六自由度機械臂運動學分析與軌跡優化

李珺茹，齊立群，韓文波

（長春理工大學 光電工程學院，長春 130022）

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

1 機械臂D-H模型建立

機械臂由一系列的關節和連桿按任意的順序連接而成，這些關節可能是平移的或旋轉的，連桿可能是任意長度的。所以，在對機械臂進行運動學分析時，必須先對機械臂進行建模。為了確定D-H參數模型，首先對機械臂的每個關節指定一個本地參考坐標系。

1.1 坐標系與參數的確立

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

圖1 連桿坐標系

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

1.2 機械臂D-H參數模型

用D-H法來對機械臂建模分析，在每個關節建立坐標系，通過齊次變換描述坐標系之間的位姿變化[2]。本文采用的六自由度機械臂D-H參數如表1所示。

表1 六自由度機械臂D-H參數

1.3 六自由度機械臂正運動學分析

機械臂正運動學是指已知各關節的參數，求末端執行器相對于基坐標系的位姿[3]。由式（1）可得機械臂末端坐標系相對于基坐標系的變換矩陣。正運動學公式表示為：

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

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

其中：

1.4 機械臂仿真模型搭建

對機械臂運動學的仿真研究，首先要建立機械臂的仿真模型。利用MATLAB Robotics Toolbox工具箱中的link（）函數和seriallink（）函數來搭建機械臂的仿真對象[4]。設定初始六個關節角分別為0，pi/2，-pi/2，-pi/2，0，pi/2；根據表1的D-H參數得如圖2所示仿真模型。

圖2 機械臂仿真模型

2 逆運動學求解

逆運動學求解是根據給定的機械臂末端姿態和位置，求得六關節軸轉動角度。根據模型圖，可以將末端位置和姿態反解到后三個關節軸的交點，從而滿足了Pieper準則[5]。將機械臂逆運動學的求解分為兩部分，第一部分根據三軸相交點位置求解前三個關節角；第二部分通過已經求出的前三個關節角和給定的機械臂末端姿態求出后三個關節角。

2.1 前三個關節角的求解

根據已知末端執行器位姿反解出后三軸交點處的位置信息，表示為：

已知：

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

對于i=1,2,3由式（1）得：

將0A11A2帶入（7）得：

式中，

將（9）平方得：

令：

得：

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

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

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

2.2 后三個關節角的求解

后三個關節角的求解選擇歐拉角變換的方法[6]。末端姿態矩陣與其他旋轉矩陣有如下關系：

在求出θ1,θ2,θ3后，當θ4=θ5=θ6=0 時，可以由連桿坐標系6相對于基坐標系方位計算出0R6θ4=θ5=θ6=0 ，即

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

由這個矩陣可求得后三個關節角θ4=θ5=θ6。若可得到：

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

或

2.3 仿真驗證

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

表2 逆運動學驗證

3 笛卡爾空間軌跡規劃

機械臂末端執行器在直角坐標的位姿對時間函數進行的規劃稱作笛卡爾空間軌跡規劃[7]。規劃流程如圖3所示，其中常用的插補算法有直線插補和圓弧插補。

圖3 笛卡爾空間軌跡規劃

3.1 空間直線插補軌跡規劃

直線插補是機械臂的姿態變化按照給定的步長從初始姿態均勻向目標點姿態變化的過程。已知起點A(x0,y0,z0)，目標點B(x1,y1,z1)，插補次數N，則

對于該直線上的任意點有

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

圖4 直線插補

3.2 空間圓弧插補軌跡規劃

已知不共線的三點A,B,C。它們確定了一個空間圓弧，用平面圓弧替換掉空間圓弧，從而將三維空間轉化到二維平面空間，最后將規劃好的坐標點轉回到三維空間中去，即完成機械臂空間圓弧插補。

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

圖5 二維平面坐標圖

通過三元一次方程組，可得到圓心坐標O=(a,b,c)，圓弧半徑，設點A到C對應的圓心角大小為φ，由圖5得：

設插補次數為N，計算出每個插補點的坐標：

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

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

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

最后將局部坐標變換到全局坐標中：

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

圖6 空間圓弧插補

3.3 軌跡優化

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

圖7 預期軌跡和算法擬合軌跡比較圖

現給出如圖7所示預期軌跡，應用優化算法按固定位置間隔分段選取插值點得到對應軌跡偏差。

如表3所示，通過比對各個插值點組的誤差值，可以得到在插值中間點為（10.72，14.21，6.11）時誤差最小，選擇該點完成軌跡擬合，即可實現機械臂軌跡曲線的優化。

表3 插值點擬合軌跡誤差

3.4 優化算法應用分析

六自由度機械臂軌跡優化算法為機械臂運動系統在空間的路徑優化提供編程依據，在應用中，可根據本文提出的優化方案，將所有路徑點按固定位置間隔分段，固定起點和終點，對選取的不同中間路徑點分別進行曲線擬合，計算出軌跡偏差，通過比較各個擬合曲線誤差值得到誤差最小的插值點組合，完成該分段的軌跡優化。本文將該算法在機械臂運動控制軟件編程環境LabVIEW下實現，實現流程圖見圖8.

圖8 軌跡優化算法流程圖

如圖9所示，在LabVIEW中輸入預期路徑點和分段插值點數后，可得最小誤差值和對應的插值點組合，實現軌跡曲線的優化。

圖9 笛卡爾空間軌跡優化

4 結論

針對一類具有典型結構的六自由度機械臂，在已知機械臂結構參數的基礎上，選用D-H法建立機械臂的運動模型，采用PIEPER準則對逆運動學問題進行求解；通過對空間直線插補算法和空間圓弧插補算法的分析，使機械臂能夠按照預先設定連續路徑進行作業；針對空間曲線中軌跡偏移量較大的問題提出了一種優化方法，在LabVIEW中的Math-Script節點中導入該算法，直觀的得到最優插值點組合，實現了軌跡優化，對機械臂在工業領域中的實際應用具有一定的指導意義。