劉 琛,劉 宇,宋立濱,張繼文

(1.清華大學(xué)機(jī)械工程系,北京 100084；2.北京跟蹤與通信技術(shù)研究所,北京 100094)

0 引 言

目前,空間技術(shù)已經(jīng)取得了較大發(fā)展,但在軌服務(wù)、空間對(duì)接等多飛行器之間的復(fù)雜任務(wù)仍然是一個(gè)非常大的挑戰(zhàn)。由于空間環(huán)境的特殊性以及空間飛行器高昂的成本,在進(jìn)行相關(guān)技術(shù)的研究和驗(yàn)證時(shí),往往需要先在地面進(jìn)行充分的實(shí)驗(yàn)[1]。利用半物理(Hardware-in-the-loop,HIL)系統(tǒng)進(jìn)行空間技術(shù)的驗(yàn)證是一種較為常見的方式[2-3],如文獻(xiàn)[4]中對(duì)航天器逼近階段的導(dǎo)航研究和半物理仿真驗(yàn)證。半物理仿真系統(tǒng)可以結(jié)合實(shí)際硬件測試和計(jì)算機(jī)仿真,在地面搭建代仿真空間對(duì)象的等效硬件系統(tǒng),并且實(shí)現(xiàn)完整的控制閉環(huán),模擬真實(shí)情況下的地面控制和空間操作過程,對(duì)研究空間對(duì)接、在軌服務(wù)等衛(wèi)星間的相關(guān)技術(shù)有非常重要的作用[5]。

目前國內(nèi)外在半物理系統(tǒng)的研究和搭建方面均有一些成果。德國宇航中心開發(fā)了一套EPOS(European proximit operations simulator)系統(tǒng)[6-7],該系統(tǒng)基于兩臺(tái)機(jī)械臂和一個(gè)直線導(dǎo)軌搭建而成,用于研究在軌條件下的衛(wèi)星之間的交會(huì)對(duì)接任務(wù)。加拿大航天局為了研究交會(huì)對(duì)接中的接觸動(dòng)力學(xué)過程,搭建了主要由兩臺(tái)固定基座的機(jī)械臂組成的STVF(SPDM task verification facility)系統(tǒng)[8-9],此外,還對(duì)利用空間操作臂抓捕自由翻滾的衛(wèi)星的全過程進(jìn)行了仿真[10]。文獻(xiàn)[11]中同樣通過基于兩臺(tái)機(jī)械臂的半物理系統(tǒng)對(duì)抓捕受損衛(wèi)星的過程進(jìn)行研究。在國內(nèi)方面,哈工大通過力補(bǔ)償方式構(gòu)建了用于模擬捕獲目標(biāo)衛(wèi)星的微重力地面驗(yàn)證系統(tǒng)[12],并利用該系統(tǒng)對(duì)空間操作臂抓捕自由漂浮衛(wèi)星進(jìn)行研究。此外,空間技術(shù)研究院[13]、中國科學(xué)院[14]等單位也對(duì)半物理系統(tǒng)進(jìn)行了相關(guān)的研究,但從結(jié)構(gòu)上來看,均是以兩臺(tái)固定基座的機(jī)械臂為基礎(chǔ)進(jìn)行搭建。

為了滿足對(duì)空間飛行器之間的大范圍相對(duì)運(yùn)動(dòng)進(jìn)行運(yùn)動(dòng)學(xué)仿真的需求,在保留現(xiàn)有半物理系統(tǒng)操作能力的同時(shí)擴(kuò)大半物理系統(tǒng)的工作空間十分必要。目前這種被廣泛采用的基于兩臺(tái)固定基座的機(jī)械臂的結(jié)構(gòu),保證了飛行器之間的接觸動(dòng)力學(xué)仿真的可靠性,但同時(shí)嚴(yán)重限制了半物理系統(tǒng)工作空間的大小,在仿真較大范圍的相對(duì)運(yùn)動(dòng)時(shí)存在一定不足。部分半物理系統(tǒng)通過將機(jī)械臂安裝在直線導(dǎo)軌上的方式擴(kuò)展工作空間,但由于直線導(dǎo)軌自身特點(diǎn),在繼續(xù)擴(kuò)展其他方向平移自由度時(shí)面臨結(jié)構(gòu)復(fù)雜、成本過高等一系列問題。

文獻(xiàn)[15]對(duì)半物理仿真系統(tǒng)的基本問題和解決方案進(jìn)行了分析和總結(jié),對(duì)半物理系統(tǒng)的設(shè)計(jì)和調(diào)試過程提供了指導(dǎo)意義。在此基礎(chǔ)上,結(jié)合移動(dòng)機(jī)器人在平面運(yùn)動(dòng)的靈活性,如圖1所示的基于移動(dòng)機(jī)器人的半物理系統(tǒng)的工作空間可以被極大擴(kuò)展,且平面內(nèi)的運(yùn)動(dòng)具有較高靈活性,避免了直線導(dǎo)軌的復(fù)雜結(jié)構(gòu)設(shè)計(jì),同時(shí),移動(dòng)機(jī)器人的引入增加了半物理系統(tǒng)的平移關(guān)節(jié),增加了系統(tǒng)的冗余性,可以滿足復(fù)雜的大范圍的相對(duì)運(yùn)動(dòng)軌跡的需求,在仿真較大范圍內(nèi)的空間飛行器之間的相對(duì)運(yùn)動(dòng)時(shí)具有較大優(yōu)勢。

圖1 基于移動(dòng)機(jī)器人的半物理系統(tǒng)整體框架Fig.1 The framework of the mobile-robots-based hardware-in-the-loop simulation system

但是,基于移動(dòng)機(jī)器人的半物理系統(tǒng)也存在明顯的缺點(diǎn)：移動(dòng)機(jī)器人的運(yùn)動(dòng)精度明顯低于機(jī)械臂等執(zhí)行機(jī)構(gòu)的精度,與直線導(dǎo)軌相比也存在一定差距,將移動(dòng)機(jī)器人引入半物理系統(tǒng)后將面臨整體系統(tǒng)精度下降的問題。

針對(duì)上述問題,本文采取通過外部測量設(shè)備實(shí)時(shí)測量和機(jī)械臂運(yùn)動(dòng)補(bǔ)償?shù)姆绞?在離線規(guī)劃過程中,借鑒了超冗余機(jī)械臂的規(guī)劃方法,提前為機(jī)械臂留出實(shí)時(shí)運(yùn)動(dòng)補(bǔ)償?shù)挠嗔?并且保證其末端的靈活運(yùn)動(dòng)能力,從而完成整個(gè)半物理系統(tǒng)的實(shí)時(shí)運(yùn)動(dòng)補(bǔ)償控制。

1 問題描述

基于移動(dòng)機(jī)器人的半物理系統(tǒng)可以將移動(dòng)機(jī)器人與機(jī)械臂等執(zhí)行機(jī)構(gòu)相結(jié)合,通過兩個(gè)機(jī)械臂末端的相對(duì)運(yùn)動(dòng)模擬空間中飛行器之間的相對(duì)運(yùn)動(dòng),并且通過動(dòng)力學(xué)等效的方式模擬復(fù)雜任務(wù)中的飛行器之間的接觸動(dòng)力學(xué)過程。這些任務(wù)對(duì)飛行器各自的運(yùn)動(dòng)精度提出了較高的要求。但是移動(dòng)機(jī)器人的運(yùn)動(dòng)存在較多不確定性,精度較差,需要通過外部測量設(shè)備實(shí)時(shí)測量和機(jī)械臂等精度較高的執(zhí)行機(jī)構(gòu)實(shí)時(shí)運(yùn)動(dòng)補(bǔ)償?shù)姆绞教岣咂渚?因此在規(guī)劃過程中需要保證機(jī)械臂基本實(shí)時(shí)補(bǔ)償?shù)倪\(yùn)動(dòng)能力。

基于移動(dòng)機(jī)器人的半物理系統(tǒng)具有較多冗余自由度,對(duì)于任意相對(duì)目標(biāo)位姿,均存在對(duì)應(yīng)的構(gòu)型子空間,因此可以額外設(shè)計(jì)性能指標(biāo),在每一個(gè)構(gòu)型子空間中根據(jù)性能指標(biāo)計(jì)算最符合要求的構(gòu)型,從而保證整個(gè)仿真過程的順利進(jìn)行。

2 算法框架

為了實(shí)現(xiàn)在規(guī)劃過程中優(yōu)化基于移動(dòng)機(jī)器人的半物理系統(tǒng)的操作能力、避關(guān)節(jié)極限性能,為后續(xù)的運(yùn)動(dòng)補(bǔ)償環(huán)節(jié)創(chuàng)造條件,本文采取了一種基于超冗余機(jī)械臂的規(guī)劃方法,通過一套完整的方法簡化當(dāng)前問題,提出的規(guī)劃方法框架如圖2所示。

圖2 算法框架Fig.2 The framework of the proposed algorithm

具體步驟如下：

1)通過運(yùn)動(dòng)學(xué)等效的方式,將基于移動(dòng)機(jī)器人的半物理系統(tǒng)等效為一個(gè)串聯(lián)超冗余機(jī)械臂。

2)根據(jù)該超冗余機(jī)械臂的物理參數(shù)建立其運(yùn)動(dòng)學(xué)模型。

3)建立性能指標(biāo),并將該性能指標(biāo)引入運(yùn)動(dòng)學(xué)模型中。

4)針對(duì)稀疏位姿點(diǎn),利用五次多項(xiàng)式插值及球面線性插值等方式進(jìn)行插值計(jì)算每個(gè)時(shí)刻的操作空間位姿及速度。

5)根據(jù)帶性能指標(biāo)的運(yùn)動(dòng)學(xué)公式計(jì)算該時(shí)刻的關(guān)節(jié)空間構(gòu)型及速度。

6)循環(huán)計(jì)算整條軌跡。

7)輸出規(guī)劃結(jié)果。

3 運(yùn)動(dòng)學(xué)模型的等效與建立

本文中的規(guī)劃問題均基于整體系統(tǒng)等效而成的超冗余機(jī)械臂,該超冗余機(jī)械臂的構(gòu)建方法如下：以飛行器1為超冗余機(jī)械臂的基座,飛行器2為超冗余機(jī)械臂的末端,將移動(dòng)機(jī)器人之間的相對(duì)運(yùn)動(dòng)等效為兩個(gè)平移關(guān)節(jié)和一個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)的運(yùn)動(dòng)。此時(shí),半物理系統(tǒng)被抽象為一個(gè)具有多自由度的超冗余機(jī)械臂,具有多個(gè)平移和轉(zhuǎn)動(dòng)關(guān)節(jié),給定的空間飛行器之間的大范圍相對(duì)運(yùn)動(dòng)軌跡即為超冗余機(jī)械臂的末端相對(duì)于基座的運(yùn)動(dòng)軌跡。等效過程示意圖如圖3所示。

圖3 超冗余機(jī)械臂等效圖(僅用于表示等效過程,不代表真實(shí)關(guān)節(jié)類型及數(shù)量)Fig.3 The equivalent process of the super redundant manipulator (only used to represent the equivalent process,not the real type and number of joints)

由于該機(jī)械臂高度冗余,對(duì)于任意給定的操作空間下的末端位姿,均有對(duì)應(yīng)的無窮組關(guān)節(jié)構(gòu)型。這些關(guān)節(jié)構(gòu)型構(gòu)成了該超冗余機(jī)械臂構(gòu)型空間的一個(gè)子空間,在該子空間內(nèi)可以根據(jù)特定目的設(shè)計(jì)性能指標(biāo)以尋找對(duì)應(yīng)的最優(yōu)構(gòu)型[16]。

此時(shí),根據(jù)Denavit-Hartenberg方法建立超冗余機(jī)械臂各關(guān)節(jié)連桿的坐標(biāo)系,根據(jù)該超冗余機(jī)械臂關(guān)節(jié)尺寸的參數(shù),可以確定其D-H參數(shù)表,進(jìn)而根據(jù)D-H參數(shù)表可以建立超冗余機(jī)械臂的運(yùn)動(dòng)學(xué)模型：

(1)

(2)

其中:J+(θ)為當(dāng)前關(guān)節(jié)構(gòu)型的雅可比矩陣的偽逆;(I-J+(θ)J(θ))為當(dāng)前關(guān)節(jié)構(gòu)型的雅可比矩陣的零空間;φ為任意優(yōu)化向量;(I-J+(θ)J(θ))φ被稱為當(dāng)前構(gòu)型的超冗余機(jī)械臂的自運(yùn)動(dòng)。

在對(duì)超冗余機(jī)械臂求解過程中,考慮到機(jī)械臂需要對(duì)移動(dòng)平臺(tái)的運(yùn)動(dòng)誤差進(jìn)行補(bǔ)償,在對(duì)超冗余機(jī)械臂進(jìn)行規(guī)劃時(shí)需要在保證末端位姿準(zhǔn)確的情況下,使超冗余機(jī)械臂的操作靈活性盡可能高、各關(guān)節(jié)角度盡可能遠(yuǎn)離關(guān)節(jié)極限,因此這里選定可操作度ω[17]與避關(guān)節(jié)極限系數(shù)h為性能指標(biāo),其定義分別為：

(3)

(4)

可操作度是機(jī)械臂的末端可操作度橢球的體積,當(dāng)可操作度越接近零表示當(dāng)前構(gòu)型的末端可操作度橢球的某一個(gè)軸長度接近于零,該機(jī)械臂即將喪失沿該軸方向的運(yùn)動(dòng)能力,即奇異位形。計(jì)算可操作度的矢量梯度[18]可以得到：

(5)

(6)

其中:A=J(θ)JT(θ)=(aij)n×n,θi為超冗余機(jī)械臂的第i個(gè)關(guān)節(jié)角度。計(jì)算避關(guān)節(jié)極限系數(shù)的矢量梯度可以得到：

(7)

(8)

(9)

其中:a,b為調(diào)節(jié)系數(shù),J和J+分別為當(dāng)前關(guān)節(jié)構(gòu)型對(duì)應(yīng)的雅可比矩陣以及雅可比矩陣的偽逆。對(duì)公式進(jìn)行離散化可以得到：

(10)

進(jìn)一步整理可以得到:

θt+1-θt=J+(Pt+1-Pt)+α(I-J+J)·

(11)

其中:α為時(shí)間調(diào)節(jié)因子。此時(shí)即可根據(jù)上述公式并且結(jié)合當(dāng)前關(guān)節(jié)構(gòu)型計(jì)算下一時(shí)刻的構(gòu)型,并且可以一定程度上保證超冗余機(jī)械臂的遠(yuǎn)離關(guān)節(jié)極限性能和可操作度性能。

4 軌跡規(guī)劃方法

為了保證超冗余機(jī)械臂的末端運(yùn)動(dòng)按照給定的相對(duì)運(yùn)動(dòng)軌跡運(yùn)動(dòng),這里采取在超冗余機(jī)械臂的操作空間內(nèi)直接進(jìn)行規(guī)劃的方式。通常情況下,給定的相對(duì)運(yùn)動(dòng)軌跡為一系列較為稀疏的路徑點(diǎn),每個(gè)路徑點(diǎn)均包含時(shí)間、位姿、速度及加速度。為了實(shí)現(xiàn)操作空間內(nèi)的較為平滑的運(yùn)動(dòng),需要對(duì)路徑點(diǎn)之間進(jìn)行插值以計(jì)算密集的插值點(diǎn)信息。路徑點(diǎn)之間的位置信息采用對(duì)每個(gè)方向的坐標(biāo)分別進(jìn)行五次多項(xiàng)式插值,姿態(tài)信息采用姿態(tài)四元數(shù)的球面線性插值(Spherical linear interpolation,SLERP)[19]與直線插值相結(jié)合的方式,插值方式為等時(shí)間間隔插值。該規(guī)劃過程具體如下：

對(duì)于任意相鄰路徑點(diǎn),將相鄰路徑點(diǎn)之間的各方向位置坐標(biāo)分別進(jìn)行五次多項(xiàng)式插值[20]。五次多項(xiàng)式插值的表示形式為：

S(t)=At5+Bt4+Ct3+Dt2+Et+F

(12)

其中:S(t)為插值得到的關(guān)于時(shí)間的五次多項(xiàng)式,A,B,C,D,E,F分別為五次多項(xiàng)式的各階系數(shù)。根據(jù)插值結(jié)果可以得到相鄰路徑點(diǎn)之間的任意插值點(diǎn)的位置向量[px,py,pz]T和速度向量[vx,vy,vz]T。

相鄰路徑點(diǎn)之間任意一個(gè)插值點(diǎn)的姿態(tài)四元數(shù)可以根據(jù)如下公式計(jì)算：

qk=

(13)

其中:q表示姿態(tài)四元數(shù),Ω=arccos(qt·qt+1)為相鄰路徑點(diǎn)姿態(tài)四元數(shù)的夾角,·表示四元數(shù)乘法。θthreshold為直線插值與球面圓弧插值的臨界角度。根據(jù)四元數(shù)運(yùn)算規(guī)律,對(duì)于任意四元數(shù)q,有:

(14)

此時(shí),經(jīng)過整理可以得到當(dāng)前插值點(diǎn)的轉(zhuǎn)動(dòng)速度：

(15)

其中:q*為當(dāng)前姿態(tài)四元數(shù)的共軛,[ωx,ωy,ωz]T為機(jī)械臂末端在笛卡爾坐標(biāo)系下的沿各個(gè)方向的轉(zhuǎn)動(dòng)角速度。

圖4 在各構(gòu)型子空間中規(guī)劃的示意圖Fig.4 The schematic diagram of the planning in each configuration subspace

5 實(shí)驗(yàn)系統(tǒng)搭建及結(jié)果分析

為了對(duì)上述規(guī)劃方法進(jìn)行驗(yàn)證和分析,本文設(shè)計(jì)并搭建了一套基于移動(dòng)機(jī)器人的半物理系統(tǒng)并基于該系統(tǒng)對(duì)本文提出的方法展開詳細(xì)分析與驗(yàn)證。該基于移動(dòng)機(jī)器人的半物理系統(tǒng)整體結(jié)構(gòu)示意圖如圖5所示。

圖5 基于移動(dòng)機(jī)器人的半物理系統(tǒng)設(shè)計(jì)圖Fig.5 The overall design of the mobile robots based hardware-in-the-loop simulation system

該半物理系統(tǒng)主要分為三個(gè)主要組成部分：動(dòng)作捕捉子系統(tǒng)、飛行器1子系統(tǒng)和飛行器2子系統(tǒng)。動(dòng)作捕捉子系統(tǒng)可以提供實(shí)時(shí)精確測量結(jié)果,其原理為通過一定數(shù)量固定的高速相機(jī)捕捉粘貼于實(shí)驗(yàn)環(huán)境中物體上的標(biāo)志點(diǎn)位置,并且根據(jù)標(biāo)志點(diǎn)相對(duì)位置關(guān)系解算出試驗(yàn)場地內(nèi)目標(biāo)物體的實(shí)時(shí)精確位置姿態(tài)[21],同時(shí)測量頻率可以達(dá)到120 Hz,基本滿足實(shí)時(shí)性要求。該系統(tǒng)主要用來實(shí)時(shí)監(jiān)測半物理系統(tǒng)中的移動(dòng)機(jī)器人的運(yùn)動(dòng),提供實(shí)時(shí)精確外部測量數(shù)據(jù),結(jié)合控制及運(yùn)動(dòng)補(bǔ)償部分改善移動(dòng)機(jī)器人的運(yùn)動(dòng)精度。飛行器1子系統(tǒng)主要包括移動(dòng)機(jī)器人、并聯(lián)機(jī)構(gòu)及飛行器1模型,飛行器2子系統(tǒng)主要包括移動(dòng)機(jī)器人、機(jī)械臂及飛行器2模型。移動(dòng)機(jī)器人基于麥克納姆輪設(shè)計(jì),具有完整非約束的三個(gè)自由度,可以在平面內(nèi)保證靈活性的自由運(yùn)動(dòng)。并聯(lián)機(jī)構(gòu)為Stewart構(gòu)型,用來小范圍調(diào)整飛行器1的位姿,機(jī)械臂為七軸冗余機(jī)械臂,可以在一定范圍內(nèi)對(duì)飛行器2的位姿進(jìn)行調(diào)整。

在上述整體系統(tǒng)框架下,基于ROS搭建了該基于移動(dòng)機(jī)械臂的半物理系統(tǒng),如圖6、圖7所示。圖8所示為動(dòng)作捕捉系統(tǒng)軟件中的高速相機(jī)布置以及測量得到的移動(dòng)機(jī)器人剛體位姿。各個(gè)子系統(tǒng)以及集控臺(tái)之間通過ROS的多機(jī)通信框架實(shí)現(xiàn)信息交互。子系統(tǒng)上均搭載微型計(jì)算機(jī)完成低層次的規(guī)劃和控制,保證控制頻率,微型計(jì)算機(jī)和集控臺(tái)之間通信,通過集控臺(tái)協(xié)調(diào)實(shí)現(xiàn)整體系統(tǒng)的控制。整體系統(tǒng)的軌跡規(guī)劃過程在集控臺(tái)上離線完成。該半物理系統(tǒng)的工作空間不會(huì)受到系統(tǒng)本身的限制,但由于實(shí)驗(yàn)場地的限制,實(shí)際半物理系統(tǒng)的仿真空間為所用實(shí)驗(yàn)場地的可用空間。

圖6 飛行器1子系統(tǒng)實(shí)物圖Fig.6 The subsystem for the aircraft 1

圖7 飛行器2子系統(tǒng)實(shí)物圖Fig.7 The subsystem for the aircraft 2

圖8 動(dòng)作捕捉系統(tǒng)及測量到的移動(dòng)機(jī)器人位姿Fig.8 The motion capture system and the measured pose of the mobile robots

針對(duì)上述基于移動(dòng)機(jī)器人的半物理系統(tǒng),根據(jù)第3節(jié)中的運(yùn)動(dòng)學(xué)等效方式將其等效為超冗余機(jī)械臂,移動(dòng)機(jī)器人之間的平面內(nèi)的運(yùn)動(dòng)等效為兩個(gè)平移和一個(gè)旋轉(zhuǎn)共三個(gè)自由度,由于并聯(lián)機(jī)構(gòu)的六自由度運(yùn)動(dòng)的耦合性及一般情況下僅小范圍調(diào)整飛行器1的俯仰角度的功能,這里僅將并聯(lián)機(jī)構(gòu)等效為超冗余機(jī)械臂中的一個(gè)轉(zhuǎn)動(dòng)關(guān)節(jié)。此時(shí),根據(jù)上述等效過程得到的超冗余機(jī)械臂的物理參數(shù),建立該超冗余機(jī)械臂各連桿坐標(biāo)系,如圖9所示,以及D-H參數(shù)表,見表1。

表1 超冗余機(jī)械臂D-H參數(shù)表Table 1 D-H parameters table for super redundant manipulator

圖9 超冗余機(jī)械臂的連桿坐標(biāo)系Fig.9 Joint coordinate system of the super redundant manipulator

為了直觀地對(duì)上述規(guī)劃算法進(jìn)行性能分析,這里給定一條典型路徑用來通過實(shí)例驗(yàn)證算法性能。該路徑分為兩個(gè)階段,第一階段為飛行器2相對(duì)于飛行器1做直線運(yùn)動(dòng),第二階段飛行器2以飛行器1周圍一點(diǎn)為圓心進(jìn)行繞飛,第一階段持續(xù)時(shí)間為10 s,第二階段持續(xù)時(shí)間為10 s。其定義分別如下：

第一階段的平移部分軌跡為:

姿態(tài)四元數(shù)為[0,0,0.707,0.707]T并且保持不變。第二階段的平移部分軌跡為:

初始姿態(tài)四元數(shù)為[0,0,0.707,0.707]T,最終姿態(tài)四元數(shù)為[0,0,0,1]T,第二階段全過程中機(jī)械臂末端勻速轉(zhuǎn)動(dòng)。

對(duì)上述軌跡進(jìn)行等時(shí)間間隔均勻采點(diǎn)并根據(jù)軌跡公式進(jìn)一步計(jì)算生成目標(biāo)軌跡點(diǎn),軌跡點(diǎn)之間的時(shí)間間隔為1 s。對(duì)生成的路徑點(diǎn)使用上述規(guī)劃及優(yōu)化逆解方法進(jìn)行求解和規(guī)劃,可以得到如圖10所示的各關(guān)節(jié)規(guī)劃結(jié)果。圖中優(yōu)化前表示的數(shù)據(jù)為式(1)中的優(yōu)化向量φ為零向量時(shí)獲得的規(guī)劃結(jié)果,優(yōu)化后所表示的數(shù)據(jù)為根據(jù)式(10)中的方法計(jì)算得到的數(shù)據(jù)。由于關(guān)節(jié)2、3、4的運(yùn)動(dòng)由移動(dòng)平臺(tái)完成,并且未列入優(yōu)化向量中,規(guī)劃結(jié)果幾乎完全相同,因此并未直接畫出關(guān)節(jié)2、3、4三個(gè)關(guān)節(jié)的角度。從關(guān)節(jié)1及關(guān)節(jié)6的數(shù)據(jù)可以看出,相比較于未經(jīng)過優(yōu)化的方法,經(jīng)過優(yōu)化的規(guī)劃方法可以有效避免關(guān)節(jié)角度超限,同時(shí)對(duì)于其他原本遠(yuǎn)離關(guān)節(jié)極限的關(guān)節(jié),在原有位置的基礎(chǔ)上基本保持不變。因此,規(guī)劃結(jié)果驗(yàn)證了該方法為后續(xù)控制過程中為移動(dòng)機(jī)器人的運(yùn)動(dòng)誤差提供運(yùn)動(dòng)補(bǔ)償預(yù)留了足夠余量的結(jié)論。

圖10 各關(guān)節(jié)的規(guī)劃結(jié)果Fig.10 The planning results of every joint

在實(shí)際搭建出來的半物理系統(tǒng)上執(zhí)行上述規(guī)劃結(jié)果,在實(shí)驗(yàn)過程中,移動(dòng)機(jī)器人、機(jī)械臂及并聯(lián)機(jī)構(gòu)均完全根據(jù)規(guī)劃結(jié)果通過自身控制其進(jìn)行運(yùn)動(dòng),并未通過機(jī)械臂和并聯(lián)機(jī)構(gòu)對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)進(jìn)行誤差補(bǔ)償。移動(dòng)機(jī)器人通過實(shí)時(shí)獲取動(dòng)作捕捉系統(tǒng)的測量數(shù)據(jù)并進(jìn)行閉環(huán)控制,其運(yùn)動(dòng)曲線如圖11所示,可以看出,移動(dòng)機(jī)器人的各個(gè)方向的運(yùn)動(dòng)誤差范圍約為±10 mm。實(shí)驗(yàn)過程中的半物理系統(tǒng)如圖12所示。

圖11 飛行器1子系統(tǒng)中的移動(dòng)機(jī)器人運(yùn)動(dòng)曲線(上)和飛行器2子系統(tǒng)中的移動(dòng)機(jī)器人運(yùn)動(dòng)曲線(下)Fig.11 The motion curve of the mobile robot in aircraft 1 subsystem (upper)and the motion curve of the mobile robot in aircraft 2 subsystem (lower)

圖12 運(yùn)行過程中的半物理仿真系統(tǒng)Fig.12 The hardware-in-the-loop simulation system during the experiment

利用動(dòng)作捕捉系統(tǒng)對(duì)實(shí)驗(yàn)過程中兩個(gè)子系統(tǒng)的移動(dòng)平臺(tái)位姿進(jìn)行測量,并且結(jié)合機(jī)械臂及并聯(lián)機(jī)構(gòu)高精度編碼器的測量數(shù)據(jù),經(jīng)過計(jì)算后可以得到兩個(gè)飛行器模型之間的相對(duì)位姿數(shù)據(jù),將測量數(shù)據(jù)與規(guī)劃結(jié)果、期望位姿點(diǎn)進(jìn)行比對(duì),結(jié)果如圖13所示。期望路徑點(diǎn)表示對(duì)本文實(shí)驗(yàn)所用的典型路徑直接采樣生成的目標(biāo)軌跡點(diǎn),規(guī)劃結(jié)果表示本文算法規(guī)劃得到的結(jié)果對(duì)應(yīng)的超冗余機(jī)械臂的末端相對(duì)于基座的相對(duì)位姿數(shù)據(jù),實(shí)際運(yùn)動(dòng)表示使用本系統(tǒng)進(jìn)行仿真的過程中飛行器2模型相對(duì)于飛行器1模型的相對(duì)位姿數(shù)據(jù)。可以看出,在本文提出的規(guī)劃算法的基礎(chǔ)上,該半物理系統(tǒng)可以連續(xù)、平穩(wěn)地再現(xiàn)飛行器模型之間的相對(duì)運(yùn)動(dòng),兩個(gè)飛行器模型之間的相對(duì)運(yùn)動(dòng)與期望的運(yùn)動(dòng)存在一定誤差,各方向的位置誤差范圍約為±15 mm,姿態(tài)角偏差范圍約為±0.5°。對(duì)整體系統(tǒng)的運(yùn)動(dòng)誤差來源進(jìn)行分析可以發(fā)現(xiàn),移動(dòng)機(jī)器人的誤差為當(dāng)前系統(tǒng)運(yùn)動(dòng)誤差的主要來源,另外,動(dòng)作捕捉系統(tǒng)的測量坐標(biāo)系的X-Y平面與實(shí)際實(shí)驗(yàn)場地的地面并未完全重合,導(dǎo)致移動(dòng)機(jī)器人以及飛行器模型的運(yùn)動(dòng)出現(xiàn)了豎直方向上的運(yùn)動(dòng)偏差。因此,在當(dāng)前情況下使用該半物理系統(tǒng)進(jìn)行半物理仿真時(shí),運(yùn)動(dòng)誤差較大,不能滿足精度要求,需要在后續(xù)過程中通過機(jī)械臂和并聯(lián)機(jī)構(gòu)對(duì)移動(dòng)機(jī)器人的誤差進(jìn)行實(shí)時(shí)補(bǔ)償,以提高運(yùn)動(dòng)精度,這也驗(yàn)證了本文提出的算法的重要性。

圖13 規(guī)劃和實(shí)驗(yàn)結(jié)果對(duì)比Fig.13 Comparison between the planning and experiment results

6 結(jié) 論

本文針對(duì)能在極大范圍內(nèi)擴(kuò)展工作空間的基于移動(dòng)機(jī)器人的半物理系統(tǒng)進(jìn)行研究,面向引入移動(dòng)機(jī)器人后可能引起的精度下降的問題,在規(guī)劃層面提前預(yù)留空間,為后續(xù)通過高精度外部測量設(shè)備反饋和各子系統(tǒng)之間地實(shí)時(shí)運(yùn)動(dòng)補(bǔ)償創(chuàng)造了條件。主要工作可以分為以下三點(diǎn)：

1)設(shè)計(jì)了一套基于超冗余機(jī)械臂的整體半物理系統(tǒng)的規(guī)劃方法,將半物理系統(tǒng)中的移動(dòng)機(jī)器人和機(jī)械臂等效為一個(gè)串聯(lián)超冗余機(jī)械臂,利用超冗余機(jī)械臂的操作空間規(guī)劃方法對(duì)系統(tǒng)進(jìn)行規(guī)劃。

2)設(shè)計(jì)了用于改進(jìn)特定性能的超冗余機(jī)械臂逆解求解方法,求解時(shí)最大化超冗余機(jī)械臂的可操作度并且避免關(guān)節(jié)極限,為后續(xù)控制過程中的機(jī)械臂主動(dòng)補(bǔ)償移動(dòng)平臺(tái)誤差創(chuàng)造了充足的條件。

3)在自行搭建的基于移動(dòng)機(jī)器人的半物理系統(tǒng)上進(jìn)行了實(shí)驗(yàn)驗(yàn)證,實(shí)驗(yàn)結(jié)果驗(yàn)證了所提出的規(guī)劃方法的必要性和有效性。