基于改進適應度函數(shù)組合法的機器人逆運動學求解

李 光 譚薪興 肖 帆 易 靜 薛晨慷 于權偉

(1.湖南工業(yè)大學機械工程學院, 株洲 412007; 2.武漢科技大學機械自動化學院, 武漢 430081)

0 引言

傳統(tǒng)的逆向運動學的求解理論和方法有解析法[1-3]、幾何法[4-5]、數(shù)值法[6]等。解析法主要以消元的方式將高維方程組轉換為低維方程組，PIEPER等[7]提出了一類具有解析解的6R串聯(lián)機械手，雖不滿足Pieper準則的機器人逆向運動學問題，也獲得了廣泛的研究[8-9]，其給出的方法需要進行大量的三角代換，計算過程十分復雜耗時，很難在工業(yè)機械手中實現(xiàn)，且一般結構的機械臂無法得到解析解。幾何法針對機器人的特殊結構進行簡化再求解，一般無法單獨使用甚至無法使用[10]。數(shù)值法可以精確搜索到一組逆解，求解速度與初始值相關，甚至存在無解，且數(shù)值法需要的運算量大，不適合實時求解的場合[11]。

近年來，利用智能算法求解機器人逆向運動學的方法取得一定的成果[12-20]。神經(jīng)網(wǎng)絡算法的計算代價高，計算能力很大程度取決于數(shù)據(jù)的量，同時也取決于網(wǎng)絡的深度和復雜程度；遺傳算法全局搜索能力強，局部搜索能力較弱，容易誤入局部最優(yōu)解；蟻群算法和魚群算法參數(shù)設置復雜，如果設置不當，容易偏離最優(yōu)解；粒子群算法和果蠅優(yōu)化算法操作簡單，收斂速度快，在機器人逆向運動學中也得到了應用，但存在提前收斂、維數(shù)災難等問題。自適應協(xié)方差矩陣進化策略(Covariance matrix adaptation evolution strategy，CMA-ES)算法[21-22]是在 ES算法[23]的基礎上發(fā)展而來的無約束優(yōu)化算法，在全局優(yōu)化、多峰優(yōu)化、多目標優(yōu)化、大規(guī)模優(yōu)化和結構性工程等領域得到了大量應用[24-25]，該算法搜索性能較高，并且能夠達到比較高的搜索精度。

為克服單一的傳統(tǒng)算法和單一的智能算法求解逆向運動學的不足，組合算法引起了許多學者的關注。趙瑜[26]基于布谷鳥算法和牛頓法組合算法求解6R機器人運動學逆解，可以得到較高精度的逆運動學解，求解效率較低；陳禹含等[27]結合旋量理論和代數(shù)法的求解逆運動學求解，求解過程較為復雜；裴九芳等[28]采用解析解和數(shù)值解求解三指機器人逆向運動學，求解效率較高但通用性較差。

智能算法中適應度函數(shù)決定了機器人逆向運動學分析結果的性能，文獻[29-31]闡述了建立適應度函數(shù)的常用方式，可以高精度地求出逆向運動學解，但是該適應度函數(shù)較為復雜，增加了求解時間。

一般結構的機械臂沒有解析解，用已有的代數(shù)法求封閉解的推導過程十分復雜，而單一的智能算法和組合算法求逆解均存在不足。一般結構機器人是指至少有一組相鄰關節(jié)軸線相交于一點的機器人[32]。本文提出一種基于分離-重構方法獲得智能算法的適應度函數(shù)，與解析法相組合的方法求一般結構多自由度機械臂運動學逆解。利用分離-重構的方法[33]，構建分離-重構適應度函數(shù)，求解出部分關節(jié)角，最后利用解析法求解余下的關節(jié)角。選用改進的CMA-ES算法[31]驗證分離-重構適應度函數(shù)，以驗證方法的有效性。在一般結構多自由度機器人上進行仿真，在相同條件下的點對點運動中，使用改進適應度函數(shù)的組合法和原方法求解，同時為模擬復雜程度不同的工作環(huán)境，對二維和三維空間中的連續(xù)軌跡進行跟蹤，比較組合法和原方法的求解精度和關節(jié)角運動平穩(wěn)性；在具有一般結構的REBot-V-6R機器人實驗平臺上進行三維空間連續(xù)軌跡的跟蹤實驗。

1 適應度函數(shù)與解析法

1.1 適應度函數(shù)

目前，DH法是機器人運動學常用的建模方法，可以確定機器人的關節(jié)參數(shù)和關節(jié)變量，它與機器人結構順序和復雜程度無關，通過齊次變換矩陣描述兩個相鄰坐標系間的空間位姿關系。

首先建立連桿坐標系，然后通過4次旋轉平移變換，根據(jù)DH參數(shù)，定義第i連桿的參數(shù)ai為連桿長度，αi為相鄰兩關節(jié)軸線的夾角，di為第i根連桿和第i-1連桿的偏置距離，θi為第i連桿的旋轉角。變換過程順序如下：繞zi-1軸旋轉θi，沿zi-1軸平移di,沿xi軸平移ai, 繞xi軸旋轉αi, 可得到機器人連桿i坐標系相對于連桿i-1坐標系的變換矩陣

(1)

式中R——4×4的旋轉矩陣A——移動矩陣

式中s表示正弦函數(shù)，c表示余弦函數(shù)。

根據(jù)式(1)得到一般結構多自由度機器人正向運動學數(shù)學模型，通過正向運動學控制機器人末端執(zhí)行器到達指定位置，末端位姿齊次變換矩陣為

(j=1,2,…,n)

(2)

式中p——3×1位置向量

a——3×1接近向量

為了建立分離-重構適應度函數(shù)，需要對具有一般結構的多自由度機械臂按一定方式進行分離-重構：在相鄰軸線交點處進行分離-重構，相鄰相交軸的坐標系如圖1所示，子鏈i和i+1交于點O。分離后為使兩子鏈重構，需使得兩子鏈的末端位姿一致。首先根據(jù)重構后兩子鏈末端位置約束條件，建立約束等式

pi=pi+1

(3)

圖1 相鄰相交軸的坐標系Fig.1 Coordinate system of adjacent intersecting axes

兩子鏈z軸之間夾角為α，在實際應用中，α一般不等于0，為使子鏈i繞xi軸旋轉-α，使子鏈i和i+1的z軸重合，建立約束等式

(4)

根據(jù)分離-重構點位置不同，一般結構自由度n機器人有n-1種情況,以一般結構六自由度機器人為例，交點O有5種位置情況：關節(jié)軸1和軸2的交點、關節(jié)軸2和軸3的交點、關節(jié)軸3和軸4的交點、關節(jié)軸4和軸5的交點、關節(jié)軸5和軸6的交點，結合齊次變換矩陣式(2)可以得到以下5種情況

(5)

(6)

(7)

(8)

(9)

確定了分離-重構點位置，在式(5)～(9)選擇一種合適的情況，將坐標變換矩陣式(1)代入，得到兩子鏈的展開式Ti和Ti+1，對應式(2)中p、a矩陣的位置，根據(jù)位姿約束得到式(3)、(4)，完成構建分離-重構適應度函數(shù)

(10)

圖2 組合法流程圖Fig.2 Flow diagram of combination method

在文獻[29-31]提出的算法中，使用位姿誤差建立適應度函數(shù)JPP求解逆運動學，JPP表示兩種誤差范數(shù)之和，計算式為

JPP=‖pdes-pcur‖+b‖Rdes-Rcur‖

(11)

式中R——3×3的旋轉矩陣[34]

b——罰函數(shù)

其中‖pdes-pcur‖表示位置誤差范數(shù)， ‖Rdes-Rcur‖表示姿態(tài)誤差范數(shù)，使用智能算法直接求出全部關節(jié)角，該方法的流程圖如圖3所示。

圖3 原方法流程圖Fig.3 Flowchart of original method

JPP中包含全部關節(jié)變量，比JDR多2個關節(jié)變量，JPP求解過程復雜，R是一個3×3的旋轉矩陣，而a是3×1的姿態(tài)矩陣，JPP的代數(shù)式維度更大，通過推導得到的代數(shù)式更復雜，在智能算法中求逆解時會明顯增加耗時。

除此之外，JPP中還需設置位置誤差函數(shù)的罰函數(shù)b，是為了保證姿態(tài)誤差‖Rdes-Rcur‖和位置誤差‖pdes-pcur‖在求解過程中處于同一數(shù)量級，防止局部收斂和發(fā)散等情況[35]，但JDR不存在這種情況。

1.2 解析法分析

通過求解JDR，確定了多自由度機械臂非相鄰相交軸線的關節(jié)角，再通過解析法求解剩余的兩個關節(jié)角，避免了全部采用解析法求解的繁瑣復雜，解析法求解的兩個關節(jié)角和智能算法求解出的其他關節(jié)角精度在同一數(shù)量級。機械臂已經(jīng)確定了非相鄰相交軸線的關節(jié)角，則兩子鏈末端的αi和αi+1將保持不變，即zi軸和zi+1軸的位姿確定，根據(jù)幾何約束條件知αi和oi+1之間成π/2-α，故可以得到

(12)

式(12)展開后，只有θi+1未知，因此可以通過簡單的數(shù)學計算解得，且θi+1只有一個解。旋轉坐標系關系圖如圖4所示。

圖4 旋轉坐標系關系圖Fig.4 Relationship of rotating coordinate system

θi+1在周期內繞zi+1旋轉，只有當軸yi+1位于zizi+1平面內才能滿足約束條件，因此在一個旋轉周期內，θi+1只有一個解。同理，oi和αi+1之間呈π/2+α，故可以得到

(13)

式中只有θi未知，θi也僅有一個解。使用解析法求剩下的θi+1和θi，只需求解兩個一元一次方程，求解過程簡單方便，解決了使用解析法求一般結構機器人逆運動學解計算繁瑣復雜的問題。

2 改進的CMA-ES算法

2.1 算法原理

CMA-ES算法是一種進化策略算法，采用正態(tài)分布N(m,C)生成一定數(shù)量的搜索種群，N(m,C)表示：搜索分布均值為m，以協(xié)方差矩陣為C的多元正態(tài)分布，且C=BDBT，其中BB=I，D為對角矩陣，化簡多元正態(tài)分布N(m,C)得

N(m,C)～m+N(0,C)～m+BDN(0,I)

(14)

式(14)的“～”表示服從相同的分布，由式(14)的逆序可知，I確定等概率密度圓球面，通過B、D的變化可以確定C的橢球面；球面的分布尺度由D中的對角元素決定，主軸的方向由B確定，變化B、D實現(xiàn)球面的分布旋轉。

2.2 算法步驟

步驟如下：

(1)參數(shù)設置及初始化、靜態(tài)參數(shù)[36]。

(2)突變，突變的目的是使個體之間產(chǎn)生差異。CMA-ES算法的突變過程是以當代的均值為中心，產(chǎn)生下一代種群。

(k=1,2,…,λ)

(15)

m(g)——第g代種群中適應度排名前μ個個體的加權平均值

σ(g)——第g代種群進化步長

C(g)——第g代種群進化協(xié)方差矩陣

μ——父代數(shù)量

(3)選擇和重組，CMA-ES算法采用(μ，λ)策略，父代不參與當前代的適應度排名競爭，優(yōu)秀個體只從當前子代中選擇，對λ個子代進行適應度評價，根據(jù)適應能力由高到低排序，選取適應能力強的前μ個個體，作為下一代種群的父代，更新策略參數(shù)m(g)、σ(g)、C(g)，以此來傳遞優(yōu)良的基因，其過程如下：

②協(xié)方差矩陣自適應調整。

③全局步長控制

(16)

(17)

式中μeff——方差有效選擇質量

ε——縮放系數(shù)

ε的主要作用是：當代個體中存在越界行為時，在下一代個體更新中，及時阻止越界行為，保證結果在要求范圍內，而ε<0.56容易導致出現(xiàn)“早熟”或者無法收斂；ρ可看作步長變化的伸縮因子，δb表示越界敏感因子，個體每個維度在不可行域內出現(xiàn)次數(shù)的總和，即不在關節(jié)范圍內的次數(shù)總數(shù)，cσ為qσ更新學習率，dσ為接近于1的阻尼系數(shù)，E‖N(0,I)‖為歸一化進化路徑在隨機選擇下的期望長度。

(4)判斷是否達到最大迭代次數(shù)或精度，若是，則停止，輸出最優(yōu)解和最優(yōu)決策向量，否則返回步驟(2)，算法求解詳細流程如圖5所示。

圖5 改進的CAM-ES算法流程圖Fig.5 Flowchart of improved CAM-ES algorithm

3 仿真實驗與結果分析

3.1 適應度函數(shù)求解

使用改進適應度函數(shù)的組合法和原方法對一般結構FANUC CRX-10iA型協(xié)作機器人進行逆向運動學計算。圖6為FANUC CRX-10iA型機器人結構簡圖，可以看出是一般結構的機器人，且不滿足Pieper準則，無法得到逆向運動學的完備解析式，表1為機器人連桿參數(shù)。

圖6 FANUC CRX-10iA型機器人結構簡圖Fig.6 Structure diagram of FANUC CRX-10iA robot

表1 機器人連桿參數(shù)Tab.1 Link parameters of robot

根據(jù)1.1節(jié)中對兩種適應度函數(shù)的分析，由圖6可得關節(jié)4和關節(jié)5相交于一點，JDR計算式為

(18)

經(jīng)過計算，建立

(19)

其中a1=0.1 m,a2=0.29 m,a3=0.121 m，d1=0.32 m,d4=0.31 m,d5=0.1 m,d6=0.111 5 m,使用適應度函數(shù)JDR求解出θ1、θ2、θ3和θ6，通過解析式(12)、(13)求解相鄰相交兩軸的θ4和θ5，可得計算式為

(20)

(21)

3.2 改進的CMA-ES算法參數(shù)設置

使用改進的CMA-ES算法驗證分離-重構適應度函數(shù)，設置算法的種群數(shù)量λ=100，優(yōu)秀個體μ=50，初始步長σ=0.5，初始均值在0～1內隨機產(chǎn)生，求解過程中，前一個逆向運動學解作為當前初始點均值，初始均值縮放系數(shù)ε=0.65，算法停止條件為進化次數(shù)G=100或者適應度函數(shù)J≤10-6。

3.3 點到點運動仿真

在工作空間內，機器人從指定初始位置θ=[0 0 0 0 0 0] rad運動到指定目標關節(jié)位置：θ=[0.82 0.93 0.66 0.73 0.88 0.99] rad，分別使用組合法和原方法求解，獨立運行50次仿真結果如表2所示，算法收斂情況如圖7所示。結果表明，同原方法比較，組合法的求解速度更快更穩(wěn)定。組合法迭代次數(shù)在40次以前就能完全滿足精度要求，且大部分迭代次數(shù)在25～30次之間收斂，而原方法迭代100次仍然會出現(xiàn)不收斂的情況；組合法的平均求解時間為0.004 s，原方法平均求解時間為0.032 s，速度提升了7倍，從表2的標準差和圖7可以看出，組合法求解機器人運動學逆解更穩(wěn)定。

表2 兩種方法獨立運行50次結果Tab.2 Results of 50 independent runs of two methods

圖7 迭代收斂情況Fig.7 Situation of iterative convergence

3.4 連續(xù)軌跡跟蹤仿真

為驗證組合法在面對不同復雜程度的工作環(huán)境時軌跡跟蹤的有效性，在工作空間內分別使用二維空間的圓軌跡方程式和三維空間的圓錐螺旋軌跡方程式進行仿真，公式為

(22)

(23)

采用兩種方法對空間曲線進行跟蹤，結果如表3、4所示，結果表明，在二維平面圓軌跡跟蹤上，組合法單點的平均求解時間為0.006 8 s，比使用原方法求解的速度提升4倍左右；在三維圓錐螺旋軌跡跟蹤上，組合法速度提升2倍左右。

表3 平面圓軌跡跟蹤結果Tab.3 Tracking result of plane circular trajectory

表4 圓錐螺旋軌跡跟蹤結果Tab.4 Tracking results of conical spiral trajectory

圖8 軌跡跟蹤Fig.8 Trajectory tracking

圖9 軌跡跟蹤誤差曲線Fig.9 Error curves of trajectory tracking

圖10、11為組合法和原方法連續(xù)跟蹤兩種軌跡的關節(jié)角度變化曲線，從圖10b和圖11b可以看出，原方法在關節(jié)角度變化率較大的地方存在鋸齒波動，而組合法跟蹤過程中關節(jié)角曲線平穩(wěn)光滑。

圖10 平面圓軌跡關節(jié)角變化曲線Fig.10 Changes of joint angle of plane circular trajectory

圖11 圓錐螺旋軌跡關節(jié)角變化曲線Fig.11 Changes of joint angle of conical spiral trajectory

4 實驗

以REBot-V-6R型六自由度機器人為實驗平臺，使用所提的組合法在三維空間中進行軌跡跟蹤實驗，實驗裝置見圖12，使用VC++6.0搭建的軟件環(huán)境。

圖12 實驗裝置Fig.12 Experimental device

首先在VC++6.0平臺上基于MFC框架編寫的程序，將求解的逆向運動學數(shù)據(jù)轉換成各關節(jié)對應的脈沖數(shù)量，寫入軟件程序中，通過GALIL運動控制卡提供的函數(shù)接口，建立上位機與機械臂之間的通信，然后進行機械臂末端位置標定，最后進行實驗。實驗過程中各關節(jié)運動平穩(wěn)，順利地實現(xiàn)了對給定空間軌跡的跟蹤，通過計算得到，跟蹤誤差變化曲線如圖13所示，誤差結果控制在10-7m數(shù)量級，實驗結果如圖14所示，機械臂末端完全跟蹤上了斜圓軌跡，結果表明了改進適應度函數(shù)組合法的有效性和準確性。

圖13 跟蹤誤差變化曲線Fig.13 Tracking error

圖14 實驗結果Fig.14 Experimental results

5 結論

(1)提出了一種智能算法的適應度函數(shù)，與解析法組合求解具有一般結構多自由度的機器人運動學逆解，使用分離-重構法建立適應度函數(shù)JDR，JDR具有簡單可靠的特點，使用改進適應度函數(shù)的組合法求逆解，求解速度更優(yōu)越，求解性能更穩(wěn)定，軌跡更平滑；在點對點運動求得的逆解中，組合法求解速度比原方法提升7倍左右；為驗證組合法在不同復雜環(huán)境中的有效性，分別在二維平面圓軌跡和三維圓錐螺旋軌跡進行仿真實驗，在兩種連續(xù)的三維軌跡跟蹤中，組合法求解速度比原方法分別提升4倍左右和2倍左右。

(2)在相同條件下，兩種方法求解三維連續(xù)軌跡的適應度函數(shù)值均在10-7數(shù)量級，但組合法求解得到的位置精度比原方法高出3個數(shù)量級，且組合法不需要設置罰函數(shù)；通過在REBot-V-6R型六自由度機器人實驗平臺上進行三維軌跡跟蹤實驗，驗證了改進適應度函數(shù)的組合法在求解一般結構六自由度機器人逆運動學中的有效性和準確性。