基于連桿變換矩陣的空間機械臂運動誤差分析及辨識

劉 茜,孫 軍,劉傳凱,謝 圓,張 寬,成子青

(1.中國科學院自動化研究所,北京 100190; 2.北京航天飛行控制中心,北京 100094;3.航天飛行動力學技術重點實驗室,北京 100094)

1 引言

空間機械臂承擔著艙外設備巡檢、安裝與更換,輔助航天員出艙和航天器回收與釋放等在軌任務。目前,空間機械臂的任務規劃和控制通過地面遙操作方式,同時通過遙測下傳關節角數據,掌握空間機械臂在軌狀態。準確的運動學模型是地面遙操作進行空間機械臂任務規劃的基礎。盡管在發射入軌前,空間機械臂已在地面完成標定和仿真驗證,但由于太空重力釋放和復雜在軌環境的影響,空間機械臂的運動學特性會發生改變,導致末端位姿精度降低,影響遙操作的準確性和安全性。有必要開展空間機械臂在軌辨識,獲得能夠精確反映空間機械臂在軌運行狀態的運動學模型,提高末端位姿精度,支持在軌任務的順利開展。

國內外研究人員針對機械臂運動學參數辨識開展了大量研究工作,主要歸納為基于DH模型和基于指數積公式的運動學參數辨識方法[1]。早期的運動學參數辨識大多采用基于DH 模型的方法,但傳統DH 模型在處理相鄰平行關節時會出現奇異性問題。Hayati[2]提出了一種改進DH 模型,在相鄰平行關節處引入繞y軸轉動的參數β,有效解決了上述奇異性問題。以改進DH 模型為基礎,Zhuang 等[3-4]提了基于關節軸線單位向量表示的CPC 模型和基于關節姿態角度表示的MCPC 模型。Ma等[5]進一步對機械臂運動學誤差來源進行了分析和建模,推導了誤差傳遞方程。不同于一次性辨識全部運動學參數的方法,Jiang 等[6]提出了多維度辨識空間的概念,將不同量級的誤差參數進行分組,分別辨識各組誤差參數,以提高辨識精度。另一方面,基于指數積的運動學模型利用旋量描述運動,避免了DH 模型的奇異性問題。根據參考坐標系的不同,指數積的運動參數辨識方法又可分為基于全局指數積的辨識[7-8]和基于關節本體指數積的辨識[9-10]。盡管基于DH 模型和基于指數積公式的辨識方法得到的誤差模型各不相同,但其核心思想都是通過末端位姿誤差求解關節誤差。

為保證誤差模型求解過程的穩定性,廣泛采用的方法是排除耦合運動誤差參數,僅針對可辨識的運動誤差參數進行辨識。Everett等[11]證明了R個轉動關節和P個平動關節的機械臂具有(4R+2P+6)個可辨識的運動誤差參數。但耦合參數對末端位姿誤差的影響是不可忽略的,有必要分析運動誤差參數,尤其是具有耦合特性的誤差參數,評估其對末端位姿誤差的影響。

從遙操作實踐的角度考慮,現有的研究方法并未提供辨識構型優選的理論依據,沒有解決辨識構型如何設計以及需要設計多少個辨識構型的具體問題。不同辨識構型下的末端位姿誤差再現運動學誤差的能力不同[12],有必要分析并建立辨識構型的優化目標,以提高空間機械臂運動誤差參數的辨識精度。

針對空間機械臂由于太空重力釋放和復雜在軌環境影響導致的運動學參數與地面重力環境不一致的問題,本文提出一種基于連桿變換矩陣的運動誤差參數辨識方法。通過運動誤差傳遞方程的建立及對運動誤差參數的分析,研究關節誤差參數耦合關系以及關節誤差對末端位姿誤差映射關系;設計基于辨識矩陣及避障空間約束等條件下的最優辨識構型選取原則,實現對運動誤差參數的準確辨識及空間機械臂在軌運動的準確建模,從而有效提高地面遙操作對空間機械臂末端位姿的精確仿真和高精度控制。

2 空間機械臂運動誤差傳遞方程

2.1 運動學模型

本文研究的空間機械臂是具有N自由度(N≥6)的串聯機械臂。根據改進DH 參數[13]構建空間機械臂連桿坐標系,連桿兩端相鄰坐標系的變換矩陣如式(1)所示:

式中,i表示第i個連桿,i =1,2,…,N;αi-1表示沿xi-1軸,從zi-1軸旋轉到zi軸的角度;ai-1表示沿xi-1軸,從zi-1軸移動到zi軸的距離;di表示沿zi軸,從xi-1軸移動到xi軸的距離;θi表示沿zi軸,從xi-1軸旋轉到xi軸的角度。

空間機械臂末端工具坐標系相對于基坐標系的變換矩陣0TTool可以通過將連桿坐標系的變換矩陣依次相乘得到,如式(2)所示:

式中,NTN+1表示工具坐標系相對空間機械臂末端坐標系的變換矩陣。

2.2 運動誤差傳遞方程

已知連桿坐標系變換矩陣的理論值為(i =1,2,…,N +1),則真實值i-1Ti可表示為理論值與誤差值di-1Ti之和,如式(3)所示:

其中,i-1Δ 表示誤差矩陣[14],見式(4):

式中,i-1δ和i-1m分別表示姿態誤差矢量和位置誤差矢量,i-1δ=[i-1δx,i-1δy,i-1δz]T,i-1m=[i-1mx,i-1my,i-1mz]T;S(δ)表示矢量δ的反對稱矩陣,見式(5):

空間機械臂末端工具坐標系變換矩陣的真實值0TTool可表示為式(6):

忽略高階誤差項,公式(6)可推導出式(7):

其中,平移矢量P表示為P=[px,py,pz]T;旋轉矩陣R表示為式(9):

進一步將公式(7)寫為多元線性方程組的形式,得到從關節位姿誤差到末端位姿誤差的運動誤差傳遞方程,如式(10)所示:

式中,0ΓTool和Γ分別表示末端位姿誤差向量和關節位姿誤差向量,0ΓTool= [0mTool,0δTool]T,Γ=[0Γ,1Γ,…,N-1Γ,NΓ]T;i-1Γ表示第i個關節的誤差向量,i-1Γ=[i-1m,i-1δ]T(i=1,2,…,N+1);JΓ表示關節位姿誤差雅可比矩陣,JΓ=[0JΓ,1JΓ,…,N-1JΓ,NJΓ] ;i-1JΓ表示第i個關節的位姿誤差雅可比矩陣,如式(11)所示:

3 空間機械臂運動誤差分析及辨識構型優選

3.1 關節誤差參數可辨識性分析

關節誤差參數之間的相互獨立性決定了各個誤差參數的可辨識性,而參數獨立性取決于誤差傳遞方程中關節位姿誤差雅可比矩陣中列向量的相關性。本文將通過推導關節位姿誤差雅可比矩陣的列向量相關性,對關節位姿誤差參數的可辨識性展開分析。

式中,i-1fk(i =1,2,3,…,N;k =1,2,…,6)表示關節位姿誤差雅可比矩陣的6 個列向量,如式(13)所示:

在DH 參數中,和均為常數,空間機械臂第i +1 關節的位姿誤差雅可比矩陣第三列向量iJΓ3與第i關節的位姿誤差雅可比矩陣第二列向量i-1JΓ2和第三列向量i-1JΓ3存在線性相關關系;第i +1 關節的位姿誤差雅可比矩陣第六列向量iJΓ6與第i關節的位姿誤差雅可比矩陣第二列向量i-1JΓ2、第三列向量i-1JΓ3、第五列向量i-1JΓ5和第六列向量i-1JΓ6存在線性相關關系。

空間機械臂第一關節的位姿誤差向量具有4 個獨立誤差參數和2 個耦合誤差參數,最后一個關節的位姿誤差向量具有4 個獨立誤差參數。除第一關節和最后一個關節外,空間機械臂的任一關節位姿誤差向量具有2 個獨立誤差參數和2 個耦合誤差參數,且耦合誤差參數與上一關節的誤差參數存在線性相關關系。從關節位姿誤差雅可比矩陣列向量相關性的角度得到了與Everett 等[11]推導的(4R+2P+6)個可辨識元素相同的結論。

消除位姿誤差雅可比矩陣的線性相關列向量,公式(10)可改寫為式(14):

式中,J*Γ表示改寫后的關節位姿誤差雅可比矩陣,有J*Γ= [0JΓ,1J*Γ,…,N-1J*Γ,NJ*Γ] ,且各關節的位姿誤差雅可比矩陣i-1J*Γ=[i-1JΓ1,i-1JΓ2,i-1JΓ4,i-1JΓ5](i =2,3,…,N +1);Γ*∈R(6+4N)×1表示改寫后的關節位姿誤差向量,是由6N個位姿誤差參數構成的具有(6+4N)行元素的列向量,有Γ*= [0Γ*,1Γ*,…,N-1Γ*,NΓ*]T;i-1Γ*表示第i個關節改寫后的誤差向量,見式(15):

根據最小二乘法原理,得到式(16):

式中,K表示用于辨識的測量數據量。通過相同辨識構型重復測量的方式[15]考慮隨機誤差的影響,則對于N自由度空間機械臂,僅需要設計(6N)/N=6 組辨識構型對誤差參數進行辨識,即K =6k,k表示相同辨識構型下的重復測量次數;J*Γi和0ΓTooli分別表示第i次測量得到的關節位姿誤差雅可比矩陣和末端位姿誤差向量,i =1,2,…,K;J表示關節位姿誤差參數的辨識矩陣,J=[(J*Γ1)T(J*Γ2)T…(J*ΓK)T]T。通過測量多組的關節構型和末端位姿誤差,可實現對空間機械臂關節位姿誤差參數的辨識。

3.2 辨識構型優選

最小二乘法辨識結果的精度取決于辨識矩陣J的選取,而辨識矩陣由辨識構型計算得到。因此,辨識構型的優選有利于提高最小二乘法的辨識精度,從而提高誤差參數辨識結果的準確性。辨識構型優選問題可以描述為如式(17)所示的優化問題:

辨識構型優選問題不需要考慮測量隨機誤差的影響,因此取重復測量次數k =1,則K =6;FK(·)表示空間機械臂從關節空間到笛卡爾空間的正運動學方程;θ表示空間機械臂關節角構型,θ= [θ1,θ2,…,θN]T;xtool表示末端在基坐標系下表示的位姿;χmea表示具備末端位姿測量條件的位姿集合;θimin和θimax分別表示第i關節的最小值和最大值;χrobot表示空間機械臂外包絡空間;χobs表示障礙空間。

上述優化問題可通過基于改進粒子群優化算法求解[16],求解流程如圖1 所示。

圖1 改進粒子群優化算法求解流程圖Fig.1 The flow chart of improved particle swarm optimization algorithms

改進粒子群優化算法中對粒子適應度Wi的定義為式(18):

式中,i為在總體為N的粒子群中的第i個粒子。

算法中的速度Vi和粒子Yi的更新分別由式(19)和(20)實現,即:

式中,u為迭代次數;c1和c2為加速度常數,一般取c1=c2=[1,2.5] ,本文中取c1=c2=1.5;pBesti和gBesti分別為粒子i的個體極值和整個粒子群的全局極值。

隨后需要進行粒子間基因的交叉和變異操作。粒子Ykj與粒子Ylj之間進行交叉操作以及粒子Yij發生基因變異操作的定義分別由式(21)和(22)實現,即

式中,e為[0,1] 間的隨機數;Ymax和Ymin分別為粒子Yij的上界和下界;g(u)為迭代次數u和最大迭代次數umax的函數,即g(u)= r2(1-u/umax)2;r和r2為[0,1] 間的隨機數。

4 仿真驗證

為驗證本文提出的運動誤差辨識方法的有效性,搭建了七自由度空間機械臂仿真模型,坐標系關系如圖2 所示。空間機械臂運動學參數的理論值如表1 所示[17]。

表1 空間機械臂運動學參數理論值Table 1 Theoretical values of kinematic parameters

圖2 七自由度空間機械臂DH 坐標系Fig. 2 DH coordinates frames of the 7 - DOF space manipulator

空間機械臂各個關節的誤差由姿態誤差矢量和位置誤差矢量組成,仿真預設的誤差參數如表2 所示,誤差參數的量級參考文獻[18]。

表2 空間機械臂仿真預設誤差參數Table 2 Presupposed errors of space manipulator

一般辨識構型表示通過試湊法選的一組關節運動在[- 120°,120°] 范圍內的辨識構型(表3)。本文采用的試湊原則為:首先根據關節角度運動范圍隨機選取關節構型;在此基礎上驗證空間機械臂是否與障礙空間發生碰撞,如果所選構型下空間機械臂發生碰撞,則需要重新進行關節構型的隨機選取,直到所選構型滿足碰撞規避。

表3 空間機械臂一般辨識構型Table 3 Identifiable configuration of space manipulator單位:(°)

同時,根據3.2 節提出的辨識構型優選原則,采用基于改進粒子群優化算法對上述空間機械臂進行求解,得到關節運動在[- 120°,120°] 范圍內的最優辨識構型,如表4 所示。

表4 空間機械臂最優辨識構型Table 4 Optimized identifiable configuration of space manipulator單位:(°)

根據3.1 節的分析,上述七自由度空間機械臂的關節位姿誤差向量Γ*具有(6 + 4 × 7)=34個元素。第一個關節位姿誤差向量由6 個元素組成,表示為0Γ*= [0mx,0my,0mz +1mz,0δx,0δy,0δz +1δz]T,其中包含4 個獨立誤差參數和2 個耦合誤差參數;末端位姿誤差向量由4 個獨立誤差參數組成,表示為7Γ*= [7mx,7my,7δx,7δy]T;其他關節位姿誤差各由2 個獨立誤差參數和2 個耦合誤差參數組成,見式(23):

基于最小二乘法原理辨識得到上述20 個獨立誤差參數和14 個耦合誤差參數的辨識結果,如表5 所示。其中,辨識值的誤差γerror見式(24):

表5 誤差參數辨識結果Table 5 Identification of error parameters

式中,γreal和γsim分別表示預設真實值和仿真辨識值。

如表5 中所示,一般辨識構型下得到的辨識結果精度較差,平均誤差為11.95%,最大誤差為70.83%;最優辨識構型下的辨識結果的精度顯著提高,平均誤差為1.11%,最大誤差為6.67%。上述結果驗證了本文提出的運動誤差分析方法及辨識構型優選方法的有效性。

在僅有關節構型和末端位姿數據的情況下,耦合誤差中的參數不能唯一確定。但由于耦合誤差參數中各個參數的選取不影響機械臂末端位姿的精度,因此,基于以最優辨識構型得到辨識結果,本文給出一種可能的空間機械臂辨識誤差參數,如表6 所示。

表6 空間機械臂辨識誤差參數Table 6 Identification errors of space manipulator

采用梯形速度算法[19]規劃空間機械臂從初始構型[10,-100,80,100,-80,90,5]運動到目標構型[60,30,20,20,30,75,15]的軌跡。運動誤差補償前后的末端位姿曲線如圖3 所示。

圖3 空間機械臂末端位姿及誤差圖Fig.3 Pose and pose error of space manipulator in conditions with or without compensation

運動誤差補償前,空間機械臂末端位姿精度較差,最大位置誤差達到25 mm,最大姿態誤差達到0.45°。采用本文提出的方法對空間機械臂進行運動誤差參數辨識,提高了空間機械臂的末端位姿精度,最大位置誤差減小到1 mm 以下,最大姿態誤差減小到0.1°以下。綜上,通過七自由度空間機械臂運動仿真,驗證了基于連桿變換矩陣的空間機械臂運動誤差辨識方法的有效性和準確性。

5 結論

1)基于運動誤差傳遞方程,分析了關節運動誤差到末端運動誤差的傳導機理,開展了運動誤差參數的可辨識性分析,實現了耦合運動誤差參數對末端位姿誤差的線性表達。

2)提出了空間機械臂辨識構型優選原則,通過改進粒子群算法實現對空間機械臂最優辨識構型的求解。

3)通過七自由度空間機械臂的運動仿真實例,驗證了本文所提出的基于連桿變換矩陣的運動誤差參數辨識方法的有效性,該方法能夠有效提高空間機械臂運動學模型的準確性。

4)本文仿真驗證基于理想的末端位姿測量結果,并未考慮測量噪聲的影響。另一方面,盡管耦合運動誤差中各個參數并不直接影響空間機械臂末端位姿精度,但會影響動力學仿真的精度。因此,未來將進一步考慮測量噪聲的影響,同時開展耦合運動誤差的分析,以期得到更加準確的運動學模型。