葛佳昊，劉莉，董欣心，田維勇，陸天和

北京理工大學(xué) 宇航學(xué)院，北京 100081

隨著人類對(duì)宇宙空間探索的不斷深入，高效的后勤保障成為空間在軌服務(wù)的熱點(diǎn)方向之一[1]，空間機(jī)器人成為低成本在軌運(yùn)行的關(guān)鍵[2]。根據(jù)機(jī)器人基座的控制策略不同，可將空間機(jī)器人分為基座位姿固定、基座位姿機(jī)動(dòng)、基座姿態(tài)受控和自由漂浮等模式[3]。自由漂浮空間機(jī)器人因節(jié)省燃料、姿態(tài)調(diào)整靈活等優(yōu)點(diǎn)，有更高的實(shí)際應(yīng)用價(jià)值，其軌跡規(guī)劃問題是關(guān)鍵技術(shù)之一。

自由漂浮空間機(jī)器人(Free Floating Space Robot，F(xiàn)FSR)軌跡規(guī)劃即對(duì)機(jī)械臂末端機(jī)械手的運(yùn)動(dòng)軌跡規(guī)劃，需要在滿足多體動(dòng)力學(xué)系統(tǒng)動(dòng)力學(xué)特性和非完整約束條件的同時(shí)避開障礙并到達(dá)目標(biāo)位置，F(xiàn)FSR工作環(huán)境比地面機(jī)器人更為惡劣，軌跡規(guī)劃更為復(fù)雜。關(guān)于FFSR模型，Vafa和Dubowsky[4]提出了基于虛擬機(jī)械臂(Virtual Manipulator，VM)的分析建模方法，實(shí)現(xiàn)了對(duì)FFSR運(yùn)動(dòng)/動(dòng)力學(xué)方程的更簡(jiǎn)易描述。Dubowsky和Papadopoulos[5]建立了完整的自由飛行和自由漂浮機(jī)器人運(yùn)動(dòng)/動(dòng)力學(xué)和控制模型。梁斌等[6]提出動(dòng)態(tài)等效機(jī)械手(Dynamically Equivalent Manipulator，DEM)的概念，將FFSR轉(zhuǎn)化為基座鉸接的機(jī)械臂，以克服VM不能直觀模擬空間機(jī)械臂的缺點(diǎn)。

根據(jù)規(guī)劃模型的不同，F(xiàn)FSR的軌跡規(guī)劃可大致分為基于運(yùn)動(dòng)/動(dòng)力學(xué)模型規(guī)劃和基于幾何模型規(guī)劃兩類。

基于運(yùn)動(dòng)學(xué)模型的規(guī)劃通常借助智能算法對(duì)FFSR軌跡規(guī)劃進(jìn)行求解，如粒子群優(yōu)化算法[7-9]、遺傳算法[10-11]和差分進(jìn)化算法[12]等智能算法被用來規(guī)劃得到單臂和雙臂機(jī)器人運(yùn)動(dòng)軌跡；黃興宏等[13]將非完整約束軌跡規(guī)劃問題轉(zhuǎn)化為最優(yōu)控制問題，并采用牛頓迭代法得到全程恒定基座姿態(tài)零擾動(dòng)的空間機(jī)械臂軌跡?；谶\(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型，F(xiàn)FSR軌跡規(guī)劃問題通常轉(zhuǎn)化為最優(yōu)控制問題，戈新生和陳凱捷[14]采用Legendre偽譜法實(shí)現(xiàn)機(jī)器人軌跡規(guī)劃；曾祥鑫[15]通過高斯偽譜法得到機(jī)器人移動(dòng)軌跡，并以此為輸入實(shí)現(xiàn)考慮機(jī)械臂撓性的FFSR姿態(tài)控制?；贒EM模型，Benevides和Grassi[16]引入雙向策略的快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Tree, RRT)算法得到等效機(jī)械手的無碰撞軌跡，但是難以還原成FFSR的關(guān)節(jié)運(yùn)動(dòng)情況。Xu等[17]針對(duì)旋轉(zhuǎn)目標(biāo)的抓取問題，提出面向區(qū)域捕獲的雙臂空間機(jī)器人協(xié)調(diào)路徑規(guī)劃方法。至此，考慮高維非完整約束的FFSR軌跡規(guī)劃問題已得到基本解決，但現(xiàn)有方法還存在著規(guī)劃效率較低的問題。

另一類為僅考慮機(jī)器人幾何模型的快速路徑規(guī)劃。賈慶軒等[18]采用A*算法實(shí)現(xiàn)了對(duì)機(jī)械臂軌跡的毫秒級(jí)避障規(guī)劃，滿足控制周期的實(shí)時(shí)性要求。陽涵疆[19]和鄒宇星[20]等基于機(jī)械臂關(guān)節(jié)構(gòu)形空間，分別采用RRT和改進(jìn)的概率路線圖法實(shí)現(xiàn)了對(duì)六自由度機(jī)械臂的無碰路徑規(guī)劃。謝碧云等[21]改進(jìn)了雙樹隨機(jī)樹搜索算法的目標(biāo)點(diǎn)樹根生成機(jī)制，在不考慮機(jī)械臂末端姿態(tài)的情況下，實(shí)現(xiàn)了對(duì)典型空間冗余機(jī)器人的路徑規(guī)劃。僅考慮幾何模型可以較快得到可行路徑，但規(guī)劃結(jié)果不滿足系統(tǒng)動(dòng)力學(xué)特性。

為提高考慮運(yùn)動(dòng)/動(dòng)力學(xué)模型的FFSR軌跡規(guī)劃效率，提出一種基于動(dòng)力學(xué)RRT*算法的FFSR軌跡規(guī)劃方法。首先，將FFSR的運(yùn)動(dòng)/動(dòng)力學(xué)方程偽線性化為機(jī)械手狀態(tài)方程，并設(shè)計(jì)同時(shí)考慮關(guān)節(jié)能量消耗和軌跡完成時(shí)間的代價(jià)函數(shù)。其次，為提高碰撞檢測(cè)效率，針對(duì)機(jī)械手初末位置間障礙，提出機(jī)械臂避障和機(jī)械手避障的兩層次避障策略，并設(shè)計(jì)FFSR動(dòng)力學(xué)RRT*軌跡規(guī)劃器。最后，以2自由度(Degree of Freedom，DoF)自由漂浮空間機(jī)器人的軌跡規(guī)劃為例進(jìn)行數(shù)值仿真。結(jié)果表明，采用動(dòng)力學(xué)RRT*方法可在較少的迭代步數(shù)內(nèi)得到可行解，隨迭代步數(shù)增加，可獲得漸進(jìn)最優(yōu)軌跡，具有較高的求解效率。

1 FFSR軌跡規(guī)劃問題假設(shè)及模型

選用單臂自由漂浮機(jī)器人通用模型，如圖1所示。圖中：CM為機(jī)器人的質(zhì)心位置，并將其選為慣性系坐標(biāo)原點(diǎn)；CMi為FFSR各部件的質(zhì)心位置；Pi為第i個(gè)機(jī)械臂關(guān)節(jié)；ρi為各部件質(zhì)心在慣性系的位矢；qi為各部件的相對(duì)轉(zhuǎn)角，正方向規(guī)定如圖1所示；ri為第i個(gè)部件質(zhì)心到下一個(gè)關(guān)節(jié)的位矢；li為第i個(gè)部件質(zhì)心到前一個(gè)關(guān)節(jié)的位矢。

圖1 單臂FFSR通用模型Fig.1 General model of single arm FFSR

1.1 FFSR模型

自由漂浮模式下，系統(tǒng)線動(dòng)量、角動(dòng)量守恒：

(1)

(2)

由式(1)和式(2)可得

(3)

末端機(jī)械手的坐標(biāo)可表示為

ρE=ρn+rn

(4)

對(duì)式(4)求導(dǎo)可得系統(tǒng)運(yùn)動(dòng)學(xué)方程為

(5)

式中：J(q)為FFSR的廣義Jacobian矩陣。

忽略重力，考慮系統(tǒng)拉格朗日方程和總動(dòng)能：

(6)

可得系統(tǒng)動(dòng)力學(xué)方程：

(7)

1) 忽略重力，忽略太陽風(fēng)等阻力，F(xiàn)FSR始終動(dòng)量守恒且初始動(dòng)量為0。

2) 忽略FFSR部件撓度，各部件間由一個(gè)單自由度關(guān)節(jié)連接，每個(gè)關(guān)節(jié)僅受一臺(tái)電機(jī)控制。

3) 系統(tǒng)質(zhì)量參數(shù)已知，運(yùn)動(dòng)狀態(tài)可觀測(cè)。

4) 基座質(zhì)量遠(yuǎn)大于機(jī)械臂桿質(zhì)量，且FFSR運(yùn)動(dòng)緩慢，故當(dāng)前節(jié)點(diǎn)的機(jī)器人位姿可作為到下一個(gè)節(jié)點(diǎn)間狀態(tài)的估計(jì)值。

1.2 系統(tǒng)線性化

由式(5)和式(7)可得

(8)

式中：H(q)為對(duì)稱正定矩陣，其逆矩陣必存在。

本研究結(jié)果也顯示，觀察組患者Baumann角、臨床效果、手術(shù)時(shí)間等均優(yōu)于對(duì)照組，差異有統(tǒng)計(jì)學(xué)意義（P＜0.05），這是因?yàn)樾∏锌谳o助復(fù)位經(jīng)皮克氏針固定治療中，切口能夠有效避免太靠上，減少對(duì)橈神經(jīng)的損傷，同時(shí)術(shù)中能夠有效保護(hù)干骺端骨膜，從肱骨小頭進(jìn)針會(huì)事先捫清確定尺神經(jīng)溝，進(jìn)而避免了因手術(shù)操作對(duì)尺神經(jīng)的損傷，術(shù)后會(huì)依據(jù)情況予以患者抗生素或者引流。

(9)

假設(shè)4)保證每個(gè)節(jié)點(diǎn)之間都可近似為線性系統(tǒng)。

1.3 代價(jià)函數(shù)

選取以權(quán)系數(shù)μ∈[0，1]加權(quán)的Bolza形式性能指標(biāo)作為代價(jià)函數(shù)，在應(yīng)用過程中，可通過調(diào)整權(quán)系數(shù)使FFSR具有更大工作靈活性。代價(jià)函數(shù)為

minJ=μJ1+(1-μ)J2

(10)

式中：J為總代價(jià)函數(shù)；J1和J2分別為使位姿調(diào)整過程中能量消耗最小的Lagrange型性能指標(biāo)和增強(qiáng)位姿調(diào)整的時(shí)效性的Mayer型性能指標(biāo)：

(11)

(12)

式中：tf為機(jī)器人位姿調(diào)整最終時(shí)間；R為指定的正定常數(shù)矩陣，代表控制輸入成本的權(quán)衡；t0為系統(tǒng)初始時(shí)刻。

2 FFSR系統(tǒng)的避障軌跡規(guī)劃

FFSR是典型的非完整多體動(dòng)力學(xué)系統(tǒng)，系統(tǒng)表現(xiàn)出的高維度和非完整特性增加了路徑規(guī)劃的難度與控制的復(fù)雜性。規(guī)劃時(shí)不應(yīng)只考慮幾何避障，也應(yīng)考慮系統(tǒng)動(dòng)力學(xué)特性。

對(duì)于實(shí)際應(yīng)用，強(qiáng)調(diào)軌跡規(guī)劃的效率和軌跡的可行性而非最優(yōu)性。相比于圖搜索方法(如A*和D*等)、局部規(guī)劃算法(如人工勢(shì)場(chǎng)法等)和求解最優(yōu)控制問題的偽譜法等，RRT*算法作為一種基于采樣的規(guī)劃方法，可通過重選父節(jié)點(diǎn)和重布線等操作降低路徑代價(jià)，有效提高算法效率，在考慮對(duì)象非完整約束限制的條件下，快速搜索解空間并得到可行軌跡，對(duì)FFSR軌跡規(guī)劃具有較高的適應(yīng)性。因此，加入機(jī)器人動(dòng)態(tài)約束的RRT*算法在收斂的時(shí)效性和高維問題求解的適應(yīng)性方面獨(dú)具優(yōu)勢(shì)。

2.1 最優(yōu)軌跡的計(jì)算

對(duì)于能控線性質(zhì)點(diǎn)系統(tǒng)，動(dòng)力學(xué)RRT*算法[23]通過求得最短抵達(dá)時(shí)間的閉式解，構(gòu)造末狀態(tài)固定、抵達(dá)時(shí)間自由的軌跡規(guī)劃問題，自由給定抵達(dá)時(shí)間以降低代價(jià)函數(shù)值。對(duì)于非線性系統(tǒng)，每次迭代時(shí)圍繞假定平衡點(diǎn)一階泰勒展開為能控線性系統(tǒng)。作為運(yùn)動(dòng)非線性強(qiáng)的多體動(dòng)力學(xué)系統(tǒng)，采用上述方法不能準(zhǔn)確描述FFSR運(yùn)動(dòng)狀態(tài)。

針對(duì)此問題，采用偽線性化方法得到解析的FFSR狀態(tài)空間模型，相比于文獻(xiàn)[23]，模型描述更精確，適用范圍更廣，迭代時(shí)只需代入新廣義關(guān)節(jié)角值即可更新模型，提升運(yùn)算速度，使規(guī)劃過程更簡(jiǎn)潔快速，規(guī)劃結(jié)果更加符合實(shí)際。

對(duì)于能控的離散動(dòng)力學(xué)系統(tǒng)：

(13)

式中：u(t)為系統(tǒng)輸入；x0為系統(tǒng)初始狀態(tài)。

2.1.1 最優(yōu)抵達(dá)時(shí)間求解[23]

對(duì)于末狀態(tài)x(tf)=x1固定、機(jī)器人位姿調(diào)整最終時(shí)間tf固定的最優(yōu)控制問題，構(gòu)造Lyapunov函數(shù)：

(14)

其閉式解為加權(quán)能控的Gramian矩陣：

exp(AT(t-t′))dt′

(15)

式中：t′為時(shí)間變量t之前的某時(shí)刻(t′

那么，最優(yōu)控制輸入為

u(t)=R-1BTexp(AT(tf-t))d(tf)

(16)

將式(16)代入式(13)，得到代價(jià)函數(shù)的閉式解：

(17)

(18)

經(jīng)式(18)計(jì)算得到最優(yōu)的抵達(dá)時(shí)間，那么，原問題擴(kuò)展為了末狀態(tài)固定且抵達(dá)時(shí)間自由的軌跡規(guī)劃問題。

2.1.2 最優(yōu)軌跡的計(jì)算[23]

令y(t)=exp(AT(tf-t))d(tf)，則有：

u(t)=R-1BTy(t)

(19)

將式(19)代入式(13)，得到復(fù)合微分方程：

(20)

從而得到最優(yōu)軌跡的狀態(tài)量與控制輸入為

(21)

(22)

至此，得到的狀態(tài)量和輸入量實(shí)現(xiàn)了對(duì)最優(yōu)軌跡的唯一確定。

2.2 FFSR避障策略

僅考慮機(jī)械手初末位置間存在障礙的情況。Rybus[24]總結(jié)了空間機(jī)器人避障的發(fā)展現(xiàn)狀，為進(jìn)一步簡(jiǎn)化計(jì)算，將障礙物抽象成長(zhǎng)方體(在2維空間內(nèi)為矩形)；為保證運(yùn)動(dòng)安全性，對(duì)障礙物進(jìn)行膨化作為真實(shí)障礙。

提出FFSR機(jī)械臂避障與機(jī)械手避障兩層次避障策略。若機(jī)械臂與障礙物干涉或機(jī)械手位于障礙物內(nèi)部，則認(rèn)為機(jī)器人與障礙碰撞。

2.2.1 機(jī)械臂避障策略

若每次通過求解機(jī)械臂幾何位置判斷是否與障礙干涉，會(huì)大大增加計(jì)算量，降低算法效率。因此，對(duì)于單臂自由漂浮機(jī)器人通用模型，結(jié)合工程應(yīng)用實(shí)際，采用障礙幾何延拓的策略避免機(jī)械臂碰撞。

如圖2所示，對(duì)原障礙進(jìn)行膨化，面a為膨化后障礙距離機(jī)器人最近的面，基于面a將障礙延拓至機(jī)械手可達(dá)最遠(yuǎn)端。那么，原可行的軌跡a、b中僅有軌跡a可行。

圖2 機(jī)械臂的避障策略Fig.2 Obstacle avoidance strategy of robotic arms

2.2.2 機(jī)械手避障策略

如圖3所示，對(duì)原障礙進(jìn)行膨化。機(jī)械手當(dāng)前的位置為點(diǎn)1，E為點(diǎn)1與障礙四角連線的包絡(luò)區(qū)域，其他點(diǎn)為機(jī)械手下一狀態(tài)點(diǎn)xnew。點(diǎn)2位于E外，點(diǎn)3位于E內(nèi)點(diǎn)1與障礙之間，點(diǎn)4位于障礙內(nèi)，點(diǎn)5位于E內(nèi)障礙后方。易得，位于點(diǎn)4、5時(shí)軌跡不成立。

圖3 機(jī)械手的避障策略Fig.3 Obstacle avoidance strategy of manipulator

2.3 動(dòng)力學(xué)RRT*規(guī)劃器

將FFSR偽線性模型與兩層次避障策略融入動(dòng)力學(xué)RRT*算法，使其具備對(duì)多體系統(tǒng)的規(guī)劃能力。設(shè)計(jì)基于FFSR多體運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)的RRT*算法求解器，以找到式(21)和式(22)定義的最優(yōu)軌跡。通過較少的迭代次數(shù)獲得軌跡的可行解，隨著迭代次數(shù)增加，軌跡逐步趨向最優(yōu)。

圖4為求解器節(jié)點(diǎn)生成及隨機(jī)樹更新示意圖圖中r為搜索子節(jié)點(diǎn)時(shí)設(shè)置的鄰域半徑。結(jié)合圖4，給出算法步驟流程如下。

步驟1設(shè)迭代次數(shù)k=1，xstart和xgoal分別作為軌跡的初末狀態(tài)點(diǎn)。

步驟2在第k次迭代時(shí)，在隨機(jī)樹上各狀態(tài)節(jié)點(diǎn)x處更新FFSR系統(tǒng)偽線性模型并獲得對(duì)應(yīng)的代價(jià)函數(shù)值cost[x]，以x為父節(jié)點(diǎn)生成新的隨機(jī)狀態(tài)點(diǎn)xrand。

步驟3碰撞檢測(cè)。若發(fā)生碰撞，則重新生成隨機(jī)狀態(tài)點(diǎn)并進(jìn)行步驟3；反之，設(shè)xrand為合法隨機(jī)狀態(tài)點(diǎn)xnew。

步驟5隨機(jī)樹重新布線。對(duì)已有狀態(tài)點(diǎn)及xgoal，在滿足避碰條件下，若cost[xnew,xnear]

步驟6判斷隨機(jī)樹是否已延伸至xgoal，若是，則可行軌跡生成，算法可停止；若否，則轉(zhuǎn)至步驟7。若希望完成全部迭代次數(shù)，則忽略步驟6，轉(zhuǎn)至步驟7。

步驟7根據(jù)節(jié)點(diǎn)間父子關(guān)系求解軌跡，更新隨機(jī)樹；k+1→k，轉(zhuǎn)至步驟2，如圖4(c)所示。

考慮到FFSR狀態(tài)方程的系統(tǒng)矩陣A為冪零矩陣，exp(At)可表達(dá)為一個(gè)n-1自由度的矩陣多項(xiàng)式；因此，由式(15)、式(17)、式(21)和式(22)可得到確定的閉式多項(xiàng)式解，最優(yōu)軌跡可解析表達(dá)。那么，規(guī)劃得到的軌跡也可用多項(xiàng)式平滑連接。與經(jīng)典RRT*算法不同的是，規(guī)劃過程中軌跡直接延伸至新的可用隨機(jī)點(diǎn)，而不是向新的可用隨機(jī)點(diǎn)前進(jìn)固定距離，這樣可以快速連接初末位姿，得到可行軌跡，進(jìn)而根據(jù)代價(jià)函數(shù)值更新或更新隨機(jī)樹以獲得更優(yōu)軌跡。

圖4 節(jié)點(diǎn)生成及隨機(jī)樹更新示意圖Fig.4 Schematic diagram of node generation and random tree updating

迭代期間通過式(3)和式(23)求解機(jī)器人狀態(tài)信息：

(23)

為提高運(yùn)算速度，算法采用Doolittle分解法對(duì)式(9)的H-1(q)進(jìn)行求解。

3 仿真驗(yàn)證與結(jié)果分析

為在驗(yàn)證所提方法有效性的同時(shí)不失一般性，選取簡(jiǎn)化的平面2連桿自由漂浮機(jī)器人作為研究對(duì)象，此模型能夠充分證明設(shè)計(jì)理論的概念和性能，已廣泛應(yīng)用于空間機(jī)器人理論驗(yàn)證[15]。

3.1 仿真參數(shù)和結(jié)果

系統(tǒng)的模型參數(shù)如表1所示。

表1 2連桿FFSR模型參數(shù)Table 1 Parameters of 2DoF FFSR model

仿真是基于PC機(jī)(3.41 GHz/Core8)上的MATLAB R2019b進(jìn)行。仿真迭代次數(shù)選取1 000次。對(duì)μ=0, 0.5, 1.0這3種目標(biāo)情況進(jìn)行數(shù)值仿真。仿真結(jié)果如表2和圖5所示。

表2 迭代1 000次的規(guī)劃結(jié)果Table 2 Planning results of 1 000 iterations

結(jié)合表2，從系統(tǒng)沿軌跡規(guī)劃結(jié)果運(yùn)動(dòng)過程圖(圖5(a)～圖5(c)，采用FFSR部件顏色的由淺至深表明系統(tǒng)從起點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)的時(shí)序變化)上看，生成的軌跡光滑且滿足約束條件，能有效避開障礙，并以預(yù)設(shè)的狀態(tài)到達(dá)目標(biāo)點(diǎn)。觀察可發(fā)現(xiàn)，能量消耗最低的軌跡最貼近障礙物，過程調(diào)整幅度最??；調(diào)整時(shí)間最短的軌跡偏離障礙物的距離最遠(yuǎn)，過程調(diào)整幅度最大。其原因在于:為了使能量消耗最低，電機(jī)扭矩的數(shù)值和變化幅度小(圖5(g))，姿態(tài)變化幅度小，使得軌跡完成的時(shí)間長(zhǎng)，軌跡貼近障礙；調(diào)整時(shí)間最短的軌跡則相反，電機(jī)的扭矩輸出和變化幅度較大，甚至到達(dá)扭矩的約束邊界(圖5(h))，F(xiàn)FSR姿態(tài)調(diào)整速度快，姿態(tài)調(diào)整幅度大(圖5(e))，使得軌跡完成的時(shí)間短，軌跡偏離障礙較遠(yuǎn)；能量消耗和調(diào)整時(shí)間折中的軌跡規(guī)劃結(jié)果則在姿態(tài)調(diào)整幅度(圖5(f))、所需時(shí)間和電機(jī)扭矩輸出(圖5(i))等方面指標(biāo)居中，符合代價(jià)函數(shù)設(shè)置預(yù)期。此外，對(duì)于不同權(quán)系數(shù)，初末位置關(guān)節(jié)角角速度和加速度近似為0，規(guī)劃結(jié)果符合工程應(yīng)用需要。

圖5 能量消耗最低、調(diào)整時(shí)間最短和兩者折中時(shí)的軌跡、關(guān)節(jié)角和電機(jī)扭矩Fig.5 Path, joint angle, and motor torque with minimum energy consumption, minimum adjusting time, and tradeoff between energy consumption and adjusting time

3.2 仿真結(jié)果分析

3.2.1 漸進(jìn)最優(yōu)性

為了驗(yàn)證規(guī)劃方法的漸進(jìn)最優(yōu)性，選取上述代價(jià)函數(shù)權(quán)系數(shù)μ=0.5的算例(迭代次數(shù)1 000 次)，最終代價(jià)函數(shù)值為2.901 6。代價(jià)函數(shù)值隨迭代次數(shù)增加的變化情況如圖6所示。結(jié)果表明，代價(jià)函數(shù)值隨迭代次數(shù)的增加逐步降低，可得到從初始位姿到期望位姿的漸進(jìn)最優(yōu)調(diào)整軌跡。

圖6 代價(jià)函數(shù)值與迭代次數(shù)的關(guān)系Fig.6 Cost function value versus number of iterations

3.2.2 算法有效性

在實(shí)際應(yīng)用時(shí)，通常不需要最優(yōu)軌跡而是快速得到可行軌跡。如圖6所示，在第13次迭代時(shí)，生成了有效的代價(jià)函數(shù)值，即此時(shí)生成了可行的軌跡。根據(jù)式(23)，最初得到的軌跡誤差較大，隨著迭代次數(shù)的增多，路徑上隨機(jī)點(diǎn)數(shù)增多，誤差逐步降低。值得注意的是，快速隨機(jī)擴(kuò)展類方法的隨機(jī)性影響了生成可行軌跡的迭代次數(shù)，但是經(jīng)多次計(jì)算，在此場(chǎng)景下，算法迭代35次以內(nèi)可得到可行軌跡，且單次迭代時(shí)長(zhǎng)約0.61 s。

3.3 算法對(duì)比

選取經(jīng)典RRT*算法、高斯偽譜法與本文算法對(duì)比。本文算法與高斯偽譜法均包含系統(tǒng)動(dòng)力學(xué)模型，代價(jià)函數(shù)選為姿態(tài)調(diào)整時(shí)間最短；而經(jīng)典RRT*算法僅能考慮FFSR的關(guān)節(jié)幾何約束，不包含動(dòng)力學(xué)信息，難以獲取姿態(tài)調(diào)整時(shí)間及能量消耗，代價(jià)函數(shù)為路徑最短。將三者結(jié)果進(jìn)行對(duì)比，自由漂浮機(jī)器人的初末位姿與障礙物特征相同。其中，經(jīng)典RRT*算法節(jié)點(diǎn)數(shù)為分別為3 500與5 000，步進(jìn)長(zhǎng)度為0.005 m；高斯偽譜法的配點(diǎn)數(shù)分別為20與30。

圖7為各方法規(guī)劃得到的末端機(jī)械手路徑對(duì)比，由對(duì)比可得，高斯偽譜法可得到規(guī)劃的最優(yōu)解；經(jīng)典RRT*規(guī)劃得到的路徑更短，但是由于無法考慮系統(tǒng)動(dòng)力學(xué)特性，因而規(guī)劃得到的路徑不平滑，難以滿足系統(tǒng)動(dòng)力學(xué)約束，不能保證運(yùn)動(dòng)的連續(xù)性。

圖7 不同方法機(jī)械手軌跡規(guī)劃結(jié)果對(duì)比Fig.7 Comparison of trajectory planning results of manipulator with different methods

以同樣仿真情景和硬件環(huán)境下規(guī)劃所需的CPU計(jì)算時(shí)間為規(guī)劃效率的評(píng)價(jià)標(biāo)準(zhǔn)，各方法的規(guī)劃效率及代價(jià)函數(shù)值展示于表3。

由表3可得，動(dòng)力學(xué)RRT*算法可以快速生成可行軌跡；經(jīng)典RRT*算法只生成路徑而沒有代價(jià)函數(shù)值(姿態(tài)調(diào)整時(shí)間)，且隨著節(jié)點(diǎn)數(shù)增加，CPU計(jì)算時(shí)間增加；高斯偽譜法產(chǎn)生規(guī)劃結(jié)果所需CPU計(jì)算時(shí)間隨著配點(diǎn)數(shù)的增加而增加，配點(diǎn)數(shù)為20時(shí)約為動(dòng)力學(xué)RRT*算法初次得到可行軌跡所需時(shí)間的4倍。

表3 不同算法的CPU計(jì)算時(shí)間與代價(jià)函數(shù)值對(duì)比Table 3 Comparison of CPU calculation time and cost function values of different methods

4 結(jié) 論

1) 提出了兼顧自由漂浮空間機(jī)器人能量消耗和姿態(tài)調(diào)整時(shí)間的加權(quán)Bolza形式性能指標(biāo)作為軌跡規(guī)劃的代價(jià)函數(shù)，增強(qiáng)了規(guī)劃的目的性和靈活性。

2) 對(duì)算法原有的模型線性化方法進(jìn)行改進(jìn)，針對(duì)性地采用偽線性化方法獲得FFSR的狀態(tài)空間模型，模型描述更加清晰、精確，運(yùn)算更加簡(jiǎn)便。提出了FFSR機(jī)械臂避障與機(jī)械手避障兩層次避障策略，提高碰撞檢測(cè)效率。

3) 使動(dòng)力學(xué)RRT*算法具備了對(duì)多體系統(tǒng)的軌跡規(guī)劃能力，通過多次迭代的優(yōu)化策略構(gòu)建了無碰最優(yōu)軌跡的逼近求解框架。仿真結(jié)果表明，該方法能夠以較快的速度生成可行的FFSR移動(dòng)軌跡。