一種基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法
【專利摘要】本發(fā)明公開了一種基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法，通過計算約束矩陣CVF，復(fù)雜操作任務(wù)分解，最終設(shè)計約束控制器；本發(fā)明針對復(fù)雜的空間操控任務(wù)，與一般的機械臂控制方法相比較，具有任務(wù)層面和控制方法的兩大有益效果。首先，機械臂在空間中要完成的操控任務(wù)包括目標(biāo)接近、路徑跟蹤及躲避障礙等，本發(fā)明根據(jù)操作任務(wù)的不同選取合適的運動旋量集合來構(gòu)造約束矩陣，較以往方法更具有適應(yīng)復(fù)雜任務(wù)的能力；其次，非完整約束控制方法可以使操作更加靈活，操作者可以控制機械臂僅在平移或旋轉(zhuǎn)方向運動，無需提供額外的約束，從而減輕操作者壓力。
【專利說明】-種基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法 【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于空間遙操作技術(shù)領(lǐng)域，設(shè)及一種基于復(fù)雜操作任務(wù)的非完整遙操作約 束控制方法。 【【背景技術(shù)】】
[0002] 在諸多機器人領(lǐng)域中已經(jīng)證明了虛擬夾具在任務(wù)執(zhí)行時間及總體精準(zhǔn)度方面都 有提高，然而夾具也是非靈敏型的輔助，如在空間遙操作過程中，需要通過虛擬夾具對機械 臂末端的位置及姿態(tài)同時進行控制，在保證位置控制最優(yōu)的情況下往往會失去對末端姿態(tài) 的準(zhǔn)確控制。為了在空間遙操作中實現(xiàn)良好的操作效果，需要給機械臂末端添加合適的輔 助，使其靈活的根據(jù)任務(wù)環(huán)境及操作任務(wù)進行位置和姿態(tài)協(xié)調(diào)。
[0003] 在非結(jié)構(gòu)化環(huán)境中，機器人的運動受到幾何及動力學(xué)約束，平移和旋轉(zhuǎn)自由度受 到限制。在空間遙操作任務(wù)過程時，機械臂末端一般只在平動方向約束情況下便能完成操 作任務(wù)，然而針對一些復(fù)雜的空間操控任務(wù)，僅僅平動方向的約束限制難W保證操作的高 效性和安全性，故要求在機械臂末端旋轉(zhuǎn)方向也添加輔助作用，使操作者能夠動態(tài)的根據(jù) 實際操作環(huán)境改變平動和旋轉(zhuǎn)方向的虛擬夾具輔助效果，從而更好的完成遙操作任務(wù)。 【
【發(fā)明內(nèi)容】
】
[0004] 本發(fā)明的目的在于克服上述現(xiàn)有技術(shù)的缺點，提供一種基于復(fù)雜操作任務(wù)的非完 整遙操作約束控制方法。
[0005] 為達到上述目的，本發(fā)明采用W下技術(shù)方案予W實現(xiàn)：
[0006] -種基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法，包括W下步驟：
[0007] 1)計算約束矩陣Cvf;
[000引定義約束矩陣Cvf表示對應(yīng)的約束能力，且Cvf是6X6半正定對稱矩陣，并將空間作 用力映射成Cvf的矩陣元素;操作者控制機械臂末端向著最優(yōu)方向和非最優(yōu)方向的運動構(gòu)成 了空間幾何約束，且都由S e ( 3 )中的單位旋量表示，給出旋量集合S :
弦m個rank-1的半正定矩陣之和：
[0010] 其中，正比例系數(shù)Cl代表了每個約束矩陣Cl的約束能力大小，強約束使操作者控制 機械臂末端沿著最優(yōu)方向運動，弱約束使沿著非最優(yōu)方向發(fā)生偏離；
[0011]操作者控制力巧=(心，/6)的大小決定了機械臂末端在旋轉(zhuǎn)和平移方向發(fā)生的位 移變化時b= Snb) _有
[001。 （2)
[0013]其中，巧表示作用在機械臂末端的操作者控制力，姑b表示旋轉(zhuǎn)和平移方向的微小 位移;虛擬夾具輔助下的完整約束等同于約束矩陣C為滿秩矩陣，而非完整約束則對應(yīng)約束 矩陣C為非滿秩矩陣；
[0014] 2)復(fù)雜操作任務(wù)分解；
[0015] 遙操作中的復(fù)雜操作任務(wù)能夠分解成多個任務(wù)的組合;針對幾個不同的任務(wù)，選 取最優(yōu)的運動旋量集合；
[0016] 3)約束控制器設(shè)計；
[0017] 設(shè)機械臂的空間運動速度為yb=( cAyb)，其中wb、vb分另懐肅肢性空間中機械臂 的旋轉(zhuǎn)角速度和切向速度;控制器設(shè)計如下：
[001引
巧）
[0019] 其中，約束矩陣CvF(g，gd)表示對機械臂的約束能力，操作者控制力定義為Fb=(mb, fb)，比例系數(shù)Kc = diagkrl3X3，Cpl3X3)控制操作者約束作用力的大小。
[0020] 2.根據(jù)權(quán)利要求1所述的基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法，其特 征在于，所述步驟2)中，選取最優(yōu)的運動旋量集合的具體方法如下：
[0021] 2-1)目標(biāo)接近
[0022] 設(shè)機械臂在位形空間中的參考軌跡為gd，其當(dāng)前位形為g，則距離參考軌跡的誤差 表不為
[0023] Oj
[0024] 取誤差的對數(shù)表達，有
[0025] (4)
[00%] 對應(yīng)的約巧矩陣Cvf = CS由運動旋量集合構(gòu)成，C的大小決定了約束作用力的強弱；
[0027] 2-2)軌跡跟蹤
[002引設(shè)機械臂運動的參考軌跡為gr(A，t)，機械臂運動的軸線為滬+ls:leR}，r為運 動軸線上任意一點，A為平動方向的進動參數(shù)，M S M =1，則相對應(yīng)的切向速度為
[0029]
(5)
[0030] 記切向速度為單位旋量集合Si, S2為誤差的對數(shù)形式：
[0031] S^=IogC?,'.?) 貨)
[0032] 則約束矩陣為
[0033] CvF = ciSi+C2S2 (7)
[0034] 其中，正比例系數(shù)Cl的大小決定了機械臂受虛擬夾具約束的運動速度大小，且Cl越 大對應(yīng)的運動速度會減小，C2的大小決定了機械臂跟蹤參考軌跡的能力；
[0035] 2-3)平面運動
[0036] 假設(shè)機械臂末端的運動速度向量為Vi和V2,則span {VI, V2}是指Wvi, V2為基生成的 平面，操作者控制機械臂在該二維平面內(nèi)運動，定義運動旋量C = (? ,V)，當(dāng)沿著Vl方向運 動時，取W = S = 0，則Vl方向的運動旋量集合為Sl =(0，Vl )，同理V2方向的運動旋量集合為S2 = (0，V2)，則機械臂在約束作用下沿軸向s運動的旋量集合為S3 = (s，rXs)，r為s上任意一 點' ；
[0037] 從而，約束矩陣定義為
[003引 CvF = ClSl+C2S2+C3S3 (8)
[0039] 其中，Cl的大小決定機械臂沿Vl方向運動的能力，C2的大小決定機械臂沿V2方向運 動的能力，C3的大小決定機械臂在約束軸向S運動的能力，其中包括繞軸向轉(zhuǎn)動的約束作 用。
[0040] 與現(xiàn)有技術(shù)相比，本發(fā)明具有W下有益效果：
[0041] 本發(fā)明針對復(fù)雜的空間操控任務(wù)，與一般的機械臂控制方法相比較，具有任務(wù)層 面和控制方法的兩大有益效果。首先，機械臂在空間中要完成的操控任務(wù)包括目標(biāo)接近、路 徑跟蹤及躲避障礙等，本發(fā)明根據(jù)操作任務(wù)的不同選取合適的運動旋量集合來構(gòu)造約束矩 陣，較W往方法更具有適應(yīng)復(fù)雜任務(wù)的能力;其次，非完整約束控制方法可W使操作更加靈 活，操作者可W控制機械臂僅在平移或旋轉(zhuǎn)方向運動，無需提供額外的約束，從而減輕操作 者壓力。 【【附圖說明】】
[0042] 圖1約束功率原理圖。
[0043] 圖2平面約束運動示意圖。
[0044] 圖3球面及其切平面示意圖。
[0045] 圖4球面截面操作示意圖。 【【具體實施方式】】
[0046] 下面結(jié)合附圖對本發(fā)明做進一步詳細描述：
[0047] 參見圖1-4,基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法，其特征在于，包括 W下步驟：
[004引步驟一:計算約束矩陣CvF。
[0049] 操作者控制機械臂在空間環(huán)境中完成復(fù)雜任務(wù)，要求對機械臂末端控制力進行有 效約束作用，運樣的一種約束作用構(gòu)成虛擬夾具，定義約束矩陣Cvf表示對應(yīng)的約束能力，且 Cvf是6X6半正定對稱矩陣，并將空間作用力映射成Cvf的矩陣元素。操作者控制機械臂末端 向著最優(yōu)方向和非最優(yōu)方向的運動構(gòu)成了空間幾何約束，且都可W由se(3)中的單位旋量 表示，給出旋量集合S:
'^=I,2,…，6，則媒可W分解成m個rank-1的半正定
矩陣之
[0050] (1)
[0051 ] 其中，正比例系數(shù)Ci代表r每個約束矩陣Ci的約束能力大小，強約束使操作者控制 機械臂末端沿著最優(yōu)方向運動，弱約束使沿著非最優(yōu)方向發(fā)生偏離。運樣的運動優(yōu)先級區(qū) 分降低了操作者的操作壓力，即操作者只需控制機械臂在虛擬夾具約束作用范圍內(nèi)運動。 [0化2]操作者控制力巧=如6.，/)的大小決定了機械臂末端在旋轉(zhuǎn)和平移方向發(fā)生的位 移變化姑b=(Sd)b，Sqb)，有
[0053]
(2)
[0054] 其中，巧表示作用在機械臂末端的操作者控制力，姑b表示旋轉(zhuǎn)和平移方向的微小 位移。虛擬夾具輔助下的完整約束等同于約束矩陣C為滿秩矩陣，而非完整約束則對應(yīng)約束 矩陣C為非滿秩矩陣。
[0055] 步驟二:復(fù)雜操作任務(wù)分解。
[0056] 基于上述理論知識，遙操作中的復(fù)雜操作任務(wù)可W分解成多個簡單任務(wù)(如目標(biāo) 接近、路徑跟蹤、躲避障礙等）的組合。針對幾個不同的簡單任務(wù)，選取最優(yōu)的運動旋量集 合：
[0057] (1)目標(biāo)接近
[0058] 設(shè)機械臂在位形空間中的參考軌跡為gd，其當(dāng)前位形為g，則距離參考軌跡的誤差 表示為
[0059] (3)
[0060] ^取誤差的對數(shù)表達，有
[006。 （4)
[0062] 對應(yīng)的約束矩陣Cvf = CS由運動旋量集合構(gòu)成，C的大小決定了約束作用力的強弱， 操作者只需按照虛擬夾具提供的約束作用力隨動完成操作任務(wù)即可，不需要提供額外的控 制力。
[0063] (2)軌跡跟蹤
[0064] 設(shè)機械臂運動的參考軌跡為g;r(>,t)，機械臂運動的軸線為.《=貧，r為運 動軸線h件意一點，A為平動方向的進動參數(shù)，I IsM =1，則相對應(yīng)的切向速度為
[0065； 巧
[0066； I旋量集合Si，S2同4.3.1節(jié)中一樣，定義為誤差的對數(shù)形式
[0067； (6)
[0068] 則約束矩陣為
[0069] CvF = ciSi+C2S2 (7)
[0070] 其中，正比例系數(shù)Cl的大小決定了機械臂受虛擬夾具約束的運動速度大小，且Cl越 大對應(yīng)的運動速度會減小，C2的大小決定了機械臂跟蹤參考軌跡的能力。
[0071] (3)平面運動
[0072] 假設(shè)機械臂末端的運動速度向量為Vi和V2,則span {VI, V2}是指Wvi, V2為基生成的 平面，操作者控制機械臂在該二維平面內(nèi)運動，如圖2所示，定義運動旋量C = (?，v)，當(dāng)沿 著Vl方向運動時，取O = S = 0，則vi方向的運動旋量集合為Sl = (0，Vl )，同理V2方向的運動旋 量集合為S2=(0，V2)，則機械臂在約束作用下沿軸向S運動的旋量集合為S3=(s，rXs)，r為 S上任意一點。
[0073] 從而，約束矩陣可定義為
[0074] CvF = ClSl+C2S2+C3S3 (8)
[0075 ]其中，C1的大小決定機械臂沿Vi方向運動的能力，C2的大小決定機械臂沿V2方向運 動的能力，C3的大小決定機械臂在約束軸向S運動的能力，其中包括繞軸向轉(zhuǎn)動的約束作 用。
[0076] 步驟S:約束控制器設(shè)計。
[0077] 操作者控制機械臂完成給定任務(wù)時，需要機械臂在旋轉(zhuǎn)和平移方向都保持在穩(wěn)定 的速度闊值內(nèi)，基于此，設(shè)機械臂的空間運動速度為Vb=(cAvb)，其中cAvb分別表示慣性 空間中機械臂的旋轉(zhuǎn)角速度和切向速度。控制器設(shè)計如下：
[007引 ㈱
[0079] 共T，巧氷W件LVKg,gd;表示對機械臂的約束能力，操作者控制力定義為Fb= (mb, fb)，比例系數(shù)Kc = diag(Crl3X3，Cpl3X3)控制操作者約束作用力的大小，由于強約束使機械臂 沿著最優(yōu)方向運動速度較快，為防止速度過快導(dǎo)致碰撞等意外情況的發(fā)生，選取合適的。、 Cp使操作者約束力施加的大小得W控制，防止機械臂因失去控制而導(dǎo)致任務(wù)的失敗。
[0080] 本發(fā)明的原理：
[0081] 經(jīng)典分析力學(xué)中，約束可分為完整約束和非完整約束?？臻g遙操作中機械臂在操 作者控制下能否實現(xiàn)給定的運動，與約束控制力的完整性和簡化操作任務(wù)等有關(guān)。在非完 整約束情況下，即對機械臂進行部分約束限制，具體表現(xiàn)為參考軌跡的操作自由度小于機 械臂位形空間的維數(shù)，如在二維曲面上控制機械臂末端從A點運動至B點，給定任務(wù)序列Tr ([mx，my,mz] ,Pr)，其中（mx，my,mz)指的是任務(wù)序列上的任一點坐標(biāo)，Pr表示約束功率，如圖1 所示，對于被約束對象是否能實現(xiàn)給定的運動，其判別方法通常是考察約束力旋量鄭'與給 定速度旋量WkS的乘積即瞬時約束功率(簡稱約束功率)是否為零，若約束功率為零則該運動 為約束允許的運動。
[0082] 為了驗證所設(shè)計控制器在變化操作環(huán)境中的對機械臂的有效約束作用，設(shè)計動態(tài) 軌跡跟蹤實驗如下：
[0083] (1)實驗中，操作者控制末端操作工具沿參考軌跡運動，并設(shè)計控制末端所在的操 作環(huán)境是動態(tài)變化的。在動態(tài)軌跡跟蹤實驗中，首先設(shè)置實驗環(huán)境(動態(tài)變化球面）的動態(tài) 變化頻率為O (0~45rad/s)，設(shè)參考軌跡為球面上r = 50mm的締線圓（球體半徑為ro = IOOmm)，如圖3所示，
[0084] (2)機械臂末端在由起始點向著目標(biāo)點運動時，運動旋量集合由兩部分組成，記切 向速度為單位旋量集合Si，S2定義為誤差的對數(shù)形式- k，g.化i,g)，則約束矩陣為
[00 化]CvF = ClSl+C2S2
[0086] 其中，令Cl = 0.6,C2 = 0.8,正比例系數(shù)Cl的大小決定了機械臂受虛擬夾具約束下 的運動速度大小，C2的大小決定了機械臂跟蹤參考軌跡的能力。
[0087] (3)在二維曲面中，使約束功率Pr = O的操作狀態(tài)是控制機械臂末端沿某一軸向運 動（如Z軸），而圍繞Z軸的所有轉(zhuǎn)動對Z方向的運動沒有約束影響，則機械臂末端可沿著Z軸 做連續(xù)運動。
[008引設(shè)定變化的球體半徑為r = ；r0+Ar sinot，其中Ar = 20mm。比例系數(shù)Kc = diag (Crl3X3,Cpl3X3)控制操作者約束作用力的大小，試驗過程中，選取Cr = O. Irad/s,Cp=IOOmm/ S，操作者控制末端操作工具從起始位置go向著參考軌跡gd運動，如圖4所示為球面直徑方向 截平面操作示意圖：
[0089] 實驗結(jié)果表明，在變化環(huán)境中，操作者根據(jù)不同操作任務(wù)選取相應(yīng)的運動旋量集 合構(gòu)造約束矩陣，從而可W很好的約束末端操作工具沿著指定的方向運動。
[0090] W上內(nèi)容僅為說明本發(fā)明的技術(shù)思想，不能W此限定本發(fā)明的保護范圍，凡是按 照本發(fā)明提出的技術(shù)思想，在技術(shù)方案基礎(chǔ)上所做的任何改動，均落入本發(fā)明權(quán)利要求書 的保護范圍之內(nèi)。
【主權(quán)項】
1. 一種基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法，其特征在于，包括W下步驟： 1) 計算約束矩陣Cvf; 定義約束矩陣Cvf表示對應(yīng)的約束能力，且Cvf是6 X 6半正定對稱矩陣，并將空間作用力 映射成Cvf的矩陣元素;操作者控制機械臂末端向著最優(yōu)方向和非最優(yōu)方向的運動構(gòu)成了空 間幾何約束，且都由S e ( 3 )中的單位旋量表示，給出旋量集合S : S = WhA = W，《,T_r，!' = 1，2，。.，6，則Cf';分解成m個rank-1的半正定矩陣之和：(1) 其中，正比例《數(shù)Ci代表/每個約巧矩陣Ci的約束能力大小，強約束使操作者控制機械 臂末端沿著最優(yōu)方^々束使沿著非最優(yōu)方向發(fā)生偏離； 操作者控制；9大小決定了機械臂末端在旋轉(zhuǎn)和平移方向發(fā)生的位移變 化姑6= (Sd) b，Sqb),右(2) 其中，持表示作用在機械臂末端的操作者控制力，SXb表示旋轉(zhuǎn)和平移方向的微小位 移;虛擬夾具輔助下的完整約束等同于約束矩陣C為滿秩矩陣，而非完整約束則對應(yīng)約束矩 陣C為非滿秩矩陣； 2) 復(fù)雜操作任務(wù)分解； 遙操作中的復(fù)雜操作任務(wù)能夠分解成多個任務(wù)的組合;針對幾個不同的任務(wù)，選取最 優(yōu)的運動旋量集合； 3) 約束控制器設(shè)計； 設(shè)機械臂的空間運動速度為yb=( C〇b，vb)，其中c〇b、vb分別表示慣性空間中機械臂的旋 轉(zhuǎn)角速度和切向速度，梓制盤期i+々n下，(9) 其中，約束矩陣CvF(g，gd)表示對機械臂的約束能力，操作者控制力定義為Fb=(mb，fb)， 比例系數(shù)Kc = diag(Crl3X3，Cpl3X3)控制操作者約束作用力的大小。2. 根據(jù)權(quán)利要求1所述的基于復(fù)雜操作任務(wù)的非完整遙操作約束控制方法，其特征在 于，所述步驟2)中，選取最優(yōu)的運動旋量集合的具體方法如下： 2-1)目標(biāo)接近設(shè)機械臂在位形空間中的參考軌跡為gd，其當(dāng)前位形為g，則距離參考軌跡的誤差表示 為 (3) 對應(yīng)的運動旋 ；，有 (4) 對應(yīng)的約束矩陣Cvf = cS由運動旋量集合構(gòu)成，C的大小決定了約束作用力的強弱； 2-2)軌跡跟蹤 設(shè)機械臂運動的參考軌跡為grO，t)，機械臂運動的軸線為/ = -!f +J.S': i m，r為運動軸 線上任意一點，人為平動方向的進動參數(shù)，I |s I I =1，則相對應(yīng)的切向速度為巧 記切向速度為單^ I誤差的對數(shù)形式： 樹 則約束矩陣為 CvF=ClSl+C2S2 (7) 其中，正比例系數(shù)Cl的大小決定了機械臂受虛擬夾具約束的運動速度大小，且Cl越大對 應(yīng)的運動速度會減小，C2的大小決定了機械臂跟蹤參考軌跡的能力； 2-3)平面運動 假設(shè)機械臂末端的運動速度向量為Vi和V2,則span {Vi ,V2}是指Wvi, V2為基生成的平 面，操作者控制機械臂在該二維平面內(nèi)運動，定義運動旋量C = (? ,V)，當(dāng)沿著Vl方向運動 時，取W = S = O，則Vl方向的運動旋量集合為Sl = (O，Vl )，同理V2方向的運動旋量集合為S2 = (0，V2)，則機械臂在約束作用下沿軸向s運動的旋量集合為S3=(s，rXs)，r為s上任意一點； 從而，約束矩陣定義為 Cvf= C1S1+C2S2+C3S3 (8) 其中，Cl的大小決定機械臂沿Vl方向運動的能力，C2的大小決定機械臂沿V2方向運動的 能力，C3的大小決定機械臂在約束軸向S運動的能力，其中包括繞軸向轉(zhuǎn)動的約束作用。
【文檔編號】B25J9/16GK105904458SQ201610323695
【公開日】2016年8月31日
【申請日】2016年5月16日
【發(fā)明人】黃攀峰, 潘吉祥, 劉正雄, 孟中杰
【申請人】西北工業(yè)大學(xué)