一種改進(jìn)RRT結(jié)合四次樣條的協(xié)調(diào)路徑規(guī)劃方法1)

余 敏 羅建軍 王明明 高登巍

(西北工業(yè)大學(xué)深圳研究院，廣東深圳 518057)

(西北工業(yè)大學(xué)航天學(xué)院，西安 710072)

引言

隨著在軌維修、空間裝配等任務(wù)需求的與日俱增，空間機(jī)器人技術(shù)被驗(yàn)證是執(zhí)行特殊惡劣空間環(huán)境下任務(wù)最有效的手段之一.典型實(shí)例有“工程試驗(yàn)衛(wèi)星(engineering test satellite VII,ETS-VII)”[1]和“軌道快車(orbit express,OE)”[2].空間機(jī)器人在軌服務(wù)任務(wù)中最具挑戰(zhàn)性的工作之一是其抓捕過程中的運(yùn)動規(guī)劃問題[3-7].

過去的文獻(xiàn)中已提出過多種空間機(jī)器人運(yùn)動規(guī)劃方法.基于零作用空間，Yoshida 等[8]提出了無反作用機(jī)械臂，解決了運(yùn)動學(xué)非冗余臂和冗余臂考慮時間最少和機(jī)械臂速度限制的規(guī)劃問題.基于零反作用空間的概念，Wang 等[9-13]提出了一種考慮任務(wù)優(yōu)先級的運(yùn)動學(xué)冗余機(jī)械臂路徑規(guī)劃方法，并基于Bézier 曲線提出抓捕自旋衛(wèi)星后的最優(yōu)消旋與路徑規(guī)劃策略.羅建軍等[14]提出了一種利用傅里葉級數(shù)對空間機(jī)械臂關(guān)節(jié)運(yùn)動軌跡進(jìn)行參數(shù)化的規(guī)劃方法.Xu 等[15]利用自由漂浮空間機(jī)器人非完整特性提出了一種點(diǎn)到點(diǎn)的路徑規(guī)劃方法，同時調(diào)節(jié)基座姿態(tài)和末端的位姿.Nakamura 等[16]最早在機(jī)器人學(xué)中引入了任務(wù)優(yōu)先級的概念，基于二次規(guī)劃框架，僅考慮兩個優(yōu)先級水平，解決了冗余機(jī)械臂關(guān)節(jié)軌跡優(yōu)化問題.戈新生等[17]基于Legendre 偽譜法研究自由漂浮空間機(jī)器人非完整路徑規(guī)劃的最優(yōu)控制問題.程靖等[18]分析了雙臂空間機(jī)器人捕獲衛(wèi)星的力學(xué)，并研究其鎮(zhèn)定控制問題.Salehian 等[19]研究了雙臂機(jī)器人的協(xié)調(diào)操作機(jī)制與自碰撞規(guī)避策略.楊冬等[20]研究了冗余雙臂機(jī)器人的實(shí)時協(xié)調(diào)避碰方法.方健[21]研究雙臂機(jī)器人的在線運(yùn)動規(guī)劃與協(xié)調(diào)操作方法.

近年來，非結(jié)構(gòu)化環(huán)境下的機(jī)器人自主導(dǎo)航控制技術(shù)成為亟需.基于隨機(jī)采樣的規(guī)劃方法在機(jī)器人領(lǐng)域中興起，它適合于解決高自由度機(jī)器人在復(fù)雜環(huán)境中的規(guī)劃問題，而且對帶有微分約束的規(guī)劃問題也具有較強(qiáng)的解決能力.例如，RRT 算法[22]，其本質(zhì)上是單一查詢的規(guī)劃方法，其基本算法的主要思想是：由起始位形點(diǎn)出發(fā)構(gòu)造搜索樹，而后再從整個位形空間中隨機(jī)選擇一個位形點(diǎn)，找出與該點(diǎn)在搜索樹中最近的位形點(diǎn)，以此不斷的產(chǎn)生新位形狀態(tài)，直到到達(dá)目標(biāo)，搜索樹構(gòu)造結(jié)束.由于在搜索樹構(gòu)造過程中可以考慮多種微分約束(如非完整約束、運(yùn)動學(xué)及動力學(xué)約束等)，因而獲得的規(guī)劃結(jié)果合理性較強(qiáng).Vahrenkamp 等[23]基于RRT 算法，提出一種適用于地面雙臂機(jī)器人的抓捕和運(yùn)動規(guī)劃策略.James 等[24]將RRT 方法應(yīng)用于雙臂空間機(jī)器人抓捕前階段的路徑規(guī)劃過程中.然而，基于RRT 算法的運(yùn)動規(guī)劃方法產(chǎn)生的初始軌跡不平滑，不能直接應(yīng)用于機(jī)器人運(yùn)動規(guī)劃過程中.常見的平滑處理技巧有優(yōu)化方法和樣條曲線擬合技巧[25]等，然而采用優(yōu)化方法較為耗時.Kolter 等[26]利用三次樣條曲線設(shè)計(jì)四足機(jī)器人的在任務(wù)空間下的運(yùn)動軌跡.Vaz 等[27]結(jié)合三次樣條和半無限規(guī)劃方法進(jìn)行機(jī)器人的軌跡規(guī)劃.基于三次非均勻B 樣條曲線，趙皓等[28]和賴永林等[29]分別進(jìn)行6DOF 機(jī)械臂和工業(yè)機(jī)器人的軌跡規(guī)劃.上述研究表明，樣條函數(shù)是最有效的平滑處理技巧之一，且在機(jī)器人運(yùn)動規(guī)劃中得到廣泛的成功應(yīng)用.

本文主要研究空間雙臂機(jī)器人的協(xié)調(diào)路徑規(guī)劃問題，基于RRT*算法分別規(guī)劃雙臂的末端執(zhí)行器在笛卡爾空間中的初始可行路徑，并利用四次樣條曲線對由RRT*算法生成的初始路徑進(jìn)行平滑處理，通過滿足樣條曲線的一階、二階及三階微分連續(xù)約束，保證機(jī)械臂末端執(zhí)行器的初末速度、加速度約束條件，得到適合于機(jī)器人執(zhí)行的平滑路徑.本工作采用RRT*和四次樣條曲線方法，研究了空間多臂機(jī)器人的快速路徑規(guī)劃問題，考慮雙臂規(guī)劃過程中的協(xié)調(diào)性操作和路徑平滑處理，以期為空間多臂機(jī)器人協(xié)調(diào)運(yùn)動規(guī)劃問題以及多智能體協(xié)調(diào)運(yùn)動規(guī)劃問題提供參考數(shù)據(jù).

1 自由漂浮雙臂空間機(jī)器人建模

如圖1 所示，雙臂空間機(jī)器人系統(tǒng)由1 個基座和2 個n自由度的機(jī)械臂構(gòu)成，共計(jì)2n+1 個剛體，且系統(tǒng)不受任何外力作用(本文研究中n=7).

圖1 自由漂浮雙臂空間機(jī)器人系統(tǒng)示意圖Fig.1 Sketch of a dual-arm free-floating space robot

空間機(jī)器人建模方面的現(xiàn)有研究很成熟，根據(jù)泊松公式[30]，空間機(jī)器人的運(yùn)動學(xué)方程表示如下

其中，上標(biāo)a和b分別表示變量屬于機(jī)械臂a和b.下標(biāo)b 和e 分別表示變量屬于基座(base)和末端執(zhí)行器(end-effector).分別為基座和末端執(zhí)行器的線速度、角速度合矢量，為各機(jī)械臂關(guān)節(jié)角速度.若無特別說明，文中矢量(除姿態(tài)相關(guān)矢量)選擇向慣性坐標(biāo)系投影，其他矢量已在表1 中列舉.

表1 本文所用運(yùn)動學(xué)和動力學(xué)符號Table 1 Symbols used in this paper

根據(jù)第二類拉格朗日方程[31]，空間機(jī)器人的動力學(xué)方程表示如下

式中，M0=[P0L0]T為系統(tǒng)的初始線動量、角動量.由于矩陣Hb可逆，可以將基座的運(yùn)動方程改寫如下

將式(2)代入式(1)，那么末端執(zhí)行器的運(yùn)動表達(dá)如下

式中，Jg為系統(tǒng)的廣義雅克比矩陣，Jd為系統(tǒng)初始動量M0引起的耦合矩陣.

2 協(xié)調(diào)RRT*結(jié)合四次樣條的路徑規(guī)劃

2.1 RRT*協(xié)調(diào)路徑規(guī)劃

基于隨機(jī)采樣原理的快速搜索隨機(jī)樹方法具有概率完備性、計(jì)算量小、計(jì)算難度低等特點(diǎn)，非常適合空間自主航天器完成復(fù)雜空間的近距離操作服務(wù)任務(wù).此外，它對解決高維狀態(tài)規(guī)劃問題有著較強(qiáng)的適應(yīng)性，這一能力是目前很多規(guī)劃算法所不具備的，因此它非常適合空間機(jī)器人高維運(yùn)動狀態(tài)規(guī)劃的實(shí)際情況.鑒于RRT*算法不僅加快了原RRT 算法的收斂速度，還增強(qiáng)了算法的最優(yōu)性能，因此本文選取RRT*作為空間機(jī)器人路徑規(guī)劃的主要方法，這里簡單給出RRT*算法的基本流程，關(guān)于RRT*算法的細(xì)則可以參考文獻(xiàn)[32].

本文研究中，利用RRT*規(guī)劃算法生成機(jī)械臂末端執(zhí)行器在笛卡爾空間下的期望路徑，具體工作流程如下：

(1)將系統(tǒng)的初始狀態(tài)(末端執(zhí)行器的位置信息)作為樹的第一個頂點(diǎn)；

(2)當(dāng)無障礙環(huán)境時在狀態(tài)空間中隨機(jī)生成一個采樣點(diǎn)；

(3)在原樹與該采樣點(diǎn)最近點(diǎn)處延伸固定步長(初始時即為頂點(diǎn))；

(4)對采樣點(diǎn)周圍的點(diǎn)進(jìn)行檢測與優(yōu)化，若有更短的航跡，則用新節(jié)點(diǎn)替代舊節(jié)點(diǎn)；

(5)考慮障礙物等運(yùn)動學(xué)約束，進(jìn)行碰撞檢測，以保證生成路徑的安全性；

(6)經(jīng)過有限次迭代優(yōu)化求解，直至達(dá)到目標(biāo)狀態(tài).

通過以上步驟，可以在非結(jié)構(gòu)環(huán)境中快速搜索到一條無碰撞的從初始狀態(tài)到期望狀態(tài)的優(yōu)化運(yùn)動路徑.

然而，將RRT*算法直接應(yīng)用于雙臂空間機(jī)器人的末端執(zhí)行器在任務(wù)空間下的路徑規(guī)劃，雙末端執(zhí)行器在規(guī)劃的過程中由于隨機(jī)采樣極有可能會發(fā)生碰撞.因此本文針對原有RRT*算法進(jìn)行改進(jìn)，提出一種適用于多臂路徑規(guī)劃的協(xié)調(diào)RRT*算法，改進(jìn)的RRT*算法同樣適用于其它多智能體系統(tǒng)的協(xié)調(diào)路徑規(guī)劃問題.

基于原有RRT*算法框架，在設(shè)置雙臂的采樣空間時，本文對雙臂各自采樣空間預(yù)留了約0.1 m 寬度的歐氏距離，通過在不同的采樣空間中分別進(jìn)行各末端執(zhí)行器的路徑搜索，這相當(dāng)于對雙末端施加了具有一定邊界范圍的笛卡爾空間約束.值得注意的是，此空間約束不是固定的，應(yīng)隨著末端執(zhí)行器的運(yùn)動而連續(xù)變化.此空間約束可以保證整個規(guī)劃執(zhí)行過程中雙臂空間機(jī)器人不發(fā)生自身碰撞，包括兩個末端執(zhí)行器不發(fā)生碰撞，以及雙臂各連桿不發(fā)生自身碰撞.改進(jìn)的協(xié)調(diào)RRT*路徑規(guī)劃算法流程如下：

(1)Fork=1 toKdo;

(2)隨機(jī)在不同的任務(wù)空間中對雙末端分別采樣;

(3)在原樹結(jié)構(gòu)中搜索新生采樣點(diǎn)附近的點(diǎn);

(4)進(jìn)行碰撞檢測，生成新的樹節(jié)點(diǎn);

(5)判斷是否有更少代價(jià)的航跡，若有則替代原航跡;

(6)Return Tree.

注意，由上述RRT*算法得到的末端初始路徑十分不平滑，不能直接施加于空間機(jī)器人，需要對得到的初始路徑進(jìn)行平滑處理.常見的平滑處理技巧分為兩類，一類是基于優(yōu)化方法，例如，共軛梯度法、凸優(yōu)化方法等.但此類方法比較耗時，因此本文采用第二類方法：曲線擬合技巧.

2.2 四次樣條平滑

樣條曲線被認(rèn)為是平滑機(jī)器人路徑的最有效的手段之一，在機(jī)器人規(guī)劃中也得到了很多應(yīng)用，包括關(guān)節(jié)規(guī)劃和末端規(guī)劃.實(shí)際應(yīng)用中最常見的樣條曲線是三次樣條曲線，本文研究表明，為了滿足路徑的三階導(dǎo)數(shù)連續(xù)條件，針對末端的路徑規(guī)劃至少需要用到四次及以上的樣條曲線.

首先，回顧樣條曲線的基本知識.假設(shè)有n+1 個時間位置相關(guān)的路徑點(diǎn)如下

其中，xi表示機(jī)器人末端執(zhí)行器在時間ti的期望位置.

給定這些路徑點(diǎn)，四次樣條曲線可以平滑地銜接這些路徑點(diǎn)，且保證在曲線銜接處滿足一階、二階微分連續(xù)約束等.四次條件曲線的基本形式表達(dá)如下

其中，ai,bi,ci,di,ei分別是四次樣條曲線的參數(shù).最終的路徑由若干段四次樣條曲線級聯(lián)而成.給定路徑點(diǎn)，存在唯一的樣條參數(shù)集合，使得規(guī)劃的路徑通過各個路徑點(diǎn)，且在各個路徑點(diǎn)處滿足特定的速度連續(xù)，加速度連續(xù)等約束條件.針對機(jī)器人運(yùn)動而言，還需要保持加加速度連續(xù)，以防機(jī)械臂產(chǎn)生抖振現(xiàn)象.

如圖2 所示，給定一系列末端執(zhí)行器的期望路徑點(diǎn)(本文研究中，這些路徑點(diǎn)由RRT*算法給出)，假設(shè)i=3，那么則有4 個路徑點(diǎn)，即需要級聯(lián)3 段四次樣條曲線，共計(jì)15 個未知參數(shù).

圖2 四次樣條曲線示意圖Fig.2 Sketch of quartic splines

通過保證各個路徑點(diǎn)位置約束(6 個約束)；中間路徑點(diǎn)還需滿足速度、加速度、加加速度連續(xù)約束(6 個約束)，以保證機(jī)器人能夠平滑地執(zhí)行全部路徑；此外，本文對末端的初始速度、加速度和終止速度施加限制(3 個約束).由上述15 個約束構(gòu)建方程組，可以求解15 個未知的樣條參數(shù).構(gòu)建的約束方程組具體表達(dá)如下

其中，h0,h1,h2是僅與時間相關(guān)的常量，取決于時間序列t0,t1,t2,t3.通過上述方程組即可求解樣條參數(shù)，進(jìn)而得到機(jī)器人末端的運(yùn)動軌跡.

綜上所述，自由漂浮雙臂空間機(jī)器人的路徑規(guī)劃問題可描述為以下形式

式中，目標(biāo)函數(shù)f(xe)為由RRT*算法給出的路徑點(diǎn)的位置函數(shù)，保證路徑點(diǎn)位置與樣條曲線中的位置匹配.規(guī)劃過程中，還需要滿足末端執(zhí)行器的初末狀態(tài)約束，以及機(jī)械臂關(guān)節(jié)物理極限約束.

基于RRT*算法，在非結(jié)構(gòu)化環(huán)境中生成若干可行路徑點(diǎn)，再由四次樣條曲線擬合這些路徑點(diǎn)，即可得到平滑可行的機(jī)器人路徑，本文所提的路徑規(guī)劃算法，效果如圖3 所示.其中，紅色曲線是原始RRT*算法生成的路徑，藍(lán)色曲線是由RRT*結(jié)合樣條設(shè)計(jì)生成的路徑，可以明顯看出所提算法生成的路徑不僅平滑，而且極大地保留了原RRT*生成路徑的最優(yōu)性.此外，圖4 驗(yàn)證了所提路徑規(guī)劃算法的協(xié)調(diào)性，基于分離的采樣空間，有效地避免了可能發(fā)生的碰撞.

圖3 RRT*結(jié)合四次樣條路徑規(guī)劃示意圖Fig.3 Sketch of spline-RRT*algorithm

圖4 協(xié)調(diào)RRT*路徑規(guī)劃示意圖Fig.4 Sketch of coordinated RRT*algorithm

3 仿真結(jié)果

為驗(yàn)證所提協(xié)調(diào)路徑規(guī)劃方法的有效性，本章給出了采用運(yùn)動學(xué)冗余雙機(jī)械臂抓捕空間目標(biāo)的演示實(shí)例，如圖5 所示.表2 給出了本文所用雙臂空間機(jī)器人的運(yùn)動學(xué)和動力學(xué)參數(shù)，兩機(jī)械臂參數(shù)相同.

假設(shè)基座的初始位姿是0，末端執(zhí)行器的初始位姿分別為

終止位姿分別為

圖5 空間機(jī)器人抓捕目標(biāo)示意圖Fig.5 Sketch of capturing task by space robot

表2 空間機(jī)器人運(yùn)動學(xué)和動力學(xué)參數(shù)Table 2 Kinematica and dynamic parameters of space robot

注意，本文所提的基于RRT*路徑規(guī)劃方法只用于末端執(zhí)行器的位置規(guī)劃，對于其姿態(tài)規(guī)劃，仍采用前期工作中[33]的用到的Bézier 曲線.基于五階Bézier 曲線，規(guī)劃末端執(zhí)行器在笛卡爾空間下的姿態(tài)路徑x(t)，描述如下

其中，多項(xiàng)式bj,m(τ)是m階Bernstein 基本多項(xiàng)式，是二項(xiàng)式系數(shù).Pij是給定構(gòu)建Bézier 曲線的控制點(diǎn).

給定末端的初末位姿，考慮機(jī)械臂的速度以及加速度限制條件，可以計(jì)算出規(guī)劃路徑的執(zhí)行時間

圖6 給出了由規(guī)劃的末端執(zhí)行器的路徑，以及路徑的執(zhí)行速度.可以看出，規(guī)劃的末端路徑較為平滑，且速度保持在機(jī)器人末端執(zhí)行速度的最大限制范圍(0.1 m/s)內(nèi)，且路徑的初始速度、加速度和終止速度都為零，符合所設(shè)計(jì)的情況，由此說明了規(guī)劃路徑的有效性.

圖6 規(guī)劃的末端執(zhí)行器的路徑及其速度Fig.6 Planned path and velocity for end-effectors

圖7 給出了機(jī)械臂執(zhí)行所規(guī)劃路徑過程中，機(jī)械臂關(guān)節(jié)角、關(guān)節(jié)角速度以及關(guān)節(jié)角加速度的變化曲線.可以看出，各機(jī)械臂的關(guān)節(jié)角、關(guān)節(jié)角速度以及關(guān)節(jié)角加速度變化平滑，且速度和加速度大小均被限制在給定范圍內(nèi)，分別是0.1 rad/s 和0.1 rad/s2，且關(guān)節(jié)角速度和關(guān)節(jié)角加速度初始為零，符合實(shí)際情況.

圖8 給出了機(jī)械臂末端執(zhí)行器的實(shí)際跟蹤誤差曲線.可以看出，整個執(zhí)行過程中，末端執(zhí)行器的實(shí)際跟蹤誤差保持在10?4量級，由此說明，末端能夠跟蹤上規(guī)劃的路徑，證明了所設(shè)計(jì)的規(guī)劃算法的正確性.此外，機(jī)械臂各連桿在執(zhí)行過程中亦未發(fā)生碰撞，各部件均保持在安全距離(di=0.1 m)以外，如圖9 所示.

圖7 各機(jī)械臂關(guān)節(jié)變化曲線Fig.7 Joint angle,velocity and acceleration

圖8 末端執(zhí)行器的跟蹤誤差Fig.8 Tracking error of end-effectors

圖9 機(jī)械臂各連桿之間的相對距離Fig.9 The minimum distance between robot links

4 結(jié)論

考慮到安全性和最優(yōu)性因素，本文提出了一種雙臂空間機(jī)器人協(xié)調(diào)路徑規(guī)劃方法.基于RRT*算法在非結(jié)構(gòu)化環(huán)境中快速生成末端執(zhí)行器安全可行的初始路徑，并利用四次樣條曲線，平滑了由RRT*生成的初始路徑，保證機(jī)器人以良好的狀態(tài)執(zhí)行規(guī)劃的路徑，同時考慮了機(jī)械臂末端以及關(guān)節(jié)的物理極限約束.仿真結(jié)果說明了所提協(xié)調(diào)路徑規(guī)劃方法的有效性.

附錄A 雙臂空間機(jī)器人運(yùn)動學(xué)建模

雙臂空間機(jī)器人各連桿質(zhì)心和末端執(zhí)行器的位置矢量可以表示為

同時，各關(guān)節(jié)和末端執(zhí)行器的角速度可以表示為

將末端執(zhí)行器的線速度和角速度統(tǒng)一表示為

其中，上標(biāo)a和b分別表示變量屬于機(jī)械臂a和b.

附錄B 雙臂空間機(jī)器人動力學(xué)建模

基于拉格朗日方程進(jìn)行自由漂浮雙臂空間機(jī)器人的動力學(xué)建模，第二類拉格朗日方程如下式所示

其中，qi為描述系統(tǒng)位形的獨(dú)立的廣義坐標(biāo)，L稱為拉格朗日函數(shù)，與系統(tǒng)的動能T以及勢能V滿足關(guān)系

雙臂空間機(jī)器人系統(tǒng)處于空間微重力環(huán)境中，因此V=0，雙臂空間機(jī)器人系統(tǒng)的動能包括基座和各連桿運(yùn)動產(chǎn)生的動能

將式(A3)和式(A5)代入上式，得到緊湊形式的雙臂空間機(jī)器人動能表達(dá)式

對于空間機(jī)器人系統(tǒng)，L=T，代入式(B3)表示的拉格朗日方程，可以得到雙臂空間機(jī)器人的一般動力學(xué)方程

其中，cb,分別為與基座、機(jī)械臂運(yùn)動相關(guān)的非線性力，包括向心力和哥氏力；為作用在基座上的外力和外力矩；為機(jī)械臂關(guān)節(jié)的輸入力矩；當(dāng)機(jī)械臂末端執(zhí)行器和目標(biāo)發(fā)生碰撞時，為碰撞產(chǎn)生的外力和外力矩.