一種基座姿態(tài)可控空間機(jī)器人的通用避奇異算法

趙龍澤，佘浩平，*，黃良偉，黃龍飛

（1.北京理工大學(xué) 宇航學(xué)院，北京 100081； 2.中國空間技術(shù)研究院 錢學(xué)森空間技術(shù)實(shí)驗(yàn)室，北京 100094）

對于基座姿態(tài)可控的關(guān)節(jié)型空間機(jī)器人，在對其進(jìn)行笛卡兒空間連續(xù)運(yùn)動(dòng)規(guī)劃的過程中，需要利用基于雅可比矩陣求逆或偽逆的速度級運(yùn)動(dòng)學(xué)求解來實(shí)現(xiàn)。然而，當(dāng)機(jī)器人經(jīng)過一些特定的臂型時(shí)，不可避免地會(huì)遇到運(yùn)動(dòng)學(xué)奇異，主要是因?yàn)檠趴杀染仃嚦霈F(xiàn)了病態(tài)，此時(shí)如果繼續(xù)采用經(jīng)典的速度級逆運(yùn)動(dòng)學(xué)求解方法，則在機(jī)械臂的奇異點(diǎn)附近會(huì)導(dǎo)致規(guī)劃關(guān)節(jié)角速度和角加速度出現(xiàn)無窮大的極端情況，機(jī)械臂會(huì)因此出現(xiàn)振動(dòng)，影響機(jī)械臂的軌跡跟蹤精度，同時(shí)也不利于機(jī)械臂的關(guān)節(jié)運(yùn)動(dòng)控制，甚至造成控制算法失效［1］。因此，尋求一種有效的回避算法是極為重要的。

早期常采用偽逆法進(jìn)行機(jī)械臂奇異回避的路徑規(guī)劃，該方法雖然能夠解決一些超定或欠定的問題，但關(guān)節(jié)角的解仍會(huì)在機(jī)械臂的奇異區(qū)內(nèi)出現(xiàn)跳變，造成較大誤差［2］。隨后Nakamura等［3］和Wampler［4］分別提出了阻尼最小方差法（DLS）和奇異性魯棒逆解（SRI）的方法。Chiaverini［5］對SRI方法進(jìn)行了改進(jìn)，采用“阻尼最小方差+數(shù)值濾波”的方法，通過判定最小奇異值施加阻尼系數(shù)，提升了規(guī)劃精度。SRI方法雖然能夠有效保障機(jī)械臂在奇異區(qū)內(nèi)關(guān)節(jié)角度的連續(xù)性和有限性，但仍存在機(jī)械臂末端跟蹤精度較差的問題，且由于需要對雅可比矩陣進(jìn)行實(shí)時(shí)的SVD分解，運(yùn)算量也較大。楊震等［6］引入雅可比矩陣的條件數(shù)作為DLS法中選取阻尼系數(shù)的衡量指標(biāo)，有效提高了求解精度，但由于其需要頻繁地對雅可比矩陣進(jìn)行奇異值分解，極大地提高了計(jì)算量。董伯麟和彭航［7］給出了雅可比矩陣條件數(shù)的一個(gè)更小的上界估計(jì)式，提高了運(yùn)算速度，但犧牲了一定的位姿精度。

Cheng等［8］以PUMA型機(jī)械臂為研究對象，提出了“奇異分離+緊密二次型規(guī)劃”（SCIQP）法，避免了對雅可比矩陣進(jìn)行實(shí)時(shí)SVD分解，有效減少了算法的運(yùn)算量。徐文福等［9］針對PUMA型機(jī)器人的特點(diǎn)，對奇異分離的算法進(jìn)行了改進(jìn)，提出了“奇異分離+阻尼倒數(shù)”的方法，采用阻尼倒數(shù)代替普通倒數(shù)，僅犧牲末端部分方向的精度，以較小的運(yùn)算量完成了奇異回避。崔洪新等［10］采用指數(shù)級阻尼倒數(shù)代替普通阻尼倒數(shù)的方法，進(jìn)一步減小了跟蹤誤差。劉成良等［11］采用協(xié)調(diào)控制的算法回避了機(jī)器人的腕部奇異。Choi等［12］引入高斯分布阻尼因子，進(jìn)一步提高了機(jī)械臂在奇異點(diǎn)附近的算法穩(wěn)定性，但該算法本身就存在一定的不連續(xù)性。吳戈等［13］對高斯分布阻尼因子進(jìn)行了改進(jìn)，減小了跟蹤誤差，但魯棒性并不高。Qiu等［14］通過引入與最小奇異值相關(guān)的修正向量，從而控制規(guī)劃角速度，其性能僅受最小奇異值下限選擇的影響，但損失了一定的實(shí)時(shí)性。Nanos和Papadopoulos［15-16］從 初 始 機(jī) 械 臂 的 關(guān) 節(jié)角和基座位姿出發(fā)，通過合理設(shè)置初值的方法避免了雅可比矩陣的奇異，但該方法未考慮基座受到擾動(dòng)的情況。

本文針對不同自由度的基座姿態(tài)可控空間機(jī)械臂，在進(jìn)行一般建模后，通過建立雅可比矩陣，進(jìn)行速度級運(yùn)動(dòng)學(xué)逆解分析，在進(jìn)行路徑規(guī)劃的同時(shí)判斷雅可比矩陣行列式的值，給出奇異判定條件，從而判定機(jī)械臂的奇異區(qū)。隨后，在奇異區(qū)內(nèi)提取各關(guān)節(jié)角的微分項(xiàng)，通過設(shè)計(jì)二次擬合方法在奇異區(qū)內(nèi)對關(guān)節(jié)角進(jìn)行分段規(guī)劃，最終實(shí)現(xiàn)了一種通用性較強(qiáng)的奇異回避算法。

1 運(yùn)動(dòng)學(xué)模型分析

1.1 基座姿態(tài)可控空間機(jī)械臂的建模

在慣性坐標(biāo)系下建立空間機(jī)械臂的一般模型，如圖1所示?？臻g機(jī)械臂由n個(gè)僅有一個(gè)自由度的旋轉(zhuǎn)關(guān)節(jié)和n+1個(gè)剛體桿件串聯(lián)而成，其中航天器平臺(tái)記為機(jī)械臂的基座，記為連桿0，連桿i為空間機(jī)械臂所對應(yīng)的第i個(gè)桿件，關(guān)節(jié)i為連接第i-1個(gè)連桿與第i個(gè)連桿的關(guān)節(jié)。為方便問題分析，可對模型做出以下幾點(diǎn)假設(shè)：①可以同時(shí)對基座姿態(tài)及各關(guān)節(jié)力矩進(jìn)行控制，實(shí)時(shí)保證基座姿態(tài)可控；②忽略模型的柔性，將模型的所有部分都視為剛體；③忽略各項(xiàng)外力的影響［17］。

圖1 空間機(jī)械臂的一般模型Fig.1 General model of a space manipulator

圖1中的符號說明如下：B0為空間機(jī)器人的基座；Bi（i=1，2，…，n）為機(jī)械臂的第i個(gè)連桿；Ji（i=1，2，…，n）為機(jī)械臂的第i個(gè)關(guān)節(jié)；Ci（i=0，1，2，…，n）為第i個(gè)連桿的質(zhì)心；OI為慣性坐標(biāo)系的原點(diǎn)；ΣI為慣性坐標(biāo)系；ΣE為機(jī)械臂末端執(zhí)行器所固連的坐標(biāo)系；Σi（i=0，1，2，…，n）為與連桿i固連的坐標(biāo)系，本文采用剛體固連坐標(biāo)系的建立原則；ki（i=1，2，…，n）為Ji旋轉(zhuǎn)方向的單位矢量；rg為空間機(jī)械臂系統(tǒng)質(zhì)心在慣性坐標(biāo)系中的位置矢量；ri（i=1，2，…，n）為Bi的質(zhì)心位置矢量；pi（i=0，1，2，…，n）為關(guān)節(jié)i的位置矢量；pe為機(jī)械臂末端位置矢量；θi（i=1，2，…，n）為關(guān)節(jié)i的關(guān)節(jié)角；Θ為由θi到θn依次排列組成的關(guān)節(jié)角向量；b0為Σ1的原點(diǎn)在Σ0坐標(biāo)系中的位置矢量；ai和bi（i=1，2，…，n）分別為從關(guān)節(jié)i指向連桿i質(zhì)心的位置矢量和從連桿i質(zhì)心指向關(guān)節(jié)i+1的位置矢量。

定義叉乘操作數(shù)：若s=［x y z］T，則

由式（1）可得，矢量叉乘運(yùn)算表示為

各連桿質(zhì)心的線速度vi可表示為

式中：v0、ω0分別為基座線速度、角速度。

機(jī)械臂的末端線速度ve可表示為

末端角速度ωe可表示為

將式（4）與式（5）聯(lián)立，可得

式中：Jb、Jm分別為基座運(yùn)動(dòng)雅可比矩陣、機(jī)械臂運(yùn)動(dòng)雅可比矩陣。

由于空間機(jī)械臂系統(tǒng)滿足線動(dòng)量守恒，則有

式中：P為系統(tǒng)的線動(dòng)量；mi（i=0，1，2，…，n）為剛體i的質(zhì)量。

將式（3）代入式（7）并整理，可得

系統(tǒng)總質(zhì)量為

式中：M為系統(tǒng)總質(zhì)量。

由假設(shè)③可知，系統(tǒng)不受任何外力，故系統(tǒng)的質(zhì)心為

將式（8）代入式（7），可得

將式（9）和式（11）代入式（8），可得系統(tǒng)的線動(dòng)量守恒方程為

式中：

由于基座姿態(tài)是指處于受控狀態(tài)，基座角速度可近似忽略，即

將式（15）代入式（12），可得

將式（15）、式（16）代入式（6），可得

采用虛擬機(jī)械臂法［18］建立基座受控空間機(jī)器人的微分運(yùn)動(dòng)方程為

聯(lián)立式（17）和式（18），可得

1.2 奇異性分析

2 基于“微分項(xiàng)提?。螖M合”的奇異回避算法

2.1 總體思路

在對機(jī)械臂進(jìn)行笛卡兒路徑規(guī)劃時(shí)，已經(jīng)預(yù)先得到每一迭代步的末端位姿狀態(tài)向量。由第1節(jié)分析可知，在機(jī)械臂進(jìn)行笛卡兒路徑跟蹤時(shí)，通過確定關(guān)節(jié)角發(fā)生跳變的臨界點(diǎn)，從而判定奇異區(qū)的關(guān)節(jié)角范圍，即可確定在雅可比矩陣不發(fā)生奇異的情況下其行列式所對應(yīng)的閾值。

在確定奇異區(qū)的關(guān)節(jié)角范圍后，實(shí)時(shí)讀取每一迭代步已預(yù)先規(guī)劃好的末端位姿狀態(tài)向量，采用基于Newton-Raphson迭代法的機(jī)械臂逆運(yùn)動(dòng)學(xué)的數(shù)值解法可以得到奇異區(qū)內(nèi)任意迭代步所對應(yīng)的關(guān)節(jié)角［19］。隨后，采用一種“微分項(xiàng)提取+二次擬合”的方法，對機(jī)械臂在奇異區(qū)內(nèi)的關(guān)節(jié)角序列進(jìn)行擬合，最終得到完整的奇異區(qū)角度序列和角速度序列。在脫離奇異區(qū)后，保持期望末端位姿不變，以當(dāng)前末端位姿矩陣為初始時(shí)刻末端位姿，重新進(jìn)行笛卡兒空間內(nèi)的連續(xù)位姿跟蹤，最終完成奇異回避任務(wù)。

2.2 機(jī)械臂逆運(yùn)動(dòng)學(xué)的數(shù)值解法

在本文所述的奇異回避算法中，需要對奇異區(qū)內(nèi)已規(guī)劃好的末端位姿進(jìn)行運(yùn)動(dòng)學(xué)求逆，得到關(guān)節(jié)角，從而為奇異區(qū)內(nèi)的關(guān)節(jié)角擬合提供輸入。

對任意自由度的空間機(jī)械臂，已知機(jī)械臂當(dāng)前時(shí)刻的關(guān)節(jié)角序列qr，即可得到其所對應(yīng)末端位姿矩陣為

假設(shè)期望末端位姿狀態(tài)變量為

可得到末端姿態(tài)矩陣為

期望末端位置矢量為

當(dāng)前時(shí)刻與終止時(shí)刻的末端位置矢量差值為

記下當(dāng)前時(shí)刻的姿態(tài)矩陣 Rr=［nrorar］與終止時(shí)刻的期望姿態(tài)矩陣Rf=［nfofaf］，計(jì)算由Rr到Rf的姿態(tài)變換的歐拉軸角參數(shù)（kf，φf），由此計(jì)算當(dāng)前時(shí)刻與終止時(shí)刻的末端姿態(tài)矢量差值Δe為

聯(lián)立式（26）、式（27）和式（20），得到基于速度級的逆運(yùn)動(dòng)學(xué)求解方程為

機(jī)械臂逆運(yùn)動(dòng)學(xué)的數(shù)值解法算法流程如圖2所示。

圖2 機(jī)械臂逆運(yùn)動(dòng)學(xué)的數(shù)值解法算法流程Fig.2 Flowchart of inverse kinematics numerical solution algorithm of manipulator

需要說明的是，對于相同的末端位姿矩陣，可能存在關(guān)節(jié)角的多解情況。本文算法所得到的關(guān)節(jié)角迭代值與給定初始值的相關(guān)程度較大，因此在使用本文算法時(shí)，應(yīng)保證在關(guān)節(jié)角初值的鄰域內(nèi)至少存在一組解，防止在迭代過程中出現(xiàn)角度突變。

2.3 奇異回避算法的實(shí)現(xiàn)流程

2.3.1 奇異區(qū)的判斷

需要指出，設(shè)定εΔq的值越大，奇異區(qū)越?。?0］。

2.3.2 進(jìn)入奇異區(qū)后的機(jī)械臂路徑規(guī)劃

步驟1 以機(jī)械臂當(dāng)前關(guān)節(jié)角為輸入，進(jìn)行笛卡兒空間連續(xù)位姿跟蹤，若不出現(xiàn)奇異，則轉(zhuǎn)步驟6，反之轉(zhuǎn)步驟2。

步驟2 判定機(jī)械臂進(jìn)入奇異區(qū)后，讀取上一迭代時(shí)刻的關(guān)節(jié)角qj-1，作為2.2節(jié)所述算法的初始關(guān)節(jié)角輸入。

步驟3 初始化機(jī)械臂末端位姿矩陣誤差閾值εT，每隔k步讀取一次奇異區(qū)內(nèi)所規(guī)劃的末端位姿狀態(tài)變量Xek，并記下相鄰2次讀取時(shí)間間隔Δt，作為2.2節(jié)所述算法的期望末端位姿矩陣輸入，可得到期望關(guān)節(jié)角qek。

將式（31）代入式（30），可得終端時(shí)刻微分項(xiàng)方程組為

式（32）提取出了期望關(guān)節(jié)角的微分項(xiàng)，在一些特殊情況下，用式（30）所得到的關(guān)節(jié)角速度曲線會(huì)出現(xiàn)龍格現(xiàn)象［21］，同樣會(huì)造成關(guān)節(jié)角突變問題，因此需要對關(guān)節(jié)角進(jìn)行二次擬合，避免此種問題的產(chǎn)生。

式中：ti為奇異區(qū)內(nèi)第i次迭代時(shí)所對應(yīng)的的仿真時(shí)間。令

假設(shè)θ″（t）在［ti，ti+1］區(qū)間內(nèi)為線性函數(shù)，則

將式（35）積分2次，可得

由式（36）可得

聯(lián)立式（36）和式（37），可得

記

聯(lián)立式（36）～式（38），可得

由式（40）和式（41）整理，可得

通過求解式（36）的方程組，即可完成所有I=［ti，ti+1］，i=0，1，2，…，l-1區(qū)間內(nèi)的關(guān)節(jié)角度、角速度和角加速度序列的求解。

需要指出，k的值越小，跟蹤精度越高，同時(shí)計(jì)算量也會(huì)相應(yīng)增大。

步驟5 記錄機(jī)械臂在奇異區(qū)內(nèi)的每一迭代步的機(jī)械臂末端位姿Td，若滿足：

則停止規(guī)劃，輸出機(jī)械臂的關(guān)節(jié)角序列。反之，規(guī)劃繼續(xù)進(jìn)行，直至離開奇異區(qū)后，轉(zhuǎn)步驟1。

步驟6 記錄全部規(guī)劃過程的關(guān)節(jié)角和關(guān)節(jié)角速度序列，完成機(jī)械臂的路徑跟蹤。

經(jīng)過上述6個(gè)步驟，可完成機(jī)械臂在笛卡兒連續(xù)位姿跟蹤過程中的奇異回避?？臻g機(jī)械臂奇異回避算法流程如圖3所示。

圖3 空間機(jī)械臂奇異回避算法流程Fig.3 Flow-chart of singularity avoidance algorithm for space manipulator

3 仿真驗(yàn)證

3.1 機(jī)械臂模型及仿真參數(shù)設(shè)置

本文以常用的6R空間機(jī)械臂為研究對象，其系統(tǒng)模型如圖4所示，圖4中坐標(biāo)系基于DH方法創(chuàng)建。

圖4 6R機(jī)械臂的結(jié)構(gòu)簡圖Fig.4 Structure of 6R manipulator

文獻(xiàn)［19］給出了其質(zhì)量特性參數(shù)，其中L1、L4、L6、L7的值分別為0.30、0.15、0.10、0.234 5 m，d1、d2、d4的值分別為0.30、0.15、0.70 m，a2、a3分別為0.83、0.03 m。

表1 6R機(jī)械臂的DH參數(shù)Table 1 DH parameters of 6R manipulator

3.2 算法校驗(yàn)

本文采用直線位姿跟蹤，以梯形速度插值規(guī)劃機(jī)械臂的末端位姿，驗(yàn)證本文算法的奇異回避效果。

設(shè)定基座姿態(tài)為ψb=［0，0，0］，初始時(shí)刻的系統(tǒng)關(guān)節(jié)角為

Θ=［47.61° 110.2° -78.07° 85.23°-30° 77.68°］

系統(tǒng)初始時(shí)刻的末端位姿Pe0及期望末端位姿Pef分別為

Pe0=［0.384 6 m -0.384 6 m 1.283 0 m 55.67° 61.10° -18.13°］

Pef=［0.597 7 m -0.129 3 m 1.306 5 m 51.86° 51.92° 37.49°］

式中：位姿向量的前3項(xiàng)為機(jī)械臂末端相對于基坐標(biāo)系的位置坐標(biāo)；后3項(xiàng)為末端姿態(tài)歐拉角，設(shè)定規(guī)劃時(shí)間為20 s，加減速時(shí)間為4 s。

采用經(jīng)典笛卡兒路徑跟蹤算法，得到機(jī)械臂各關(guān)節(jié)的角速度曲線，如圖5所示?？梢姡?.8 s左右，關(guān)節(jié)角速度開始出現(xiàn)跳變，仿真過程出現(xiàn)奇異。設(shè)定εΔq=5，εT=10-8，判定奇異區(qū)在8.8～11.2 s區(qū)間內(nèi)。選取k=1，在采用2.3節(jié)所述的奇異回避算法進(jìn)行重新規(guī)劃后，關(guān)節(jié)角速度曲線如圖6所示。

圖5 未進(jìn)行奇異回避時(shí)機(jī)械臂各關(guān)節(jié)角速度Fig.5 Angular velocity of each joint of manipulator without singularity avoidance

在進(jìn)入奇異區(qū)后，發(fā)生奇異的關(guān)節(jié)角速度仍會(huì)出現(xiàn)小幅振蕩，這是由于在進(jìn)入奇異區(qū)以前，雅可比矩陣已經(jīng)逐漸趨于病態(tài)，隨著迭代的進(jìn)行出現(xiàn)誤差積累，在進(jìn)入奇異區(qū)后，機(jī)械臂需要較快地調(diào)整關(guān)節(jié)角，以減小迭代誤差，完成位姿跟蹤。

由圖6可見，機(jī)械臂在經(jīng)過奇異區(qū)時(shí)，關(guān)節(jié)1到關(guān)節(jié)3的角速度未受影響，關(guān)節(jié)5的角速度出現(xiàn)了［-5°，3°］區(qū)間內(nèi)的小幅振蕩，原本發(fā)生奇異的關(guān)節(jié)4和關(guān)節(jié)6的角速度出現(xiàn)了在［-5°，3°］區(qū)間內(nèi)的小幅振蕩，但其幅值仍然得到了明顯的控制，避免了奇異的發(fā)生。

圖6 進(jìn)行奇異回避后的機(jī)械臂各關(guān)節(jié)角速度Fig.6 Angular velocity of each joint of manipulator after singularity avoidance

分別選取k=1、5、10時(shí)，代入2.3節(jié)所述算法中，得到的末端位置跟蹤誤差和末端姿態(tài)跟蹤誤差曲線分別如圖7和圖8所示。

由圖7和圖8可見，本文算法可以較好地完成連續(xù)位姿跟蹤任務(wù)。k值選取越小，機(jī)械臂在剛好進(jìn)入奇異區(qū)時(shí)的跟蹤誤差越大，這同樣是由于迭代過程中已經(jīng)產(chǎn)生誤差積累的緣故，上文已進(jìn)行詳細(xì)說明。在奇異區(qū)內(nèi)經(jīng)過短暫規(guī)劃后，其末端位姿跟蹤精度則隨著k值的減小顯著提升。

圖7 k取不同值時(shí)末端位置誤差對比Fig.7 Comparison of terminal position errors when k is taken as different values

圖8 k取不同值時(shí)末端姿態(tài)誤差對比Fig.8 Comparison of terminal attitude errors when k is taken as different values

實(shí)驗(yàn)結(jié)果表明，本文算法能夠?qū)崿F(xiàn)回避基座可控空間機(jī)械臂的奇異，完成預(yù)期跟蹤任務(wù)。

4 結(jié) 論

1）與傳統(tǒng)的基于奇異分離和阻尼倒數(shù)的算法相比，本文算法可適用于不同自由度、不同空間構(gòu)型的空間機(jī)械臂，并能夠以較小的跟蹤誤差完成機(jī)械臂的奇異回避，具備通用性。

2）通過合理設(shè)定各項(xiàng)參數(shù)，本文算法可以以不同的計(jì)算時(shí)間和跟蹤精度完成奇異回避問題的求解，使用者可以根據(jù)計(jì)算機(jī)性能進(jìn)行權(quán)衡，選取適用于自己的參數(shù)取值。

3）由于本文算法采取在奇異區(qū)內(nèi)分段規(guī)劃的策略對末端軌跡進(jìn)行規(guī)劃，在期望末端位姿處在奇異區(qū)內(nèi)時(shí)，機(jī)械臂仍能夠完成位姿跟蹤任務(wù)。