自由漂浮空間機器人無逆運動學基于采樣的運動規劃

張紅文, 朱戰霞, 袁建平

(1.西北工業大學 航天學院, 陜西 西安 710072; 2.航天飛行動力學技術國家級重點實驗室, 陜西 西安 710072)

FFSR具備節省燃料,延長系統壽命等優點,被廣泛用于在軌服務。FFSR構型參數中,關節角和基座姿態間存在一階導關系,需直接考慮微分約束進行運動規劃。

FFSR運動規劃方法中,研究較多的包含3類。早期學者研究了系統的操控(steer)問題,即在不考慮障礙物的情況下,生成一系列的動作,讓FFSR從初始構型運動到目標構型。如Vafa等基于“虛擬機械臂(virtual manipulator,VM)”[1]的模型,發展了自校正運動法[1]及“擾動圖法(disturbance map,DM)”[2]。Nakamura 和Mukherjee[3]基于李雅普諾夫函數設計了雙向方法。Fernandes等[4]研究了耦合剛體的運動規劃方法，此類方法著眼于利用關節運動操控基座姿態;但未考慮碰撞,不能形成完整的運動規劃。第二類方法是基于多項式參數化的方法,即以FFSR的初始關節角和最終關節角為端點,利用帶未知系數的多項式對關節軌跡進行參數化,并以最終時刻基座姿態擾動為適應度函數,將規劃問題轉換為優化問題;利用遺傳算法[5]、粒子群算法[6]、微分進化算法[7]等解優化問題,可得最終軌跡。該類方法不易控制軌跡的形狀,因此很難考慮碰撞問題;此外,由于適應度函數計算需進行積分,因此效率低下。第三類方法是通過直接法或間接法將運動規劃問題轉換為非線性優化問題[8-9]。Misra等[10]于2017年基于非線性優化研究了空間機器人考慮任務約束的運動規劃問題,并將問題構造為一個凸二次規劃問題。該類方法求解效率高,但也存在局部最優問題。

基于采樣的運動規劃方法,能高效解決有高自由度構型空間的機器人運動規劃問題。其中RRT(rapidly-exploring random tree)對有微分約束運動規劃有很強的適用性[11]。一般情況下,運動規劃多采用“先求逆運動學,再規劃路徑/軌跡”的框架;但這一框架存在逆運動學所求目標構型與初始構型未必在同一連通域內的隱患。針對這一問題,Bertram、Weghe等[12-13]基于有目標偏置的RRT(RRT with Goal Bias,GB-RRT)給出了解決方案,即不求逆運動學,而是直接引入目標末端位姿導引生長。在該方案中,用于目標末端位姿導引生長的局部規劃含一個迭代過程,每次迭代通過雅克比矩陣的轉置,將末端位姿誤差映射到構型空間,從而產生一系列越來越接近目標末端位姿的構型，但該方案針對固定基座機械臂的路徑規劃問題,而FFSR的運動規劃需直接考慮微分約束。此外,固定基座機械臂執行機構自由度大于末端位姿自由度,但FFSR末端末端位姿導引生長,除要求具備最終收斂到目標末端位姿的能力外,還需保證基座姿態擾動不能過大,因此執行機構自由度不足。

本文以GB-RRT為框架設計規劃算法,解決FFSR從初始構型到目標末端位姿的運動規劃問題。為適應FFSR直接考慮微分約束的需求,在Weghe等[13]提出的方法基礎上,在每次迭代中引入積分機制;為克服FFSR執行機構自由度不足的問題,提出適時調節基座姿態的目標末端位姿導引生長局部規劃器,以調節末端位姿為主,同時在必要時兼顧考慮基座姿態的調節。

1 自由漂浮空間機器人的運動學方程

FFSR如圖1所示,符號定義與文獻[14]相同。

圖1 FFSR模型簡圖

1.1 僅考慮幾何關系的運動學方程

1.1.1 姿態方程

IRe=feR(q)

(1)

FFSR的EE姿態角Ψe與IRe對應,因此

Ψe=feΨ(q)

(2)

1.1.2 位置方程

FFSR末端執行器位置pe為

(3)

1.1.3 速度方程

FFSR末端執行器角速度ωe為

(4)

末端執行器線速度ve度為

(5)

(6)

Jb,Jm的表達式見文獻[14]。

1.2 動量守恒方程

FFSR的動量方程為

(7)

式中，P與L分別為FFSR的線動量、角動量,相關矩陣的定義間文獻[14]。令

1.3 考慮動量守恒后的運動學方程

1.3.1 基座姿態角一階導方程

(8)

取上述方程后3行,可得基座角速度ωb為

(9)

(10)

式中

(11)

式中，JbΨ為與基座姿態角相關的雅克比矩陣。

1.3.2 狀態轉移方程

考慮(11)式,則FFSR的狀態轉移方程為

(12)

式中，Jx為與狀態轉移相關的雅克比矩陣。

1.3.3 廣義雅克比矩陣

考慮(6)式以及(8)式,可得

(13)

Jg為廣義雅克比矩陣。

1.3.4 考慮線動量守恒積分形式后的方程

FFSR的線動量可積分為位置形式

(14)

(15)

進而

(16)

給出構型參數q,可計算在慣性空間之中的表示Ibi,Iai,進而可計算r0,即q為構型參數q的函數。再考慮(3)式,可知pe為q的函數

pe=fep(q)

(17)

綜合考慮(1)式以及(17)式,可得端位姿xe與q間的映射關系

(18)

2 FFSR無逆運動學基于采樣的運動規劃

算法偽代碼如Algorithm 1所示,在每次迭代中,利用隨機采樣構型qSample或目標末端位姿xed來導引樹的生長,這2種生長分別稱為隨機導引生長(Extend-Randomly()函數)與目標末端位姿導引生長(Extend-Toward-Goal())。每次迭代生成一個隨機數p∈[0,1],若p

algorithm 1 GB-RRT for FFSR1tree.V.init(qI)2for i=1 to K do3 if rand()

2.1 局部規劃器設計

2.2.1 隨機構型導引生長局部規劃器

隨機構型導引生長選擇離qsample最近的節點作為待擴展點qextended,并從qextended向qsample進行局部規劃,以生成局部軌跡以及新的節點。

algorithm 2 extend-randomly1qsample=random-config()2qextend=nearest-node(tree,qsample)3qpresent=qextend4last-checked-q=qpresent5shall-end=false6while all(shall-end=false)7 Jb=Jacobian(qpresent)8 Jx=[Jb,eye(n,n)]9 Δq=qsample-qpresent10 JV=inv(Jx)*Δq11 BV=Jb*JV12 qpresent=qpresent+[BV;JV]*time-step13 If max(qpresent-last-checked-q)>= collision-check-thresh14 is-collision=collision-check(qpresent)15 last-checked-q=qpresent17 shall-end=check-termination()19return

(19)

(20)

從而得到構型速度

(21)

通過積分,更新構型

(22)

對qpresent進行碰撞檢測,以保證所生成的局部軌跡處于自由構型空間。若滿足下述任何一個條件,則停止迭代:①檢測出碰撞;②構型超出限制,即基座姿態擾動超出限制,或者關節角度超出范圍;③構型參數之中最大擴展值達到一定的閾值。

2.2.2 適時調節基座姿態的目標末端位姿導引生長局部規劃器

algorithm3 extend-toward-goal1qextend=nearest-node(tree,Xgoal) 2qpresent=qextend 3last-checked-q=qpresent 4shall-end=false 5while all(shall-end=false) 6 JB=Jacobian(qpresent) 7 JG=general-Jacobian(qpresent) 8 ΔX=Xgoal-f(qpresent) 9 ΔqB=qBgoal-qBpresent 10 JV-for-EE=pinv(JG)*ΔX 11 if ΔqB>B-adjust-thresh 12 JV-for-BA=pinv(JB*null(JG))*ΔqB 13 else 14 JV-for-BA=0 15 JV=JV-for-EE+null(JG)*JV-for-BA 16 BV=Jb*JV 17 qpresent=qpresent+[BV;JV]*time-step 18 if max(qpresent-last-checked-q)>=C-check-tresh 19 is-collision=collision-check(qpresent) 20 last-checked-q=qpresent 21 shall-end=check-termination() 22return

目標末端位姿導引生長,即選擇離xed最近的點作為待擴展點qextended,并從qextended向xed進行局部規劃;其要求盡可能消除相對于xed的誤差,同時保證Ψb不超出取值范圍。本文提出適時調節基座姿態的目標末端位姿導引生長其局部規劃器,其偽代碼如Algorithm 3所示。作為執行機構的FFSR操作臂關節僅有7個自由度,而末端位姿任務為六自由度,基座姿態為三自由度。相對于到達xed以及保持Ψb處于一定取值范圍的要求,執行機構自由度不足;因此若在局部規劃的每一次迭代中,同時調節xe和Ψb,則末端位姿誤差無法收斂。

(23)

(24)

(25)

獲得動作后,后續其他步驟與2.2.1節中相應的步驟相同。本局部規劃器的終止條件包含:①檢測出碰撞;②構型超出限制,即基座姿態擾動超出限制,或者關節角度超出范圍;③到達目標。

3 仿 真

表1 FFSR的動力學以及幾何參數

表2 FFSR的D-H參數

3.1 場景1的結果

36次迭代后,FFSR的末端到達目標位置,規劃結束。如圖2a)所示,圖中淡藍色為初始構型,紅色為最終構型。從初始構型到目標末端位置的運動歷程如圖2b)所示,整個過程中未發生碰撞。關節角、基座姿態隨時間的變化分別如圖3所示,從圖3b)可看出,基座姿態滿足取值范圍限制。

圖2 場景1：初始構型與最終構型，從初始圖3 場景1：關節角及基座姿態變化曲線構型向目標末端位置的運動歷程

3.2 場景2的結果

116次迭代后,到達目標末端位姿,規劃成功;圖4a)中,紅色為最終構型,青色為初始構型。從初始構型向目標末端位姿的運動歷程如圖4b)所示,整個過程未發生碰撞。關節角、基座姿態隨時間的變化如圖5所示,可發現基座姿態處于取值范圍內。

圖4 場景2：初始構型與最終構型，從初始圖5 場景2：關節角及基座姿態變化曲線構型向目標末端位姿的運動歷程

4 結 論

本文基于GB-RRT設計了FFSR無逆運動學的運動規劃算法,并提出適時調節基座姿態的目標末端位姿導引局部規劃器,引入參考目標基座姿態、基座姿態誤差閾值。在該局部規劃器的每一次迭代中,只有相對于參考目標基座姿態誤差達到閾值,才調節基座姿態,否則只調節末端位姿。該局部規劃器所生成的軌跡能滿足基座姿態取值限制,且具備讓末端位姿誤差收斂的能力,克服了FFSR執行機構自由度不足的問題。