NMPC算法下六自由度機(jī)械臂避障軌跡規(guī)劃

喬棟豪，仲志丹，張浩博，趙耀

(河南科技大學(xué)機(jī)電工程學(xué)院，河南洛陽 471003)

0 前言

隨著現(xiàn)代化生產(chǎn)的高速發(fā)展，機(jī)械臂成為了不可或缺的生產(chǎn)工具，在各個(gè)領(lǐng)域中都出現(xiàn)了機(jī)械臂的身影。機(jī)械臂在制造市場(chǎng)中的需求量連年增加，對(duì)其要求也越來越多，工作環(huán)境也更加復(fù)雜。對(duì)于六自由度機(jī)械臂來說，如何在復(fù)雜的環(huán)境中實(shí)現(xiàn)路徑規(guī)劃及避障是一個(gè)技術(shù)性難題。

關(guān)于機(jī)械臂避障方法的研究有很多，常見的有DONG和DU[1]提出的通過計(jì)算機(jī)械臂工作空間密度，進(jìn)而得到機(jī)械臂在有多個(gè)障礙物復(fù)雜環(huán)境中的無碰撞路徑，并通過仿真驗(yàn)證了該方法的可行性；LIU等[2]提出了一種以圓柱包圍盒模型作為檢測(cè)手段，通過將理想軌跡進(jìn)行分段處理，把每段軌跡的中間點(diǎn)作為參數(shù)，并對(duì)參數(shù)進(jìn)行優(yōu)化達(dá)到避障目的的方法。這種基于圓柱包圍盒模型的機(jī)械臂避障算法在對(duì)機(jī)械臂進(jìn)行碰撞檢測(cè)時(shí)也較為常見。ISMAIL等[3]將動(dòng)態(tài)避障算法應(yīng)用于纜索串聯(lián)機(jī)械臂，在保證執(zhí)行器纜索張力有界的前提下，尋找最短最快的避障路徑。此外，經(jīng)典的避障算法還有人工勢(shì)場(chǎng)法，在機(jī)器人的運(yùn)行環(huán)境中加入了引力場(chǎng)；快速隨機(jī)搜索樹算法(RRT)[4]，可以快速搜索空間當(dāng)中沒有障礙的路徑；概率路線圖法(PRM)[5]，是基于圖搜索的方法。上述這些算法能夠計(jì)算出機(jī)械臂最優(yōu)或次優(yōu)軌跡，但是計(jì)算量較大，無法對(duì)動(dòng)態(tài)變化做出快速反應(yīng)，同時(shí)無法將機(jī)械臂的軌跡優(yōu)化和運(yùn)動(dòng)學(xué)優(yōu)化聯(lián)系在一起。雖然后來CHEN等[6]采用分段描述機(jī)械臂軌跡，并通過優(yōu)化各段軌跡中間點(diǎn)選取的方法，在保證避障的同時(shí)對(duì)運(yùn)動(dòng)控制進(jìn)行優(yōu)化，但機(jī)械臂在運(yùn)動(dòng)過程中需要在中間點(diǎn)停頓，不符合實(shí)際應(yīng)用需求。國(guó)內(nèi)對(duì)于機(jī)械臂軌跡規(guī)劃的研究很多，尤其機(jī)械臂的安全問題是一個(gè)主要研究方向，其中碰撞檢測(cè)[7]和運(yùn)動(dòng)控制[8-9]都有很大的進(jìn)展，但是關(guān)于機(jī)械臂自主避障的研究相對(duì)較少。

非線性模型預(yù)測(cè)控制(Nonlinear Model Predictive Control, NMPC)[10]方法對(duì)系統(tǒng)模型不確定性具有較強(qiáng)的魯棒性，在控制相關(guān)領(lǐng)域[11]已經(jīng)有了廣泛應(yīng)用，同時(shí)對(duì)非線性系統(tǒng)控制效果明顯，是解決機(jī)械臂軌跡規(guī)劃問題的有效手段。NMPC控制算法是將復(fù)雜的問題分為多個(gè)階段，通過滾動(dòng)優(yōu)化策略，逐步得到每個(gè)階段的最優(yōu)解，減少了計(jì)算的復(fù)雜性，對(duì)動(dòng)態(tài)環(huán)境適應(yīng)能力強(qiáng)，因此該方法也可以用于解決機(jī)械臂動(dòng)態(tài)避障問題。該控制算法也曾被應(yīng)用于移動(dòng)機(jī)器人的軌跡跟蹤過程中，表現(xiàn)出良好的控制效果。

如今機(jī)械臂軌跡規(guī)劃研究中大多數(shù)使用PID控制與強(qiáng)化學(xué)習(xí)相結(jié)合，使得機(jī)械臂能夠按照預(yù)定軌跡運(yùn)動(dòng)并實(shí)現(xiàn)自主避障。NMPC控制算法與其他算法相比更適用于六自由度機(jī)械臂這種復(fù)雜的非線性控制系統(tǒng)，在運(yùn)動(dòng)過程中，運(yùn)動(dòng)的軌跡精度、響應(yīng)速度、對(duì)擾動(dòng)的抑制能力都有明顯提升，在避障過程省去強(qiáng)化學(xué)習(xí)中間訓(xùn)練環(huán)節(jié)，節(jié)約時(shí)間的同時(shí)保證機(jī)械臂不會(huì)在訓(xùn)練中受到災(zāi)難性的損壞。

本文作者在以上研究的基礎(chǔ)上提出一種基于NMPC六自由度機(jī)械臂避障軌跡規(guī)劃算法。首先對(duì)六自由度機(jī)械臂建立動(dòng)力學(xué)模型，然后通過使用圓柱包絡(luò)法對(duì)機(jī)械臂與障礙物間距離進(jìn)行判斷，當(dāng)判斷機(jī)械臂與障礙物相撞沖突時(shí)，以最小的代價(jià)實(shí)現(xiàn)沖突消解，同時(shí)使得機(jī)械臂快速重回預(yù)定軌跡，最后通過MATLAB對(duì)機(jī)械臂防撞控制進(jìn)行仿真。仿真結(jié)果表明，該算法能夠使得機(jī)械臂在保證運(yùn)動(dòng)穩(wěn)定前提下完成自主避障。

1 機(jī)械臂動(dòng)力學(xué)模型

六自由度機(jī)械在工業(yè)生產(chǎn)中應(yīng)用較廣，其系統(tǒng)構(gòu)造也極其復(fù)雜。在對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)仿真和控制算法的設(shè)計(jì)過程中，機(jī)械臂動(dòng)力學(xué)模型的推導(dǎo)至關(guān)重要。常見機(jī)械臂建模方法有許多種，此次采用拉格朗日公式法建立系統(tǒng)模型。

簡(jiǎn)化拉格朗日方程可以推導(dǎo)出機(jī)械臂的動(dòng)力學(xué)方程，形式如下：

(1)

用矢量形式表達(dá)，機(jī)械臂輸入約束如下：

τmin≤τ≤τmax

(2)

2 碰撞檢測(cè)方案設(shè)計(jì)

此次設(shè)計(jì)采用圓柱包絡(luò)法進(jìn)行碰撞檢測(cè)，用適當(dāng)高度和直徑的圓柱將機(jī)械臂和待測(cè)障礙物包裹起來，通過這種方法，把空間中的物體碰撞問題簡(jiǎn)化為兩個(gè)圓柱之間的位置關(guān)系的計(jì)算問題，當(dāng)障礙物進(jìn)入機(jī)械臂的探測(cè)范圍時(shí)，通過計(jì)算兩個(gè)圓柱之間的距離，判斷機(jī)械臂和障礙物發(fā)生碰撞的可能性。兩圓柱之間的位置關(guān)系如圖1所示。

圖1 兩圓柱之間的位置關(guān)系

在圖1中，圓柱體Ai和Bi分別是對(duì)機(jī)械臂和空間中遇到的障礙進(jìn)行簡(jiǎn)化后得到的模型，C為圓柱表面的圓心，兩個(gè)圓柱體半徑分別為ri和rj。對(duì)于圓柱體Ai來說，用O(x0,y0,z0)表示其質(zhì)心的坐標(biāo),那么(xi，yi,zi)和(xi+1，yi+1,zi+1)則表示該圓柱體上下兩端面的坐標(biāo)，該圓柱體Ai軸線方程表示如下：

(3)

m=xi+1-xi,n=yi+1-yi,p=zi+1-zi

(4)

對(duì)方程進(jìn)行簡(jiǎn)化可得：

Li=[Xi(t),Yi(t),Zi(t)]=[xi,yi,zi]+t[m,n,p]

(5)

通過相同的方法，也可以計(jì)算出圓柱體Bi軸線所處直線的方程為

Lj=[Xj(t),Yj(t),Zj(t)]=[xj,yj,zj]+t×[u,v,w]

(6)

其中：t∈(0,1)。

那么兩軸線間的垂直距離為

(7)

若兩圓柱體軸線間的垂直距離Dij小于兩圓柱體半徑之和ri+rj，則表示機(jī)械臂存在碰撞風(fēng)險(xiǎn)，需要啟動(dòng)避障控制，規(guī)劃新的軌跡；反之則表示不會(huì)與障礙物發(fā)生碰撞。

3 機(jī)械臂自主防撞最優(yōu)控制模型

當(dāng)判斷機(jī)械臂和障礙物將要發(fā)生碰撞沖突時(shí)，機(jī)械臂需要進(jìn)行自主控制實(shí)現(xiàn)沖突消解。在該過程中需要使機(jī)械臂以最小代價(jià)避開障礙物，然后繼續(xù)沿著預(yù)定軌跡運(yùn)動(dòng)。

(8)

其中：φ(tf)為距離代價(jià)函數(shù)，為tf時(shí)刻機(jī)械臂末端到達(dá)結(jié)束點(diǎn)遠(yuǎn)近程度；R為單位矩陣。定義tf時(shí)刻機(jī)械臂轉(zhuǎn)角位置向量為

Xu=

[q1(tf)q2(tf)q3(tf)q4(tf)q5(tf)q6(tf)]

目標(biāo)位置為

Xc=[q1q2q3q4q5q6]

Q為單位矩陣，則φ(tf)可改寫為

φ(tf)=[Xu(tf)-Xc]TQ[Xu(tf)-Xc]

(9)

φTRφ為機(jī)械臂防撞過程中的能量代價(jià)，g(t)為防撞代價(jià)，定義為

(10)

(11)

聯(lián)立上述各式，機(jī)械臂自主防撞最優(yōu)控制模型如下：

(12)

通過求解該非線性優(yōu)化模型，可以得到機(jī)械臂規(guī)避障礙物成功到達(dá)目標(biāo)位置的最優(yōu)驅(qū)動(dòng)力矩τ(i)。但是，對(duì)于六自由度機(jī)械臂復(fù)雜的非線性模型來講，龐大的計(jì)算量難以保證避障過程中的時(shí)效性。為此，需要對(duì)模型進(jìn)行離散處理，分步求得每一步的最優(yōu)控制結(jié)果。

4 基于NMPC機(jī)械臂防撞控制算法

4.1 線性化處理

采用常用的泰勒公式展開法進(jìn)行輸入輸出線性化處理[12-13]，通過該方法提高計(jì)算效率。在模型的線性化處理過程中引入滾動(dòng)優(yōu)化策略，實(shí)時(shí)優(yōu)化機(jī)械臂的空間狀態(tài)模型，保證線性化模型正確的同時(shí)增強(qiáng)了系統(tǒng)的魯棒性。

對(duì)機(jī)械臂的狀態(tài)變量進(jìn)行定義：

則機(jī)械臂連續(xù)狀態(tài)空間方程為

(13)

通過利用泰勒公式展開得到離散化系統(tǒng)模型如下：

(14)

其中：

C=[I6O6×6]

x(0)=[x1x2]T，x(i)表示機(jī)械臂在第i個(gè)采樣時(shí)刻的預(yù)測(cè)輸出，x1、x2分別表示位置和速度。

4.2 控制約束

在以式(14)為系統(tǒng)預(yù)測(cè)模型的基礎(chǔ)上提出了一種新的軌跡跟蹤控制器的設(shè)計(jì)方法，通過這種方法可以減小機(jī)械臂預(yù)測(cè)輸出和實(shí)際輸出之間的偏差，當(dāng)偏差到達(dá)最小時(shí)為最優(yōu)控制率。對(duì)最優(yōu)控制率評(píng)價(jià)的性能指標(biāo)如下：

(15)

τ=Kxref+Fx(t)+D

(16)

其中:x(t)=[x1x2]為當(dāng)前時(shí)刻系統(tǒng)狀態(tài)。

D=-M(x1)f(x1,x2)

4.3 軌跡約束

機(jī)械臂沿著預(yù)定軌跡運(yùn)動(dòng)過程中，如果遇到障礙物，應(yīng)當(dāng)使得機(jī)械臂能夠自主制定新的軌跡，繞開障礙物，然后重新快速回到預(yù)定軌跡上來。在滿足控制約束的前提下，機(jī)械臂軌跡約束控制器設(shè)計(jì)如下：

(17)

(18)

(19)

τmin≤Kxopt(1)+Fxs+D≤τmax

(20)

τmin≤Kxopt(i+1)+Fxopt(i)+D≤τmax

(21)

其中：J=(X-Xd)TQ(X-Xd)T+ΔUTRΔUT；xs為機(jī)械臂系統(tǒng)在當(dāng)前時(shí)刻的狀態(tài)；τold記錄上一時(shí)刻輸出力矩。

通過式(16)可以計(jì)算得到K、F、D的值，代入上述各式當(dāng)中，保證機(jī)械臂在預(yù)測(cè)時(shí)域內(nèi)預(yù)測(cè)輸出滿足各個(gè)力矩約束條件。同時(shí)在計(jì)算得到機(jī)械臂的最優(yōu)控制增量后，可以計(jì)算出機(jī)械臂下一時(shí)刻的期望軌跡xopt(1)。由于機(jī)械臂各關(guān)節(jié)轉(zhuǎn)動(dòng)存在范圍約束，記為qmax和qmin，則期望軌跡應(yīng)滿足：

qmin≤xopt1(i)≤qmaxi=1,…,N

4.4 機(jī)械臂動(dòng)機(jī)決策

機(jī)械臂在避障過程當(dāng)中，對(duì)算法的時(shí)效性要求較高。由于六自由度機(jī)械臂系統(tǒng)復(fù)雜，對(duì)搜索空間進(jìn)行剪切可以幫助快速找到最優(yōu)避障策略，減少計(jì)算量。文中定義5種防撞機(jī)動(dòng)策略，U、D、L、R、C，分別為機(jī)械臂以最大轉(zhuǎn)動(dòng)速率向上、下、左、右轉(zhuǎn)動(dòng)以及呆在原地5種方式。

上述5種策略構(gòu)成了機(jī)械臂的可選機(jī)動(dòng)策略集合E={U,D,L,R,C}，并且每一控制量:

對(duì)于每層預(yù)測(cè)時(shí)域都有5條分支數(shù)，當(dāng)預(yù)測(cè)時(shí)域?yàn)镹時(shí)，分支數(shù)為5N。隨著預(yù)測(cè)時(shí)域增大，計(jì)算量也不斷增加，需要對(duì)動(dòng)機(jī)決策樹進(jìn)行減枝進(jìn)而減少計(jì)算量。

機(jī)械臂在避障過程中逐漸向結(jié)束點(diǎn)靠近，在每一層預(yù)測(cè)時(shí)域中，代價(jià)函數(shù)J應(yīng)逐漸減小，在k時(shí)刻應(yīng)滿足以下關(guān)系：

Jk(τ)

(22)

式中：φ(x(k|k))為當(dāng)前位置x(k|k)的終端代價(jià)。

令U*(k)為機(jī)械臂在k時(shí)刻計(jì)算得到的最優(yōu)控制序列，則在k+1時(shí)刻控制序列為

(23)

綜上所述，建立基于NMPC的六自由度機(jī)械臂避障軌跡規(guī)劃算法，如下所示：

第一步，設(shè)定圓柱包絡(luò)法中圓柱半徑為dm，對(duì)預(yù)測(cè)時(shí)域、采樣周期、各個(gè)加權(quán)參數(shù)進(jìn)行初始化設(shè)置，同時(shí)確立5種動(dòng)機(jī)策略的對(duì)應(yīng)值。

最后，令k=k+1，返回步驟1，循環(huán)執(zhí)行。

5 仿真實(shí)驗(yàn)分析

5.1 搭建仿真環(huán)境

通過MATLAB Robotics Toolbox(version 10.1)工具箱來對(duì)機(jī)械臂避障算法進(jìn)行仿真驗(yàn)證，同時(shí)與PID和強(qiáng)化學(xué)習(xí)組成的控制避障算法的效果進(jìn)行對(duì)比。其中強(qiáng)化學(xué)習(xí)選用較為先進(jìn)的DDPG算法[14]進(jìn)行實(shí)驗(yàn)，該算法具有訓(xùn)練速度快、準(zhǔn)確率高的特點(diǎn)。

采用PUMU560機(jī)械臂作為控制對(duì)象，該機(jī)械臂的相關(guān)參數(shù)信息可以參考文獻(xiàn)[15]。通過MATLAB Robotics Toolbox(version 10.1)搭建模型，如圖2所示。

圖2 MATLAB中PUMU560機(jī)械臂模型

5.2 仿真結(jié)果分析

在空間中指定起始點(diǎn)和結(jié)束點(diǎn)，同時(shí)給出預(yù)定軌跡，將機(jī)械臂末端當(dāng)作成一個(gè)點(diǎn)，可以觀測(cè)到在二維平面中機(jī)械臂在誤差允許的范圍內(nèi)，沿著預(yù)定軌跡運(yùn)動(dòng)，如圖3所示?？梢悦黠@看出：機(jī)械臂末端在NMPC算法下運(yùn)動(dòng)更準(zhǔn)確，誤差較小，效果更好。

圖3 機(jī)械臂未遇到障礙物運(yùn)行軌跡

通過觀測(cè)機(jī)械臂在約束條件下轉(zhuǎn)動(dòng)位置、角速度、角加速度3個(gè)物理量來判斷機(jī)械臂在避障過程中的穩(wěn)定性。在實(shí)驗(yàn)運(yùn)動(dòng)過程中，第二關(guān)節(jié)轉(zhuǎn)動(dòng)幅度最大，因此對(duì)其轉(zhuǎn)動(dòng)角度、角速度、角加速度進(jìn)行分析，具體變化如圖4所示。

圖4 機(jī)械臂轉(zhuǎn)動(dòng)過程中第二關(guān)節(jié)角位置、角速度、角加速度圖像

從圖4可以看出隨著運(yùn)動(dòng)的進(jìn)行，機(jī)械臂在兩種控制算法下都可以到達(dá)指定位置，但是相對(duì)于圖(b)中的PID控制，圖(a)中在NMPC算法控制下，機(jī)械臂運(yùn)動(dòng)過程連續(xù)平滑，角速度、角加速度沒有出現(xiàn)跳點(diǎn)，關(guān)節(jié)、運(yùn)動(dòng)部件運(yùn)行更加平穩(wěn)。因此通過與PID算法對(duì)比可以發(fā)現(xiàn)，機(jī)械臂在響應(yīng)速度，控制精度以及抗擾動(dòng)等方面，NMPC算法的控制效果更好。

在避障過程中，強(qiáng)化學(xué)習(xí)DDPG算法需要提前進(jìn)行訓(xùn)練，其訓(xùn)練次數(shù)與成功率的關(guān)系如圖5所示。可以看出：當(dāng)訓(xùn)練次數(shù)達(dá)到7 000次時(shí)，避障成功率在90%以上，隨后再增加訓(xùn)練次數(shù)，成功率不再明顯增加，一直趨近平穩(wěn)。選用訓(xùn)練7 000次后的DDPG算法進(jìn)行避障實(shí)驗(yàn)。

圖5 DDPG算法下機(jī)械臂避障成功率

在預(yù)定軌跡中加入障礙物，機(jī)械臂在兩種算法下都可以提前對(duì)障礙物進(jìn)行規(guī)避，然后重新回到預(yù)定軌跡，到達(dá)結(jié)束點(diǎn)，如圖6所示。

圖6 機(jī)械臂進(jìn)行避障運(yùn)動(dòng)軌跡

隨后再反復(fù)多次進(jìn)行實(shí)驗(yàn)，發(fā)現(xiàn)機(jī)械臂在強(qiáng)化學(xué)習(xí)算法下會(huì)偶爾出現(xiàn)避障失敗的情況，然而NMPC算法卻能保證機(jī)械臂每次都能成功避開障礙物，到達(dá)結(jié)束點(diǎn)。通過對(duì)比可以發(fā)現(xiàn)，NMPC算法不但省去了訓(xùn)練過程，同時(shí)在避障的成功率上更有保證。

6 結(jié)論

通過對(duì)NMPC算法進(jìn)行改進(jìn)，將其應(yīng)用在六自由度機(jī)械臂的軌跡規(guī)劃當(dāng)中進(jìn)而達(dá)到空間避障的效果。通過采用圓柱包絡(luò)法對(duì)機(jī)械臂周圍存在的障礙物進(jìn)行碰撞檢測(cè)，同時(shí)為了減少計(jì)算量，將復(fù)雜的機(jī)械臂系統(tǒng)進(jìn)行了線性化處理。在機(jī)械臂動(dòng)力學(xué)模型中加入控制約束和軌跡約束，保證了運(yùn)動(dòng)精度與系統(tǒng)穩(wěn)定性。在機(jī)械臂避障過程中加入動(dòng)機(jī)決策，減少了搜索范圍，提高了避障的時(shí)效性。通過MATLAB仿真顯示，機(jī)械臂實(shí)際軌跡與預(yù)定軌跡近似一致，當(dāng)遇到障礙物時(shí)，可以避開障礙物然后重新回到預(yù)定軌跡，到達(dá)結(jié)束點(diǎn)。同時(shí)與PID+DDPG算法進(jìn)行對(duì)比，結(jié)果表明NMPC控制效果更好，而且機(jī)械臂動(dòng)態(tài)避障的成功率更高。但是NMPC算法的計(jì)算量較大，在未來的研究中會(huì)對(duì)該算法進(jìn)行進(jìn)一步優(yōu)化。