基于避障偽距離的自由漂浮空間機(jī)器人規(guī)劃-跟蹤一體化控制

羊 帆，張國(guó)良，田 琦，王保明

(1. 火箭軍工程大學(xué)，西安 710025； 2. 寶雞市高新技術(shù)研究所，寶雞 721000; 3. 成都信息工程大學(xué)，成都 610225)

0 引 言

伴隨著人類探索太空進(jìn)程的加快，利用空間機(jī)器人代替人類完成相關(guān)在軌服務(wù)任務(wù)，成為目前空間技術(shù)研究的方向之一。自由漂浮空間機(jī)器人(Free-Floating Space Robot, FFSR)由于其不需消耗額外能量穩(wěn)定基座位姿，具有較好的長(zhǎng)期在軌服務(wù)能力，成為目前空間機(jī)器人研究的主要對(duì)象[1-2]。

不同于地面機(jī)械臂系統(tǒng)， FFSR特有的漂浮基座特性使其規(guī)劃和控制問(wèn)題更為復(fù)雜。為此，一些學(xué)者開(kāi)展了相關(guān)研究并取得了豐碩的研究成果。在控制問(wèn)題的研究上，文獻(xiàn)[3]在對(duì)FFSR運(yùn)動(dòng)學(xué)模型分析的基礎(chǔ)上，提出了分解速度和加速度控制方法。文獻(xiàn)[4]在FFSR動(dòng)力學(xué)建模的基礎(chǔ)上，利用PD控制實(shí)現(xiàn)了FFSR的計(jì)算力矩控制。文獻(xiàn)[5]利用FFSR擴(kuò)展動(dòng)力學(xué)模型提出了規(guī)范形式擴(kuò)展自適應(yīng)控制方法方法。文獻(xiàn)[6]利用積分降階技術(shù)，將FFSR動(dòng)力學(xué)方程進(jìn)行低通濾波，改進(jìn)了文獻(xiàn)[5]所提方法，從而避免加速度信號(hào)的測(cè)量。文獻(xiàn)[7]研究了同時(shí)存在運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)參數(shù)不確定情況的自適應(yīng)控制方法。文獻(xiàn)[8]以桿件最近距離為約束條件，基于NMPC理論提出了FFSR的避障控制方法。此外，一些智能控制方法如神經(jīng)網(wǎng)絡(luò)[9]、確定學(xué)習(xí)[10-11]、模糊控制[12]被研究者采用以實(shí)現(xiàn)FFSR的軌跡跟蹤控制。另一方面，針對(duì)FFSR的非完整約束以及存在動(dòng)力學(xué)奇異問(wèn)題，其軌跡規(guī)劃方法被學(xué)者所重視，提出了圓周運(yùn)動(dòng)方法[13]，雙向規(guī)劃法[14]，增強(qiáng)干擾圖[15]等經(jīng)典方法。在這些方法基礎(chǔ)上，文獻(xiàn)[16]利用PSO優(yōu)化實(shí)現(xiàn)了FFSR在抓取后基座姿態(tài)的穩(wěn)定，進(jìn)一步在文獻(xiàn)[17]提出了FFSR的自主路徑規(guī)劃方法。文獻(xiàn)[18]提出了笛卡爾空間下FFSR點(diǎn)到點(diǎn)運(yùn)動(dòng)的規(guī)劃方法。文獻(xiàn)[19]提出了一種最小姿態(tài)干擾的目標(biāo)接觸軌跡規(guī)劃方法。文獻(xiàn)[20]基于人工勢(shì)場(chǎng)法，對(duì)地面機(jī)械臂避障規(guī)劃方法進(jìn)行擴(kuò)展用于FFSR的末端避障規(guī)劃。文獻(xiàn)[21]提出了一種針對(duì)滾動(dòng)目標(biāo)的避碰規(guī)劃方法，實(shí)現(xiàn)了對(duì)滾動(dòng)目標(biāo)的末端同步接近規(guī)劃。文獻(xiàn)[22]將多項(xiàng)式插值與PSO方法相結(jié)合提出了一種FFSR關(guān)節(jié)空間下的非完整路徑規(guī)劃方法，從而保證關(guān)節(jié)軌跡的連續(xù)平滑。

雖然上述規(guī)劃和控制方法的研究初步解決了FFSR運(yùn)動(dòng)控制問(wèn)題，但是依然存在一些不足。首先，在規(guī)劃方法上，多考慮在非完整約束下，F(xiàn)FSR的機(jī)械臂運(yùn)動(dòng)與基座姿態(tài)的相互關(guān)系以及動(dòng)力學(xué)奇異回避問(wèn)題，未考慮FFSR運(yùn)動(dòng)過(guò)程中的避障問(wèn)題。其次，在運(yùn)動(dòng)控制上，基本遵循離線規(guī)劃—軌跡跟蹤思想，但是由于傳感器探測(cè)距離，目標(biāo)移動(dòng)以及可能存在的避障需求，在工程上這一思路無(wú)法有效實(shí)現(xiàn)FFSR的運(yùn)動(dòng)控制。最后，少數(shù)如文獻(xiàn)[8]提出的避障控制方法，也存在避障約束構(gòu)造困難等問(wèn)題。

為此，針對(duì)FFSR的避障規(guī)劃與控制問(wèn)題，本文提出一種FFSR的避障規(guī)劃-跟蹤一體化控制方法。首先，基于障礙物偽距離技術(shù)[23]，采用FFSR逆幾何模型求解期望末端位姿下的連桿偽距離估計(jì)值構(gòu)造避障約束條件，進(jìn)而通過(guò)求解非線性優(yōu)化問(wèn)題，獲得FFSR避障期望軌跡。其次，將全局軌跡規(guī)劃與局部在線避障相結(jié)合，輔以離散狀態(tài)依賴?yán)锟ㄌ岱匠?Discrete State Dependent Riccati Equation DSDRE)控制方法實(shí)現(xiàn)FFSR的避障規(guī)劃-跟蹤一體化控制，以克服傳統(tǒng)離線規(guī)劃在跟蹤所存在的對(duì)動(dòng)態(tài)障礙，未知障礙避障能力不足等問(wèn)題。

1 模型與假設(shè)

1.1 空間機(jī)器人模型

1)FFSR的運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型

FFSR一般由基座和機(jī)械臂組成，由于基座處于漂浮狀態(tài)，系統(tǒng)無(wú)外力輸入，滿足動(dòng)量守恒，基座運(yùn)動(dòng)與機(jī)械臂運(yùn)動(dòng)相耦合[3-4]。

典型的空間機(jī)器人運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型為

(1)

FFSR在軌工作時(shí)，由于其姿態(tài)控制系統(tǒng)關(guān)閉，工作過(guò)程中無(wú)外力輸入，由動(dòng)量守恒原理，系統(tǒng)(1)還滿足：

(2)

其中，Μ0表示系統(tǒng)的初始動(dòng)量，包括初始線動(dòng)量和初始角動(dòng)量。

2)FFSR幾何模型

考慮如圖1所示的空間機(jī)器人一般模型，由n個(gè)連桿機(jī)械臂及作為其基座的航天器平臺(tái)共n+1個(gè)剛體組成。其中，剛體B0代表空間機(jī)器人基座，剛體Bi(i=1,…,n)按照從與基座相連的第一個(gè)剛體開(kāi)始向機(jī)械臂末端進(jìn)行編號(hào)，依次代表機(jī)械臂的第i個(gè)連桿。連桿i-1與連桿i之間通過(guò)關(guān)節(jié)Ji相連接，第i個(gè)連桿質(zhì)心記為Ci，基座質(zhì)心記為C0。

則FFSR末端位姿在慣性系下可表示為：

ye=IA00A11A2…n-1AnΣE=
h(q1,q2,…,qn,qb)

(3)

其中，iAi+1為坐標(biāo)系Σi+1到坐標(biāo)系Σi的位姿轉(zhuǎn)移矩陣；IA0為基座坐標(biāo)系統(tǒng)到慣性系的坐標(biāo)轉(zhuǎn)移矩陣；ye為慣性系下末端位姿。

則式(3)為FFSR的幾何模型，進(jìn)一步可得FFSR逆幾何模型

qm=h*(qb,ye)

一般而言，上式求解很復(fù)雜甚至無(wú)法求解，通常在已知末端與基座位姿情況采用迭代求解式(3),獲得qm。

1.2 障礙建模

文獻(xiàn)[23]提出了障礙物的偽距離建模方法，該方法將空間中的障礙規(guī)則化為空間中的橢球、椎體、圓柱、方體等幾何體，進(jìn)而通過(guò)求解規(guī)則化障礙物表面方程函數(shù)值作為確定機(jī)械臂與障礙物之間關(guān)系的依據(jù)。

由于橢圓，橢球具有較好的逼近真實(shí)障礙常常作為障礙描述的基本形狀。若利用空間位置坐標(biāo)x,y,z的標(biāo)量方程替代橢球方程中的半軸參數(shù)a,b,c，則可在橢球方程基礎(chǔ)上表征復(fù)雜障礙的表面方程，障礙物表面一般方程具有如下形式：

(4)

1)空間中點(diǎn)與障礙物關(guān)系

將空間中任意一點(diǎn)?X0∈R3代入障礙物表面方程(4)中可得S(X0)，則點(diǎn)X0與障礙關(guān)系可由S(X0)函數(shù)值表示為：

若S(X0)=0，則點(diǎn)X0位于障礙物表面；

若S(X0)>0，則點(diǎn)X0位于障礙物外；

若S(X0) < 0，則點(diǎn)X0位于障礙物內(nèi)；

且有S(X0)越大點(diǎn)與障礙物實(shí)際距離越遠(yuǎn)，故可將S(X0)稱為點(diǎn)X0與障礙物的偽距離。

2)連桿與障礙物關(guān)系

如圖2所示，t時(shí)刻考慮連桿Ji,Ji+1上點(diǎn)Xm可表示為如下關(guān)系：

Xm=ri+α(ri+1-ri)

(5)

其中，rm,ri,ri+1∈R3,為XmJi,Ji+1相對(duì)于參考系的位置矢量，標(biāo)量α∈[0,1]為點(diǎn)位置參數(shù)。

考慮將障礙物ob規(guī)則化為一橢球體，由公式(4)，可得點(diǎn)Xm與障礙物之間的偽距離為：

(6)

其中，矩陣Qm,向量Bm，標(biāo)量Cm為由橢球表面方程中半軸參數(shù)a,b,c導(dǎo)出。

由公式(5)、(6)并有U=ri-ri+1可得：

H(λ)= (ri+αU)TQm(ri+αU)+

(7)

則連桿Ji,Ji+1與障礙物ob之間的最短距離滿足

(8)

進(jìn)一步可獲得連桿避障最小距離點(diǎn)位置參數(shù)：

(9)

1.3 問(wèn)題描述及假設(shè)

本文一體化控制器設(shè)計(jì)主要目的是通過(guò)設(shè)計(jì)FFSR末端軌跡規(guī)劃-跟蹤控制器，實(shí)現(xiàn)FFSR由起點(diǎn)至目標(biāo)的避障軌跡控制。

為便于問(wèn)題研究設(shè)計(jì)且不失一般性，提出如下假設(shè)：

1)FFSR屬于非固定基座機(jī)械臂系統(tǒng)，為便于規(guī)劃器設(shè)計(jì)，假設(shè)在兩次很短的采樣間隔內(nèi)基座位置姿態(tài)變化很小。即忽略兩次采樣間的基座位姿變化。

注1文獻(xiàn)[4]中指出對(duì)于FFSR而言，基座相對(duì)于機(jī)械臂的質(zhì)量越大，F(xiàn)FSR的運(yùn)動(dòng)學(xué)特性越接近地面固定機(jī)械臂，實(shí)際中通?；倔w質(zhì)量遠(yuǎn)大于機(jī)械臂連桿質(zhì)量，因此在很短的采樣間隔內(nèi)上述假設(shè)并不失一般性。

4)系統(tǒng)均為剛體，忽略微重力影響。

2 偽距離求解和避障約束

為實(shí)現(xiàn)FFSR的避障規(guī)劃，在本文中基于障礙物的偽距離模型，采用FFSR逆幾何模型(IGM)求解FFSR的連桿最近偽距離估計(jì)，進(jìn)而獲得FFSR的避障約束條件。

2.1 基于IGM的連桿最近偽距離估計(jì)

在傳統(tǒng)的6DOF固定基座機(jī)械臂系統(tǒng)中，若末端位姿確定，可通過(guò)迭代求解機(jī)械臂的IGM獲得關(guān)節(jié)角運(yùn)動(dòng)的唯一解。由于FFSR特有的漂浮基座特性，使得該方法受到一定限制，由文獻(xiàn)[4]可知，若FFSR基座相對(duì)于機(jī)械臂連桿質(zhì)量越大其動(dòng)力學(xué)特性越接近于固定機(jī)械臂系統(tǒng)。為此，本文在獲得基座位姿反饋情況下，結(jié)合假設(shè)1條件，在一定誤差允許范圍內(nèi)通過(guò)IGM求解連桿偽距離估計(jì)。

由假設(shè)1，可得t=kT時(shí)，期望關(guān)節(jié)角

qmr(k)=h*(qbk,yer(k))

(10)

其中，h*(qb,xe)為FFSR的IGM。

則迭代求解式(10)以及FFSR幾何模型可求得t=kT時(shí)，第i關(guān)節(jié)期望位姿

怎樣捕捉人物的“神”；各種燈光的打法；氛圍營(yíng)造；強(qiáng)大的后期處理；攝影美學(xué)理論；市場(chǎng)現(xiàn)狀和流行趨勢(shì)；現(xiàn)場(chǎng)拍攝及創(chuàng)作指導(dǎo)；犀利點(diǎn)評(píng)；獲獎(jiǎng)經(jīng)驗(yàn)；等等。

(11)

其中，0hi(qb,qm)為慣性系中第i關(guān)節(jié)相對(duì)于慣性系的正向幾何模型。

由式(6)，(9)，(11)可得，t=kT期望末端位姿的連桿最近偽距離估計(jì)為

(12)

其中，g(qbk,yer(k))為由式(11)，(9)推得的最近偽距離計(jì)算函數(shù)。

2.2 避障約束

為實(shí)現(xiàn)FFSR末端位姿運(yùn)動(dòng)過(guò)程中的避障，在本節(jié)中定義兩個(gè)偽距離

定義1：FFSR障礙物安全偽距離，Duf=[duf1,duf2…dufn]T。

當(dāng)連桿偽距離D≤Duf時(shí)，認(rèn)為FFSR發(fā)生碰撞。

定義2：FFSR避障處理偽距離，Dif=[dif1,dif2…difn]T。

(本文中D≤Duf(if)表示D中每一個(gè)元素均不大于Duf(if)中每一個(gè)元素。)

則由定義1、定義2可知FFSR的末端及連桿運(yùn)動(dòng)可以進(jìn)入由Dif構(gòu)成的區(qū)域，但不能進(jìn)入由Duf構(gòu)成的區(qū)域。為了在末端軌跡規(guī)劃器中引入避障約束，引入下述罰函數(shù)：

p(D)=[p1(d1),p2(d2)…pn(dn)]T

(13)

其中,

(14)

則，式(13)、(14)構(gòu)成了避障規(guī)劃的約束函數(shù)。

注2.式(13)、(14)說(shuō)明當(dāng)各連桿的pi(di)越小，則FFSR距離障礙物越遠(yuǎn)，反之越接近障礙，故避障問(wèn)題求解中，只需滿足各連桿pi(di)極小即可。

3 規(guī)劃-跟蹤控制器設(shè)計(jì)

本文中規(guī)劃-跟蹤控制器設(shè)計(jì)分為規(guī)劃器和控制器兩部分。首先根據(jù)目標(biāo)點(diǎn)位姿通過(guò)規(guī)劃器產(chǎn)生FFSR的末端避障軌跡規(guī)劃，然后將規(guī)劃軌跡結(jié)果引入FFSR的末端跟蹤控制器中，實(shí)現(xiàn)FFSR的控制。最后基于FFSR相關(guān)運(yùn)動(dòng)反饋信息，計(jì)算偽距離，產(chǎn)生避障約束修正規(guī)劃軌跡。

3.1 規(guī)劃器設(shè)計(jì)

3.1.1全局規(guī)劃器設(shè)計(jì)

若FFSR處于非避障工作空間，則規(guī)劃當(dāng)前點(diǎn)與終點(diǎn)之間的末端運(yùn)動(dòng)軌跡，可采用5次多項(xiàng)式(15)規(guī)劃

(15)

滿足如下約束條件：

(16)

則根據(jù)式(15)、(16)將全局軌跡規(guī)劃問(wèn)題，轉(zhuǎn)化為式(17)所示的非線性尋優(yōu)問(wèn)題。

minf(x)=tf,x=[A0,A1,…A5,tf]T

s.t.

(17)

其中Ai為多項(xiàng)式系數(shù)矩陣。

求解式(17)的非線性優(yōu)化問(wèn)題，即可獲得全局規(guī)劃的軌跡的5次多項(xiàng)式系數(shù)，進(jìn)而獲得全局規(guī)劃軌跡。則全局規(guī)劃器為

(18)

3.1.2避障規(guī)劃器設(shè)計(jì)

考慮tk時(shí)，F(xiàn)FSR任意連桿或末端偽距離di≤difi，則表明FFSR進(jìn)入障礙影響區(qū)，原全局規(guī)劃軌跡可能會(huì)造成FFSR的碰撞，因此需要進(jìn)行避障軌跡規(guī)劃重新確定yer(tk)使得FFSR既有一定的避障能力，同時(shí)又保證FFSR末端位姿向目標(biāo)運(yùn)動(dòng)。

考慮yer(tk+1)與yer(tk)存在如下關(guān)系

(19)

其中，T=tk+1-tk為采樣間隔。

由式(13)、(14)及(19)，可確定避障規(guī)劃目標(biāo)函數(shù)為

(20)

為保證控制力矩的連續(xù)性，以及使FFSR具有一定的避障能力，在式(20)目標(biāo)函數(shù)的基礎(chǔ)上給出相應(yīng)的求解約束條件,

1)加速度約束

由式(20)可知加速度a的搜索范圍一方面決定了FFSR避障規(guī)劃的可行解空間,另一方面也影響著進(jìn)入障礙影響區(qū)域后FFSR進(jìn)行避障運(yùn)動(dòng)的控制力矩大小。若a的搜索范圍較大，則FFSR可以獲得更大的避障能力，相應(yīng)的控制力矩也較大。反之，若a選擇較小的搜索范圍，則FFSR的控制力矩更光滑，其避障能力相對(duì)減弱。為此，給定如下加速度約束。

(21)

注4.通過(guò)調(diào)整kl,kA的值可以實(shí)現(xiàn)加速度搜索空間的調(diào)整，kl,kA可根據(jù)FFSR控制力矩輸出限制、Dif和Duf所確定的障礙區(qū)域大小以及FFSR運(yùn)動(dòng)時(shí)間綜合考慮。

2)錐形搜索區(qū)域約束

FFSR的期望避障規(guī)劃軌跡，不僅應(yīng)使FFSR具有一定的避障能力，同時(shí)又要保證其趨向于目標(biāo)位姿運(yùn)動(dòng)。雖然在式(20)的目標(biāo)函數(shù)中通過(guò)權(quán)系數(shù)γ,βi調(diào)整可以在一定程度上滿足要求，為縮短FFSR末端運(yùn)動(dòng)時(shí)間,在加速度約束的基礎(chǔ)上增加錐形搜索約束。

(22)

至此，則根據(jù)式(20)、(21)以及(22)將避障規(guī)劃問(wèn)題，轉(zhuǎn)化為如下非線性優(yōu)化問(wèn)題。

s.t.

(23)

3.2 控制器設(shè)計(jì)

由FFSR運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，可得

(24)

則采用歐拉法對(duì)式(24)，進(jìn)行離散化可得：

(25)

其中Ad=TA+I，Bd=TB，Cd=C，xk=x(kT)，I為單位陣，T為采樣間隔。

由SDRE相關(guān)理論可知，當(dāng)J*可逆時(shí)，Ad,Bd,Cd為系統(tǒng)(24)的狀態(tài)依賴系數(shù)(State Dependent Coefficient, SDC)矩陣[24]。

為設(shè)計(jì)DSDRE優(yōu)化跟蹤控制器，引入滑膜變量z。

定義3：滑膜變量z滿足

(26)

其中，ek表示跟蹤誤差。λ為滑膜增益，其滿足λ=diag(λ1,λ2,…,λn),λi>0。

由式(24)，(26),系統(tǒng)輸出方程可寫(xiě)為

(27)

則由式(27)可重寫(xiě)系統(tǒng)(25)的期望跟蹤軌跡如下：

(28)

設(shè)計(jì)目標(biāo)函數(shù)為

(29)

則由DSDRE方法可得控制律為

(30)

注5：關(guān)于Pk+1，sk的求解，根據(jù)DSDRE理論可按照無(wú)限時(shí)間調(diào)節(jié)器理論方法求解上述黎卡提方程，此時(shí)認(rèn)為Pk+1=Pk,sk+1=sk。由式(25)表明控制律(30)使得limt→∞z→0，則FFSR末端運(yùn)動(dòng)軌跡能夠?qū)崿F(xiàn)對(duì)期望末端軌跡的跟蹤。Q,R為適當(dāng)維數(shù)的正定矩陣。

定理1. 對(duì)于具有不確定參數(shù)的FFSR系統(tǒng)(25)，滿足假設(shè)1)～4)及相應(yīng)的SDC條件，則存在控制律(30)使得FFSR末端運(yùn)動(dòng)軌跡漸進(jìn)穩(wěn)定跟蹤期望軌跡。

證. 由系統(tǒng)(25)及控制律(30)，系統(tǒng)閉環(huán)方程可寫(xiě)為

(31)

閉環(huán)方程(31)包含反饋?lái)?xiàng)與前饋?lái)?xiàng)，由于前饋?lái)?xiàng)不影響Pk的求解，閉環(huán)系統(tǒng)穩(wěn)定性僅與反饋?lái)?xiàng)相關(guān)，故可得系統(tǒng)閉環(huán)反饋方程為：

xk+1=Adkxk-AdkFkxk=Adk(I-Fk)xk

(32)

構(gòu)造Lyapunov 函數(shù)

其中V(xk)>0,?xk≠0并且有xk=0，V(xk)=0。則有

Pk+1FkAdk]xk。

由于R>0，故可得V(xk+1)-V(xk)≤0。表明系統(tǒng)在標(biāo)稱控制律作用下可實(shí)現(xiàn)閉環(huán)反饋系統(tǒng)的漸進(jìn)穩(wěn)定。

注6.定理1成立需要系統(tǒng)(1)滿足SDC條件，即保證FFSR的廣義雅克比矩陣非奇異。為此，本文將奇異點(diǎn)等效為一個(gè)障礙，計(jì)算跟蹤過(guò)程中廣義雅克比矩陣的最小奇異值作為等效障礙的偽距離。通過(guò)選取合適等效避障處理偽距離，采用在線避障規(guī)劃方法實(shí)現(xiàn)回避奇異。

3.3 規(guī)劃-跟蹤一體化控制算法

限于傳感器觀測(cè)范圍等因素，采用傳統(tǒng)離線規(guī)劃再跟蹤控制方法無(wú)法有效實(shí)現(xiàn)FFSR的避障運(yùn)動(dòng)控制，為此本文將避障規(guī)劃和優(yōu)化跟蹤相結(jié)合提出FFSR規(guī)劃-跟蹤一體化控制算法，流程如圖3所示。

為了進(jìn)一步說(shuō)明FFSR的規(guī)劃-跟蹤一體化控制方法，給出算法步驟：

5)利用式(26)求解優(yōu)化跟蹤控制律，控制FFSR運(yùn)動(dòng)。

6)是否到達(dá)目標(biāo)位姿。若到達(dá)目標(biāo)位姿停止，反之轉(zhuǎn)步驟7)。

7)k+1→k。轉(zhuǎn)步驟2)。

4 數(shù)值仿真

以文獻(xiàn)[25]所提的6R空間機(jī)器人為例，驗(yàn)證所提方法的有效性。機(jī)器人結(jié)構(gòu)及參數(shù)與文獻(xiàn)[25]中相同。

給定系統(tǒng)初始與終止末端位姿分別為：

y0=[1.5950,-0.0095,-0.2166,0,-1.3962]T

yf=[1.0,0.0520,0,1,-1.3963,0]T

給定系統(tǒng)初始末端速度為0，基座初始位姿為0；關(guān)節(jié)角初始值為：qm0=[0,0,-1.04 rad,0,-0.34 rad,0]T

假定障礙中心坐標(biāo)為

PO=[1.3205m,0.06m,0.1195m]T;

設(shè)定障礙物安全偽距離為Duf=[1,1,1,1,1,1]T設(shè)定避障處理偽距離為Dif=[3,3,3,3,3,3]T

控制器選取權(quán)值矩陣：

仿真采用MATLAB Spacedyn工具箱[26]，仿真時(shí)間為25 s，結(jié)果如圖4～8所示。圖4為控制力矩;圖5為末端軌跡跟蹤誤差;圖6為機(jī)器人跟蹤過(guò)程中連桿運(yùn)動(dòng)示意圖;圖7為腕部和末端的偽距離變化;圖8為基座的位姿運(yùn)動(dòng)情況。

從圖5中可以看出一體化控制方法能夠?qū)崿F(xiàn)末端軌跡對(duì)期望軌跡的穩(wěn)定跟蹤，且能夠滿足跟蹤精度。若采用下式計(jì)算末端位姿控制精度，則etrack≤1×10-3。

其中，yef為實(shí)際終點(diǎn)末端位姿，yf為期望終點(diǎn)末端位姿。

圖4中可以明顯看出在4 s和8 s左右控制力矩發(fā)生了兩次振蕩，對(duì)比圖7可以發(fā)現(xiàn)FFSR在3.4 s左右其腕部進(jìn)入了障礙影響范圍，在8.8 s左右末端進(jìn)入障礙影響范圍。這反映出由于進(jìn)行在線避障規(guī)劃使控制力矩發(fā)生了較強(qiáng)烈的變化，從而控制FFSR運(yùn)動(dòng)進(jìn)行避障。從圖7可看到在短時(shí)間內(nèi)FFSR離開(kāi)了障礙區(qū)域，實(shí)現(xiàn)了避障。

圖6的連桿示意圖中整體反映了FFSR避障的桿件運(yùn)動(dòng)過(guò)程。圖8中的基座位姿變化可以看出，由于基座質(zhì)量相對(duì)較大， FFSR桿件運(yùn)動(dòng)導(dǎo)致基座位姿運(yùn)動(dòng)緩慢，基座位置變化較小。

5 結(jié) 論

針對(duì)FFSR的避障規(guī)劃與控制問(wèn)題，本文提出一種基于避障偽距離的FFSR的規(guī)劃-跟蹤一體化控制方法。將FFSR的在線避障規(guī)劃與優(yōu)化跟蹤控制相結(jié)合，解決了FFSR點(diǎn)到點(diǎn)的避障規(guī)劃與控制問(wèn)題。

[1] Flores-Abad A, Ma O, Pham K, et al. A review of space robotics technologies for on-orbit servicing[J]. Progress in Aerospace Sciences, 2014,68(8): 1-26.

[2] Xu W, Liang B, Xu Y. Survey of modeling, planning, and ground verification of space robotic systems[J]. Acta Astronautica, 2011,68(11-12): 1629-1649.

[3] Umetani Y, Yoshida K. Resolved motion rate control of space manipulators with generalized Jacobian matrix[J]. IEEE Transactions on Robotics & Automation, 2002,5(3): 303-314.

[4] Dubowsky S, Papadopoulos E. The kinematics, dynamics, and control of free-flying and free-floating space robotic systems[J]. Robotics & Automation IEEE Transactions 1993,9(5): 531-543.

[5] Gu YL, Xu Y. A normal form augmentation approach to adaptive control of space robot systems[J]. Dynamics & Control, 1995,5(3): 275-294.

[6] Parlaktuna O, Ozkan M. Adaptive control of free-floating space robots in Cartesian coordinates[J]. Advanced Robotics, 2004,18(9): 943-959.

[7] Wang H, Xie Y. Passivity based adaptive Jacobian tracking for free-floating space manipulators without using spacecraft acceleration[J]. Automatica, 2009,45(6): 1510-1517.

[8] Wang M, Luo J, Walter U. A non-linear model predictive controller with obstacle avoidance for a space robot[J]. Advances in Space Research, 2016,57(8): 1737-1746.

[9] 史也. 空間機(jī)器人自主捕獲目標(biāo)的軌跡規(guī)劃與控制研究[D]: 哈爾濱工業(yè)大學(xué), 2013. [Shi Ye. Research on trajectory planning and control of Space Robot for autonomous capturing target[D].Harbin： Institute of Technology, 2013]

[10] 吳玉香, 王聰. 基于確定學(xué)習(xí)的機(jī)器人任務(wù)空間自適應(yīng)神經(jīng)網(wǎng)絡(luò)控制[J]. 自動(dòng)化學(xué)報(bào), 2013,39(6): 806-815. [Wu Yu-Xiang, Wang Cong. Deterministic learning based adaptive network control of robot in task space[J]. Acta Automatica Sinica, 2013, 39(6): 806-815.]

[11] 楊飛飛, 王聰, 曾瑋. 空間機(jī)械臂的確定性學(xué)習(xí)控制[J]. 宇航學(xué)報(bào), 2015,(07): 811-818.[Yang Fei-fei, Wang Cong, Zeng Wei. Learning from Adaptive Control of a Class of Space Manipulator Systems[J]. Journal of Astronautics, 2015,(07): 811-818.]

[12] 張福海, 付宜利, 王樹(shù)國(guó). 慣性參數(shù)不確定的自由漂浮空間機(jī)器人自適應(yīng)控制研究[J]. 航空學(xué)報(bào), 2012,33(12): 2347-2354. [Zhang Fu-hai, Fu Yi-li, Wang Shu-guo. Adaptive control of free-floating space robot with inertia parameter uncertainties. Acta Aeronautica et Astronautica Sinica, 2012, 33(12):2347-2354.]

[13] Vafa Z, Dubowsky S. On the dynamics of space manipulators using the virtual manipulator, with applications to path planning[J]. 1993,38(4): 45-76.

[14] Nakamura Y, Mukherjee R. Nonholonomic path planning of space robots via a bidirectional approach[J]. IEEE Transactions on Robotics & Automation, 2002,7(4): 500-514.

[15] Dubowsky S, Torres M A. Path planning for space manipulators to minimize spacecraft attitude disturbances[C]. Proceedings of the IEEE International Conference on Robotics and Automation, Sacramento,CA,1991.

[16] Xu W, Li C, Liang B, et al. Target berthing and base reorientation of free-floating space robotic system after capturing[J]. Acta Astronautica, 2009,64(2): 109-126.

[17] Xu W, Liang B, Li C, et al. Autonomous target capturing of free-floating space robot: theory and experiments[J]. Robotica, 2009,27(3): 425-445.

[18] Tortopidis I, Papadopoulos E. On point-to-point motion planning for underactuated space manipulator systems[J]. Robotics & Autonomous Systems, 2007,55(2): 122-131.

[19] Zhang L, Jia Q, Chen G, et al. Pre-impact trajectory planning for minimizing base attitude disturbance in space manipulator systems for a capture task[J]. Chinese Journal of Aeronautics, 2015,28(4): 1199-1208.

[20] Liu S, Zhang Q, Zhou D. Obstacle avoidance path planning of space manipulator based on improved artificial potential field method[J]. Journal of the Institution of Engineers, 2014,95(1): 31-39.

[21] Luo J, Zong L, Wang M, et al. Optimal capture occasion determination and trajectory generation for space robots grasping tumbling objects[J]. Acta Astronautica, 2017,136: 380-386.

[22] 石忠, 王永智, 胡慶雷. 基于多項(xiàng)式插值的自由漂浮空間機(jī)器人軌跡規(guī)劃粒子群優(yōu)化算法[J]. 宇航學(xué)報(bào), 2011,32(7): 1516-1521.[Shi Zhong, Wang Yong-zhi, Hu Qing-lei. A polynomial interpolation based particle swarm optimization algorithm for trajectory planning of free-floating space robot[J]. Journal of Astronautics, 2011,32(7): 1516-1521.]

[23] Perdereau V, Passi C, Drouin M. Real-time control of redundant robotic manipulators for mobile obstacle avoidance[J]. Robotics & Autonomous Systems, 2002,41(1): 41-59.

[24] ?imen T. State-dependent riccati equation (SDRE) control: A survey[J]. Ifac Proceedings Volumes, 2008,41(2): 3761-3775.

[25] 徐文福. 空間機(jī)器人目標(biāo)捕獲的路徑規(guī)劃與實(shí)驗(yàn)研究[D].哈爾濱: 哈爾濱工業(yè)大學(xué), 2007. [ Xu Wen-fu, Path planning and experiment study of space robot for target capturing[D].Harbin: Harbin Institute of Technology, 2007.]

[26] Yoshida K. The SpaceDyn : A MATLA toolbox for space and mobile robots[J]. Journal of the Society of Instrument & Control Engineers, 1999,38(7): 138-143.