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

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

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

0 前言

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

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

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

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

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

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

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

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

(1)

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

τmin≤τ≤τmax

(2)

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

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

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

在圖1中，圓柱體Ai和Bi分別是對機(jī)械臂和空間中遇到的障礙進(jìn)行簡化后得到的模型，C為圓柱表面的圓心，兩個圓柱體半徑分別為ri和rj。對于圓柱體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)

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

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，則表示機(jī)械臂存在碰撞風(fēng)險，需要啟動避障控制，規(guī)劃新的軌跡；反之則表示不會與障礙物發(fā)生碰撞。

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

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

(8)

其中：φ(tf)為距離代價函數(shù)，為tf時刻機(jī)械臂末端到達(dá)結(jié)束點遠(yuǎn)近程度；R為單位矩陣。定義tf時刻機(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ī)械臂防撞過程中的能量代價，g(t)為防撞代價，定義為

(10)

(11)

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

(12)

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

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

4.1 線性化處理

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

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

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

(13)

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

(14)

其中：

C=[I6O6×6]

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

4.2 控制約束

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

(15)

τ=Kxref+Fx(t)+D

(16)

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

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

4.3 軌跡約束

機(jī)械臂沿著預(yù)定軌跡運(yùn)動過程中，如果遇到障礙物，應(yīng)當(dāng)使得機(jī)械臂能夠自主制定新的軌跡，繞開障礙物，然后重新快速回到預(yù)定軌跡上來。在滿足控制約束的前提下，機(jī)械臂軌跡約束控制器設(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為機(jī)械臂系統(tǒng)在當(dāng)前時刻的狀態(tài)；τold記錄上一時刻輸出力矩。

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

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

4.4 機(jī)械臂動機(jī)決策

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

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

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

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

Jk(τ)

(22)

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

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

(23)

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

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

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

5 仿真實驗分析

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

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

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

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

5.2 仿真結(jié)果分析

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

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

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

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

從圖4可以看出隨著運(yùn)動的進(jìn)行，機(jī)械臂在兩種控制算法下都可以到達(dá)指定位置，但是相對于圖(b)中的PID控制，圖(a)中在NMPC算法控制下，機(jī)械臂運(yùn)動過程連續(xù)平滑，角速度、角加速度沒有出現(xiàn)跳點，關(guān)節(jié)、運(yùn)動部件運(yùn)行更加平穩(wěn)。因此通過與PID算法對比可以發(fā)現(xiàn)，機(jī)械臂在響應(yī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次時，避障成功率在90%以上，隨后再增加訓(xùn)練次數(shù)，成功率不再明顯增加，一直趨近平穩(wěn)。選用訓(xùn)練7 000次后的DDPG算法進(jìn)行避障實驗。

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

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

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

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

6 結(jié)論

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