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

藍啟杰 劉宜成 張 濤

1.四川大學電氣信息學院，成都，610065

2.清華大學自動化系，北京，100084

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

0 引言

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

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

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

1 三臂空間機器人模型

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

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

1.1 運動學模型

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

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

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

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

整個系統(tǒng)的線動量

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

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

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

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

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

1.2 誤差運動學模型

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

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

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

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

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

定義位姿誤差矢量e=（ep，eq）T，綜合式（7）、式（10）可得誤差運動學方程：

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

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

同理，可得基座質(zhì)心位姿誤差運動方程：

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

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

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

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

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

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

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

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

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

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

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

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

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

當SQ，3不可逆時，采用與任務(wù)臂相同的避奇異算法進行奇異回避。

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

3 仿真分析

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

3.1 仿真模型

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

圖2 機械臂D-H坐標系Fig.2 D-H coordinate system of manipulator

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

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

3.2 軌跡規(guī)劃仿真

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

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

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

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

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

任務(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可以看出，機械臂關(guān)節(jié)角速度變化趨勢連續(xù)平穩(wěn)，系統(tǒng)模型準確，算法有效。機械臂最大關(guān)節(jié)角速度為18°/s，滿足機械臂運動限制要求。

由圖6、圖7可看出，任務(wù)臂1在5.1 s時產(chǎn)生腕部奇異，任務(wù)臂2在8.9 s時產(chǎn)生腕部奇異，平衡臂在3.1 s時產(chǎn)生腕部奇異。由于阻尼系數(shù)的引入，任務(wù)臂1末端在4.1 s時開始出現(xiàn)位姿跟蹤誤差，位置跟蹤最大誤差（三軸合成）為2.1×10-4m，末端姿態(tài)跟蹤最大誤差（軸角法）為0.2°。任務(wù)臂2末端在7.1 s時開始出現(xiàn)位姿跟蹤誤差，位置跟蹤最大誤差（三軸合成）為6.3×10-4m，末端姿態(tài)跟蹤最大誤差（軸角法）為1.1°。任務(wù)臂渡過奇異區(qū)域后，末端位姿誤差收斂至0。基座在1.7 s時開始出現(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ù)”回避奇異時，任務(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°。機械臂末端和基座位姿誤差在機械臂產(chǎn)生奇異后一直存在。引入位姿反饋控制后，奇異區(qū)域內(nèi)位姿跟蹤誤差減小，機械臂渡過奇異域后誤差均收斂至0。

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

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

4 結(jié)論

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