一種求解冗余機械臂逆運動學的自適應動態差分進化算法

東輝， 李晨， 姚立綱， 郭金泉， 孫浩

(福州大學機械工程及自動化學院, 福建 福州 350108)

0 引言

冗余機械臂逆運動學求解是利用機械臂末端執行器位置及姿態反向求解各關節角的過程， 具有非線性、 高耦合的特點， 直接求解存在難度. 目前， 相關求解方法主要分為三類： 幾何法、 代數數值解法和智能算法.

幾何法通常是通過分析機械臂幾何結構， 結合空間幾何知識與逆運動學方程來求解逆運動學解. 針對S-R-S構型七自由度機械臂， 胡奎等[1]引入臂角變量， 推導出含有臂角的逆運動學封閉解， 并將關節限位與臂角可行范圍進行映射， 定義一種避免關節限位目標優化函數， 最后將臂角和關節角關系進行線性化展開， 提出一種冗余機械臂逆運動學優化算法. Kofinas等[2]把整個NAO類機械臂劃分為頭部、 兩只手、 兩只腳一共5個部分， 使用Denavit-Hartenberg(D-H)參數， 對系統非線性方程以及身體和關節對稱性進行分析， 提出一種高效、 準確并且能夠避奇異的逆運動學求解方法. Faria等[3]通過求取自運動范圍邊界與當前自運動位置距離最小值來實現避關節限位并求逆運動學解， 可實現避奇異以及軌跡規劃. 霍希建等[4]把七自由度冗余機械臂簡化結構為肩、 肘、 腕， 新定義避關節限位評價函數， 提出一種關節角評價體制和逆運動學求解算法. 然而， 幾何法雖求解速度快、 精度高且不存在誤差積累， 但對不同構型機械臂， 幾何算法需重新推導計算， 其通用性存在一定局限.

代數數值解法包括雅可比矩陣偽逆法、 增廣雅可比法、 梯度投影法、 加權最小范數法等. Ignacy等[5]以雅可比矩陣為基礎， 對比雅可比矩陣轉置法和雅可比矩陣偽逆法， 此外還基于接近目標點短距離進行評估， 改進Levenberg-Marquardt方法， 提出了可靠且計算量較小的算法. Ayusawa等[6]利用迭代法去迭代梯度向量得到近似逆運動學解， 精度較高且求解速度較快. 東輝等[7-8]首先利用平面冗余機械臂結構參數結合傅里葉變換和卷積定理， 構建非線性工作空間密度函數， 再以該密度函數作為目標函數， 提出一種迭代的逆運動學求解算法. Wan等[9]在原有最小二乘范數法基礎上增加一個彈性場函數， 提出一種改進最小二乘范數法. 陽方平等[10]基于加權最小范數法， 推導出一種避免計算雅可比矩陣偽逆優化方法， 通過七自由度冗余機械臂證明了該算法在精度、 避免關節限位等方面效果. 代數數值法盡管精度高且較為可靠， 但計算量巨大且求解過程復雜， 其所需的雅克比偽逆矩陣計算困難.

隨著計算機技術發展， 許多智能算法也被應用到冗余機械臂逆運動學求解問題上， 其中很多源于自然生物規律， 比如神經網絡算法(NN)、 遺傳算法(GA)、 蟻群算法(ACO)等. K?ker等[11]利用10組神經網絡和對應數據組對一冗余機械臂進行逆運動學求解， 提出一種基于神經網絡算法的冗余機械臂逆運動學求解機制. 此外， K?ker還分別融合使用遺傳算法[12]與模擬退火算法[13]對神經網絡算法進行優化并用于冗余機械臂逆運動學求解. Parker等[14]較早使用遺傳算法求解逆運動學問題. 謝習華等[15]利用改進差分進化算法求解冗余機械臂逆運動學， 并與基本差分進化算法進行比較. Huang等[16]利用粒子群算法求解七自由度冗余機械臂逆運動學解， 展示了粒子群算法強大粒子間合作及更新能力. Bayati等[17]提出基于布谷鳥算法和帝國主義算法的混合方法解決機械臂逆解問題. 智能算法具有精度高且速度較快等優勢. 但是， 對于冗余機械臂， 常規啟發式搜索算法易陷入局部搜索， 難以尋得最優解， 最終將影響逆運動學求解精度. 由此可知， 盡管多種方法成功應用于冗余機械臂逆運動學求解中， 但仍有完善空間.

本研究針對差分進化算法易陷入局部搜索等問題， 在基本差分進化算法模型基礎上引入自適應變異操作與隨機變動交叉操作, 改進了逆運動學算法適應度函數與邊界處理方法， 根據迭代進程調整搜索強度. 同時, 兼顧機械臂末端執行器位置與姿態兩類誤差， 借助改進后的自適應動態差分進化算法在關節空間范圍內進行可行解搜索， 以達到高效求解冗余機械臂逆運動學的研究目標. 應用KUKA LBR iiwa七自由度機械臂模型， 采用自適應動態差分進化算法對其進行逆運動學求解， 在仿真實驗中重點研究和定量評價了求解精度和穩定性. 計算結果表明， 位置求解精度最高可達10-7mm， 姿態誤差不超過10-5rad， 200次迭代計算僅需0.93 s. 在30次重復定位實驗中， 最低位姿綜合誤差和最高位姿綜合誤差分別是10-7和2.5×10-5， 平均位姿綜合誤差為1.6×10-5. 求解精度和穩定性結果數據驗證了方法可行性. 此外， 本方法具有良好普適性， 僅由機械臂D-H參數即可求解機械臂逆運動學問題， 而無需機械臂結構分解與處理. 因此， 本方法可推廣至多領域內相關機械臂運動分析與控制研究.

1 機械臂運動學分析

機械臂運動學包含正運動學以及逆運動學， 正運動學是利用已知的各關節角值求取機械臂末端執行器位姿. 使用D-H方法建立機械臂模型， 以KUKA LBR iiwa 七自由度機械臂為研究對象， 如圖1所示， 其D-H參數如表1所示.

圖1 KUKA LBR iiwa機械臂及其運動學簡圖Fig.1 KUKA LBR iiwa manipulator and the schematic diagram

表1 機械臂D-H參數

在機械臂每個關節處建立坐標系， 轉角α表示繞Z軸轉動量， 關節角θ代表繞X軸旋轉量， 偏距d和連桿長度a分別代表沿X，Z軸移動量， 相鄰連桿位姿變換可用齊次變換矩陣i-1Ti表示：

(1)

其中： 末端執行器位姿由所有關節共同作用產生， 故機械臂末端執行器在基座標中的位姿0T7可以表示為：

(2)

式中：Px，Py，Pz分別表示末端執行器在基坐標系中X，Y，Z方向的位置.為簡便表示姿態， 采用Z-Y-Z歐拉角進行定義， 即按順序圍繞基坐標系中Z，Y，Z軸分別旋轉α，β，γ三個歐拉角得到最終姿態， 歐拉角計算方法如下式所示.若已知機械臂各關節角度值， 代入運動學方程中即可求得末端執行器在此關節角組合情況下的位姿 (x,y,z,α,β,γ).

(3)

逆運動學求解則是根據末端執行器位置與姿態反向求解關節角， 與正運動學不同， 冗余機械臂逆運動學解并不具有唯一性. 由公式(2)、 (3)機械臂自由度至少含有七個自由度， 則方程數目至少為7， 故運動學方程中自變量數量大于因變量數目， 從而導致逆運動學多解現象， 給冗余機械臂逆運動學的求解造成一定難度.

2 差分進化算法

2.1 基本差分進化算法

差分進化算法(differential evolution algorithm， DE) 由Storn等學者于1995年提出， 屬于一種仿生算法， 采用基于群體的全局搜索策略， 利用實數編碼， 具有較強全局收斂能力以及魯棒性， 常用于解決非線性、 不可微等類型的問題. 目前差分進化算法已經廣泛應用在化工、 電力、 信號處理、 機械設計等領域. 差分進化算法是基于實數的演化算法， 主要步驟包括種群初始化、 變異、 交叉、 選擇， 其主要思想是在變化量范圍內隨機生成新的種群并選擇若干個體作為交叉、 變異操作對象， 產生新個體， 接著對新個體進行適應度計算， 直至找到適應度值最符合要求的個體， 從而獲得最優解. 算法進化流程可分為以下幾個步驟.

1)種群初始化. 種群由一定數量個體組成， 每個個體可以看作是問題的一個解， 并且個體維度與所求問題維度需保持一致. 假設待解問題包含m個自變量， 則個體維度也為m.若要生成一個包含N個個體種群， 且其中每個個體都是有m維， 種群初始化操作如下所示：

X(i,j)=Xj, min+(Xj, max-Xj, min)×rand(0, 1)

(4)

其中：i=1, …,N;j=1, …,m;X(i,j)表示種群中第i個個體第j維;Xj, min和Xj, max分別表示j維度取值上限與下限; rand(0, 1)表示在[0, 1]內隨機取值.

2)變異. 變異目的是產生與初始種群不同個體， 增加種群個體多樣性， 可增加尋找最到優解的概率和速度. 變異是在已有個體中隨機取若干各異個體進行相互運算， 從而產生新個體， 采用DE/best/1/bin變異機制， 其變異方式為：

Vi=Xbest+F(X1-X2)

(5)

式中：Vi表示第i代一個新個體；Xbest是當前種群中最符合適應度要求的個體；X1，X2是與Xbest相異的個體；F是變異因子， 用于控制偏差大小， 一般有0 ≤F≤ 2.雖然每個個體都是在取值范圍內隨機生成， 但經過變異操作后有些個體可能已經超出了取值范圍， 所以針對每個個體的每個維度都需要進行邊界處理， 常規邊界處理一般是以超出邊界粒子的最近邊界值替代， 具體操作為：

(6)

3)交叉. 變異是基于個體整體進行的運算操作， 可以一定程度上增加種群多樣性， 但這種方法仍然不夠靈活. 為進一步擴大種群多樣性， 差分進化算法還可以在個體維度層面進行交叉互換， 通過改變個體一部分的方式來產生新個體， 交叉的控制更依賴于交叉因子設置， 將其與隨機數進行對比， 更具有隨機性， 可有效增加個體多樣性， 交叉操作如下所示：

(7)

式中： CR為交叉因子， 取值區間為[0, 1]；Ui, j，Vi, j，Xi, j分別是三個不同個體在i個體第j維度值.

4)選擇. 差分進化算法根據貪婪原則， 從每個種群中選擇出最佳個體， 再從每次迭代的最佳個體中選擇出最優個體， 最終在指定迭代次數內或者精度要求范圍內得到最優解. 選擇依據是個體適應度函數值大小， 在最小化問題中一般選擇適應度函數值最小個體作為最優解， 最大化問題則通常篩選出適應度函數值最大個體. 以最小化問題為例， 適應度函數值越小， 說明越符合需求， 選擇方式如下：

(8)

2.2 自適應動態差分進化算法

在實際情況中， 基本差分進化算法易于陷入局部搜索， 其原因在于算法搜索方式一成不變， 隨著迭代進行， 算法一直按照固定方式進行搜索， 以至于最終可能陷入局部最優而找不到全局最優解位置. 所以本研究引入了自適應變異因子以及隨機變動交叉因子， 使算法隨著迭代進行而調整搜索強度， 提出一種自適應動態差分進化算法.

2.2.1自適應變異因子

變異因子F是控制種群多樣性以及收斂性的重要參數， 基本算法變異因子通常取值為常數， 導致算法不夠靈活.若F取值過大， 則會導致算法收斂速度變慢且收斂精度降低； 若F過小， 則種群多樣性不夠， 容易出現收斂過早現象.為取得良好收斂效果， 應該使算法先進行大跨度搜索再小范圍搜索， 即變異因子應隨著迭代進程減小， 逐漸逼近最優解， 故引入如下隨迭代進行而變化的自適應變異因子：

(9)

其中：F0為變異因子初始值；G為最大迭代次數；g為當前迭代次數.

2.2.2隨機變動交叉因子

交叉因子CR控制個體各維參與交叉操作程度以及全局種群多樣性， 基本算法交叉因子通常取固定值， 引入隨機變化機制可增加交叉隨機性， 從而增加種群多樣性. 隨機變動交叉因子操作如下：

CR=0.5×(1+rand(0, 1))

(10)

綜上所述， 自適應動態差分進化算法流程如圖2所示.

圖2 自適應動態差分進化算法流程圖Fig.2 Flow chart of the adaptive dynamic differential evolution algorithm

3 冗余機械臂逆運動學求解

冗余機械臂逆運動學問題具有高度非線性、 高耦合等特點， 機械臂每個關節都有其自身可運動范圍， 需要根據逆運動學求解問題實際特點， 對差分進化算法進行適當修改. 通過分析使用自適應動態差分進化算法求解七自由度機械臂逆運動學實際效果， 發現以下兩個問題： 1) 位置誤差和姿態誤差兩者收斂效果差距大， 往往位置收斂精度比姿態精度高出4～5倍， 即出現了兩種誤差收斂不平衡現象， 同時由于各歐拉角之間存在耦合， 導致姿態收斂發生漂移. 2) 常用邊界處理方法使得最后得出的結果中有些維度取值是邊界極值， 不符合機械臂實際使用要求， 當關節角取極值時容易造成機械臂抖動， 導致機械臂工作不穩定. 針對這些問題， 對適應度函數以及邊界處理進行改進， 適應度函數主要針對平衡對位置誤差和姿態誤差， 邊界處理將避免關節限位納入考慮.

3.1 適應度函數設計

冗余機械臂逆運動學問題可視為末端執行器位姿誤差最小化問題， 位置誤差用ΔP表示， 姿態誤差用ΔR表示：

(11)

其中： Δx， Δy， Δz分別為X，Y，Z方向位置誤差值； Δα， Δβ， Δγ為姿態誤差.

位置誤差與連桿長度有關， 姿態誤差與關節角度相關聯， 關節角度采用弧度制， 所以弧度制與機械臂連桿長度具有一定倍數關系. 正是這種數量關系的存在， 使兩種誤差收斂不平衡， 分別為均衡位置誤差與姿態誤差收斂. 改進適應度函數采用加權處理方法：

f=ωp·ΔP+ωr·ΔR

(12)

分析可知弧度制與連桿單位具有一定倍數關系， 當設置ωp=1，ωr=180/π時對位置誤差和姿態誤差具有良好平衡效果. 對KUKA LBR iiwa機械臂模型實驗結果進行分析， 由于表示冗余機械臂姿態的歐拉角之間存在耦合關系， 導致歐拉角α的擬合值始終包含0.43 rad的固定漂移增量， 故在計算過程中對歐拉角α進行漂移量補償：

(13)

3.2 關節邊界處理

常用差分進化算法對超出邊界的個體是用邊界值替代， 實際情況中， 機械臂關節角取邊界值不利于機械臂控制和有載工作， 會造成機械臂異常抖動使得工作穩定性下降， 所以為盡可能避免解中包含關節角邊界值， 對于迭代過程中超出邊界的個體進行處理， 使其重新從取值范圍中點開始取值， 從而盡可能地避免邊界取值， 關節邊界處理操作如下：

(14)

4 仿真實驗

為驗證算法各方面性能表現， 以KUKA LBR iiwa 七自由度機械臂為實驗對象， 實驗平臺基于Windows 10系統， 硬件條件為Intel Core i7-8700K @ 3.7 GHz， 內存大小為16 GB. 利用自適應動態差分進化算法進行逆運動學求解， 基于Matlab進行了三類仿真測試： 算法收斂性測試、 算法穩定性測試， 算法對比測試， 所得結果證明了算法在收斂性以及穩定性方面都具有良好效果， 適合用于冗余機械臂逆運動學求解.

4.1 算法比較測試

為驗證自適應動態差分進化算法改進后的收斂性和穩定性， 把本算法與基本差分進化算法進行比較， 對同一點位姿進行逆運動學求解. 適應度函數由位置誤差與姿態誤差組成， 為體現綜合性能， 以適應度函數值為評價標準， 分別從收斂性與穩定性兩方面進行比對.

4.1.1收斂性比對

對于位姿(400, 500, 600, π/2, π/4, π/3)， 設置自適應動態差分進化算法初始變異因子F0為2， 分別使用兩種算法迭代200次進行逆運動學求解， 對比兩種方法適應度函數值如圖3所示. 可以發現, 自適應動態差分進化算法最終收斂曲線位于基本差分進化算法下方， 適應度函數值最終收斂至1.64， 得出解為(399.941 3, 500.015 6, 599.957 7, -0.138 8, 1.114 0, 1.047 3)， 與目標位姿相差較大， 尤其是姿態誤差最大. 改進后算法適應度函數收斂值為1.53×10-5， 得出解為(399.999 9, 500.000 1, 600.000 0, 1.570 8, 0.785 4, 1.047 2). 將兩者結果進行對比可發現， 本算法在精度方面明顯優于基本差分進化算法. 說明改進后算法在收斂性方面優于基本差分進化算法， 收斂曲線下降趨勢更大， 且基本差分進化算法收斂速度一直慢于自適應動態差分進化算法， 體現了本算法優越性.

圖3 收斂性對比曲線Fig.3 Convergence plots of the two algorithms

4.1.2穩定性對比

穩定性測試采用重復定位的方法， 同樣對位姿(400, 500, 600, π/2, π/4, π/3)求解， 分別使用兩種算法迭代30次計算， 記錄每次定位的適應度函數收斂值并對比. 基本差分進化算法最大和最小收斂值分別是2.45和1.12， 平均收斂值為1.65， 收斂值變化幅度非常大. 本算法適應度函數收斂值則一直保持在1×10-5附近， 兩者收斂值曲線對比如圖4所示. 由圖4可見， 基本差分進化算法不僅收斂值高于本算法， 收斂精度波動幅度也更大， 證明了改進后方法具有更好的穩定性.

圖4 穩定性對比曲線Fig.4 Stability curves of the two algorithms

4.2 自適應差分進化算法性能測試

4.2.1求解精度測試

對于KUKA LBR iiwa機械臂末端執行器在工作空間內的目標位姿 (400, 500, 600, π/2, π/4, π/3)，設置自適應動態差分進化算法初始變異因子F0為2， 迭代次數G為200次， 對這一位姿進行逆運動學求解. 計算得到的一組解為(0.127 7， 0.945 3， 1.184 0， 1.331 4， -1.405 0， -1.148 4， 1.850 0)， 適應度函數值為1.4×10-5. 為驗證結果準確性， 把這組關節角帶入到正運動學方程中， 得到此時末端執行器位姿為(399.999 9, 500.000 1, 600.000 0, 1.570 8, 0.785 4, 1.047 2)， 位置最大誤差僅為0.000 1 mm， 姿態誤差為0 rad， 說明本方法具有較高逆運動學求解精度與計算效率. 計算過程中適應度函數值以及誤差曲線如圖5～7所示. 分別使用ε，η代表位置誤差和姿態誤差, 假設工作場合所需位姿綜合精度為10-3(圖中用紅色虛線表示)， 則全局在第73次迭代就已滿足位姿精度要求， 單獨位置精度和姿態精度分別在第82次和第43次達到要求精度. 說明改進后的算法不僅提高了姿態收斂精度， 也加快了收斂速度.

圖5 適應度函數收斂曲線 圖6 位置誤差收斂曲線 圖7 姿態誤差收斂曲線

4.2.2穩定性測試

差分進化算法屬于隨機搜索算法， 其搜索過程具有一定隨機性， 故算法計算效果也具有相應隨機變動， 這種計算精度波動情況可用算法穩定性來衡量， 穩定性越好， 多次計算平均精度更低且不同次數間的計算精度差別更為合理. 為檢驗本算法穩定性， 實施了重復定位實驗， 對(400, 500, 600, π/2, π/4, π/3)這一位姿進行了30次重復定位計算并比較適應度函數值變化情況. 在30次計算中， 最低和最高適應度函數值分別為1.0×10-7和2.5×10-5， 平均適應度值是1.6×10-5， 適應度函數值變化曲線如圖8所示， 在多次重復定位測試下， 適應度函數值波動小， 未發生較大數值偏差或陷入局部搜索現象， 穩定性表現良好.

圖8 重復定位適應度曲線Fig.8 Fitness curve of repeated positioning

4.2.3多點軌跡跟蹤測試

實際工作中， 機械臂末端執行器通過掠過由多個點組成的工作路徑來完成動作， 為測試算法對連續多點的計算效果， 選定螺旋線和蝴蝶曲線兩種不同曲線用于軌跡跟蹤實驗， 函數表達式分別為:

x=cos(t)+400；y=sin(t)+500；z=t

(15)

{y=sin(t)(ecos(t)-2cos(4t)-sin5t12)+500

z=cos(t)(ecos(t)-2cos(4t)-sin5t12)+500

0≤t≤12

首先在曲線上按固定步長選取若干計算點， 螺旋曲線和蝴蝶曲線分別均勻選取100個和500個計算點， 把所有計算點代入計算， 將輸出計算結果點與標準軌跡進行對比. 計算結果表明， 螺旋線擬合精度最低為2×10-4mm， 最高精度1×10-6mm， 平均精度為1×10-5mm， 蝴蝶線軌跡計算平均精度為1×10-5mm. 對比圖如圖9， 10所示， 兩種曲線計算結果點全部落在標準軌跡上， 表明算法對于軌跡上多點的位置計算精度高， 軌跡擬合能力較強.

圖9 空間螺旋線預設軌跡與計算點Fig.9 Preset trajectory and calculated points of a spatial spiral

圖10 “蝴蝶”形曲線預設軌跡與計算點Fig.10 Preset trajectory and calculated points of a butterfly-shaped curve

4.3 與其他逆運動學算法對比

目前冗余機械臂逆運動學求解算法眾多， 每種方法都有其自身特點. 為綜合對比不同算法效果， 列舉了幾種逆運動學算法性能表現, 如表2所示. 文獻[18]使用改進人工蜂群算法求解七自由度冗余機械臂逆運動學， 位置計算精度可達10-3mm. 文獻[19]采用粒子群算法、 遺傳算法、 量子粒子群算法以及引力搜索算法求解四自由度機械臂逆運動學問題， 求解位置精度較高可達10-5mm， 但求解時間長， 耗時約為2 s. 文獻[20]使用神經網絡算法， 對七自由度機械臂單次求解僅需0.1 s， 但位置精度僅為5 mm， 計算速度較快但精度不高. 而同樣針對七自由度冗余機械臂， 本算法位置計算精度可達10-7mm， 求解時間也較短， 僅需0.93 s.

表2 幾種逆運動學求解方法對比

5 結語

采用自適應動態差分進化算法求解冗余機械臂逆運動學. 引入自適應變異因子以及隨機變動交叉因子， 隨迭代進行自適應調整算法搜索強度， 避免算法過早陷入姿態局部搜索， 提高了算法收斂性及穩定性. 改進含加權操作的適應度函數和關節邊界處理操作， 平衡了位置誤差及姿態誤差收斂. 最后， 完成了一系列數值仿真實驗， 測試和評價算法收斂性、 穩定性， 并與基本差分進化算法和其他逆運動學算法完成了比對分析. 結果表明， 機械臂位置和姿態誤差均得到了良好收斂， 方法可普適于冗余機械臂逆運動學求解.