挖掘機(jī)時(shí)間最優(yōu)與脈動(dòng)最優(yōu)軌跡規(guī)劃

孫志毅，劉 松，王 銀，李 虹

（太原科技大學(xué)電子信息工程學(xué)院，山西 太原 030024）

1 引言

挖掘機(jī)作為一種功能典型，用途較為廣泛的工程機(jī)械之一，在建筑、工業(yè)、軍事等領(lǐng)域發(fā)揮著極其重要的作用［1］。但是復(fù)雜危險(xiǎn)的工作環(huán)境以及對(duì)人的依賴使得傳統(tǒng)挖掘機(jī)在挖掘過程中給操作員帶來了太多的挑戰(zhàn)。

對(duì)挖掘機(jī)進(jìn)行軌跡規(guī)劃研究作為其智能化發(fā)展的重要組成部分，對(duì)提高挖掘機(jī)的工作效率以及減少關(guān)節(jié)的磨損具有重要的意義。

在對(duì)6自由度的工業(yè)機(jī)器人以最優(yōu)時(shí)間和最優(yōu)脈動(dòng)為目標(biāo)的軌跡規(guī)劃方法中：文獻(xiàn)［2-3］采用七次B樣條插值的方法，規(guī)劃出了一條脈動(dòng)連續(xù)，曲線較為平滑且跟蹤誤差較小的焊接軌跡，但是隨著插值曲線次數(shù)越高，曲線容易出現(xiàn)扭擺和發(fā)散問題；文獻(xiàn)［4］采用五次非均勻有理B樣條曲線插值的方法，以機(jī)器人的運(yùn)行時(shí)間、能耗以及脈動(dòng)為目標(biāo)對(duì)其軌跡進(jìn)行優(yōu)化得到一條關(guān)節(jié)驅(qū)動(dòng)力矩變化較小的運(yùn)動(dòng)軌跡；在以挖掘機(jī)為對(duì)象的軌跡規(guī)劃研究中，文獻(xiàn)［5-6］采用3-3-5-3-3分段多項(xiàng)式插值法使挖掘機(jī)實(shí)現(xiàn)了平穩(wěn)連續(xù)的自主挖掘，但是得到的挖掘軌跡不能實(shí)現(xiàn)脈動(dòng)連續(xù)；文獻(xiàn)［7］分別采用5次非均勻有理B樣條曲線和3-3-5-3-3分段多項(xiàng)式曲線插值法，通過對(duì)比仿真發(fā)現(xiàn)5次非均勻有理B樣條插值法得到的軌跡更加光滑平穩(wěn)且能夠減少計(jì)算量。

上述對(duì)挖掘機(jī)軌跡規(guī)劃的研究中，采用分段多項(xiàng)式插值的方法在連接點(diǎn)處關(guān)節(jié)加速度及脈動(dòng)變化較大，并且低階次的曲線無法保證關(guān)節(jié)脈動(dòng)的連續(xù)；在對(duì)軌跡規(guī)劃時(shí)沒有考慮關(guān)節(jié)脈動(dòng)的性能指標(biāo)以及鏟斗齒間的姿態(tài)角。

以某小型三關(guān)節(jié)挖掘機(jī)作為研究對(duì)象，以最短運(yùn)行時(shí)間和最優(yōu)脈動(dòng)的加權(quán)和作為目標(biāo)函數(shù)，在滿足挖掘機(jī)動(dòng)臂、斗桿、鏟斗的運(yùn)動(dòng)學(xué)約束，動(dòng)作時(shí)間約束的條件下，同時(shí)在考慮鏟斗末端挖掘姿態(tài)角的情況下，采用C4連續(xù)的五次非均勻B樣條曲線來對(duì)離散點(diǎn)進(jìn)行插值，并利用自適應(yīng)粒子群優(yōu)化算法（APSO）進(jìn)行優(yōu)化求解得到一條脈動(dòng)連續(xù)且各關(guān)節(jié)啟停速度、加速度可分別設(shè)定的關(guān)節(jié)軌跡，最后通過MATLAB 仿真得到時(shí)間-脈動(dòng)最優(yōu)的末端軌跡。

2 最優(yōu)軌跡規(guī)劃問題描述

2.1 五次B樣條函數(shù)插值軌跡

設(shè)定挖掘機(jī)的末端挖掘路徑點(diǎn)已經(jīng)給出，在滿足末端挖掘軌跡精度要求的前提下，將挖掘空間的挖掘路徑進(jìn)行離散化，得到末端齒尖的位姿序列Ti和對(duì)應(yīng)的時(shí)間序列ti，再進(jìn)行逆運(yùn)動(dòng)學(xué)求解將路徑點(diǎn)的位姿矩陣Ti轉(zhuǎn)化為關(guān)節(jié)空間的角度值pi可得到每個(gè)關(guān)節(jié)的關(guān)節(jié)角度-時(shí)間節(jié)點(diǎn)序列{pi，ti}。

采用Ck-1連續(xù)的k次非均勻B樣條曲線構(gòu)造鏟斗末端軌跡。所有關(guān)節(jié)軌跡曲線均可以描述為如下式所示［2］：

式中：dj，j=0，1，…，n—曲線的控制頂點(diǎn)；可通過移動(dòng)某個(gè)或某幾個(gè)控制頂點(diǎn)來對(duì)曲線進(jìn)行局部的修改，而不需改變曲線的其他部分，u∈[ui，ui+1]?[uk，un+1]?[u0，un+k+1]—節(jié)點(diǎn)向量。

根據(jù)B樣條曲線的r階導(dǎo)數(shù)pr(u)，r=1，2，3可依次求得各個(gè)臂的速度，加速度以及脈動(dòng)曲線。根據(jù)德布爾算法，pr(u)可按照如下遞推公式計(jì)算：

根據(jù)B樣條曲線定義可知，要確定一條B樣條曲線，需要確定節(jié)點(diǎn)向量以及控制頂點(diǎn)序列。

在已知關(guān)節(jié)位置點(diǎn)序列為(ti，pi)，i=0，1，…，n，對(duì)時(shí)間序列ti進(jìn)行參數(shù)化處理得到k次B 樣條的節(jié)點(diǎn)向量為：u=[u0，u1，…，un+2k]。其中，

將節(jié)點(diǎn)向量[uk，un+k]內(nèi)的n+1個(gè)節(jié)點(diǎn)值依次代入B樣條曲線方程讓其分別等于n+1個(gè)插值數(shù)據(jù)點(diǎn)pi，得到n+1個(gè)B樣條曲線方程如下：

對(duì)于k次B 樣條插值曲線總共需要n+k個(gè)控制頂點(diǎn)di(i=0，1，…，n+k-1)，因此還需要k-1 個(gè)由切矢條件來確定的附加方程。設(shè)初始點(diǎn)速度、加速度分別為v0，a0，終止點(diǎn)速度、加速度分別為vf，af。則由B樣條的r階導(dǎo)矢公式可得；

對(duì)于五次B樣條，最終聯(lián)立以上式子可以得到n+5個(gè)關(guān)于曲線控制頂點(diǎn)的方程：

2.2 目標(biāo)函數(shù)

為了兼顧挖掘機(jī)機(jī)械臂運(yùn)行效率和關(guān)節(jié)運(yùn)動(dòng)過程中軌跡的平滑性，參考文獻(xiàn)［9］，定義兩個(gè)性能指標(biāo)S1，S2如下所示；

式中：S1—挖掘機(jī)關(guān)節(jié)軌跡運(yùn)行總時(shí)間，用來衡量挖掘機(jī)的工作效率；n—末端插值點(diǎn)個(gè)數(shù)；j—插值點(diǎn)序號(hào)；S2—挖掘機(jī)關(guān)節(jié)的平均脈動(dòng)之和，其中，N—關(guān)節(jié)個(gè)數(shù)，這里N=3；jerki(t)—第i個(gè)關(guān)節(jié)的脈動(dòng)函數(shù)。

設(shè)定第i個(gè)關(guān)節(jié)的速度最大值為vimax、加速度最大值為aimax、脈動(dòng)最大值為jimax。

根據(jù)B樣條的導(dǎo)矢方程可求得各關(guān)節(jié)速度，加速度和脈動(dòng)的曲線方程，如式（12）所示：

根據(jù)式（9）、式（10）所給定的兩個(gè)性能指標(biāo)以及根據(jù)式（12）所求得到的關(guān)節(jié)速度、加速度以及脈動(dòng)，定義優(yōu)化目標(biāo)和約束條件為：

關(guān)節(jié)約束：

2.3 自適應(yīng)粒子群算法尋優(yōu)

采用自適應(yīng)粒子群優(yōu)化算法對(duì)某個(gè)關(guān)節(jié)五段插值時(shí)間優(yōu)化的步驟為：

（1）根據(jù)經(jīng)驗(yàn)初始化粒子種群規(guī)模N，初始及結(jié)束時(shí)的慣性權(quán)重ωstart，ωend，加速度因子c1，c2，最大迭代次數(shù)等，一般N=20；

（2）根據(jù)經(jīng)驗(yàn)設(shè)定路徑點(diǎn)之間的最大運(yùn)行時(shí)間，在最大時(shí)間限制內(nèi)，隨機(jī)產(chǎn)生N組初始粒子，并初始設(shè)定種群的位置Xi=(xi1，xi2，…，xiD)T和速度VI=(vi1，vi2，…，viD)T，其中，D為粒子的維度；

（3）對(duì)每組粒子進(jìn)行時(shí)間節(jié)點(diǎn)歸一化處理，根據(jù)關(guān)節(jié)空間中的插值點(diǎn)反求B樣條曲線控制頂點(diǎn)后插值得到位置曲線，然后分別計(jì)算速度，加速度，加加速度并判斷其是否在約束條件范圍之內(nèi)，如果滿足，計(jì)算該組時(shí)間在當(dāng)前位置的適應(yīng)度值，否則將該組粒子適應(yīng)度值置為較大值，最后將每個(gè)粒子的當(dāng)前位置記為個(gè)體的最優(yōu)位置，種群最優(yōu)則在個(gè)體最優(yōu)中選取。

（4）通過式（20）更新粒子的速度向量、位置向量。依照式（21）更新慣性權(quán)重ω［10-11］，如果更新后粒子速度和位置超出其取值范圍，則用最大值代替。

（5）計(jì)算更新后每個(gè)粒子所在位置的適應(yīng)度值，然后與歷史最優(yōu)值進(jìn)行比較，如果當(dāng)前值更優(yōu)，則更新個(gè)體歷史最優(yōu)位置為當(dāng)前粒子位置，并再次與其他粒子的歷史最優(yōu)值進(jìn)行比較，更新群體最優(yōu)粒子位置；

（6）當(dāng)?shù)螖?shù)達(dá)到最大值時(shí)，則停止，得到一組最優(yōu)粒子(h1，h2，h3，h4，h5)，求得，軌跡在每個(gè)插值點(diǎn)的時(shí)間為：T=[t1t2t3t4t5t6]。否則返回步驟（4）。

3 仿真結(jié)果分析

首先在操作空間選取一組挖掘路徑點(diǎn)，其中包括鏟斗齒間在笛卡爾坐標(biāo)系中的位置(x，y，z)以及鏟斗的姿態(tài)角ζ，然后采用幾何法求運(yùn)動(dòng)學(xué)逆解得到對(duì)應(yīng)的關(guān)節(jié)空間角度值，如表1所示。各關(guān)節(jié)約束，如表2所示。

表1 關(guān)節(jié)位置序列Tab.1 Joint Position Sequence

表2 各關(guān)節(jié)的物理束條件Tab.2 Physical Constraints of Each Joint

采用APSO算法求解時(shí)間最優(yōu)和軌跡最優(yōu)問題，算法的參數(shù)設(shè)置為：種群規(guī)模為20，粒子位置的取值范圍［0.1，10］，粒子速度取值為［-1，1］之間，最大迭代次數(shù)200，慣性權(quán)重ωstart=0.9，ωend=0.4。

從大到小逐漸改變時(shí)間權(quán)重KT(KT+KJ=1)，在不同的權(quán)值下均采用粒子群算法進(jìn)行優(yōu)化，軌跡優(yōu)化結(jié)果，如表3 所示。

表3 軌跡優(yōu)化結(jié)果Tab.3 Results of Trajectory Optimization

由表3可知：隨著時(shí)間加權(quán)系數(shù)KT從1到0逐漸減小，挖掘機(jī)各關(guān)節(jié)的動(dòng)作時(shí)間從18.50s逐漸增大到40s；平均脈動(dòng)從9.04逐漸減小到0.88。

當(dāng)KT=1時(shí)，末端軌跡為時(shí)間最優(yōu)軌跡。當(dāng)KT=0時(shí)，目標(biāo)函數(shù)只含有脈動(dòng)函數(shù)。當(dāng)KT為1.0、0.5、0.3、0時(shí)得到挖掘機(jī)的各個(gè)關(guān)節(jié)軌跡，如圖1～圖4所示。

圖1 挖掘機(jī)關(guān)節(jié)角度變化曲線Fig.1 Variation Curve of Excavator Joint Angle

圖1中：a，b，c分別表示動(dòng)臂，斗桿和鏟斗關(guān)節(jié)隨權(quán)重KT從1到0的角度變化曲線；關(guān)節(jié)速度和加速度變化曲線，如圖2、圖3所示；從圖中可以看出各關(guān)節(jié)的移動(dòng)速度、加速度均滿足邊界條件，即啟停時(shí)刻均為0；且速度、加速度以及脈動(dòng)曲線的變化均在運(yùn)動(dòng)學(xué)約束范圍之內(nèi)。由圖4可知，隨著動(dòng)作時(shí)間的增加，脈動(dòng)曲線基本趨于平滑，對(duì)關(guān)節(jié)的沖擊更小。

圖2 挖掘機(jī)關(guān)節(jié)角速度變化曲線Fig.2 Variation Curve of Joint Angular Velocity of Excavator

圖3 挖掘機(jī)關(guān)節(jié)角加速度變化曲線Fig.3 Variation Curve of Joint Angular Acceleration of Excavator

圖4 挖掘機(jī)關(guān)節(jié)角加加速度變化曲線Fig.4 Variation Curve of Joint Jerk of Excavator

從結(jié)果圖可以發(fā)現(xiàn)當(dāng)權(quán)重系數(shù)KT從1變化到0.5時(shí)，各關(guān)節(jié)的動(dòng)作時(shí)間以及速度，加速度和脈動(dòng)變化基本相差不大，而當(dāng)KT從0.5變化到0.3時(shí)曲線的運(yùn)行時(shí)間卻有一個(gè)較大的提升，且從圖4鏟斗的加加速度曲線可以看出，平均脈動(dòng)權(quán)重KJ由0增加到0.5時(shí)，鏟斗的最大加加速度值基本沒有較大的變化，僅略小于約束值10，這是因?yàn)榧s束條件中已經(jīng)有考慮加加速度約束，即曲線的脈動(dòng)約束，在KJ取值較小時(shí)，約束對(duì)曲線的影響大于性能指標(biāo)，所以KJ由0增加到0.5時(shí)，優(yōu)化得到的時(shí)間變化不明顯。

根據(jù)表三以及結(jié)果圖可知當(dāng)KT從（1～0.5）變化時(shí)，脈動(dòng)函數(shù)對(duì)軌跡影響不大，當(dāng)KT從0.5變化至0.3時(shí)，關(guān)節(jié)軌跡運(yùn)行時(shí)間S1由20.17s增大到24.96s，提升了23.75%。

關(guān)節(jié)平均脈動(dòng)S2則減少了47.18%。當(dāng)KT從0.3變化至0時(shí)，S1則達(dá)到最大約束時(shí)間，運(yùn)行時(shí)間提升了60.26%，關(guān)節(jié)平均脈動(dòng)S2減少了75%。

考慮在時(shí)間最優(yōu)的條件下，為了使挖掘機(jī)各個(gè)脈動(dòng)最優(yōu)，選擇加權(quán)系數(shù)KT=0.3，KJ=0.7為挖掘機(jī)關(guān)節(jié)軌跡規(guī)劃的最佳取值。

在Matlab 中建立挖掘機(jī)機(jī)械臂模型，選擇加權(quán)系數(shù)KT=0.3，KJ=0.7時(shí)得到的挖掘機(jī)各個(gè)關(guān)節(jié)的軌跡進(jìn)行正運(yùn)動(dòng)學(xué)求解最后得到的末端挖掘路徑，如圖5所示。

圖5 挖掘機(jī)鏟斗齒尖運(yùn)動(dòng)軌跡曲線Fig.5 Movement Trajectory Curve of Excavator Bucket Tooth Tip

由圖可知，對(duì)關(guān)節(jié)空間采用五次非均勻曲線插值所得到的末端軌跡經(jīng)過所有給定路徑點(diǎn)，能夠完成軌跡規(guī)劃任務(wù)；每?jī)蓚€(gè)路徑點(diǎn)間的軌跡較為平滑。

綜上說明了以時(shí)間和脈動(dòng)為優(yōu)化目標(biāo)，采用五次非均勻B樣條曲線能夠?qū)崿F(xiàn)經(jīng)過給定路徑點(diǎn)，動(dòng)作時(shí)間較短，且軌跡平滑連續(xù)的要求。

4 結(jié)論

相比較多項(xiàng)式曲線而言，B樣條曲線擁有獨(dú)特的凸包性和局部支撐性的優(yōu)點(diǎn)；以最短運(yùn)行時(shí)間和最優(yōu)脈動(dòng)的加權(quán)和作為目標(biāo)函數(shù)，綜合考慮了挖掘機(jī)的運(yùn)行時(shí)間和關(guān)節(jié)脈動(dòng)，實(shí)際操作時(shí)可以通過調(diào)節(jié)權(quán)重系數(shù)來平衡運(yùn)動(dòng)時(shí)間和關(guān)節(jié)脈動(dòng)；考慮了鏟斗姿態(tài)角，可以保證挖掘機(jī)鏟斗在挖掘過程中更容易達(dá)到滿斗狀態(tài)；另外采用五次曲線可以保證關(guān)節(jié)初始速度，加速度均為0，且脈動(dòng)連續(xù)，規(guī)劃出來的軌跡可以降低對(duì)機(jī)械臂的磨損和沖擊，提高挖掘機(jī)的使用壽命，