自主挖掘的時間最優(yōu)軌跡規(guī)劃

蔡敢為，王芬，田軍偉，齊港，張軻忱

(1.廣西大學(xué) 機械工程學(xué)院，廣西 南寧 530004;2.河池學(xué)院 人工智能與制造學(xué)院，廣西 546300；3.湖南三一職業(yè)技術(shù)學(xué)院 工程技術(shù)學(xué)院, 湖南 長沙 410000)

0 引言

隨著挖掘機作業(yè)精度和效率要求的不斷提高以及為適應(yīng)危險惡劣環(huán)境，自動化、智能化的研究已成挖掘機的主要發(fā)展趨勢[1]。自動挖掘機的軌跡規(guī)劃是其控制的基礎(chǔ)，旨在規(guī)劃出一條通過給定路徑點并且滿足邊界約束條件的光滑的挖掘軌跡[2]。在此基礎(chǔ)上，基于時間最優(yōu)的軌跡規(guī)劃能夠提高挖掘作業(yè)的工作效率，近年來得到了國內(nèi)外學(xué)者的廣泛研究。對自動挖掘機進行時間最優(yōu)軌跡規(guī)劃，關(guān)鍵在于對其各個關(guān)節(jié)的規(guī)劃，使其滿足速度最值、加速度最值等約束條件。

挖掘機鏟斗末端的笛卡兒坐標可由各個運動關(guān)節(jié)的驅(qū)動函數(shù)確定，而驅(qū)動函數(shù)多為多項式插值函數(shù)[3-7]。徐海黎等[8]使用三次多項式將關(guān)節(jié)空間中的插值節(jié)點連接起來，但需要已知每個節(jié)點的速度和加速度，不具有普遍性。章旭[9]采用帶虛節(jié)點的三次多項式插值方法，雖然能夠保證首末位置的速度、加速度為零，但在虛節(jié)點處末端軌跡不可控，誤差較大。李海虹等[10]以角加速度為約束條件，修正3-3-5-3-3多項式的最高階階數(shù)，能更好地滿足操作的平穩(wěn)連續(xù)，但是求解相對復(fù)雜。孫志毅等[11]提出4-3-3-3-4多項式的方法，避免了采用高階函數(shù)造成的關(guān)節(jié)往復(fù)運動，但求解相對復(fù)雜。唐建業(yè)等[12]采用4-4-7-4多項式插值的方法，可以保證加加速度連續(xù)，能夠?qū)崿F(xiàn)時間和平穩(wěn)性的綜合最優(yōu)，但是計算量很大。劉涼等[13]采用分段五次樣條曲線對關(guān)節(jié)角度進行插值，雖然能保證驅(qū)動力矩的連續(xù)性以及滿足運動學(xué)和動力學(xué)的約束條件，但必須已知各插值節(jié)點的速度和加速度，計算麻煩。管成等[14]提出了一種基于NURBS插值多項式的挖掘軌跡規(guī)劃方法，通過萊布尼茲公式求解多項式的高階導(dǎo)矢，能夠保證關(guān)節(jié)加加速度連續(xù)，但求導(dǎo)公式復(fù)雜且求解效率低。Liu等[15]提出七次NURBS曲線插值方法，雖然可以有效降低進給速度波動，但是約束條件計算復(fù)雜。胡小平等[16]提出一種多項式和Newton插值法相結(jié)合的機械手軌跡規(guī)劃，雖然計算量小，但當(dāng)節(jié)點數(shù)多時，中間段Newton插值多項式計算仍然很難。綜上所述，多項式插值方法在更改目標節(jié)點后需重新計算整個優(yōu)化函數(shù)，在目標節(jié)點較多時，計算量大，而NURBS樣條插值，雖然不用計算整個優(yōu)化函數(shù)；但求解效率低，因此提出一種能夠適用于多目標節(jié)點的且算法效率高的插值方法，具有較重要的實際意義。

本文以某液壓挖掘機為研究對象，為使末端路徑更貼合規(guī)劃路徑，提出一種在關(guān)節(jié)空間內(nèi)交叉使用三次插值多項式與五次插值多項式的軌跡插值方法，其中三次多項式的參數(shù)僅以角位移確定，以覆蓋規(guī)劃軌跡上更多的點，三次多項式之間通過五次多項式連接，使角速度、角加速度連續(xù)、保證軌跡的穩(wěn)定性。最后以時間最優(yōu)為目標，采用該插值方法對軌跡進行優(yōu)化，得到貼合規(guī)劃軌跡的關(guān)節(jié)角度曲線。

1 挖掘機運動學(xué)建模

1.1 Denavit-Hartenberg(D-H)參數(shù)法建模

根據(jù)D-H坐標系原則建立某反鏟液壓挖掘機的運動學(xué)模型，如圖1所示。

圖1 挖掘機的D-H坐標系

其中，坐標系{O0:x0y0z0}設(shè)置在挖掘機回轉(zhuǎn)機構(gòu)的中心，坐標系{O1:x1y1z1}設(shè)置在動臂與機身的鉸接點處，坐標系{O2:x2y2z2}設(shè)置在動臂與斗桿的鉸接點處，坐標系{O3:x3y3z3}設(shè)置在斗桿與鏟斗的鉸接點處，坐標系{O4:x4y4z4}設(shè)置在鏟斗齒尖處。D-H坐標系中的參數(shù)ai為各連桿長度，即相鄰兩關(guān)節(jié)軸線之間的距離；αi為連桿扭角，即兩關(guān)節(jié)軸線之間的夾角；di為連桿距離，即xi軸與xi-1軸之間的距離；θi為連桿轉(zhuǎn)角，即xi軸與xi-1軸之間的夾角。挖掘機的D-H參數(shù)見表1，其中關(guān)節(jié)變量范圍通過液壓缸長度以及各部件長度、關(guān)節(jié)轉(zhuǎn)角求出。

表1 挖掘機的D-H參數(shù)

根據(jù)圖1的D-H坐標系以及表1的D-H參數(shù)可得鏟斗齒尖的坐標表達式為

(1)

式中：cθ0=cosθ0；sθ0=sinθ0；cθ1=cosθ1；sθ1=sinθ1；c12=cos(θ1+θ2)；

c123=cos(θ1+θ2+θ3)；s12=sin(θ1+θ2)；s123=sin(θ1+θ2+θ3)。

結(jié)合鏟斗齒尖的坐標表達式以及關(guān)節(jié)變量范圍，在不考慮挖掘機回轉(zhuǎn)的情況下，通過數(shù)值分析軟件繪出挖掘機鏟斗齒尖的工作范圍如圖2所示。同時在圖2中以挖坑為例取點，具體數(shù)據(jù)見位姿空間到關(guān)節(jié)空間的轉(zhuǎn)換表(表2)中的第二列數(shù)據(jù)，挖掘路徑如圖3所示。

圖2 挖掘機鏟斗齒尖的工作范圍

圖3 挖掘路徑

1.2 逆運動學(xué)的數(shù)值解

軌跡規(guī)劃是分別對各個關(guān)節(jié)進行軌跡求解，最后映射到鏟斗齒尖的軌跡[17]，因此需要通過運動學(xué)逆解將鏟斗齒尖對應(yīng)的坐標轉(zhuǎn)化為關(guān)節(jié)角度值。本文采用粒子群算法進行運動學(xué)逆解求解，考慮各角度變化盡可能小，以公式(2)作為適應(yīng)度函數(shù)。

f=10 000((a3cos(A2+B2+C2)+a2cos(A2+B2)+a1cos(A2)-X2)2+

(a3sin(A2+B2+C2)+a2sin(A2+B2)+a1sin(A2)+d0-Y2)2)+

min(|A2-A1|+|B2-B1|+|C2-C1，|

(2)

式中：X2、Y2為目標點的鏟斗位置坐標值；A1、B1、C1為初始點的關(guān)節(jié)角度值；A2、B2、C2為目標點的關(guān)節(jié)角度值。

本文給定初始點的關(guān)節(jié)角度值為(20°,-90°,-45°)，第一個目標點的坐標為(6 m, 0 m,-4 m)，并以第一個目標點對應(yīng)的關(guān)節(jié)角度值作為第二個目標點對應(yīng)的初始關(guān)節(jié)角度值，依次類推。粒子群算法基本參數(shù)設(shè)定如下：粒子數(shù)N=20,慣性權(quán)重ω=1，加速因子C1=C2=1.5，最后進化代數(shù)為1 200。位姿空間到關(guān)節(jié)空間的轉(zhuǎn)換見表2。

表2 位姿空間到關(guān)節(jié)空間的轉(zhuǎn)換表

2 交叉插值多項式函數(shù)

2.1 交叉插值多項式函數(shù)表達式

用運動學(xué)逆解方法結(jié)合粒子群算法求出每個作業(yè)路徑點對應(yīng)的關(guān)節(jié)空間角度值后，需要對各個關(guān)節(jié)擬合軌跡曲線。為保證工作效率及穩(wěn)定性，本文在關(guān)節(jié)空間內(nèi)采用一種三次插值多項式與五次插值多項式相交叉的新型軌跡插值方法對關(guān)節(jié)軌跡進行擬合。

假設(shè)一共有n段三次多項式，將五次多項式穿插于三次多項式之間后，再將整條軌跡始末各增加一段五次曲線，則五次曲線的段數(shù)為(n+1)段，第i個關(guān)節(jié)的插值表達式為公式(3)。

其中，θiqj(t)表示t時刻第i個關(guān)節(jié)的第j段五次曲線位移，θicj(t)表示t時刻第i個關(guān)節(jié)的第j段三次曲線位移，aiqjl表示第i個關(guān)節(jié)的第j段五次曲線的l次項系數(shù)，aicjl表示第i個關(guān)節(jié)的第j段三次曲線的l次項系數(shù)。

(3)

2.2 插值函數(shù)系數(shù)求解

為保證插值函數(shù)軌跡可以經(jīng)過更多規(guī)劃路徑上的點，因此僅以4個目標點的角位移確定每段三次多項式的系數(shù)，即

(4)

其中tjck(k=0，1，…，3)依次為第j段三次曲線經(jīng)過四個目標點的時刻。為計算簡便，將每段三次曲線分開計算，令每段曲線的初始時刻tjc0=0。θijk為第i個關(guān)節(jié)的第j段三次曲線在tjck時刻時的角位移。將方程組寫作矩陣形式，即

Ajxj=bj，

(5)

再考慮五次多項式，當(dāng)五次多項式處于2個三次多項式之間時，其系數(shù)由相鄰的三次多項式的速度及加速度確定，因此對式(4)在三次曲線的初始及結(jié)束時刻對時間求一階導(dǎo)及二階導(dǎo)，得

(6)

為保證軌跡段之間速度與加速度連續(xù)，使第j段五次曲線的起始時刻的速度、加速度為第j-1段三次曲線的結(jié)尾時刻的速度、加速度，其結(jié)束時刻的速度、加速度為第j段三次曲線的起始時刻的速度、加速度。

則此時五次多項式系數(shù)可由式(7)確定：

(7)

其中tjq0及tjq1分別為第j段五次曲線初始時刻與結(jié)束時刻。將每段五次曲線分開計算，令每段曲線的初始時刻tjq0=0。

將方程組寫作矩陣形式，即

Bjyj=cj，

(8)

yj=(aiqj5,aiqj4,aiqj3,aiqj2,aiqj1,aiqj0)T；

當(dāng)五次多項式處于整條軌跡起始位置或結(jié)尾時，只需將上式中的cj替換為

其中θi0及θi1n分別為第i個關(guān)節(jié)整條軌跡初始角位移及結(jié)尾角位移。

3 基于時間最優(yōu)的軌跡規(guī)劃

3.1 分段優(yōu)化設(shè)計問題的建立

通過對上述三次方程和五次方程系數(shù)求解，可以看出只有插值時間是待定的，因此可以將插值時間進行優(yōu)化確定方程系數(shù)。本文將總體時間最優(yōu)化問題，分解為各段三次插值曲線與五次插值曲線兩部分優(yōu)化設(shè)計問題的組合，并分段建立優(yōu)化設(shè)計問題。

在挖掘作業(yè)時，各關(guān)節(jié)相互聯(lián)合運動完成作業(yè)，根據(jù)事先規(guī)劃好的軌跡由液壓缸調(diào)整驅(qū)動動臂，斗桿和鏟斗三個關(guān)節(jié)同時運動且每個關(guān)節(jié)均需要滿足對應(yīng)的最大角速度和角加速度限制,因此每段三次曲線的優(yōu)化目標函數(shù)為

(9)

式中hjk表示第j段三次曲線插值節(jié)點之間的時間間隔，即

hjk=tjck-tjc(k-1)。

假設(shè)一共有N個關(guān)節(jié)，則約束條件為

(10)

再考慮五次插值曲線，每個關(guān)節(jié)每段五次曲線的優(yōu)化目標函數(shù)為

gj,min=minhj，

(11)

式中hj表示第j段五次曲線插值節(jié)點之間的時間間隔，即

hj=tjq1-tjq0。

每段五次曲線優(yōu)化設(shè)計問題的約束條件為

(12)

3.2 優(yōu)化設(shè)計問題的求解

活躍目標點粒子群優(yōu)化(APSO)算法[18]是一種改進的粒子群算法，其基本思想是,在標準PSO速度更新公式中引入第3個目標點,稱為活躍目標點,從而構(gòu)成新的基于3目標點速度更新機制的粒子速度更新公式。APSO的優(yōu)點是較好地克服了PSO的早熟收斂問題,并兼具復(fù)合形法射線搜索的能力。

本文將hjk、hj作為優(yōu)化設(shè)計變量，結(jié)合APSO算法，分別對每段三次曲線及五次曲線進行優(yōu)化，并匯總得到總體最優(yōu)時間優(yōu)化設(shè)計的結(jié)果。優(yōu)化設(shè)計問題算法流程如圖4所示。

圖4 優(yōu)化設(shè)計問題算法流程

4 仿真結(jié)果

將1.2節(jié)中規(guī)劃的10個插值節(jié)點，根據(jù)新的插值方法分配節(jié)點，得到形如5-3-5-3-5的分配結(jié)果。各個關(guān)節(jié)對應(yīng)的最大角速度和角加速度限制數(shù)值，其數(shù)值大小由挖掘機的物理約束條件(表3)給出。

表3 挖掘機的物理約束條件

由3.2節(jié)中的優(yōu)化算法得到2段三次曲線以及3段五次曲線的最短時間。第1段三次曲線所經(jīng)過的時間依次為0.891 7、1.340 9、2.176 9 s，對應(yīng)的f1,min=4.409 6 s，第2段三次曲線所經(jīng)過的時間依次為1.270 9、1.336 7、1.351 7 s，對應(yīng)的f2,min=3.959 2 s。3個五次曲線插值時間分別為1.902 6、1.223 7、2.496 9 s。同時得到每個關(guān)節(jié)2段三次曲線的系數(shù)。

動臂關(guān)節(jié)交叉插值多項式的表達式為

斗桿關(guān)節(jié)交叉插值多項式的表達式為

鏟斗關(guān)節(jié)交叉插值多項式的表達式為

本文選取10個插值節(jié)點中序號為1、3、5、6、8、10的6個關(guān)節(jié)角度值，以文獻[5]中的4-3-3-3-4多項式為關(guān)節(jié)插值函數(shù)，通過APSO算法對關(guān)節(jié)插值時間進行優(yōu)化求解，最終可以求出每個關(guān)節(jié)的插值曲線系數(shù)。動臂關(guān)節(jié)4-3-3-3-4的插值表達式為

斗桿關(guān)節(jié)4-3-3-3-4的插值表達式為

鏟斗關(guān)節(jié)4-3-3-3-4的插值表達式為

分別對本文的交叉插值多項式和4-3-3-3-4多項式關(guān)節(jié)角度函數(shù)求取一階和二階導(dǎo)數(shù)，得出了挖掘機3個關(guān)節(jié)角度、角速度、角加速度變化曲線圖，分別如圖5、6所示。

(a)交叉插值多項式最優(yōu)關(guān)節(jié)角度圖

從圖5可以看出，通過APSO算法得到的交叉插值多項式的角度、角速度、角加速度曲線連續(xù)，且都在給定的約束條件范圍內(nèi)，說明此方法的正確性和有效性。從圖6可以看出，由4-3-3-3-4插值多項式得出的關(guān)節(jié)角度、角速度、角加速度圖雖然連續(xù)，但在時間最優(yōu)算法求解中，需要22維的矩陣求解以及數(shù)量相對多的速度、加速度約束條件，計算量較大。

(a)4-3-3-3-4時間最優(yōu)關(guān)節(jié)角度

將交叉插值多項式和4-3-3-3-4的關(guān)節(jié)角度曲線表達式代入末端鏟斗位姿的表達式中，可以得到末端笛卡兒空間的軌跡曲線圖。挖掘路徑對比圖如圖7所示，從中可以看出，交叉插值多項式與4-3-3-3-4插值多項式相比，其覆蓋的路徑點多，且更貼合實際路徑曲線。為了更加準確地描述此特點，本文將規(guī)劃路徑均勻分為N個路徑點，以每段規(guī)劃路徑點與實際位置的實時誤差以及整段規(guī)劃路徑的平均誤差作為指標進行比較分析，如式(13)所示，其中，Pos_thr(i)為第i個規(guī)劃路徑點的位置，Pos(i)為第i個路徑點所對應(yīng)的實際位置，error_real(i)為第i個規(guī)劃路徑點的實時誤差，error_avg為整段規(guī)劃路徑的平均誤差。

圖7 挖掘路徑對比圖

error_real(i)=|Pos(i)-Pos_thr(i)|,

(13)

(14)

由式(13)可以分別計算出交叉插值多項式方法與4-3-3-3-4插值多項式方法的實時誤差如圖8所示。由圖中可以看出交叉插值多項式方法與4-3-3-3-4插值多項式方法的實時誤差最大值分別為0.428 5、0.580 1 m，本文方法的與規(guī)劃路徑的最大實時誤差減少了26.1%。由式(14)可以分別計算出交叉插值多項式方法與4-3-3-3-4插值多項式方法的平均誤差分別為0.197 3、0.281 7 m，本文方法的與規(guī)劃路徑的平均誤差減少了30.0%。綜上所述，本文方法比傳統(tǒng)方法誤差值小，更接近規(guī)劃路徑。

圖8 交叉插值多項式與4-3-3-3-4多項式誤差對比圖

5 結(jié)論

根據(jù)挖掘機各液壓缸長度范圍以及構(gòu)件長度算出三個關(guān)節(jié)角度范圍值，利用粒子群算法得到各個末端位姿點到關(guān)節(jié)空間角度的轉(zhuǎn)化值。本文中提出一種新型的交叉插值多項式軌跡函數(shù)，介紹了插值函數(shù)表達式的建立以及表達式中系數(shù)的求解方法。利用新型軌跡插值函數(shù)，建立時間最優(yōu)軌跡規(guī)劃優(yōu)化問題，結(jié)合APSO算法給出優(yōu)化問題的求解流程。給出算例，得到滿足運動約束條件的最優(yōu)時間，以及各關(guān)節(jié)角度、角速度及角加速度曲線圖，并與4-3-3-3-4多項式方法對比，其最大誤差減少26.1%，平均誤差減少30.0%，說明此方法計算量小且末端軌跡更貼合實際軌跡。