一種改進RRT結合四次樣條的協調路徑規劃方法1)

2020-08-11 02:32:36羅建軍王明明高登巍

力學學報 2020年4期

余敏羅建軍王明明高登巍

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

(西北工業大學航天學院，西安 710072)

引言

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

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

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

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

1 自由漂浮雙臂空間機器人建模

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

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

空間機器人建模方面的現有研究很成熟，根據泊松公式[30]，空間機器人的運動學方程表示如下

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

表1 本文所用運動學和動力學符號Table 1 Symbols used in this paper

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

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

將式(2)代入式(1)，那么末端執行器的運動表達如下

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

2 協調RRT*結合四次樣條的路徑規劃

2.1 RRT*協調路徑規劃

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

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

(1)將系統的初始狀態(末端執行器的位置信息)作為樹的第一個頂點；

(2)當無障礙環境時在狀態空間中隨機生成一個采樣點；

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

(4)對采樣點周圍的點進行檢測與優化，若有更短的航跡，則用新節點替代舊節點；

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

(6)經過有限次迭代優化求解，直至達到目標狀態.

通過以上步驟，可以在非結構環境中快速搜索到一條無碰撞的從初始狀態到期望狀態的優化運動路徑.

然而，將RRT*算法直接應用于雙臂空間機器人的末端執行器在任務空間下的路徑規劃，雙末端執行器在規劃的過程中由于隨機采樣極有可能會發生碰撞.因此本文針對原有RRT*算法進行改進，提出一種適用于多臂路徑規劃的協調RRT*算法，改進的RRT*算法同樣適用于其它多智能體系統的協調路徑規劃問題.

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

(1)Fork=1 toKdo;

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

(3)在原樹結構中搜索新生采樣點附近的點;

(4)進行碰撞檢測，生成新的樹節點;

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

(6)Return Tree.

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

2.2 四次樣條平滑

樣條曲線被認為是平滑機器人路徑的最有效的手段之一，在機器人規劃中也得到了很多應用，包括關節規劃和末端規劃.實際應用中最常見的樣條曲線是三次樣條曲線，本文研究表明，為了滿足路徑的三階導數連續條件，針對末端的路徑規劃至少需要用到四次及以上的樣條曲線.

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

其中，xi表示機器人末端執行器在時間ti的期望位置.

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

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

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

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

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

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

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

式中，目標函數f(xe)為由RRT*算法給出的路徑點的位置函數，保證路徑點位置與樣條曲線中的位置匹配.規劃過程中，還需要滿足末端執行器的初末狀態約束，以及機械臂關節物理極限約束.

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

圖3 RRT*結合四次樣條路徑規劃示意圖Fig.3 Sketch of spline-RRT*algorithm

圖4 協調RRT*路徑規劃示意圖Fig.4 Sketch of coordinated RRT*algorithm

3 仿真結果

為驗證所提協調路徑規劃方法的有效性，本章給出了采用運動學冗余雙機械臂抓捕空間目標的演示實例，如圖5 所示.表2 給出了本文所用雙臂空間機器人的運動學和動力學參數，兩機械臂參數相同.

假設基座的初始位姿是0，末端執行器的初始位姿分別為

終止位姿分別為

圖5 空間機器人抓捕目標示意圖Fig.5 Sketch of capturing task by space robot

表2 空間機器人運動學和動力學參數Table 2 Kinematica and dynamic parameters of space robot

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

其中，多項式bj,m(τ)是m階Bernstein 基本多項式，是二項式系數.Pij是給定構建Bézier 曲線的控制點.

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

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

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

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

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

圖7 各機械臂關節變化曲線Fig.7 Joint angle,velocity and acceleration

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

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

4 結論

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

附錄A 雙臂空間機器人運動學建模

雙臂空間機器人各連桿質心和末端執行器的位置矢量可以表示為