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

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

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

0 前言

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

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

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

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

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

1 機械臂動力學(xué)模型

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

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

(1)

用矢量形式表達，機械臂輸入約束如下：

τmin≤τ≤τmax

(2)

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

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

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

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

(3)

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

(4)

對方程進行簡化可得：

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

(5)

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

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

(6)

其中：t∈(0,1)。

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

(7)

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

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

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

(8)

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

Xu=

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

目標位置為

Xc=[q1q2q3q4q5q6]

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

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

(9)

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

(10)

(11)

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

(12)

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

4 基于NMPC機械臂防撞控制算法

4.1 線性化處理

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

對機械臂的狀態(tài)變量進行定義：

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

(13)

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

(14)

其中：

C=[I6O6×6]

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

4.2 控制約束

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

(15)

τ=Kxref+Fx(t)+D

(16)

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

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

4.3 軌跡約束

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

(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為機械臂系統(tǒng)在當前時刻的狀態(tài)；τold記錄上一時刻輸出力矩。

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

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

4.4 機械臂動機決策

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

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

對于每層預(yù)測時域都有5條分支數(shù)，當預(yù)測時域為N時，分支數(shù)為5N。隨著預(yù)測時域增大，計算量也不斷增加，需要對動機決策樹進行減枝進而減少計算量。

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

Jk(τ)

(22)

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

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

(23)

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

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

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

5 仿真實驗分析

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

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

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

圖2 MATLAB中PUMU560機械臂模型

5.2 仿真結(jié)果分析

在空間中指定起始點和結(jié)束點，同時給出預(yù)定軌跡，將機械臂末端當作成一個點，可以觀測到在二維平面中機械臂在誤差允許的范圍內(nèi)，沿著預(yù)定軌跡運動，如圖3所示。可以明顯看出：機械臂末端在NMPC算法下運動更準確，誤差較小，效果更好。

圖3 機械臂未遇到障礙物運行軌跡

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

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

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

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

圖5 DDPG算法下機械臂避障成功率

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

圖6 機械臂進行避障運動軌跡

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

6 結(jié)論

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