六自由度機(jī)械臂軌跡規(guī)劃算法研究

任 偉，江 明

(安徽工程大學(xué) 電氣工程學(xué)院，安徽 蕪湖 241000)

無(wú)論是結(jié)構(gòu)型機(jī)器人(機(jī)械臂)，還是移動(dòng)型機(jī)器人，想要實(shí)現(xiàn)其連續(xù)平穩(wěn)地運(yùn)行，對(duì)于其軌跡的提前規(guī)劃都不容忽視。近年來(lái)，很多工業(yè)制造行業(yè)中已經(jīng)大量投入使用工業(yè)應(yīng)用型機(jī)械臂。由于應(yīng)用場(chǎng)景和應(yīng)用目的不同，使得軌跡規(guī)劃出現(xiàn)了多種算法。對(duì)于比較簡(jiǎn)單的點(diǎn)到點(diǎn)(PTP)操作，軌跡規(guī)劃中更多的是使用一些簡(jiǎn)單的插值方法。文獻(xiàn)[1-4]中針對(duì)機(jī)器人不同的應(yīng)用場(chǎng)景，采用多項(xiàng)式、S曲線(xiàn)、3次B樣條等插值算法，實(shí)現(xiàn)了機(jī)械臂的軌跡規(guī)劃。而對(duì)于較為復(fù)雜目的的軌跡規(guī)劃，通常采用的是基于分段插值的軌跡規(guī)劃思想。針對(duì)工業(yè)機(jī)器人運(yùn)行不平穩(wěn)問(wèn)題，文獻(xiàn)[5]中采用3-3-3-3-3多項(xiàng)式算法，對(duì)搬運(yùn)機(jī)器人實(shí)現(xiàn)軌跡規(guī)劃，但仍存在中間過(guò)程較多、復(fù)雜度較高的問(wèn)題。同樣基于運(yùn)行不平穩(wěn)問(wèn)題，為了改善算法復(fù)雜度，文獻(xiàn)[6]中采用5-3-5多項(xiàng)式算法，實(shí)現(xiàn)了機(jī)械臂的關(guān)節(jié)角在關(guān)節(jié)空間的分段插值，完成碼垛功能。而對(duì)于標(biāo)準(zhǔn)B樣條插值算法存在啟停位置振動(dòng)問(wèn)題，文獻(xiàn)[7]中運(yùn)用了高階次多項(xiàng)式與B樣條優(yōu)化相結(jié)合的插值算法，很好地解決了這一問(wèn)題。對(duì)于機(jī)器人在應(yīng)用過(guò)程中出現(xiàn)的問(wèn)題，還有很多學(xué)者進(jìn)行了一系列的優(yōu)化研究。

針對(duì)PUMA560機(jī)器人，分別從運(yùn)動(dòng)學(xué)建模和軌跡規(guī)劃兩個(gè)方面進(jìn)行探究。首先，對(duì)于運(yùn)動(dòng)學(xué)建模問(wèn)題，主要是利用D-H參數(shù)法和解析法構(gòu)建其正逆向運(yùn)動(dòng)學(xué)模型。其次，對(duì)于軌跡規(guī)劃問(wèn)題，分析討論了4-5-4多項(xiàng)式分段插值算法原理，并與傳統(tǒng)的3-5-3多項(xiàng)式分段插值算法進(jìn)行了對(duì)比研究。最終采用4-5-4多項(xiàng)式分段插值算法實(shí)現(xiàn)各關(guān)節(jié)角度插值，并利用正向運(yùn)動(dòng)學(xué)獲得機(jī)器人末端的運(yùn)行軌跡，實(shí)現(xiàn)了機(jī)器人平穩(wěn)、連續(xù)運(yùn)行。

1 運(yùn)動(dòng)學(xué)分析

機(jī)器人的運(yùn)動(dòng)學(xué)問(wèn)題，指在不關(guān)注力和力矩的前提下，從機(jī)器人的幾何結(jié)構(gòu)角度去研究機(jī)器人在不同空間(關(guān)節(jié)、笛卡爾)下的運(yùn)行狀況。對(duì)于機(jī)器人運(yùn)動(dòng)學(xué)的研究，主要從正向和逆向兩個(gè)方面進(jìn)行研究分析，前者是指在已知連桿參數(shù)和關(guān)節(jié)相關(guān)變量(角度或位移)前提下，求解機(jī)器人末端的位姿；后者是指在已知連桿參數(shù)和末端位姿前提下，求解機(jī)器人所有的關(guān)節(jié)變量值[8]。

1.1 連桿關(guān)系

(1)

1.2 PUMA560機(jī)器人

PUMA560是一款通用六自由度關(guān)節(jié)型機(jī)器人，末端執(zhí)行器位置是由前3個(gè)關(guān)節(jié)所決定，末端執(zhí)行器方位是由腕部的3個(gè)關(guān)節(jié)所決定，其連桿結(jié)構(gòu)圖如圖2所示。

圖1 連桿相鄰坐標(biāo)系變換 圖2 PUMA560連桿坐標(biāo)系

PUMA560機(jī)器人具體的D-H參數(shù)變量以及關(guān)節(jié)角度運(yùn)行變化范圍如表1所示。由表1可知，相關(guān)變量參數(shù)d2=149.09 mm,d4=433.07 mm,d6=56.25 mm,a2=431.8 mm,a3=20.32 mm。

表1 PUMA560連桿參數(shù)

(2)

(3)

(4)

(5)

2 4-5-4多項(xiàng)式分段插值

機(jī)械臂在運(yùn)行過(guò)程中一旦出現(xiàn)不平穩(wěn)的問(wèn)題，不僅會(huì)影響運(yùn)行任務(wù)的完成，還會(huì)影響其本體使用壽命。此項(xiàng)研究設(shè)計(jì)的目的是使得PUMA560機(jī)器人能夠在任務(wù)空間準(zhǔn)確、平穩(wěn)地完成搬運(yùn)任務(wù)。傳統(tǒng)的3-5-3多項(xiàng)式分段插值算法對(duì)機(jī)器人的啟停位置無(wú)法很好地控制，且不能對(duì)關(guān)節(jié)角度運(yùn)行變化的速度和加速度同時(shí)進(jìn)行配置，可能會(huì)引起機(jī)器人出現(xiàn)運(yùn)行不連續(xù)和不平穩(wěn)的問(wèn)題。因此提出一種4-5-4多項(xiàng)式分段插值算法，實(shí)現(xiàn)關(guān)節(jié)空間的各關(guān)節(jié)角度分段式軌跡規(guī)劃。具體設(shè)計(jì)思想：

首先，在任務(wù)空間中確定起始點(diǎn)和終止點(diǎn)，并選取兩個(gè)中間路徑點(diǎn)。然后，根據(jù)逆向運(yùn)動(dòng)學(xué)，將任務(wù)空間軌跡關(guān)鍵點(diǎn)進(jìn)行逆解，求出關(guān)節(jié)空間4組關(guān)鍵點(diǎn)的關(guān)節(jié)角度值。再利用分段思想，在關(guān)節(jié)空間中，分別對(duì)每個(gè)關(guān)節(jié)的角度變化進(jìn)行三段式的4-5-4多項(xiàng)式插值規(guī)劃，從而獲得各關(guān)節(jié)的角度插值序列。最后利用運(yùn)動(dòng)學(xué)正解將各關(guān)節(jié)角度插值序列轉(zhuǎn)化為末端位姿，從而獲得末端運(yùn)動(dòng)軌跡。對(duì)4個(gè)關(guān)鍵點(diǎn)利用逆向運(yùn)動(dòng)學(xué)反解，可以得到各關(guān)節(jié)變量值，具體的空間位置點(diǎn)及其對(duì)應(yīng)關(guān)節(jié)角度值如表2所示。

表2 空間點(diǎn)及相應(yīng)關(guān)節(jié)角度

4-5-4多項(xiàng)式分段插值算法，具體是指對(duì)機(jī)器人的各關(guān)節(jié)角度插值運(yùn)算，在起始點(diǎn)Q0與上升點(diǎn)Q1關(guān)節(jié)角度間，運(yùn)用4次多項(xiàng)式進(jìn)行插值；在上升點(diǎn)Q1與下降點(diǎn)Q2的關(guān)節(jié)角度間，運(yùn)用5次多項(xiàng)式進(jìn)行插值；在下降點(diǎn)Q2與終止點(diǎn)Q3關(guān)節(jié)角度間，運(yùn)用4次多項(xiàng)式進(jìn)行插值。

為了在關(guān)節(jié)空間實(shí)現(xiàn)機(jī)器人的4-5-4多項(xiàng)式分段插值，需要對(duì)各個(gè)關(guān)節(jié)分別進(jìn)行三段式的多項(xiàng)式插值分析，設(shè)這三段目標(biāo)軌跡的角度插值函數(shù)為[12]:

(6)

式中,qi1(t),qi2(t),qi3(t)為第i關(guān)節(jié)的第1,2,3段的關(guān)節(jié)角度插值函數(shù);aijk為第i關(guān)節(jié)的第j段插值函數(shù)的第k項(xiàng)系數(shù)，i可取1,2,3,4,5,6;j可取1,2,3;k可取0,1,2,3,4,5。

對(duì)式(6)關(guān)于時(shí)間求導(dǎo)，獲得相應(yīng)關(guān)節(jié)的角速度以及角加速度插值函數(shù)，如式(7)所示。

(7)

為了實(shí)現(xiàn)機(jī)器人運(yùn)行的連續(xù)性、平穩(wěn)性和高效性，需要對(duì)各關(guān)節(jié)運(yùn)行的三段軌跡進(jìn)行約束：①三段軌跡的起點(diǎn)和末端點(diǎn)的關(guān)節(jié)角度必須與給定關(guān)鍵點(diǎn)位置保持一致。②各段軌跡起點(diǎn)的角速度、角加速度與前一段軌跡末端點(diǎn)的角速度、角加速度保持一致。③關(guān)節(jié)運(yùn)行的第1段軌跡起點(diǎn)和第3段軌跡末端點(diǎn)的角速度和角加速度都為0。④確保整個(gè)過(guò)程規(guī)劃的相應(yīng)關(guān)節(jié)運(yùn)行角度、速度和加速度變化保持連續(xù)。

對(duì)于3個(gè)插值區(qū)間運(yùn)行時(shí)間的設(shè)定，當(dāng)設(shè)定值較小時(shí)，會(huì)使得關(guān)節(jié)運(yùn)行的角速度和角加速度變化過(guò)大；當(dāng)設(shè)定值較大時(shí)，會(huì)增大計(jì)算數(shù)值，延長(zhǎng)運(yùn)行時(shí)間。為了便于算法計(jì)算研究，分別將3個(gè)插值區(qū)間的運(yùn)行時(shí)間都分配為2s。根據(jù)上述約束條件以及式(6)、式(7)分析獲得各關(guān)節(jié)在3個(gè)階段角度插值函數(shù)求解系數(shù)的公式如式(8)、式(9)、式(10)所示。

在0～2 s，第一階段求系數(shù)參數(shù)的公式為

(8)

在2～4 s，第二階段求系數(shù)參數(shù)的公式為

(9)

在4～6 s，第三階段求系數(shù)參數(shù)的公式為

(10)

3 仿真驗(yàn)證

3.1 運(yùn)動(dòng)學(xué)驗(yàn)證

(11)

對(duì)于6軸機(jī)器人的正逆向運(yùn)動(dòng)學(xué)模型的驗(yàn)證，主要是借助Robotics Toolbox 9.10，正向求解是運(yùn)用fkine( )函數(shù)，逆向求解是運(yùn)用ikine( )函數(shù)[13],部分程序如下：

robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','puma560');%建立機(jī)器人模型

Q0=[pi/2 0 -pi/2 0 0 0];%起始點(diǎn)角度值

T=robot.fkine(Q0);%利用正向運(yùn)動(dòng)學(xué)函數(shù)獲得起始點(diǎn)位姿

Q1=robot.ikine(T);%利用逆向運(yùn)動(dòng)學(xué)函數(shù)獲得各關(guān)節(jié)角度

Robot.plot(Q0);%獲得機(jī)器人在Q0處位姿圖

運(yùn)行結(jié)果得到的末端位姿T矩陣與正向運(yùn)動(dòng)學(xué)推算的結(jié)果一致，而得到的Q1與Q0一致，證明了運(yùn)動(dòng)學(xué)模型的正確性。同樣，也可以利用teach( )函數(shù)觀察各關(guān)節(jié)角度與位姿的轉(zhuǎn)換關(guān)系，仿真結(jié)果如圖3所示，進(jìn)一步驗(yàn)證所建機(jī)器人運(yùn)動(dòng)學(xué)模型的準(zhǔn)確性。

3.2 任務(wù)空間仿真

PUMA560的各關(guān)節(jié)運(yùn)行變化范圍有限，使得機(jī)器人在運(yùn)行過(guò)程中有限定的任務(wù)空間，所以使用蒙特卡洛法[14]對(duì)PUMA560任務(wù)空間進(jìn)行分析，各關(guān)節(jié)角度通過(guò)式(12)進(jìn)行計(jì)算。

Qi=Qimin+(Qimax-Qimin)*rand(N,1),

(12)

式中，Qi表示第i關(guān)節(jié)的角度值；Qimin,Qimax表示第i關(guān)節(jié)角度值的最小和最大值，i可取1,2,3,4,5,6;rand(N,1)隨機(jī)產(chǎn)成N組(0,1)之間的隨機(jī)數(shù)，N取30 000。

利用式(12)依次對(duì)6個(gè)關(guān)節(jié)角度變量取值，然后利用fkine( )函數(shù)求解出關(guān)節(jié)角度對(duì)應(yīng)的任務(wù)空間點(diǎn)，MATLAB仿真得到的任務(wù)空間散點(diǎn)分布如圖4所示[15],仿真得到的散點(diǎn)都是機(jī)器人在任務(wù)空間中可以到達(dá)的點(diǎn)。

圖3 PUMA560在(pi/2 0 -pi/2 0 0 0)處位姿圖 圖4 PUMA560工作空間

3.3 軌跡規(guī)劃仿真驗(yàn)證

針對(duì)PUMA560，基于相同數(shù)據(jù)和約束條件，分別利用4-5-4多項(xiàng)式分段插值和傳統(tǒng)的3-5-3多項(xiàng)式分段插值算法進(jìn)行軌跡規(guī)劃，得到前3個(gè)關(guān)節(jié)角度、角速度和角加速度的插值結(jié)果如圖5、圖6、圖7所示。由圖5、圖6、圖7對(duì)比分析得出：

(1)兩種不同算法在第2段(2～4 s)的關(guān)節(jié)變量控制上采用的都是5次多項(xiàng)式。通過(guò)對(duì)比分析，這兩種算法在第2段控制所起的作用效果差不多。

(2)兩種不同算法在第1段和第3段(0～2 s、4～6 s)的關(guān)節(jié)變量控制上，4-5-4多項(xiàng)式分段插值比3-5-3多項(xiàng)式分段插值算法控制得更好。在第1段和第3段關(guān)節(jié)變量控制上，如果采用3次多項(xiàng)式，它將無(wú)法對(duì)關(guān)節(jié)的角速度和角加速度同時(shí)進(jìn)行配置，即第1段起始位置和第3段結(jié)束位置都無(wú)法準(zhǔn)確同時(shí)控制角速度和角加速度為0，這可能會(huì)引起機(jī)器人振動(dòng)，影響其正常運(yùn)行。而利用4次多項(xiàng)式控制可以很好地彌補(bǔ)角速度和角加速度的同時(shí)配置問(wèn)題，且4-5-4多項(xiàng)式分段插值算法控制的關(guān)節(jié)變量在相應(yīng)時(shí)間段呈現(xiàn)的函數(shù)特性要比后者階次更高，從圖5、圖6、圖7中不難看出，在控制變量變化上呈現(xiàn)出平穩(wěn)性和柔性更好的特點(diǎn)。

圖5 axis1的角度、角速度、角加速度 圖6 axis2的角度、角速度、角加速度

圖7 axis3的角度、角速度、角加速度

通過(guò)上面分析得出結(jié)論，在控制關(guān)節(jié)運(yùn)行的相應(yīng)變量方面，4-5-4多項(xiàng)式分段插值算法要比傳統(tǒng)的3-5-3多項(xiàng)式分段插值算法更有優(yōu)勢(shì)。

根據(jù)表2中的數(shù)據(jù)，利用4-5-4多項(xiàng)式分段插值算法對(duì)各關(guān)節(jié)變量分別進(jìn)行軌跡規(guī)劃，得到的各關(guān)節(jié)角度、角速度以及角加速度插值結(jié)果如圖8、圖9、圖10所示。得到的各關(guān)節(jié)變量變化都呈現(xiàn)出連續(xù)性和平穩(wěn)性，且滿(mǎn)足所要求的約束條件，仿真結(jié)果也進(jìn)一步驗(yàn)證了4-5-4多項(xiàng)式分段插值算法的可行性。

對(duì)于4-5-4多項(xiàng)式分段插值得到的關(guān)節(jié)角度變化序列，利用運(yùn)動(dòng)學(xué)正解分析、計(jì)算和仿真得到PUMA560末端運(yùn)行軌跡，機(jī)器人將在任務(wù)空間按規(guī)劃軌跡依次通過(guò)各個(gè)位置序列點(diǎn)，具體如圖11所示。

圖8 各關(guān)節(jié)角度變化 圖9 各關(guān)節(jié)角速度變化

圖10 各關(guān)節(jié)角加速度變化 圖11 PUMA560末端運(yùn)行圖

而對(duì)于機(jī)械臂在任務(wù)空間的穩(wěn)定運(yùn)行，最重要的就是控制其位置和速度的平穩(wěn)連續(xù)變化。圖12和圖13分別是PUMA560末端在X/Y/Z 3個(gè)方向上的位置和速度變化圖。通過(guò)分析可以清楚地觀察到末端在X/Y/Z 3個(gè)方向上的位移、速度曲線(xiàn)都具有平滑連續(xù)等特性，能夠滿(mǎn)足PUMA560運(yùn)行平穩(wěn)條件，同時(shí)也進(jìn)一步驗(yàn)證了4-5-4多項(xiàng)式分段插值算法的可行性、可靠性與準(zhǔn)確性。

圖12 在X/Y/Z方向上位置變化 圖13 在X/Y/Z方向上速度變化

4 結(jié)論

研究為了解決機(jī)器人運(yùn)行過(guò)程中所出現(xiàn)的不連續(xù)和不平穩(wěn)問(wèn)題，以PUMA560為研究對(duì)象，通過(guò)對(duì)PUMA560連桿結(jié)構(gòu)進(jìn)行分析得到了機(jī)器人運(yùn)動(dòng)學(xué)模型。并在所建模型的基礎(chǔ)上，提出了一種4-5-4多項(xiàng)式分段插值算法，詳細(xì)闡述了此算法原理。最后運(yùn)用MATLAB完成了仿真驗(yàn)證工作，得到結(jié)論如下：

(1)通過(guò)對(duì)機(jī)器人運(yùn)動(dòng)學(xué)模型和運(yùn)行空間的仿真，驗(yàn)證了所建運(yùn)動(dòng)學(xué)模型的正確性，并以散點(diǎn)圖形式顯示了機(jī)器人的正常運(yùn)行空間范圍。

(2)通過(guò)對(duì)4-5-4多項(xiàng)式分段插值和傳統(tǒng)的3-5-3多項(xiàng)式分段插值算法進(jìn)行仿真、對(duì)比、分析，驗(yàn)證了4-5-4多項(xiàng)式分段插值算法的可行性和準(zhǔn)確性，也證明了4-5-4多項(xiàng)式分段插值算法較傳統(tǒng)算法更具優(yōu)勢(shì)。

(3)最終采用4-5-4多項(xiàng)式分段插值算法對(duì)PUMA560的各關(guān)節(jié)角度進(jìn)行插值規(guī)劃，再利用正向運(yùn)動(dòng)學(xué)將關(guān)節(jié)角度插值序列轉(zhuǎn)化為機(jī)器人末端軌跡。通過(guò)仿真驗(yàn)證進(jìn)一步地詮釋了4-5-4多項(xiàng)式分段插值算法的連續(xù)性、平穩(wěn)性和可靠性，能夠有效保證機(jī)器人連續(xù)平穩(wěn)地完成任務(wù)。