六自由度機(jī)械臂運(yùn)動(dòng)學(xué)分析與軌跡優(yōu)化

李珺茹，齊立群，韓文波

（長(zhǎng)春理工大學(xué) 光電工程學(xué)院，長(zhǎng)春 130022）

隨著時(shí)代的進(jìn)步，機(jī)器人技術(shù)的應(yīng)用越來(lái)越普及，已逐漸滲透到軍事、航天、醫(yī)療、日常生活等各個(gè)領(lǐng)域。機(jī)械臂作為機(jī)器人的重要執(zhí)行機(jī)構(gòu)，它的使用不但減少了勞動(dòng)力降低了勞動(dòng)成本，而且提高了產(chǎn)品的生產(chǎn)質(zhì)量和效率，是未來(lái)發(fā)展的必然趨勢(shì)。因此，對(duì)機(jī)械臂關(guān)鍵技術(shù)的研究具有重要的現(xiàn)實(shí)意義。運(yùn)動(dòng)學(xué)分析和軌跡規(guī)劃是機(jī)械臂控制的兩個(gè)重要因素。其中，機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題是其軌跡規(guī)劃與控制的基礎(chǔ)，求解是否快速準(zhǔn)確將直接影響到后續(xù)軌跡規(guī)劃與控制的精度，而有效的軌跡規(guī)劃能夠確保運(yùn)動(dòng)軌跡在滿足約束條件的前提下平滑準(zhǔn)確[1]。基于上述內(nèi)容，從實(shí)時(shí)性出發(fā)，采用PIEPER逆運(yùn)動(dòng)學(xué)解法，縮短了程序的運(yùn)行時(shí)間。應(yīng)用插補(bǔ)算法完成運(yùn)動(dòng)軌跡規(guī)劃，并在笛卡爾空間中直觀的得到末端執(zhí)行器的路徑曲線，通過(guò)優(yōu)化算法減小軌跡偏移誤差，結(jié)合MATLAB驗(yàn)證算法的合理性，為機(jī)械臂控制系統(tǒng)的實(shí)際應(yīng)用提供了理論依據(jù)。

1 機(jī)械臂D-H模型建立

機(jī)械臂由一系列的關(guān)節(jié)和連桿按任意的順序連接而成，這些關(guān)節(jié)可能是平移的或旋轉(zhuǎn)的，連桿可能是任意長(zhǎng)度的。所以，在對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)分析時(shí)，必須先對(duì)機(jī)械臂進(jìn)行建模。為了確定D-H參數(shù)模型，首先對(duì)機(jī)械臂的每個(gè)關(guān)節(jié)指定一個(gè)本地參考坐標(biāo)系。

1.1 坐標(biāo)系與參數(shù)的確立

如圖1所示，zi軸與Ai+1關(guān)節(jié)軸重合，xi軸與公法線li重合，方向沿li由Ai+1軸線指向Ai+2軸線，yi軸按右手定則判定方向建立關(guān)節(jié)坐標(biāo)系。

圖1 連桿坐標(biāo)系

定義連桿參數(shù)如下：θi為xi-1軸到xi繞軸zi旋轉(zhuǎn)角度；di為xi-1軸到xi沿軸zi軸平移距離；li為zi-1到zi沿xi平移距離；αi為zi-1到zi繞xi旋轉(zhuǎn)角度。相鄰兩坐標(biāo)系之間的變化矩陣為

1.2 機(jī)械臂D-H參數(shù)模型

用D-H法來(lái)對(duì)機(jī)械臂建模分析，在每個(gè)關(guān)節(jié)建立坐標(biāo)系，通過(guò)齊次變換描述坐標(biāo)系之間的位姿變化[2]。本文采用的六自由度機(jī)械臂D-H參數(shù)如表1所示。

表1 六自由度機(jī)械臂D-H參數(shù)

1.3 六自由度機(jī)械臂正運(yùn)動(dòng)學(xué)分析

機(jī)械臂正運(yùn)動(dòng)學(xué)是指已知各關(guān)節(jié)的參數(shù)，求末端執(zhí)行器相對(duì)于基坐標(biāo)系的位姿[3]。由式（1）可得機(jī)械臂末端坐標(biāo)系相對(duì)于基坐標(biāo)系的變換矩陣。正運(yùn)動(dòng)學(xué)公式表示為：

式中，si代表 sinθi；ci代表 cosθi

變換矩陣T中，法線向量，指向向量，接近向量。如果直接給出有九個(gè)變量，比較麻煩，所以可通過(guò)RPY（滾動(dòng)角、俯仰角和偏航角）給出。其中繞軸旋轉(zhuǎn)φa為滾動(dòng)角，繞軸旋轉(zhuǎn)φo為俯仰角，繞軸旋轉(zhuǎn)φn為偏航角。通過(guò)繞，，軸的三個(gè)旋轉(zhuǎn)順序即可把機(jī)械臂末端調(diào)整到相應(yīng)的姿態(tài)。

其中：

1.4 機(jī)械臂仿真模型搭建

對(duì)機(jī)械臂運(yùn)動(dòng)學(xué)的仿真研究，首先要建立機(jī)械臂的仿真模型。利用MATLAB Robotics Toolbox工具箱中的link（）函數(shù)和seriallink（）函數(shù)來(lái)搭建機(jī)械臂的仿真對(duì)象[4]。設(shè)定初始六個(gè)關(guān)節(jié)角分別為0，pi/2，-pi/2，-pi/2，0，pi/2；根據(jù)表1的D-H參數(shù)得如圖2所示仿真模型。

圖2 機(jī)械臂仿真模型

2 逆運(yùn)動(dòng)學(xué)求解

逆運(yùn)動(dòng)學(xué)求解是根據(jù)給定的機(jī)械臂末端姿態(tài)和位置，求得六關(guān)節(jié)軸轉(zhuǎn)動(dòng)角度。根據(jù)模型圖，可以將末端位置和姿態(tài)反解到后三個(gè)關(guān)節(jié)軸的交點(diǎn)，從而滿足了Pieper準(zhǔn)則[5]。將機(jī)械臂逆運(yùn)動(dòng)學(xué)的求解分為兩部分，第一部分根據(jù)三軸相交點(diǎn)位置求解前三個(gè)關(guān)節(jié)角；第二部分通過(guò)已經(jīng)求出的前三個(gè)關(guān)節(jié)角和給定的機(jī)械臂末端姿態(tài)求出后三個(gè)關(guān)節(jié)角。

2.1 前三個(gè)關(guān)節(jié)角的求解

根據(jù)已知末端執(zhí)行器位姿反解出后三軸交點(diǎn)處的位置信息，表示為：

已知：

對(duì)于i=4，由式（1）的第四列有：

對(duì)于i=1,2,3由式（1）得：

將0A11A2帶入（7）得：

式中，

將（9）平方得：

令：

得：

將D-H參數(shù)帶入（12）能夠解出θ3：

將θ3值帶入（13），解出θ2：

將θ2,θ3帶入（14），解出θ1：

2.2 后三個(gè)關(guān)節(jié)角的求解

后三個(gè)關(guān)節(jié)角的求解選擇歐拉角變換的方法[6]。末端姿態(tài)矩陣與其他旋轉(zhuǎn)矩陣有如下關(guān)系：

在求出θ1,θ2,θ3后，當(dāng)θ4=θ5=θ6=0 時(shí)，可以由連桿坐標(biāo)系6相對(duì)于基坐標(biāo)系方位計(jì)算出0R6θ4=θ5=θ6=0 ，即

4R6ZYZ為Z-Y-Z歐拉角變換矩陣，其表達(dá)式為：

由這個(gè)矩陣可求得后三個(gè)關(guān)節(jié)角θ4=θ5=θ6。若可得到：

若θ5=0或θ5=180°，上式解將會(huì)退化，在這種情況下，僅能求出θ4,θ6的和或差。在這種情況下，一般取θ4=0，結(jié)果如下：

或

2.3 仿真驗(yàn)證

給定一個(gè)初始位姿(x,y,z,R,P,Y)，應(yīng)用上一節(jié)的理論可以得到各個(gè)位姿所對(duì)應(yīng)的六個(gè)關(guān)節(jié)角(θ1,θ2,θ3,θ4,θ5,θ6)；將這些關(guān)節(jié)角與輸入到機(jī)械臂正運(yùn)動(dòng)學(xué)方程中與給定位姿比較，即可驗(yàn)證逆運(yùn)動(dòng)解的正確性。給定位姿（-1.165，-0.362，0.518，45，-60，0）。

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

3 笛卡爾空間軌跡規(guī)劃

機(jī)械臂末端執(zhí)行器在直角坐標(biāo)的位姿對(duì)時(shí)間函數(shù)進(jìn)行的規(guī)劃稱作笛卡爾空間軌跡規(guī)劃[7]。規(guī)劃流程如圖3所示，其中常用的插補(bǔ)算法有直線插補(bǔ)和圓弧插補(bǔ)。

圖3 笛卡爾空間軌跡規(guī)劃

3.1 空間直線插補(bǔ)軌跡規(guī)劃

直線插補(bǔ)是機(jī)械臂的姿態(tài)變化按照給定的步長(zhǎng)從初始姿態(tài)均勻向目標(biāo)點(diǎn)姿態(tài)變化的過(guò)程。已知起點(diǎn)A(x0,y0,z0)，目標(biāo)點(diǎn)B(x1,y1,z1)，插補(bǔ)次數(shù)N，則

對(duì)于該直線上的任意點(diǎn)有

已知空間中兩點(diǎn)A(0,2,1)、B(3,12,9)，通過(guò)直線插補(bǔ)算法得到如圖4所示軌跡曲線，證實(shí)了算法的可行性。

圖4 直線插補(bǔ)

3.2 空間圓弧插補(bǔ)軌跡規(guī)劃

已知不共線的三點(diǎn)A,B,C。它們確定了一個(gè)空間圓弧，用平面圓弧替換掉空間圓弧，從而將三維空間轉(zhuǎn)化到二維平面空間，最后將規(guī)劃好的坐標(biāo)點(diǎn)轉(zhuǎn)回到三維空間中去，即完成機(jī)械臂空間圓弧插補(bǔ)。

具體步驟如下：在基礎(chǔ)坐標(biāo)系中A=(x0,y0,z0)、B=(x1,y1,z1)、C=(x2,y2,z2)三點(diǎn)確定的空間圓弧，按右手定則建立局部坐標(biāo)系O1X1Y1Z1。設(shè)圓心相對(duì)與基礎(chǔ)坐標(biāo)系的坐標(biāo)為O=(a,b,c)，借助輔助點(diǎn)Q,P，如圖5，可以得到

圖5 二維平面坐標(biāo)圖

通過(guò)三元一次方程組，可得到圓心坐標(biāo)O=(a,b,c)，圓弧半徑，設(shè)點(diǎn)A到C對(duì)應(yīng)的圓心角大小為φ，由圖5得：

設(shè)插補(bǔ)次數(shù)為N，計(jì)算出每個(gè)插補(bǔ)點(diǎn)的坐標(biāo)：

其中Δθ=φ/(N+1)。

在局部坐標(biāo)系中求出了各個(gè)插值點(diǎn)坐標(biāo)后，需要將這些點(diǎn)轉(zhuǎn)換到全局坐標(biāo)系中。令P表示由局部坐標(biāo)系向全局坐標(biāo)系的變換矩陣。P由O=(a,b,c)和X1，Y1，Z1矢量在全局坐標(biāo)系中的方向余弦構(gòu)成：

將X1，Y1，Z1化為單位向量，，即P可以表示為：

最后將局部坐標(biāo)變換到全局坐標(biāo)中：

已知不共線的三點(diǎn)A(3,12,9)、B(15,22,23)、C(25,26,25)分別為空間中不共線的三點(diǎn)，通過(guò)圓弧插補(bǔ)算法得到如圖6所示軌跡，證明了機(jī)械臂圓弧插補(bǔ)算法是可行的。

圖6 空間圓弧插補(bǔ)

3.3 軌跡優(yōu)化

如圖7所示，在機(jī)械臂軌跡規(guī)劃中，選用直線插補(bǔ)往往不能滿足路徑要求，通過(guò)對(duì)上一小節(jié)的分析，已知選取路徑上任意不共線的三點(diǎn)即可完成空間圓弧軌跡擬合，選取的插值點(diǎn)不同，得到的軌跡擬合曲線也不同。插值點(diǎn)選取的優(yōu)劣將直接影響到擬合軌跡的精度。為了得到誤差值較小的擬合軌跡曲線，本文在位置控制方式中基于連續(xù)軌跡控制提出一種優(yōu)化算法，通過(guò)選取不同插值點(diǎn)組合生成的軌跡曲線和預(yù)期軌跡曲線在空間中面積最小值的方法，得到最優(yōu)插值點(diǎn)組合，完成機(jī)械臂軌跡曲線的優(yōu)化。

圖7 預(yù)期軌跡和算法擬合軌跡比較圖

現(xiàn)給出如圖7所示預(yù)期軌跡，應(yīng)用優(yōu)化算法按固定位置間隔分段選取插值點(diǎn)得到對(duì)應(yīng)軌跡偏差。

如表3所示，通過(guò)比對(duì)各個(gè)插值點(diǎn)組的誤差值，可以得到在插值中間點(diǎn)為（10.72，14.21，6.11）時(shí)誤差最小，選擇該點(diǎn)完成軌跡擬合，即可實(shí)現(xiàn)機(jī)械臂軌跡曲線的優(yōu)化。

表3 插值點(diǎn)擬合軌跡誤差

3.4 優(yōu)化算法應(yīng)用分析

六自由度機(jī)械臂軌跡優(yōu)化算法為機(jī)械臂運(yùn)動(dòng)系統(tǒng)在空間的路徑優(yōu)化提供編程依據(jù)，在應(yīng)用中，可根據(jù)本文提出的優(yōu)化方案，將所有路徑點(diǎn)按固定位置間隔分段，固定起點(diǎn)和終點(diǎn)，對(duì)選取的不同中間路徑點(diǎn)分別進(jìn)行曲線擬合，計(jì)算出軌跡偏差，通過(guò)比較各個(gè)擬合曲線誤差值得到誤差最小的插值點(diǎn)組合，完成該分段的軌跡優(yōu)化。本文將該算法在機(jī)械臂運(yùn)動(dòng)控制軟件編程環(huán)境LabVIEW下實(shí)現(xiàn)，實(shí)現(xiàn)流程圖見(jiàn)圖8.

圖8 軌跡優(yōu)化算法流程圖

如圖9所示，在LabVIEW中輸入預(yù)期路徑點(diǎn)和分段插值點(diǎn)數(shù)后，可得最小誤差值和對(duì)應(yīng)的插值點(diǎn)組合，實(shí)現(xiàn)軌跡曲線的優(yōu)化。

圖9 笛卡爾空間軌跡優(yōu)化

4 結(jié)論

針對(duì)一類具有典型結(jié)構(gòu)的六自由度機(jī)械臂，在已知機(jī)械臂結(jié)構(gòu)參數(shù)的基礎(chǔ)上，選用D-H法建立機(jī)械臂的運(yùn)動(dòng)模型，采用PIEPER準(zhǔn)則對(duì)逆運(yùn)動(dòng)學(xué)問(wèn)題進(jìn)行求解；通過(guò)對(duì)空間直線插補(bǔ)算法和空間圓弧插補(bǔ)算法的分析，使機(jī)械臂能夠按照預(yù)先設(shè)定連續(xù)路徑進(jìn)行作業(yè)；針對(duì)空間曲線中軌跡偏移量較大的問(wèn)題提出了一種優(yōu)化方法，在LabVIEW中的Math-Script節(jié)點(diǎn)中導(dǎo)入該算法，直觀的得到最優(yōu)插值點(diǎn)組合，實(shí)現(xiàn)了軌跡優(yōu)化，對(duì)機(jī)械臂在工業(yè)領(lǐng)域中的實(shí)際應(yīng)用具有一定的指導(dǎo)意義。