冗余自由度協(xié)作機(jī)器人運(yùn)動(dòng)規(guī)劃研究

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

(1.河南科技大學(xué)機(jī)電工程學(xué)院，河南 洛陽(yáng) 471003；2.河南省機(jī)器人與智能系統(tǒng)重點(diǎn)實(shí)驗(yàn)室，河南 洛陽(yáng) 471003；3.機(jī)械裝備先進(jìn)制造河南省協(xié)同創(chuàng)新中心，河南 洛陽(yáng) 471003)

1 引言

近年來(lái)，隨著產(chǎn)品多樣化、精細(xì)化、輕量化的發(fā)展，對(duì)機(jī)器人的應(yīng)用提出了新的要求。機(jī)器人要滿足生產(chǎn)線批量生產(chǎn)的剛性需求，也要滿足客戶個(gè)性定制的柔性制造。同時(shí)機(jī)器人從獨(dú)立工作轉(zhuǎn)向共享環(huán)境中的與人協(xié)作模式，工作環(huán)境不再需要圍欄隔離，機(jī)器人能夠直接與人并肩工作[1]。協(xié)作機(jī)器人對(duì)人機(jī)交互、柔順適應(yīng)等關(guān)鍵技術(shù)提出了更高的要求，約束空間中的機(jī)器人自主的運(yùn)動(dòng)規(guī)劃研究是協(xié)作交互的重要研究?jī)?nèi)容。

運(yùn)動(dòng)特性的研究是機(jī)器人運(yùn)動(dòng)控制和路徑規(guī)劃的基礎(chǔ)[2]。國(guó)內(nèi)外對(duì)非冗余機(jī)械臂運(yùn)動(dòng)學(xué)的研究已相當(dāng)成熟，但對(duì)冗余機(jī)械臂，特別是其逆運(yùn)動(dòng)學(xué)的研究相對(duì)有限。文獻(xiàn)[3]提出固定一個(gè)關(guān)節(jié)，將機(jī)器人改為六自由度求解，但關(guān)節(jié)的減少會(huì)降低機(jī)械臂的靈活性；李寧森[4]對(duì)無(wú)特殊構(gòu)型機(jī)械臂，采用偽逆法求逆解，此方法適合實(shí)時(shí)、高精度的逆運(yùn)動(dòng)學(xué)求解；魏延輝等[5]根據(jù)構(gòu)型平面的基本理論和半解析的方法進(jìn)行逆運(yùn)動(dòng)學(xué)求解。

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

針對(duì)特殊構(gòu)型的Sawyer機(jī)器人，本文對(duì)傳統(tǒng)RRT算法新枝點(diǎn)擴(kuò)展時(shí)無(wú)方向性，搜索時(shí)間長(zhǎng)的特點(diǎn)進(jìn)行算法改進(jìn)。在新枝點(diǎn)的擴(kuò)展上加入引力系數(shù)，使其具有方向性，根據(jù)機(jī)器人工作空間障礙物復(fù)雜程度的不同，通過(guò)調(diào)節(jié)引力系數(shù)，使新枝點(diǎn)擴(kuò)展具有合適的搜索步長(zhǎng)，同時(shí)利用雙向搜索有效減少了路徑規(guī)劃時(shí)間。采用阻尼最小二乘Newton-Raphson方法對(duì)Sawyer機(jī)器人逆運(yùn)動(dòng)學(xué)進(jìn)行數(shù)值求解。在MATLAB和ROS(機(jī)器人操作系統(tǒng))平臺(tái)進(jìn)行仿真和實(shí)驗(yàn)，通過(guò)實(shí)驗(yàn)驗(yàn)證路徑規(guī)劃改進(jìn)算法的有效性。

2 機(jī)器人構(gòu)形分析

在三維空間中，機(jī)器人至少需要六個(gè)獨(dú)立變量(三個(gè)位置，三個(gè)姿態(tài))，才能完整定義機(jī)械臂的末端位姿。冗余機(jī)器人由于自由度大于6，在機(jī)械臂的奇異點(diǎn)回避，空間避障等方面具有優(yōu)勢(shì)。由于機(jī)器人的關(guān)節(jié)存在限位，在工作空間內(nèi)，非冗余機(jī)器人末端執(zhí)行器存在不可達(dá)點(diǎn)，七自由度機(jī)器人通過(guò)增加了關(guān)節(jié)個(gè)數(shù)和旋轉(zhuǎn)角度，可以克服關(guān)節(jié)不可達(dá)問題。

七自由度機(jī)器人最常用的關(guān)節(jié)配置模式為Spherial-Roll-Spherial構(gòu)型，即腕部與肩部是由三個(gè)軸線互相垂直、交于一點(diǎn)的等效球關(guān)節(jié)，肘部為旋轉(zhuǎn)關(guān)節(jié)構(gòu)成，多為非偏置式構(gòu)型。其優(yōu)點(diǎn)是制造簡(jiǎn)單，運(yùn)動(dòng)學(xué)求解方便。無(wú)偏置球關(guān)節(jié)構(gòu)型如圖1所示。但由于其肘關(guān)節(jié)運(yùn)動(dòng)范圍受限，在需要執(zhí)行較復(fù)雜工作，或要求機(jī)械臂工作范圍盡可能大時(shí)，多采用圖2所示的偏置式非球關(guān)節(jié)構(gòu)型。

圖2 有偏置非球形關(guān)節(jié)構(gòu)型

本文以七自由度協(xié)作機(jī)器人Rethink Sawyer為研究對(duì)象，如圖3所示，為有偏置非球形關(guān)節(jié)構(gòu)型，肩部關(guān)節(jié)J0與J1的軸線垂直且無(wú)交點(diǎn)，連桿偏置，使得關(guān)節(jié)運(yùn)動(dòng)角度增加。

圖3 7自由度機(jī)器人Sawyer

3 基于RRT的改進(jìn)路徑規(guī)劃算法

3.1 RRT算法基本原理

RRT算法是在待搜索區(qū)域中，對(duì)地圖進(jìn)行分裂尋找從起始位置xinit到目標(biāo)位置xgoal的隨機(jī)搜索樹。算法的基本原理是，以xinit為根節(jié)點(diǎn)，在區(qū)域內(nèi)隨機(jī)采樣得到xrand，然后找到離xrand最近的節(jié)點(diǎn)xnear；在指向xrand的方向上生成給定步長(zhǎng)ρ的新節(jié)點(diǎn)xnew，若新生節(jié)點(diǎn)與障礙物沒有碰撞，則作為擴(kuò)展節(jié)點(diǎn)加入原隨機(jī)樹，否則重新進(jìn)行采樣；當(dāng)采樣點(diǎn)距目標(biāo)點(diǎn)xgoal距離小于設(shè)定閾值時(shí)，算法結(jié)束，否則繼續(xù)擴(kuò)展。其新生節(jié)點(diǎn)計(jì)算公式為：

(1)

圖4 RRT樹生長(zhǎng)過(guò)程

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

3.2 改進(jìn)RRT算法

針對(duì)傳統(tǒng)RRT算法在新枝點(diǎn)擴(kuò)展上無(wú)目標(biāo)性，時(shí)間復(fù)雜度高的特點(diǎn)?；谌斯?shì)場(chǎng)法中目標(biāo)引力的思想，提出枝點(diǎn)擴(kuò)展具有導(dǎo)引的變步長(zhǎng)雙向RRT算法。目標(biāo)點(diǎn)方向增加引力函數(shù)，枝節(jié)點(diǎn)朝著目標(biāo)點(diǎn)方向擴(kuò)展，無(wú)需對(duì)所有區(qū)域進(jìn)行搜索，減少了對(duì)冗余點(diǎn)的計(jì)算；根據(jù)搜索區(qū)域的復(fù)雜度，人工調(diào)節(jié)引力參數(shù)，避免局部極小值的產(chǎn)生；從根節(jié)點(diǎn)與目標(biāo)點(diǎn)同時(shí)擴(kuò)展，縮短搜索時(shí)間，提高路徑搜索效率。

根據(jù)引力勢(shì)能函數(shù)

(2)

求得引力函數(shù)

(3)

其中kp為引力場(chǎng)系數(shù)。

由此改進(jìn)后新枝點(diǎn)計(jì)算方法為

(4)

在新枝點(diǎn)的擴(kuò)展中，根據(jù)空間障礙物情況，調(diào)節(jié)合適的引力場(chǎng)系數(shù)kp。當(dāng)空間中無(wú)障礙物時(shí)，增大kp提高隨機(jī)樹向目標(biāo)點(diǎn)生長(zhǎng)的概率，減小新枝點(diǎn)隨機(jī)生長(zhǎng)的概率；在遇到障礙物時(shí)，減小kp至合適值，增強(qiáng)新枝點(diǎn)隨機(jī)擴(kuò)展的能力，繞開障礙物。不但保證了避障能力，同時(shí)增強(qiáng)了方向的指向能力。

基于經(jīng)典RRT算法，引入目標(biāo)引力思想與變步長(zhǎng)策略，雖然有效地降低了搜索過(guò)程的隨機(jī)性，提升了機(jī)器人的避障能力強(qiáng)，但路徑搜索效率較低。在此基礎(chǔ)上，引入雙向生長(zhǎng)策略，在起始位置xinit與目標(biāo)位置xgoal同時(shí)生成隨機(jī)擴(kuò)展樹，直至兩樹新生枝點(diǎn)間的距離小于給定閾值，即可認(rèn)為兩樹連通。

4 逆運(yùn)動(dòng)學(xué)求解算法

逆運(yùn)動(dòng)學(xué)是機(jī)器人進(jìn)行運(yùn)動(dòng)規(guī)劃的基礎(chǔ)，其實(shí)質(zhì)是已知機(jī)器人末端執(zhí)行器在工作空間的位姿，求解關(guān)節(jié)空間角度的過(guò)程，設(shè)工作空間末端位姿為X=[x1，x2，x3，x4，x5，x6]，關(guān)節(jié)空間角度為q=[q1，q2，q3，q4，q5，q6，q7]，運(yùn)動(dòng)學(xué)逆解表達(dá)式為

q=f-1(X)

(5)

Sawyer機(jī)器人具有7自由度，關(guān)節(jié)幾何構(gòu)型不滿足Pieper準(zhǔn)則。本文采用Newton-Raphson法進(jìn)行數(shù)值迭代求解逆運(yùn)動(dòng)學(xué)。由于關(guān)節(jié)空間維數(shù)大于工作空間維數(shù)，冗余機(jī)器人具有自運(yùn)動(dòng)性，對(duì)同一位姿會(huì)產(chǎn)生無(wú)窮組關(guān)節(jié)角與其對(duì)應(yīng)。因此，需添加約束條件使機(jī)器人具有唯一逆解。Sawyer存在冗余自由度，雅克比矩陣J為6×7矩陣。

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

在滿足末端跟隨誤差最小的情況下，使關(guān)節(jié)速度范數(shù)最小，最小二乘法解得雅克比偽逆

J+=JT(JJT)-1

(6)

雅克比矩陣J奇異值分解

J=UΣVT

(7)

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

(8)

利用偽逆解求得關(guān)節(jié)最小范數(shù)為

(9)

(10)

對(duì)式(6)進(jìn)行求解得

(11)

(12)

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

(13)

(14)

根據(jù)奇異值的變化來(lái)確定阻尼系數(shù)的大小如式(15)，設(shè)定最小奇異值的閾值σ0，當(dāng)機(jī)器人遠(yuǎn)離奇異點(diǎn)時(shí)，最小奇異值大于閾值σ0，此時(shí)阻尼系數(shù)u為零，使用偽逆解，在保證跟蹤精度的情況下，使關(guān)節(jié)速度在合理范圍；當(dāng)機(jī)器人接近奇異點(diǎn)時(shí)，最小奇異值小于閾值σ0，此時(shí)阻尼系數(shù)不再為零，保證關(guān)節(jié)速度不會(huì)超限。

(15)

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

利用Newton-Raphson法對(duì)Sawyer機(jī)器人進(jìn)行逆運(yùn)動(dòng)學(xué)求解釋，基于機(jī)器人的微分雅克比變換：

Δq=J-1ΔX

(16)

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

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

(17)

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

目標(biāo)位姿由初始位姿Tstart與微分運(yùn)動(dòng)累加求解，為：

Tend=Tstart+Δ·Tstart

(18)

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

(19)

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

逆運(yùn)動(dòng)學(xué)算法流程如圖5所示。

圖5 逆運(yùn)動(dòng)算法流程圖

5 仿真驗(yàn)證

5.1 逆運(yùn)動(dòng)學(xué)仿真

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

通過(guò)奇異魯棒性逆的Newton iteration method迭代求解，經(jīng)過(guò)57次迭代后，對(duì)應(yīng)終點(diǎn)位姿的關(guān)節(jié)角度逆解趨于穩(wěn)定，如圖6所示。

圖6 逆運(yùn)動(dòng)學(xué)求解中關(guān)節(jié)角度與迭代次數(shù)關(guān)系曲線

通過(guò)逆運(yùn)動(dòng)學(xué)求解，最終得到的關(guān)節(jié)角是q=[-0.0112，1.0482，-0.0138，1.0489，0.0112，1.0478，0.0138]T，沒有出現(xiàn)關(guān)節(jié)突變的現(xiàn)象，說(shuō)明機(jī)器人在逆解過(guò)程中，各關(guān)節(jié)運(yùn)動(dòng)平穩(wěn)，可以求解出穩(wěn)定解。因此可采用所建立的逆運(yùn)動(dòng)學(xué)求解方法在機(jī)器人工作空間中進(jìn)行運(yùn)動(dòng)規(guī)劃，通過(guò)逆運(yùn)動(dòng)學(xué)解得各關(guān)節(jié)的運(yùn)動(dòng)路徑。

為驗(yàn)證所求解的關(guān)節(jié)角度是否與給出的末端位姿對(duì)應(yīng)，將終點(diǎn)機(jī)器人關(guān)節(jié)角度再次代入正運(yùn)動(dòng)學(xué)模型，求解得出此關(guān)節(jié)角度對(duì)應(yīng)的機(jī)器人末端位姿為[0.5436，0.1603，0.9838;0，0.1047，0]，解算后的機(jī)器人位姿如圖7。

圖7 終點(diǎn)位姿模型

將逆運(yùn)動(dòng)學(xué)解得關(guān)節(jié)坐標(biāo)帶入正運(yùn)動(dòng)學(xué)方程，解得此時(shí)機(jī)器人末端姿態(tài)矩陣如表1所示。

表1 逆運(yùn)動(dòng)學(xué)仿真結(jié)果

對(duì)照設(shè)定的機(jī)器人終點(diǎn)位姿，驗(yàn)證了運(yùn)動(dòng)學(xué)逆解求解方法的有效性。

5.2 RRT算法驗(yàn)證

5.2.1 仿真

實(shí)驗(yàn)環(huán)境:win10，Intel Core i5，主頻2.6GHz，8G內(nèi)存。編譯工具為MATLAB 2017a。狀態(tài)空間1000*1000，其中起始節(jié)點(diǎn)坐標(biāo)為[10，10]，目標(biāo)點(diǎn)坐標(biāo)為[900，900]，黑色斑塊為障礙物群，隨機(jī)設(shè)置障礙物形狀及位置。

實(shí)驗(yàn)一：對(duì)傳統(tǒng)RRT算法進(jìn)行仿真。從起始根節(jié)點(diǎn)開始，在空間中以給定長(zhǎng)度為生長(zhǎng)步長(zhǎng)隨機(jī)進(jìn)行擴(kuò)展，當(dāng)隨機(jī)樹枝點(diǎn)到達(dá)目標(biāo)點(diǎn)時(shí)，擴(kuò)展停止，規(guī)劃過(guò)程如圖8所示。從起始節(jié)點(diǎn)起，以20為步長(zhǎng)，隨機(jī)生成新枝點(diǎn)，連接起始點(diǎn)與目標(biāo)點(diǎn)規(guī)劃出的路徑如圖9。

圖8 傳統(tǒng)RRT 圖9 規(guī)劃路徑

實(shí)驗(yàn)二：對(duì)改進(jìn)RRT算法進(jìn)行仿真。從初始根節(jié)點(diǎn)與目標(biāo)點(diǎn)同時(shí)擴(kuò)展，生成新枝點(diǎn)，當(dāng)兩枝點(diǎn)相遇時(shí)隨機(jī)樹停止擴(kuò)展，連接起始根節(jié)點(diǎn)與目標(biāo)點(diǎn)的規(guī)劃過(guò)程如圖10，生成改進(jìn)規(guī)劃路徑如圖11。

圖10 改進(jìn)RRT 圖11 改進(jìn)路徑

表2 RRT與改進(jìn)算法的特性對(duì)比

通過(guò)表2中兩種算法的特性數(shù)據(jù)對(duì)比可知，基于改進(jìn)RRT算法在新枝點(diǎn)擴(kuò)展上，有效地減少了新生枝點(diǎn)的數(shù)量，降低了算法的時(shí)間復(fù)雜度與計(jì)算量，提高了搜索效率，路徑相對(duì)傳統(tǒng)規(guī)劃方法更加平緩。

5.2.2 機(jī)械臂運(yùn)動(dòng)規(guī)劃實(shí)驗(yàn)

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

圖12 運(yùn)動(dòng)規(guī)劃仿真過(guò)程

圖13 無(wú)碰撞狹小空間的改進(jìn)RRT算法搜索路徑

與仿真中所構(gòu)建的任務(wù)空間一致，在實(shí)際環(huán)境中利用鋁型材搭建400*400的狹小空間作為障礙物，初始機(jī)械臂末端伸入框架內(nèi)，機(jī)械臂無(wú)碰撞運(yùn)動(dòng)至框架外作為實(shí)驗(yàn)?zāi)繕?biāo)，對(duì)運(yùn)動(dòng)規(guī)劃方法進(jìn)行實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)過(guò)程如圖14所示。

圖14 實(shí)際環(huán)境下運(yùn)動(dòng)規(guī)劃過(guò)程

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

圖15 關(guān)節(jié)空間中的軌跡規(guī)劃曲線

6 結(jié)論

本文以七自由度協(xié)作機(jī)器人Rethink Sawyer為研究對(duì)象，運(yùn)用改進(jìn)RRT算法在狹小操作空間內(nèi)考慮存在的路徑約束對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)規(guī)劃。實(shí)驗(yàn)結(jié)果表明文中設(shè)計(jì)的七自由度協(xié)作機(jī)器人的逆運(yùn)動(dòng)學(xué)算法能夠有效的對(duì)機(jī)器人關(guān)節(jié)空間和操作空間中的運(yùn)動(dòng)關(guān)系進(jìn)行求解，逆解的計(jì)算精度高，改進(jìn)RRT算法有效縮短了運(yùn)動(dòng)規(guī)劃的時(shí)間，在復(fù)雜環(huán)境下有效避免與障礙物的碰撞，為七自由度協(xié)作機(jī)器人的運(yùn)動(dòng)規(guī)劃提供了可行的理論方法。