基于位姿反饋的三臂空間機(jī)器人抓捕軌跡規(guī)劃

藍(lán)啟杰 劉宜成 張 濤

1.四川大學(xué)電氣信息學(xué)院，成都，610065

2.清華大學(xué)自動(dòng)化系，北京，100084

3.西昌衛(wèi)星發(fā)射中心，西昌，615000

0 引言

空間機(jī)器人是代替或協(xié)助人類在太空環(huán)境中執(zhí)行在軌服務(wù)的特種機(jī)器人，在降低成本和風(fēng)險(xiǎn)方面有突出的優(yōu)勢，已成為空間技術(shù)發(fā)展與應(yīng)用的熱點(diǎn)，其應(yīng)用領(lǐng)域非常廣泛，包括在軌組裝、維護(hù)、清理、實(shí)驗(yàn)、輔助航天員活動(dòng)等，由于其存在特殊的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)性質(zhì)，因此受到許多研究人員的關(guān)注［1?4］。

相比單臂和雙臂空間機(jī)器人系統(tǒng)，三臂或多臂空間機(jī)器人更靈活，具備執(zhí)行更加復(fù)雜任務(wù)的能力［5］。三臂空間機(jī)器人執(zhí)行在軌任務(wù)時(shí)，可以采用多種工作方式。采用雙臂抓捕操作、單臂穩(wěn)定基座位姿的方案時(shí)，2個(gè)機(jī)械臂完成協(xié)同抓捕的任務(wù)，1個(gè)機(jī)械臂實(shí)現(xiàn)基座位姿穩(wěn)定［6］。基于運(yùn)動(dòng)學(xué)特征方程求解關(guān)節(jié)運(yùn)動(dòng)軌跡是機(jī)器人連續(xù)路徑規(guī)劃的基本方法，但運(yùn)動(dòng)學(xué)或動(dòng)力學(xué)奇異是機(jī)械臂的固有特性，在奇異點(diǎn)附近，采用雅可比矩陣直接求逆會(huì)造成關(guān)節(jié)角速度突變而導(dǎo)致控制不可實(shí)現(xiàn)。目前，已有學(xué)者提出回避奇異的方法來保證奇異點(diǎn)附近關(guān)節(jié)角速度的連續(xù)性和有限性，比如阻尼最小方差法［7］、選擇性最小阻尼方差法［8］、“奇異分離+阻尼倒數(shù)”方法［9］、“奇異轉(zhuǎn)化+阻尼倒數(shù)”方法［10?11］，這些方法都是在奇點(diǎn)附近對設(shè)計(jì)路徑做微小改變，但都在不同程度上犧牲了位姿跟蹤精度，即使機(jī)器人離開奇異區(qū)域，誤差仍然存在。

為減小和消除上述方法的跟蹤誤差，本文在三臂空間機(jī)器人的軌跡規(guī)劃過程中，引入機(jī)械臂末端和基座的位姿反饋，將機(jī)械臂關(guān)節(jié)角速度作為控制變量，設(shè)計(jì)閉環(huán)系統(tǒng)的反饋控制率，在誤差運(yùn)動(dòng)方程上對關(guān)節(jié)角速度進(jìn)行求解，實(shí)現(xiàn)高精度的位姿誤差控制和軌跡跟蹤。

1 三臂空間機(jī)器人模型

如圖1所示，空間機(jī)器人系統(tǒng)由基座和在其上安裝的3個(gè)機(jī)械臂組成，每個(gè)機(jī)械臂有6個(gè)自由度。機(jī)械臂1和機(jī)械臂2實(shí)現(xiàn)目標(biāo)跟蹤、抓捕，以下稱為任務(wù)臂；機(jī)械臂3實(shí)現(xiàn)跟蹤階段和抓捕時(shí)刻的基座位姿平衡，以下稱為平衡臂。圖1中，j（j=1，2，3）為機(jī)械臂標(biāo)識(shí)；i（i=1，2，…，6）為連桿和關(guān)節(jié)標(biāo)識(shí)；ΣI為慣性坐標(biāo)系；ΣE為末端坐標(biāo)系；ΣB為基座參考坐標(biāo)系；B0、C0分別為基座及其質(zhì)心；Jj，i為機(jī)械臂 j連桿 i的關(guān)節(jié)；Cj，i為機(jī)械臂 j連桿 i的質(zhì)心；aj，i、bj，i分別為機(jī)械臂 j的關(guān)節(jié) Jj，i到質(zhì)心 Cj，i的位置 矢 量 和 質(zhì)心 Cj，i到關(guān)節(jié) Jj，i+1的位置矢量；bj，0為基座質(zhì)心C0到機(jī)械臂j的第一個(gè)關(guān)節(jié) Jj，1的位置矢量；φk為矢量 bj，0與基座坐標(biāo)系X軸的夾角。

圖1 三臂空間機(jī)器人結(jié)構(gòu)模型Fig.1 Structural model of three-arm space robot

1.1 運(yùn)動(dòng)學(xué)模型

根據(jù)各連桿相對位置關(guān)系，可得各機(jī)械臂末端的位置矢量：

式中，re，j為慣性系下機(jī)械臂j的末端位置矢量；r0為慣性系下基座質(zhì)心位置矢量。

根據(jù)各連桿相對速度關(guān)系，可得各機(jī)械臂末端速度矢量：

式中，Ve,j為機(jī)械臂j末端線速度ve,j和角速度ωe,j組成的矢量；V0為基座質(zhì)心線速度v0和角速度ω0組成的矢量；Jb,j為基座雅可比矩陣；Jm,j為機(jī)械臂j雅可比矩陣；j為機(jī)械臂j的類節(jié)角速度。

整個(gè)系統(tǒng)的線動(dòng)量

式中，Μ為系統(tǒng)總質(zhì)量；JTω,j為系數(shù)矩陣；rog為系統(tǒng)質(zhì)心相對于基座質(zhì)心的位移矢量；C1為常數(shù)。

整個(gè)系統(tǒng)相對于衛(wèi)星本體質(zhì)心的角動(dòng)量

式中，Hω、Hω?為耦合矩陣；C2為系統(tǒng)相對慣性系的總角動(dòng)量，為常數(shù)。

聯(lián)立式（3）、式（4），可推導(dǎo)出基座質(zhì)心速度矢量：

式中，~rg為整個(gè)系統(tǒng)質(zhì)心的位置矢量。

1.2 誤差運(yùn)動(dòng)學(xué)模型

在運(yùn)動(dòng)學(xué)模型基礎(chǔ)上，引入期望位姿指令，可以得到系統(tǒng)的誤差運(yùn)動(dòng)學(xué)模型。機(jī)械臂末端或基座質(zhì)心位置誤差及其導(dǎo)數(shù)可表示為

式中，p為末端或基座的實(shí)際位置；pd為末端或基座的期望位置；v為末端或基座的實(shí)際線速度；vd為末端或基座的期望線速度。

機(jī)械臂末端或者基座質(zhì)心姿態(tài)誤差可表示為

其中，qe為姿態(tài)誤差；qd為期望姿態(tài)；q為實(shí)際姿態(tài)，均采用四元數(shù)法表示。姿態(tài)誤差及其導(dǎo)數(shù)也可用修正的羅德里格參數(shù)表示為

式中，eq為姿態(tài)誤差；ω為實(shí)際角速度；ωd為期望角速度；R為實(shí)際姿態(tài)相對于期望姿態(tài)的旋轉(zhuǎn)矩陣。

定義位姿誤差矢量e=（ep，eq）T，綜合式（7）、式（10）可得誤差運(yùn)動(dòng)學(xué)方程：

結(jié)合式（2）和式（11）可得2個(gè)任務(wù)臂末端位姿誤差運(yùn)動(dòng)方程：

式中，k為關(guān)節(jié)角速度。

同理，可得基座質(zhì)心位姿誤差運(yùn)動(dòng)方程：

2 機(jī)械臂軌跡規(guī)劃

空間機(jī)器人軌跡規(guī)劃一般指在運(yùn)動(dòng)學(xué)或動(dòng)力學(xué)的基礎(chǔ)上，分析關(guān)節(jié)空間或笛卡兒空間軌跡的規(guī)劃和生成。給定任務(wù)臂末端初始和目標(biāo)位姿，可求出末端期望軌跡，然后通過運(yùn)動(dòng)方程反解即可得到任務(wù)臂關(guān)節(jié)角運(yùn)動(dòng)規(guī)律，當(dāng)雅可比矩陣奇異時(shí)，需要采用避奇異算法。利用動(dòng)量守恒方程對平衡臂進(jìn)行速度級(jí)規(guī)劃時(shí)也可能出現(xiàn)雅可比矩陣奇異的情況。如前所述，采用現(xiàn)有避奇異算法會(huì)產(chǎn)生末端或基座位姿跟蹤誤差。因此，本文根據(jù)誤差運(yùn)動(dòng)學(xué)方程（式（12）、式（13））分別對任務(wù)臂和平衡臂進(jìn)行軌跡規(guī)劃。首先將2個(gè)任務(wù)臂關(guān)節(jié)的角速度作為控制變量，設(shè)計(jì)利用末端位姿反饋的控制器，實(shí)現(xiàn)末端高精度位姿跟蹤。將任務(wù)臂規(guī)劃得到的關(guān)節(jié)角速度視為常數(shù)向量代入式（13），將平衡臂關(guān)節(jié)角速度作為控制變量，設(shè)計(jì)利用基座位姿反饋的控制器，實(shí)現(xiàn)基座高精度的穩(wěn)定。

2.1 任務(wù)臂軌跡規(guī)劃

由式（12）看出，任務(wù)臂末端位姿誤差運(yùn)動(dòng)狀態(tài)可以通過任務(wù)臂關(guān)節(jié)角速度來控制，由此引入末端位姿誤差，使得末端位姿運(yùn)動(dòng)滿足：

其中，λe，k為末端位姿反饋增益矩陣，其對角元素都大于零。t→∞時(shí)，末端位姿誤差ee，k將按指數(shù)速度漸進(jìn)收斂至0，能實(shí)現(xiàn)對期望末端位姿的無誤差跟蹤。聯(lián)立式（12）、式（14）可以求出任務(wù)臂關(guān)節(jié)角速度：

本文采用基座穩(wěn)定無擾動(dòng)的方案，即滿足V0=06×1。當(dāng) Jm，k不可逆時(shí)，可采用“奇異分離+阻尼倒數(shù)”［9］或者DLS方法［12］求其近似值：

式中，λm為最大阻尼值；ξ為閾值；vi,k、ui,k為矩陣Jm,k的正交奇異向量。

引入阻尼系數(shù)后，機(jī)械臂關(guān)節(jié)角速度可以連續(xù)平滑地渡過奇異區(qū)域。

由于末端位姿反饋的引入，按照式（15）規(guī)劃的任務(wù)臂關(guān)節(jié)角速度不僅可以實(shí)現(xiàn)末端位姿誤差收斂至0，還可以減小奇異區(qū)域內(nèi)的末端位姿誤差，收斂速度通過設(shè)計(jì)反饋增益矩陣λe，k元素值進(jìn)行調(diào)節(jié)。

2.2 平衡臂軌跡規(guī)劃

任務(wù)臂規(guī)劃完成后，將求得的關(guān)節(jié)角速度視為常量并代入基座質(zhì)心位姿誤差運(yùn)動(dòng)方程（式（13）），采用速度級(jí)方式對平衡臂關(guān)節(jié)角速度進(jìn)行規(guī)劃。六自由度平衡臂能同時(shí)抵消2個(gè)任務(wù)臂運(yùn)動(dòng)對基座質(zhì)心位姿的耦合影響，達(dá)到穩(wěn)定基座質(zhì)心位姿的目的。由式（13）可以看出，基座位姿誤差運(yùn)動(dòng)可以通過平衡臂關(guān)節(jié)角速度控制，由此引入基座位姿誤差，使基座位姿運(yùn)動(dòng)滿足：

與任務(wù)臂相同，λ0為對角元素都大于零的反饋增益矩陣，能使基座質(zhì)心位姿誤差收斂至0，達(dá)到穩(wěn)定基座質(zhì)心位姿的目的。

聯(lián)立式（13）、式（18）可以得出平衡臂關(guān)節(jié)角速度：

當(dāng)SQ，3不可逆時(shí)，采用與任務(wù)臂相同的避奇異算法進(jìn)行奇異回避。

由于基座位姿反饋的引入，按照式（19）規(guī)劃的平衡臂關(guān)節(jié)角速度可以使基座位姿誤差收斂至0，減小平衡臂奇異區(qū)域內(nèi)基座位姿誤差，還可以削弱外界擾動(dòng)對基座位姿的影響，收斂速度通過設(shè)計(jì)反饋增益矩陣λ0元素值進(jìn)行調(diào)節(jié)。

3 仿真分析

在建立三臂空間機(jī)器人仿真模型的基礎(chǔ)上，對本文提出的軌跡規(guī)劃算法進(jìn)行了數(shù)值仿真，并且同“奇異分離+阻尼倒數(shù)”的軌跡規(guī)劃方法［7］進(jìn)行了對比分析。

3.1 仿真模型

仿真實(shí)驗(yàn)的3個(gè)機(jī)械臂均采用PUMA560型，且均采用D?H參數(shù)建模［13］，機(jī)械臂D?H坐標(biāo)系結(jié)構(gòu)如圖2所示，D?H參數(shù)值如表1所示。基座、機(jī)械臂質(zhì)量特性參數(shù)如表2所示。

圖2 機(jī)械臂D-H坐標(biāo)系Fig.2 D-H coordinate system of manipulator

表1 D-H參數(shù)Tab.1 D-H parameters

表2 空間機(jī)器人質(zhì)量參數(shù)Tab.2 Mass properties of space robot

3.2 軌跡規(guī)劃仿真

利用MATLAB/Simulink工具對運(yùn)動(dòng)學(xué)模型和規(guī)劃算法進(jìn)行驗(yàn)證。假設(shè)系統(tǒng)初始線動(dòng)量和角動(dòng)量均為0，仿真時(shí)間tf=20 s。最大阻尼值λm=0.05，奇異閾值ξ=0.10，反饋增益矩陣為diag（2，2，2，2，2，2）。

任務(wù)臂1末端慣性系下初始位姿為

任務(wù)臂2末端慣性系下初始位姿為

兩個(gè)任務(wù)臂末端慣性系下目標(biāo)位姿為

基座慣性系下的初始位姿為

任務(wù)臂初始關(guān)節(jié)角為

平衡臂初始關(guān)節(jié)角為

任務(wù)臂與平衡臂的關(guān)節(jié)角速度如圖3～圖5所示。

圖3 任務(wù)臂1關(guān)節(jié)角速度Fig.3 Joint angular velocity of the first task arm

圖4 任務(wù)臂2關(guān)節(jié)角速度Fig.4 The joint angular velocity of the second task arm

由圖3～圖5可以看出，機(jī)械臂關(guān)節(jié)角速度變化趨勢連續(xù)平穩(wěn)，系統(tǒng)模型準(zhǔn)確，算法有效。機(jī)械臂最大關(guān)節(jié)角速度為18°/s，滿足機(jī)械臂運(yùn)動(dòng)限制要求。

由圖6、圖7可看出，任務(wù)臂1在5.1 s時(shí)產(chǎn)生腕部奇異，任務(wù)臂2在8.9 s時(shí)產(chǎn)生腕部奇異，平衡臂在3.1 s時(shí)產(chǎn)生腕部奇異。由于阻尼系數(shù)的引入，任務(wù)臂1末端在4.1 s時(shí)開始出現(xiàn)位姿跟蹤誤差，位置跟蹤最大誤差（三軸合成）為2.1×10-4m，末端姿態(tài)跟蹤最大誤差（軸角法）為0.2°。任務(wù)臂2末端在7.1 s時(shí)開始出現(xiàn)位姿跟蹤誤差，位置跟蹤最大誤差（三軸合成）為6.3×10-4m，末端姿態(tài)跟蹤最大誤差（軸角法）為1.1°。任務(wù)臂渡過奇異區(qū)域后，末端位姿誤差收斂至0?；?.7 s時(shí)開始出現(xiàn)位姿誤差，位置跟蹤最大誤差（三軸合成）為2.6×10-5m，姿態(tài)跟蹤最大誤差（軸角法）為0.05°。平衡臂渡過奇異區(qū)域后，基座位姿誤差均收斂至0。

圖5 平衡臂關(guān)節(jié)角速度Fig.5 The joint angular velocity of the balance arm

圖6 腕部奇異參數(shù)Fig.6 Singular parameter of wrist

圖7 任務(wù)臂末端位姿誤差和基座位姿誤差Fig.7 The pose error of end-effector and base

由圖8看出，僅采用“奇異分離+阻尼倒數(shù)”回避奇異時(shí)，任務(wù)臂1末端位置跟蹤最大誤差為0.5×10-3m，姿態(tài)最大誤差為0.3°。任務(wù)臂2末端位置跟蹤最大誤差為2.9×10-3m，姿態(tài)最大誤差為1.6°?；恢酶欁畲笳`差為3.9×10-5m，姿態(tài)最大誤差為0.05°。機(jī)械臂末端和基座位姿誤差在機(jī)械臂產(chǎn)生奇異后一直存在。引入位姿反饋控制后，奇異區(qū)域內(nèi)位姿跟蹤誤差減小，機(jī)械臂渡過奇異域后誤差均收斂至0。

圖8 兩種方法的誤差對比Fig.8 Error comparison of two methods

綜合上述仿真結(jié)果可以看出，本文方法不僅能減小機(jī)械臂在奇異區(qū)域內(nèi)出現(xiàn)的位姿跟蹤誤差，而且在機(jī)械臂渡過奇異區(qū)域之后，該誤差能快速收斂至0，完成末端高精度跟蹤連續(xù)路徑的任務(wù)，同時(shí)確?；蛔藷o擾動(dòng)。

4 結(jié)論

本文提出的“位姿誤差反饋”方法不需要對機(jī)械臂構(gòu)型進(jìn)行詳細(xì)的分析，對機(jī)械臂的質(zhì)量也沒有任何的特殊要求，具有很好的通用性，而且能同時(shí)應(yīng)用在單臂、雙臂等多種空間機(jī)器人系統(tǒng)構(gòu)型上。理論和仿真結(jié)果表明，對于連續(xù)路徑跟蹤，以前的回避奇異方法并未對阻尼系數(shù)所引起的誤差進(jìn)行處理。本文方法將軌跡規(guī)劃問題轉(zhuǎn)化為控制問題，不僅可以減小由阻尼系數(shù)帶來的位姿跟蹤誤差，而且在機(jī)械臂渡過奇異域之后還可以徹底消除末端或基座的位姿跟蹤誤差。