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

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

0 引言

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

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

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

1 挖掘機(jī)運(yùn)動(dòng)學(xué)建模

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

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

圖1 挖掘機(jī)的D-H坐標(biāo)系

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

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

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

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

圖2 挖掘機(jī)鏟斗齒尖的工作范圍

圖3 挖掘路徑

1.2 逆運(yùn)動(dòng)學(xué)的數(shù)值解

軌跡規(guī)劃是分別對(duì)各個(gè)關(guān)節(jié)進(jìn)行軌跡求解，最后映射到鏟斗齒尖的軌跡[17]，因此需要通過(guò)運(yùn)動(dòng)學(xué)逆解將鏟斗齒尖對(duì)應(yīng)的坐標(biāo)轉(zhuǎn)化為關(guān)節(jié)角度值。本文采用粒子群算法進(jìn)行運(yùn)動(dòng)學(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為目標(biāo)點(diǎn)的鏟斗位置坐標(biāo)值；A1、B1、C1為初始點(diǎn)的關(guān)節(jié)角度值；A2、B2、C2為目標(biāo)點(diǎn)的關(guān)節(jié)角度值。

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

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

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

2.1 交叉插值多項(xiàng)式函數(shù)表達(dá)式

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

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

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

(3)

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

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

(4)

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

Ajxj=bj，

(5)

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

(6)

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

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

(7)

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

將方程組寫(xiě)作矩陣形式，即

Bjyj=cj，

(8)

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

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

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

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

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

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

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

(9)

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

hjk=tjck-tjc(k-1)。

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

(10)

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

gj,min=minhj，

(11)

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

hj=tjq1-tjq0。

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

(12)

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

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

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

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

4 仿真結(jié)果

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

表3 挖掘機(jī)的物理約束條件

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

動(dòng)臂關(guān)節(jié)交叉插值多項(xiàng)式的表達(dá)式為

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

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

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

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

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

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

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

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

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

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

圖7 挖掘路徑對(duì)比圖

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

(13)

(14)

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

圖8 交叉插值多項(xiàng)式與4-3-3-3-4多項(xiàng)式誤差對(duì)比圖

5 結(jié)論

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