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

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

1 引言

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

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

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

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

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

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

2.1 運動學(xué)模型

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

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

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

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

2.2 運動誤差傳遞方程

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

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

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

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

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

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

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

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

3 空間機械臂運動誤差分析及辨識構(gòu)型優(yōu)選

3.1 關(guān)節(jié)誤差參數(shù)可辨識性分析

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

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

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

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

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

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

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

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

3.2 辨識構(gòu)型優(yōu)選

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

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

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

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

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

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

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

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

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

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

4 仿真驗證

為驗證本文提出的運動誤差辨識方法的有效性,搭建了七自由度空間機械臂仿真模型,坐標(biāo)系關(guān)系如圖2 所示?？臻g機械臂運動學(xué)參數(shù)的理論值如表1 所示[17]。

表1 空間機械臂運動學(xué)參數(shù)理論值Table 1 Theoretical values of kinematic parameters

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

空間機械臂各個關(guān)節(jié)的誤差由姿態(tài)誤差矢量和位置誤差矢量組成,仿真預(yù)設(shè)的誤差參數(shù)如表2 所示,誤差參數(shù)的量級參考文獻[18]。

表2 空間機械臂仿真預(yù)設(shè)誤差參數(shù)Table 2 Presupposed errors of space manipulator

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

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

同時,根據(jù)3.2 節(jié)提出的辨識構(gòu)型優(yōu)選原則,采用基于改進粒子群優(yōu)化算法對上述空間機械臂進行求解,得到關(guān)節(jié)運動在[- 120°,120°] 范圍內(nèi)的最優(yōu)辨識構(gòu)型,如表4 所示。

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

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

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

表5 誤差參數(shù)辨識結(jié)果Table 5 Identification of error parameters

式中,γreal和γsim分別表示預(yù)設(shè)真實值和仿真辨識值。

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

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

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

采用梯形速度算法[19]規(guī)劃空間機械臂從初始構(gòu)型[10,-100,80,100,-80,90,5]運動到目標(biāo)構(gòu)型[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,最大姿態(tài)誤差達到0.45°。采用本文提出的方法對空間機械臂進行運動誤差參數(shù)辨識,提高了空間機械臂的末端位姿精度,最大位置誤差減小到1 mm 以下,最大姿態(tài)誤差減小到0.1°以下。綜上,通過七自由度空間機械臂運動仿真,驗證了基于連桿變換矩陣的空間機械臂運動誤差辨識方法的有效性和準(zhǔn)確性。

5 結(jié)論

1)基于運動誤差傳遞方程,分析了關(guān)節(jié)運動誤差到末端運動誤差的傳導(dǎo)機理,開展了運動誤差參數(shù)的可辨識性分析,實現(xiàn)了耦合運動誤差參數(shù)對末端位姿誤差的線性表達。

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

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

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