冗余自由度協作機器人運動規劃研究

馬金琦，郭冰菁，2，韓建海，2，3，李向攀，2

(1.河南科技大學機電工程學院，河南 洛陽 471003；2.河南省機器人與智能系統重點實驗室，河南 洛陽 471003；3.機械裝備先進制造河南省協同創新中心，河南 洛陽 471003)

1 引言

近年來，隨著產品多樣化、精細化、輕量化的發展，對機器人的應用提出了新的要求。機器人要滿足生產線批量生產的剛性需求，也要滿足客戶個性定制的柔性制造。同時機器人從獨立工作轉向共享環境中的與人協作模式，工作環境不再需要圍欄隔離，機器人能夠直接與人并肩工作[1]。協作機器人對人機交互、柔順適應等關鍵技術提出了更高的要求，約束空間中的機器人自主的運動規劃研究是協作交互的重要研究內容。

運動特性的研究是機器人運動控制和路徑規劃的基礎[2]。國內外對非冗余機械臂運動學的研究已相當成熟，但對冗余機械臂，特別是其逆運動學的研究相對有限。文獻[3]提出固定一個關節，將機器人改為六自由度求解，但關節的減少會降低機械臂的靈活性；李寧森[4]對無特殊構型機械臂，采用偽逆法求逆解，此方法適合實時、高精度的逆運動學求解；魏延輝等[5]根據構型平面的基本理論和半解析的方法進行逆運動學求解。

常見的路徑規劃算法有概率路標圖法(probabilistic roadmap method，PRM)[6-7]、人工勢場法[8-9]、A*算法[10]、快速探索隨機數法(Rapidly-exploring random trees，RRT)[11-12]。當存在未知障礙物時PRM算法的效率較低。RRT算法收斂速度快，具有概率完備性，但因其采用的是全局均勻搜索策略，無謂的消耗大量的計算資源，增加了搜索時間，收斂速度緩慢。

針對特殊構型的Sawyer機器人，本文對傳統RRT算法新枝點擴展時無方向性，搜索時間長的特點進行算法改進。在新枝點的擴展上加入引力系數，使其具有方向性，根據機器人工作空間障礙物復雜程度的不同，通過調節引力系數，使新枝點擴展具有合適的搜索步長，同時利用雙向搜索有效減少了路徑規劃時間。采用阻尼最小二乘Newton-Raphson方法對Sawyer機器人逆運動學進行數值求解。在MATLAB和ROS(機器人操作系統)平臺進行仿真和實驗，通過實驗驗證路徑規劃改進算法的有效性。

2 機器人構形分析

在三維空間中，機器人至少需要六個獨立變量(三個位置，三個姿態)，才能完整定義機械臂的末端位姿。冗余機器人由于自由度大于6，在機械臂的奇異點回避，空間避障等方面具有優勢。由于機器人的關節存在限位，在工作空間內，非冗余機器人末端執行器存在不可達點，七自由度機器人通過增加了關節個數和旋轉角度，可以克服關節不可達問題。

七自由度機器人最常用的關節配置模式為Spherial-Roll-Spherial構型，即腕部與肩部是由三個軸線互相垂直、交于一點的等效球關節，肘部為旋轉關節構成，多為非偏置式構型。其優點是制造簡單，運動學求解方便。無偏置球關節構型如圖1所示。但由于其肘關節運動范圍受限，在需要執行較復雜工作，或要求機械臂工作范圍盡可能大時，多采用圖2所示的偏置式非球關節構型。

圖2 有偏置非球形關節構型

本文以七自由度協作機器人Rethink Sawyer為研究對象，如圖3所示，為有偏置非球形關節構型，肩部關節J0與J1的軸線垂直且無交點，連桿偏置，使得關節運動角度增加。

圖3 7自由度機器人Sawyer

3 基于RRT的改進路徑規劃算法

3.1 RRT算法基本原理

RRT算法是在待搜索區域中，對地圖進行分裂尋找從起始位置xinit到目標位置xgoal的隨機搜索樹。算法的基本原理是，以xinit為根節點，在區域內隨機采樣得到xrand，然后找到離xrand最近的節點xnear；在指向xrand的方向上生成給定步長ρ的新節點xnew，若新生節點與障礙物沒有碰撞，則作為擴展節點加入原隨機樹，否則重新進行采樣；當采樣點距目標點xgoal距離小于設定閾值時，算法結束，否則繼續擴展。其新生節點計算公式為：

(1)

圖4 RRT樹生長過程

RRT算法在理論上總能在空間中找到一條有效搜索路徑，但新枝點的產生是隨機采樣，對空間區域搜索過于平均，隨著空間維度與搜索區域的增加，將產生大量冗余點，使搜索效率將大幅下降。

3.2 改進RRT算法

針對傳統RRT算法在新枝點擴展上無目標性，時間復雜度高的特點。基于人工勢場法中目標引力的思想，提出枝點擴展具有導引的變步長雙向RRT算法。目標點方向增加引力函數，枝節點朝著目標點方向擴展，無需對所有區域進行搜索，減少了對冗余點的計算；根據搜索區域的復雜度，人工調節引力參數，避免局部極小值的產生；從根節點與目標點同時擴展，縮短搜索時間，提高路徑搜索效率。

根據引力勢能函數

(2)

求得引力函數

(3)

其中kp為引力場系數。

由此改進后新枝點計算方法為

(4)

在新枝點的擴展中，根據空間障礙物情況，調節合適的引力場系數kp。當空間中無障礙物時，增大kp提高隨機樹向目標點生長的概率，減小新枝點隨機生長的概率；在遇到障礙物時，減小kp至合適值，增強新枝點隨機擴展的能力，繞開障礙物。不但保證了避障能力，同時增強了方向的指向能力。

基于經典RRT算法，引入目標引力思想與變步長策略，雖然有效地降低了搜索過程的隨機性，提升了機器人的避障能力強，但路徑搜索效率較低。在此基礎上，引入雙向生長策略，在起始位置xinit與目標位置xgoal同時生成隨機擴展樹，直至兩樹新生枝點間的距離小于給定閾值，即可認為兩樹連通。

4 逆運動學求解算法

逆運動學是機器人進行運動規劃的基礎，其實質是已知機器人末端執行器在工作空間的位姿，求解關節空間角度的過程，設工作空間末端位姿為X=[x1，x2，x3，x4，x5，x6]，關節空間角度為q=[q1，q2，q3，q4，q5，q6，q7]，運動學逆解表達式為

q=f-1(X)

(5)

Sawyer機器人具有7自由度，關節幾何構型不滿足Pieper準則。本文采用Newton-Raphson法進行數值迭代求解逆運動學。由于關節空間維數大于工作空間維數，冗余機器人具有自運動性，對同一位姿會產生無窮組關節角與其對應。因此，需添加約束條件使機器人具有唯一逆解。Sawyer存在冗余自由度，雅克比矩陣J為6×7矩陣。

由于雅克比矩陣不是方陣，不能直接計算逆解，因此，使用最小二乘法的偽逆代替逆解進行計算。

在滿足末端跟隨誤差最小的情況下，使關節速度范數最小，最小二乘法解得雅克比偽逆

J+=JT(JJT)-1

(6)

雅克比矩陣J奇異值分解

J=UΣVT

(7)

式中：U∈Rm×m與V∈Rn×n為正交矩陣，Σ∈Rm×n由J的奇異值構成。

(8)

利用偽逆解求得關節最小范數為

(9)

(10)

對式(6)進行求解得

(11)

(12)

式中：J*為奇異魯棒性逆矩陣。奇異值分解，可得

(13)

(14)

根據奇異值的變化來確定阻尼系數的大小如式(15)，設定最小奇異值的閾值σ0，當機器人遠離奇異點時，最小奇異值大于閾值σ0，此時阻尼系數u為零，使用偽逆解，在保證跟蹤精度的情況下，使關節速度在合理范圍；當機器人接近奇異點時，最小奇異值小于閾值σ0，此時阻尼系數不再為零，保證關節速度不會超限。

(15)

式中：u0為最大阻尼系數，σ0為最小奇異值閾值。

利用Newton-Raphson法對Sawyer機器人進行逆運動學求解釋，基于機器人的微分雅克比變換：

Δq=J-1ΔX

(16)

式中：Δq為關節角度微小變量，ΔX=[dxdydzkxdθkydθkzdθ]為機器人末端位移的微小變量，其中dx，dy，dz是末端沿笛卡兒坐標系的移動量，kxdθ，kydθ，kzdθ是繞坐標系的旋轉量。機器人的微分變化矩陣可以用位姿矩陣與微分變換算子Δ的乘積表示。其中，微分變換算子Δ為：

Δ=Trans(dx，dy，dz)Rot(k，dθ)-I

(17)

式中：dx，dy，dz是在坐標軸上的量，Rot(k，dθ)是繞坐標向量k旋轉dθ角。

目標位姿由初始位姿Tstart與微分運動累加求解，為：

Tend=Tstart+Δ·Tstart

(18)

由式(18)求解微分變換算子Δ為：

(19)

根據式(17)可得機器人末端微小位移ΔX。

逆運動學算法流程如圖5所示。

圖5 逆運動算法流程圖

5 仿真驗證

5.1 逆運動學仿真

通過MATLAB進行數值仿真，通過給定機器人末端位姿變化，由式(5)～(19)，求解機器人末端到達指定位姿時各個關節角度，從而驗證逆運動學算法。仿真條件為：給出機器人末端的初始位姿為[1.01475，0.1603，0.317;0，1.571，0]，初始位姿位于奇異構型附近，其最小奇異值趨于零，終點位姿為[0.5432，0.1603，0.9839;0，1.047，0]，位姿參數中前3項為機器人末端在x，y，z方向的位置，單位為m，后三項為繞三個坐標軸的旋轉角度，單位為rad。參數初始化：u0=5，誤差ε=10-5。

通過奇異魯棒性逆的Newton iteration method迭代求解，經過57次迭代后，對應終點位姿的關節角度逆解趨于穩定，如圖6所示。

圖6 逆運動學求解中關節角度與迭代次數關系曲線

通過逆運動學求解，最終得到的關節角是q=[-0.0112，1.0482，-0.0138，1.0489，0.0112，1.0478，0.0138]T，沒有出現關節突變的現象，說明機器人在逆解過程中，各關節運動平穩，可以求解出穩定解。因此可采用所建立的逆運動學求解方法在機器人工作空間中進行運動規劃，通過逆運動學解得各關節的運動路徑。

為驗證所求解的關節角度是否與給出的末端位姿對應，將終點機器人關節角度再次代入正運動學模型，求解得出此關節角度對應的機器人末端位姿為[0.5436，0.1603，0.9838;0，0.1047，0]，解算后的機器人位姿如圖7。

圖7 終點位姿模型

將逆運動學解得關節坐標帶入正運動學方程，解得此時機器人末端姿態矩陣如表1所示。

表1 逆運動學仿真結果

對照設定的機器人終點位姿，驗證了運動學逆解求解方法的有效性。

5.2 RRT算法驗證

5.2.1 仿真

實驗環境:win10，Intel Core i5，主頻2.6GHz，8G內存。編譯工具為MATLAB 2017a。狀態空間1000*1000，其中起始節點坐標為[10，10]，目標點坐標為[900，900]，黑色斑塊為障礙物群，隨機設置障礙物形狀及位置。

實驗一：對傳統RRT算法進行仿真。從起始根節點開始，在空間中以給定長度為生長步長隨機進行擴展，當隨機樹枝點到達目標點時，擴展停止，規劃過程如圖8所示。從起始節點起，以20為步長，隨機生成新枝點，連接起始點與目標點規劃出的路徑如圖9。

圖8 傳統RRT 圖9 規劃路徑

實驗二：對改進RRT算法進行仿真。從初始根節點與目標點同時擴展，生成新枝點，當兩枝點相遇時隨機樹停止擴展，連接起始根節點與目標點的規劃過程如圖10，生成改進規劃路徑如圖11。

圖10 改進RRT 圖11 改進路徑

表2 RRT與改進算法的特性對比

通過表2中兩種算法的特性數據對比可知，基于改進RRT算法在新枝點擴展上，有效地減少了新生枝點的數量，降低了算法的時間復雜度與計算量，提高了搜索效率，路徑相對傳統規劃方法更加平緩。

5.2.2 機械臂運動規劃實驗

在改進RRT算法有效性驗證的基礎上，設計如圖13所示的狹小空間(400*400)場景，模擬機器人進行加工時的可操作任務空間，對Sawyer冗余機械臂的無碰撞運動規劃能力進行驗證。首先，在ROS機器人操作系統中利用Rviz(三維可視化工具)進行可視化仿真。根據機器人模型與環境模型，通過編寫URDF文件，創建機器人工作空間，然后采用改進RRT算法，在具有邊界約束的狹小空間下，對冗余機械臂進行運動規劃。在關節空間下給定機械臂的初始臂型與終止臂型，完成機械臂的運動規劃。運動規劃的初始與終止臂型如圖12所示，采用改進RRT算法規劃的路徑如圖13所示。

圖12 運動規劃仿真過程

圖13 無碰撞狹小空間的改進RRT算法搜索路徑

與仿真中所構建的任務空間一致，在實際環境中利用鋁型材搭建400*400的狹小空間作為障礙物，初始機械臂末端伸入框架內，機械臂無碰撞運動至框架外作為實驗目標，對運動規劃方法進行實驗驗證，實驗過程如圖14所示。

圖14 實際環境下運動規劃過程

利用改進RRT算法順利完成狹小空間下機械臂的運動規劃。初始機械臂末端伸入障礙物框架內如圖14(a)；沿規劃路徑，首先運動至框架外，機械臂臂型如圖14(b)；機械臂運動至框架頂部，繞過框架頂部與頂角如圖14(c)；最終繞過框架到達終點位姿如圖14(d)。通過機械臂的運動實驗，驗證了改進RRT算法進行路徑規劃的正確性，在操作過程中有效地避免了與運動約束框架碰撞，保障了加工過程的安全。對應機械臂末端在操作空間中的規劃路徑，冗余機器人的各關節運動軌跡如圖15所示，可以看出在關節空間中冗余機器人關節運動平穩，驗證了逆運動學算法的正確性，為機械臂運動控制提供了有效的關節運動參數。

圖15 關節空間中的軌跡規劃曲線

6 結論

本文以七自由度協作機器人Rethink Sawyer為研究對象，運用改進RRT算法在狹小操作空間內考慮存在的路徑約束對機械臂進行運動規劃。實驗結果表明文中設計的七自由度協作機器人的逆運動學算法能夠有效的對機器人關節空間和操作空間中的運動關系進行求解，逆解的計算精度高，改進RRT算法有效縮短了運動規劃的時間，在復雜環境下有效避免與障礙物的碰撞，為七自由度協作機器人的運動規劃提供了可行的理論方法。