機械臂逆運動學避障最優求解算法

楊青，周建興*，葛亮

(1.西南石油大學電氣信息學院，成都 610500；2.西南石油大學機電工程學院，成都 610500)

近年來，機械臂技術快速發展，被廣泛用于制造、醫療、航天等各個領域。目前對機械臂的研究主要包括視覺標定、軌跡規劃、運動控制等方面。實現機械臂的逆運動學求解是研究這些問題的前提，所以對機械臂逆運動學求解方法研究必不可少。因為機械臂的逆運動學求解存在非線性、多解性等特點，所以很難找到一種適用于不同結構、不同工作環境的通用逆運動學求解方法。

逆運動學求解即是根據機械臂末端位姿矩陣求取關節角矩陣，目前，求解逆運動學的方法普遍采用數值解法[1]和解析解法[2]等。周東旭等[3]、陳禹含等[4]、T?rdal等[5]采用解析法對機械臂進行求解,求解速度快，根據機械臂模型和幾何結構便可推導出相應的逆運動學求解方程。但是解析法易受到結構限制，只有滿足Pieper準則的機械臂才可以獲得解析解[6]，不具有通用性。張栩曼等[7]、K?ker[8]、Yuan等[9]采用數值解法對逆運動學進行求解，因為數值解法具有一定的通用性，數值解法主要通過迭代完成，即求得機械臂的雅克比矩陣，但部分機械臂可能存在奇異問題，所以無法求逆運動學解，且求解結果的精度容易受到隨機初值的影響[10]。近年來，智能優化算法得到了廣泛應用，主要包括遺傳算法[11]，粒子群算法[12]和差分進化算法[13]等，這些算法也逐漸被用于求機械臂逆運動學解。林陽等[14]和馮晨旭等[15]利用智能優化算法將逆運動學問題轉化為最小化位姿誤差的最優求解問題，對機械臂構型無特殊要求，可以彌補數值解法和解析解法的缺陷。

目前，前人研究方法多針對不存在障礙物的環境中機械臂逆運動學求解，而關于存在障礙物作業環境下的逆運動學求解方法研究較少。而實際工況下機械臂通常工作在障礙物環境中，如搬運、焊接、人機協作等。徐培等[16]通過解析法對逆運動學求解，再對其優選，求得最少轉角的機械臂逆解優選算法。同理，可通過數值解或解析解先求得逆運動學解，再對其進行優選得到避障逆運動學解。但是該方法存在機械臂處于奇異情況下無法求解的情況，且通用性差。為此，針對在障礙物作業環境下的逆運動學求解問題，結合避障檢測算法，將機械臂避障逆運動學求解問題轉化為多元多目標優化問題，提出一種機械臂避障逆運動學最優求解算法。

機械臂避障逆運動學最優求解算法應該滿足以下條件：機械臂全局避障而非末端一點，求解精度高，各個關節運動量小?，F將約束條件設置為各關節最大值與最小值限位，算法主要包括：①避障檢測算法：通過簡化模型，將機械臂與障礙物實體間的碰撞檢測轉化為包絡面之間的碰撞檢測問題；②逆運動學求解算法：逆運動學解是正運動學的反解，將求解結果的正向運動學結果與目標位置矩陣與姿態矩陣比較，計算出其姿態誤差，根據避障檢測結果、姿態誤差、最短行程原則，通過加權系數法建立目標函數，將目標求解問題轉化為多目標優化問題；③權重自適應算法：通過差分進化算法對目標函數進行優化，為了提高求解速度和求解精度，設計一種權重系數自適應調整方法，在優化過程中自適應調整權重系數。

1 機械臂模型的建立

選擇較為典型的六自由度串聯機械臂UR5進行研究分析。UR5機械臂的結構參數如圖1所示，連桿坐標系如圖2所示。該機械臂具有6個自由度，6個關節均為轉動關節，其中第2～4關節相互平行。

圖1 機械臂結構Fig.1 Structure of mechanical arm

由于機械臂的逆運動學求解是正向運動學求解的逆過程，因此要分析機械臂逆運動學解，首先要建立機械臂正向運動學模型，為分析機械臂正向運動學模型中從基坐標系到末端執行器的位姿變化關系，采用D-H(Denavit-Hartenberg)法對機械臂建立正運動學模型，D-H參數如表1所示。

表1 D-H參數Table 1 D-H parameters

Xi、Yi、Zi分別為機械臂各個關節坐標的坐標軸；ai、di分別為第i連桿的連桿長度和連桿偏矩;i=1,2,…,6圖2 機械臂坐標Fig.2 Coordinate of manipulator

(1)

式(1)中：RX、RZ分別為X、Z方向旋轉算子；DX、DZ分別為X、Z方向平移算子；αi-1為連桿轉角。

(2)

式(2)中：c和s分別為函數cos和sin函數；ai-1為連桿長度。

(3)

關節坐標系上的點在基坐標系中的位置可表示為

(4)

式(4)中：0P為關節坐標系上的點在基坐標系中的位置；nP為關節坐標系上的點在第n關節坐標系中的位置。

2 避障檢測算法

避障檢測算法即碰撞檢測算法，其目的是判斷機械臂連桿與障礙物之間是否發生碰撞，碰撞檢測的對象是空間中機械臂連桿和障礙物。由于機械臂的結構復雜和障礙物的形狀的不確定性，很難用準確的數學模型描述，因此，可以通過幾何模型對機械臂連桿和障礙物進行描述，最常用的方法是包圍盒技術[17]。包圍盒技術的基本思想是用體積稍大且特性簡單的幾何體(包圍盒)來近似代替復雜的幾何對象。

研究對象是六自由度串聯型機械臂，由于機械臂第一個連桿在運動過程中機械臂第一個連桿只會轉動，位置不會變化，所以不用考慮碰撞檢測。對第2～6連桿機械臂和障礙物進行碰撞檢測，采用圓柱體和球面對機械臂連桿進行包絡，將第2、3關節用圓柱體進行包絡，因后3個連桿長度較短，為簡化計算量，將后3個關節用球面進行包絡，模型簡化圖如圖3所示。采用球面對障礙物進行包絡。當機械臂包絡面與障礙物包絡面之間不發生碰撞，機械臂和障礙物實體之間就一定不會發生碰撞。

在簡化模型的基礎上，將機械臂與障礙物兩個于是將物體間的碰撞轉化為連桿直線段與球心點的距離判斷[18]、球心與球心之間的距離判斷。具體碰撞檢測方法為：第2、3關節與障礙物的碰撞檢測分為兩個類型，示意圖如圖4所示。

類型一當連桿直線與障礙物不在一條直線上時，首先計算障礙物距離機械臂每一桿向量的垂直距離，關節角確定后，可通過正向運動學計算出每一連桿末端和起始端在空間坐標系的位置坐標，計算出第i桿的末端坐標減和第i桿起始點坐標即可得到第i桿在空間中的直線向量，計算障礙物球面包絡面中心點與第i桿的直線向量的距離即可得到si，的具體計算步驟如下。

步驟1計算圖3中對應的A、B、C、D點的空間坐標，結合D-H參數，通過齊次坐標變換的方法，可以建立各連桿處的齊次變換矩陣為

(5)

A～E分別為機械臂各個關節末端在空間中的坐標點圖3 模型簡化圖Fig.3 Model simplification diagram

Li為關節中心軸線;Si為球形包圍盒到圓柱體包圍盒的距離;Ci為兩球形包圍盒之間的距離；R1為障礙物球形包圍盒半徑；R2為連桿球形包圍盒半徑；R3為障礙物球形包圍盒半徑圖4 障礙物與機械臂位置關系示意圖Fig.4 Schematic diagram of position relationship between obstacles and manipulator

式(5)中：T1～T6為第1～6連桿末端的齊次變換矩陣。

則A、B、C、D、E點的空間坐標為

(6)

式(6)中：PA為點A在關節一坐標系的位置坐標，PC為點C在關節二坐標系的位置坐標。

則直線向量L1和L2可分別表示為

L1=B-A=[xB-xA,yB-yA,zB-zA]

(7)

L2=D-C=[xD-xC,yD-yC,zD-zC]

(8)

通過空間中點到直線向量的距離公式計算si，si為球心到圓柱體軸線的距離，通過比較si與R1+R的大小，其中R為圓柱的底面半徑，若si均大于R1+R，障礙物和機械臂不會發生碰撞，否則會發生碰撞。

類型二障礙物包絡面在連桿線段的延長線上時，若si>R3+R，則障礙物與機械臂不會發生碰撞，當siR2+R3時，則障礙物與機械臂不會發生碰撞，反之則會發生碰撞，ci為兩個球形包圍盒之間的距離。

步驟2后3個關節與障礙物的碰撞檢測：后3個關節用球面包絡面包圍。由圖1可以看出，選取圓心為E點，球面半徑選取d4即可將后3個關節完全包絡。已經計算出E點坐標，d4由表1得到，將后3個關節與障礙物的碰撞檢測方法為兩個球形包絡面之間的距離。si即為球面中心之間的距離，當si大于兩球面的半徑之和時,則不會發生碰撞，反之則會發生碰撞。

3 逆運動學求解算法

逆運動學是正運動學的反解，即當給定其末端位姿數據后求其取關節角矩陣的過程。逆運動學存在多解問題,對于六自由度UR5機械臂而言,最多可能求得8組逆解,而這8組逆解都能達到該位姿點,其中可能存在位姿使得機械臂與障礙物發生碰撞。為了得到全局避障、行程最短的最優逆運動學解，需進一步根據結構及任務需求等限制求得最優解。

目標函數包含以下三部分。

第一部分為了保證算法求解的逆運動學解的精度，即所求得的逆運動學解通過正向運動學計算得到的目標位姿與期望位姿誤差最小，目標函數首先考慮目標位姿與期望位姿之間的誤差。

將機械臂在笛卡爾空間坐標系下的初始位姿矩陣Tt簡化為

(9)

式(9)中：Rt、Pt分別為目標姿態矩陣和目標位置矩陣。

所求得的位姿矩陣Ta簡化為

(10)

位置誤差Ep可用所求得的機械臂末端執行器的位置和目標位置之間的誤差平方和表示，表達式為

Ep=(Pt-Pa)2

(11)

式(11)中：Pa為位置矩陣。

姿態誤差ER可用所求得的機械臂末端執行器的姿態矩陣各元素和目標姿態矩陣各元素之間的誤差平方和表示，表達式為

ER=(Rt-Ra)2

(12)

式(12)中：Ra為姿態矩陣。

由于位置誤差和姿態誤差數量級存在差異，位姿誤差可表示為

fE=λ1EP+λ2Ea

=λ1(Pt-Pa)2+λ2(Rt-Ra)2

(13)

式(13)中：λ1、λ2分別為位置誤差、姿態誤差函數的權重系數。

第二部分為了保證機械臂全局避障，可通過碰撞檢測方法實現，標記碰撞檢測結果為fCO，若機械臂與障礙物發生碰撞，則fCO取值為1,反之取值為0，fCO可表示為

(14)

第三部分在不考慮關節轉角范圍約束的條件下，機械臂通過逆運動學求解可以求出8組逆解，這8組不同的解到達終點所需的各關節轉角不同[19]。為了保證行程最短、減少運行時間和能耗，于是引入最短行程算法[20]，即在保證避障和求解精度的前提下，實現求解目標關節角度與當前位置的關節角度之間的變化量盡可能小，又因為機械臂越靠近基坐標的連桿承受重量越重，消耗能量大，所以求最短行程時需要對每個關節角度進行加權，用fq表示加權運動量，即第三部分目標函數為

(15)

式(15)中：Δθi為每個關節角度值的變化量；ηi為每個關節角變化量的加權系數。

最后，將三部分加權求和得到最終的目標函數f可表示為

f=min|λEfE+λCOfCO+λqfq|

(16)

式(16)中：λCO、λE、λq分別為目標函數的各項權重系數。

要求首先保證機械臂全局避障，所以應該保證：λCO?λE，其次要保證求解高精度，最后考慮其行程最短，故應該滿足：λE>λq。

4 權重自適應算法

采用差分進化算法對目標函數f進行最小值優化求解,差分進化算法詳細原理可參考文獻[16]。在求解的目標位姿與初始位姿不同時，在優化后期機械臂的運動量fq始終大于0，當權重系數λq固定時，λqfq始終大于零，最終趨于穩定狀態。因為目標函數中包含了λqfq，當其大于零時會影響位姿誤差函數fE的收斂速度，所以當權重系數λq固定時存在收斂速度慢且求解精度不高的缺點。

為解決這一問題，設計了一種自適應權重系數方法，實現對逆運動學快速收斂且高精度地求解。具體方法：在優化初期選取較大的初始權重系數λq，使種群盡快被“吸引”到滿足最短行程原則且具有避障功能的峰值附近；隨著種群進化代數增加，從1/v倍最大進化代數為分界線處逐漸減小權重系數λq，直到迭代結束時λq=0，從而逐漸減小λqfq對fE的影響；迭代結束時λqfq=0，fE逐漸趨于0，最終求解到更精確的運動學逆解，v為大于1的可調參數，使得0≤G/v≤G。

所以，設計自適應權重λq在優化過程中滿足：

(17)

式(17)中：λqini為迭代初期權值；j為當前迭代步數；G為最大迭代步數。

5 避障逆運動學最優求解算法流程

采用差分進化算法對機械臂求避障最優運動學逆解，其流程圖如圖5所示，首先對機械臂、障礙物進行建模和簡化表示。同時初始化相關參數：輸入機械臂的目標位姿矩陣、初始關節角、障礙物的位置和大小、由差分進化算法產生初始種群K。其次計算碰撞檢測結果fCO、目標求解角度與當前角度的加權變化量fq、位姿誤差fE，通過差分進化算法選擇、交叉、變異得到子代，在迭代過程中不斷調整權值，多次迭代后達到終止條件。最后求解出唯一最優逆運動學解。

圖5 算法流程圖Fig.5 Algorithm flow chart

6 仿真實驗驗證

通過MATLAB Robotics Toolbox(version9.1)工具箱對算法進行仿真實驗驗證。所采用的差分進化算法參數配置如下：變量維度：6，變量定義域：[-π,-π,-π,-π,-π,-π]～[π,π,π,π,π,π]，種群規模：20，初始變異率：0.3，交叉算子：0.8，終止條件：10-5，最大迭代次數：300。

通過解析法求解得到的8組逆解如表2所示。利用本文算法對上述末端位姿隨機求取逆運動學解10次，目標函數中的權重系數設置為

10次求解結果如表3所示。解析法的求解結果在三維空間中的連桿位姿與障礙物的位置如圖6所示(此處展示第1、2組解析解在空間中的位置)。

圖6 解析解姿態圖Fig.6 Analytical solution attitude diagram

將本文算法10次求解結果的平均值代入正向運動學方程中，得到位姿矩陣，在三維空間中的各連桿的姿態與障礙物的位置如圖7所示。

圖7 最優解姿態圖Fig.7 Analytical solution attitude diagram

由圖6可以直觀地看出：末端姿態相同的情況下，求取的逆運動學解對應的機械臂姿態存在與障礙物發生碰撞的情況。由表3可以看出，機械臂在同一末端位姿情況下，由解析法求出的8組解中，存在4組解對應的機械臂連桿與障礙物發生碰撞。解析法的求解結果無法保證機械臂連桿能避開障礙物。本文方法的10次求解結果對應的機械臂姿態均未與障礙物發生碰撞。將表3和表4的數據對比不難發現，表3中第一組解析解的關節運動量最小，但該逆運動學解所對應的機械臂關節在空間中會與障礙物發生碰撞。本文算法所求得的逆運動學解對應于解析解的第2組解，對應的機械臂關節在空間中不會與障礙物發生碰撞，且在不發生碰撞的前提下，機械臂的關節的運動量最小。

表3 最優解Table 3 Optimal solution

表3 解析解Table 3 Parameter configuration

其次，當機械臂處于奇異位置時，存在解析法無法求解的情況。此時將本文算法的避障逆運動學最優求解算法與文獻[21]中不具有避障功能的最優求解算法(基于差分進化算法的逆運動學求解方法)仿真實驗對比：

當關節角θ5=0 rad時，關節4與關節6是共線的，此時UR5機械臂處于腕部奇異位置。令初始關節角K0=[-1.943 2 rad，-1.761 9 rad，2.589 1 rad，-2.398 0 rad，1.570 8 rad，-2.769 2 rad]，目標位姿Ka=[-1.943 2 rad，-1.761 9 rad，2.589 1 rad，-2.398 0 rad，0，-2.769 2 rad]，對應的位姿矩陣分別為

目標函數中的權重系數設置與前文相同。分別利用本文算法與文獻[21]中的基于差分進化的機械臂逆運動學求解方法(傳統方法)對UR5機械臂進行10次求解，10組逆運動學解對應的連桿姿態的避障性能分析結果如表4所示。

由表4可知，在機械臂處于奇異位置時，傳統的啟發式求解方法求出的結果具有隨機性，不具有避障性能。本文算法求出的逆運動學解具有良好的避障性能、運動量小且能保證較好的求解精度。

表4 奇異情況下的避障性能分析Table 4 Analysis of obstacle avoidance performance under singular conditions

最后，為了驗證權重自適應算法的優越性,在障礙物位置、差分進化算法參數、位姿矩陣等相同實驗參數條件下，將固定權重系數方法與自適應權重系數方法對比。得到優化過程中的關節角均方誤差收斂曲線如圖8所示，初始誤差不同是由于種群的隨機初始化。

由關節角均方誤差收斂曲線(圖8)可知，當權重固定時，由于收到fq的影響，在收斂后期關節角均方誤差E會上下波動，最后趨于穩定；采用自適應權重系數時，優化過程中關節角均方誤差收斂更快，且誤差更小。

圖8 誤差收斂曲線Fig.8 Error convergence curve

7 結論

以六自由度機械臂UR5為研究對象，針對機械臂在障礙物環境下的逆運動學求解問題，提出了一種結合碰撞檢測算法、最短行程算法、差分進化算法的機械臂逆運動學最優求解新方法。利用MATLAB對算法進行仿真實驗驗證，通過分析算法原理和實驗結果，得出以下結論。

(1)本文方法能夠使機械臂在避開障礙物的同時求得準確的逆運動學解。

(2)與傳統算法相比，所提出的機械臂避障最優求解算法不僅能求解出具有避障功能、行程最短的的逆運動學最優解，同時具有收斂速度快、求解精度高、通用性強的優越性。

(3)理論上，本文算法可被擴展到任意自由度的串聯機械臂的逆運動學求解中，且在無障礙物環境和復雜的多障礙物環境中均適用。下一步，將進行該算法的實際實驗研究，針對實際機械臂系統進行軌跡規劃的應用研究。