面向三自由度平面關(guān)節(jié)機(jī)器人的“S”形軌跡規(guī)劃算法研究

陳登錢，孔維賓,2，王德智，孔祥曄，平 欣

(1.鹽城工學(xué)院，江蘇 鹽城 224051；2.鹽城工學(xué)院光電信息技術(shù)研究所，江蘇 鹽城 224051；3.光馳科技(上海)有限公司，上海 200444)

1 引言(Introduction)

隨著機(jī)器人技術(shù)的迅速發(fā)展，智能機(jī)器輔助等成為現(xiàn)代機(jī)器人技術(shù)的研究熱點(diǎn)[1-2]。機(jī)器人的使用場(chǎng)合不同，對(duì)末端執(zhí)行器位移、速度等的要求也不盡相同，因此需要對(duì)機(jī)器人的運(yùn)行軌跡進(jìn)行規(guī)劃以滿足其穩(wěn)定性要求。平面關(guān)節(jié)機(jī)器人廣泛應(yīng)用于汽車等重要行業(yè)。關(guān)節(jié)空間軌跡是針對(duì)各關(guān)節(jié)實(shí)時(shí)位移角度及連續(xù)的運(yùn)動(dòng)軌跡，機(jī)器人以各關(guān)節(jié)同時(shí)啟停為目標(biāo)，保證關(guān)節(jié)同時(shí)到達(dá)指定位置；而多軸運(yùn)動(dòng)插補(bǔ)的總時(shí)長(zhǎng)取決于運(yùn)動(dòng)距離最遠(yuǎn)軸，并以關(guān)節(jié)運(yùn)動(dòng)時(shí)間為基礎(chǔ)，其余多軸通過(guò)改變規(guī)劃的最大運(yùn)動(dòng)速度與加速度實(shí)現(xiàn)時(shí)間同步[3-4]。關(guān)節(jié)空間規(guī)劃路徑中無(wú)突變點(diǎn)存在，避免規(guī)劃中出現(xiàn)瞬時(shí)速度過(guò)大導(dǎo)致關(guān)節(jié)運(yùn)動(dòng)失控[5-6]。

現(xiàn)有軌跡規(guī)劃常用的方式包括五次多項(xiàng)式和“S”形軌跡規(guī)劃[7]等。王川等[8]對(duì)五次多項(xiàng)式函數(shù)插值方法中軌跡中間點(diǎn)角速度的設(shè)置方法提出改進(jìn)。錢文學(xué)等[9]提出一種多節(jié)點(diǎn)、多樣條曲線運(yùn)動(dòng)的軌跡規(guī)劃方法。由于關(guān)節(jié)轉(zhuǎn)角范圍未確定最優(yōu)解，同時(shí)加加速度軌跡均為不連續(xù)曲線，因此在加減速的啟動(dòng)停止瞬間存在突變現(xiàn)象將會(huì)導(dǎo)致系統(tǒng)振蕩，對(duì)機(jī)械會(huì)產(chǎn)生柔性沖擊[10-13]。本文采用雙變量反正切函數(shù)規(guī)劃?rùn)C(jī)器人的最優(yōu)關(guān)節(jié)轉(zhuǎn)角范圍，利用三角函數(shù)結(jié)合傳統(tǒng)“S”形軌跡規(guī)劃算法，約束運(yùn)動(dòng)過(guò)程中的最大速度、加速度、加加速度，改善機(jī)器人運(yùn)動(dòng)過(guò)程中由于加加速度非連續(xù)軌跡導(dǎo)致的關(guān)節(jié)驅(qū)動(dòng)力矩的突變，從而保證機(jī)器人運(yùn)動(dòng)過(guò)程的穩(wěn)定性。

2 運(yùn)動(dòng)學(xué)模型(Kinematic model)

如圖1所示，平面關(guān)節(jié)機(jī)器人具有3 個(gè)自由度，其中2 個(gè)旋轉(zhuǎn)關(guān)節(jié)，1 個(gè)移動(dòng)關(guān)節(jié)。為了描述相鄰兩個(gè)坐標(biāo)之間的位置和姿態(tài)，采用D-H模型方法建立坐標(biāo)系，如圖2所示。表1給出了機(jī)器人各關(guān)節(jié)D-H參數(shù)。

圖1 平面關(guān)節(jié)機(jī)器人結(jié)構(gòu)圖Fig.1 Structural diagram of planar joint robot

圖2 平面關(guān)節(jié)機(jī)器人D-H模型Fig.2 D-H model of planar joint robot

表1 機(jī)器人各關(guān)節(jié)D-H參數(shù)值Tab.1 D-H parameter values of robot joints

以每個(gè)關(guān)節(jié)分別作為坐標(biāo)原點(diǎn)建立局部坐標(biāo)系，旋轉(zhuǎn)矩陣表示關(guān)節(jié)i相對(duì)于關(guān)節(jié)i-1的位姿矩陣變換，其表達(dá)式如下：

其中，αi-1表示連桿扭角，由于連桿無(wú)自轉(zhuǎn)，所以三關(guān)節(jié)連桿扭角均為0。表示連桿長(zhǎng)度，θi表示關(guān)節(jié)轉(zhuǎn)角，di表示關(guān)節(jié)偏置量。

每個(gè)關(guān)節(jié)相對(duì)于初始關(guān)節(jié)坐標(biāo)系的旋轉(zhuǎn)變換矩陣可以通過(guò)若干個(gè)關(guān)節(jié)的旋轉(zhuǎn)變換矩陣乘積表示，即機(jī)器人末端關(guān)節(jié)相對(duì)于初始關(guān)節(jié)坐標(biāo)系的旋轉(zhuǎn)變換矩陣可以表示為，等式兩邊同乘，可得，從而得到：

為了有效避免運(yùn)動(dòng)學(xué)逆解的復(fù)雜性及減小逆解的誤差，引入雙變量反正切函數(shù)arctan 2(y,x)，反正切函數(shù)相比反正弦和反余弦函數(shù)主要有以下優(yōu)點(diǎn)：①確定唯一解，由于雙變量規(guī)定了x和y符號(hào)，因此能夠確定角度所在象限，所以使其在內(nèi)具有唯一解，避免出現(xiàn)多解情況。②降低逆解的誤差，保證逆解的精度，函數(shù)y=f(x)的計(jì)算誤差可以表示為 Δy=Δxf＇(x)。由于f1(x) = arcsinx和f2(x)= arccosx的導(dǎo)數(shù)值域范圍內(nèi)都存在趨向于無(wú)窮的值，致使Δy變大，而f3(x)= arctanx的導(dǎo)數(shù)值域保持在[0,1)的范圍內(nèi)，因此可以保證機(jī)器人的精度。

依據(jù)式(3)可得：

記S2=sinθ2，由式(3)、式(4)和式(5)可得平面關(guān)節(jié)機(jī)器人關(guān)節(jié)1、2的轉(zhuǎn)角θ i與關(guān)節(jié)3的偏移量d3：

當(dāng)確定機(jī)器人末端位姿矩陣時(shí)，對(duì)于θ2與θ1，關(guān)節(jié)1與關(guān)節(jié)2都有兩個(gè)解，分別對(duì)應(yīng)每個(gè)關(guān)節(jié)電機(jī)的正反旋轉(zhuǎn)，因此機(jī)器人進(jìn)行路徑規(guī)劃時(shí)，需要對(duì)多組解進(jìn)行最優(yōu)化選解，使機(jī)器人的運(yùn)動(dòng)處于最佳狀態(tài)。如圖3所示，由機(jī)器人運(yùn)動(dòng)準(zhǔn)則可得最優(yōu)選解過(guò)程。

圖3 機(jī)器人最優(yōu)選解流程圖Fig.3 Flow chart of robot optimal selection

使用Matlab Robotic工具箱對(duì)機(jī)器人末端進(jìn)行仿真，將機(jī)器人末端從初始坐標(biāo)點(diǎn)運(yùn)動(dòng)到目標(biāo)點(diǎn)。如表2所示，第二組解中，關(guān)節(jié)1轉(zhuǎn)角，超出關(guān)節(jié)1的轉(zhuǎn)角允許范圍。第二組解中，關(guān)節(jié)1的轉(zhuǎn)角大于第一組解關(guān)節(jié)1的轉(zhuǎn)角，依據(jù)機(jī)器人最優(yōu)選解準(zhǔn)則，選擇第一組解作為完成該運(yùn)動(dòng)的最優(yōu)逆解。依據(jù)最優(yōu)逆解，機(jī)器人末端運(yùn)動(dòng)軌跡及各關(guān)節(jié)運(yùn)動(dòng)曲線圖，如圖4所示，關(guān)節(jié)1的位移量與速度比關(guān)節(jié)2小，并且各關(guān)節(jié)的運(yùn)動(dòng)均勻平穩(wěn)，速度及位移沒(méi)有出現(xiàn)突變現(xiàn)象，符合機(jī)器人運(yùn)動(dòng)的柔順性要求。

表2 平面關(guān)節(jié)機(jī)器人末端運(yùn)動(dòng)逆解Tab.2 Inverse solution of end motion of planar joint robot

圖4 機(jī)器人各關(guān)節(jié)運(yùn)動(dòng)曲線圖Fig.4 Motion curve of each robot joint

3 關(guān)節(jié)空間軌跡規(guī)劃(Joint space trajectory planning)

3.1 改進(jìn)“S”形軌跡規(guī)劃

如圖5所示，將“S”形軌跡分為加加速段[0,t1]、勻加速段 [t1,t2]、減加速度 [t2,t3]、勻速段 [t3,t4]、加減速段 [t4,t5]、勻減速段 [t5,t6]、減減速段 [t6,t7]七段。軌跡規(guī)劃的輸入?yún)?shù)包括機(jī)器人系統(tǒng)機(jī)械性能約束量最大速度Vm、最大加速度Am、最大加加速度Jm及目標(biāo)軌跡約束的各關(guān)節(jié)運(yùn)動(dòng)位移Qi。假設(shè)三個(gè)關(guān)節(jié)中最大位移為Qm，并以該位移規(guī)劃整體軌跡運(yùn)行時(shí)間，將各關(guān)節(jié)速度、加速度、加加速度的始、末數(shù)值均為0設(shè)置為初始條件。

圖5 改進(jìn)“S”形規(guī)劃軌跡Fig.5 Improved S-shaped trajectory planning

(3)減加速段t∈[t2,t3]。該段軌跡初始加加速度J(t2)為0，加速度為Am，結(jié)束時(shí)加加速度J(t3)為0，加速度A(t3)減為0，速度達(dá)到Vm，根據(jù)以上條件可以得到該段時(shí)間內(nèi)的軌跡，即

3.2 關(guān)節(jié)軌跡規(guī)劃流程

根據(jù)目標(biāo)軌跡約束條件，勻加速、勻速、勻減速部分時(shí)間長(zhǎng)度，即T2、T4、T6為0，軌跡規(guī)劃步驟如圖6所示。

圖6 關(guān)節(jié)軌跡規(guī)劃流程圖Fig.6 Flow chart of joint trajectory planning

步驟1：獲取機(jī)器人運(yùn)行軌跡參數(shù)，包括各關(guān)節(jié)當(dāng)前位置P1、目標(biāo)位置P2、最大速度Vm、最大加速度Am、最大加加速度Jm，判斷位移距離最遠(yuǎn)關(guān)節(jié)的運(yùn)動(dòng)范圍

步驟2：根據(jù)設(shè)定的最大速度與加加速度計(jì)算是否能達(dá)到目標(biāo)最大加速度，如果，則調(diào)整最大加速度約束量，即

步驟3：判斷軌跡中是否存在勻速段，即計(jì)算T4>0是否成立，若成立則規(guī)劃軌跡為完整軌跡，跳轉(zhuǎn)到步驟7。

步驟4：若T4>0不成立，即軌跡無(wú)勻速段，最大速度不可達(dá)，調(diào)整最大速度約束量，其條件為T4=0且S(t7) =Qm，即

步驟5：判斷是否存在勻加勻減速段，即計(jì)算T2>0，T6>0是否成立，若成立，則跳轉(zhuǎn)到步驟7。

步驟6：若T2>0，T6>0不成立，即軌跡無(wú)勻加減速段，最大加速度不可達(dá)，調(diào)整最大速度與最大加速度的約束量，根據(jù)條件T2=0、T4=0、T6=0且S(t7) =Qm，可得

步驟7：根據(jù)上述所得最終最大速度與加速度，可以得到最大移動(dòng)關(guān)節(jié)的完整軌跡、各段軌跡時(shí)間長(zhǎng)度T(i)及總時(shí)長(zhǎng)。

4 實(shí)驗(yàn)分析(Experimental analysis)

為驗(yàn)證所提出連續(xù)有界的多關(guān)節(jié)改進(jìn)“S”形時(shí)間同步軌跡規(guī)劃算法，設(shè)置平面關(guān)節(jié)機(jī)器人運(yùn)動(dòng)路徑起始點(diǎn)P1及終止點(diǎn)P2，采集并分析機(jī)械臂第一關(guān)節(jié)期望軌跡與實(shí)際反饋的關(guān)節(jié)運(yùn)動(dòng)軌跡。圖7(a)的A、B兩點(diǎn)表示機(jī)器人換向運(yùn)動(dòng)階段，此時(shí)機(jī)器人關(guān)節(jié)經(jīng)歷減速、靜止、加速三個(gè)階段，由于慣性力和減速器背隙的存在，因此會(huì)出現(xiàn)較大的誤差波動(dòng)。如圖7(b)、圖7(c)所示，傳統(tǒng)“S”形軌跡算法出現(xiàn)了較大的誤差波動(dòng)，而改進(jìn)“S”形軌跡算法的誤差變化較為穩(wěn)定。由此可知，本文所提出的加加速度連續(xù)有界的多關(guān)節(jié)改進(jìn)“S”形時(shí)間同步軌跡規(guī)劃方法能夠正確規(guī)劃出不同約束條件下的目標(biāo)軌跡，該方法適用于平面關(guān)節(jié)機(jī)器人連續(xù)有界平滑軌跡規(guī)劃，利用改進(jìn)“S”形軌跡實(shí)現(xiàn)速度、加速度和加加速度有界的連續(xù)軌跡規(guī)劃，避免階躍與沖擊，并且保證規(guī)劃的最大速度、加速度、加加速度均不超過(guò)系統(tǒng)設(shè)定的最大限制量。

圖7 平面關(guān)節(jié)機(jī)器人第一關(guān)節(jié)軌跡跟蹤曲線Fig.7 Trajectory tracking curve of the first joint of planar joint robot

5 結(jié)論(Conclusion)

本文通過(guò)引入雙變量反正切函數(shù)結(jié)合運(yùn)動(dòng)學(xué)逆解的方法規(guī)劃?rùn)C(jī)器人最優(yōu)關(guān)節(jié)轉(zhuǎn)角范圍，利用三角函數(shù)改進(jìn)“S”形軌跡規(guī)劃算法，約束運(yùn)動(dòng)過(guò)程中的最大速度、加速度、加加速度的限制值，改善了機(jī)器人運(yùn)動(dòng)過(guò)程中由于加加速度非連續(xù)軌跡導(dǎo)致關(guān)節(jié)驅(qū)動(dòng)力矩的突變。相較于傳統(tǒng)算法，改進(jìn)的“S”形軌跡規(guī)劃算法有效提升了機(jī)器人的穩(wěn)定性，改善了機(jī)器人的運(yùn)動(dòng)軌跡突變情況。