改進適應度函數的CMA－ES 算法在機器人逆運動學中的應用

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

（湖南工業大學 機械工程學院， 湖南 株洲 412007）

0 引 言

機器人運動學包括正向運動學和逆向運動學，正向運動學是由一組關節角度確定機器人末端執行器一個姿態，逆向運動學是由目標姿態求解各關節角度，逆向運動學求解復雜且存在多解。

機器人逆向運動學求解是進行機器人軌跡規劃、路徑規劃、避障等研究的基礎。 很多學者在該領域做了大量研究，有許多逆向運動學的求解理論和方法，傳統的方法有代數法、幾何法、數值法等。代數法主要以消元的方式將高維方程組轉化為低維方程組，還需要大量的三角代換，計算過程十分復雜，文獻［1］提出基于分離重構技術，降低了求解復雜度，得到了逆運動學全部解；幾何法針對機器人的特殊結構進行簡化再求解，一般無法單獨使用甚至無法使用；數值法可以精確搜索到一組逆解，求解速度與初始值相關，甚至存在無解，需要的運算量大，不適合實時性要求較高的場合。

近年來，利用智能算法求解機器人逆向運動學的方法取得一定的成果。 神經網絡算法的計算代價高，計算能力很大程度取決于數據的大小，同時也取決于網絡的深度和復雜程度；遺傳算法全局搜索能力強，局部搜索能力較弱，容易誤入局部最優解；蟻群算法和魚群算法參數設置復雜，如果參數設置不當，容易偏離最優解；粒子群算法操作簡單，收斂速度快，在眾多領域得到廣泛應用，但存在提前收斂、維數災難等問題。

自適應協方差矩陣進化策略（Covariance MatrixAdaptation Evolution Strategy， CMA－ES）是在進化策略（Evolution Strategy ，ES）算法的基礎上發展而來的無約束優化算法，在全局優化、多峰優化、多目標優化、大規模優化和結構性工程等領域得到了大量應用，該算法的搜索性能較高，并且能夠達到比較高的搜索精度。 文獻［6］提出了改進的CMA－ES 算法，在保留原算法的優點上，可以實時且高精度地求出逆向運動學解；文獻［7］引入1／5 成功理論，調整CMA－ES 算法的規模和步長，提高了算法求解效率；上述智能算法提高了機器人逆向運動學求解的效率，但是忽略了機器人在實際運動過程中的平穩性和能耗。

本文提出了一種新的適應度函數用于CMA－ES算法求逆運動學解：與原算法相比，該算法將各關節運動范圍作為約束條件，通過加權最小二乘法和位姿誤差建立新的適應度函數，求解出唯一的一組運動學逆解。 將該算法應用于REBot－V－6R－6500 型六自由度機器人，仿真結果表明，該方法可以得到機器人高精度和平穩的逆運動學解。

1 CMA－ES 算法

1.1 算法原理

CMA－ES 算法是一種進化策略類算法。 經典ES 算法尋找最優解主要依靠突變，但是如何調整突變的方向沒有成熟的理論支撐，需要依據經驗設置，因此該方法會導致無效的突變。 為克服經典ES 的不足，CMA－ES 采用正態分布（，） 生成一定數量的搜索種群，（，） 表示搜索分布均值為，以協方差矩陣為的多元正態分布，且，其中，的列向量由的特征向量正交基組成，為 對角矩陣，化簡多元正態分布（，） 得式（1）。

其中，“～”表示服從相同的分布。

由式（1）的逆序可知，確定等概率密度圓球面，通過、的變化可以確定的橢球面；球面的分布尺度由中的對角元素決定，主軸的方向由確定，變化、實現球面的分布旋轉。

1.2 算法步驟

CAM－ES 算法流程如圖1 所示。

圖1 CAM－ES 算法流程圖Fig.1 CAM－ES algorithm flow chart

突變。 突變的目的是使個體之間產生差異，CMA－ES 算法的突變過程是以當代的均值為中心，產生下一代種群，式（2）。

選擇和重組。 經典ES 算法的選擇策略有兩種：一種是（） 策略，適應度排名由上一代的父代與當前子代一起參與，同時選擇優秀的個體；另一種是（，） 策略，即父代不參與當前代的適應度排名競爭，優秀個體只從當前子代中選擇。 CMA －ES 采用的是第二種選擇策略，對個子代進行適應度評價，根據適應能力由高到低排序，選取適應能力強的前個個體，作為下一代種群的父代，更新策略參數、、，以此來傳遞優良的基因。

（1） 均值移動：下一代起始搜索點，即新的均值是從樣本，…，中選擇出來的個最優子群的加權平均值，式（3） 和式（4）。

（2）協方差矩陣自適應調整（CMA），式（5） ～式（9）。

其中，c是p的更新學習速率；h為赫維賽德階躍（）函數，用于調整‖p的過大增長；μ為方差有效選擇質量，且1 ＜μ≤，和c分別為的“秩1” 和“秩” 的更新學習速率。

（3）全局步長控制（CSA），式（10）和式（11）。

其中，可看作步長變化的伸縮因子；c為p的更新學習率；d為接近于1 的阻尼系數；‖（0，）‖為歸一化進化路徑在隨機選擇下的期望長度。

判斷是否達到最大迭代次數或精度，若是，則停止，輸出最優解和最優決策向量，否則返回Step 2。

2 逆運動學計算

2.1 正向運動學數學模型

采用標準D－H 法建立的REBot－V－6R－6500型六自由度機器人結構簡圖，如圖2 所示。 目前，普遍認為D－H 法是對機器人運動學建模最好的方法，可以確定機器人的關節參數和關節變量，與機器人結構順序和復雜程度無關，通過齊次變換矩陣描述兩個相鄰坐標系間的空間位姿關系。

圖2 REBot－V－6R－6500 結構簡圖Fig.2 Structure diagram of REBot－V－6R－6500

首先，建立連桿坐標系，通過4 次旋轉平移變換，根據DH 參數，第連桿的參數定義為： a為連桿長度；α為相鄰兩關節軸線的夾角；d為第根連桿和第1 連桿的偏置距離；θ為第連桿的旋轉角。 變換過程：繞z 軸旋轉θ角，沿z 軸平移d，沿x軸平移a， 繞x軸旋轉α，可以得到機器人連桿坐標系相對于連桿1 坐標系的變換矩陣（矩陣）即式（12）， REBot－V－6R－6500 機器人關節參數，見表1。

表1 REBot－V－6R－6500 機器人關節參數Tab.1 Joint parameters of REBot－V－6R－6500 robot

根據式（12）和表（1）中的參數，得到該型號六自由度機器人正向運動學數學模型式（13），通過正向運動學控制機器人末端執行器到達指定位置。

2.2 適應度函數

用機器人末端執行器的接近矢量和位置矢量建立適應度函數和，式（14）。 適應度函數為位置誤差范數，表示目標位置P和實際位置P之間的誤差；適應度函數為姿態誤差范數，表示目標位置a和實際位置a之間的誤差；適應度函數表示位置誤差范數和姿態誤差范數之和。 系數和是為方便調整求解過程中位置和姿態誤差而設置的權重，以便求得最合適的逆解，本文中1，1。

六自由度機器人逆運動學存在多解，為了獲得唯一的逆解，本文基于加權最小二乘法，結合“最佳柔順性”規則，通過附加約束建立目標函數，使用CMA－ES 算法求解適應度函數最小值，獲得最優解，最佳柔順性的含義為式（15）：

其中，θ（） －θ（1） 表示關節角與其自身前一個位置關節角的差值，使得關節運動更平滑，減少機器人運動路徑距離；系數是為了保證算法運行時不產生局部收斂甚至發散等情況，平衡“最佳柔順性”準則對整個適應度函數優化結果的影響。 本文中0.001；系數w遵循“少運動上臂，多運動下臂”的選取原則。 該原則的優點是運動時更為平穩，減少能源消耗，提高工作效率等。 在本研究中，加權系數w設計為［3 2 1.5 1 1 1］。

3 仿真與結果分析

3.1 求解工作空間

機器人的關節角取值范圍參考該型號機器人說明書，見表2。 求解機器人工作空間方法有：解析法、幾何法、數值法等，但這幾種計算復雜、效率低，本文在機器人各關節允許的角度值限制內，以蒙特卡洛法為基礎，用隨機函數給各關節角賦值，得到10 000組關節角，計算機器人可達工作空間；利用正向運動學公式（13）解出末端執行器在笛卡爾空間中離散點圖，從而得到機器人三維工作空間，如圖3所示。

表2 關節角取值范圍Tab.2 Range of joint angle

圖3 三維工作空間Fig.3 Three－dimensional workspace

3.2 參數設置

CMA－ES 算法的種群數量100，優秀個體50，初始步長05，初始均值在0～1 內隨機產生，求解過程中，前一個逆向運動學解作為當前初始點均值，初始均值縮放系數0.65，算法的停止條件為進化次數200 或者適應度函數≤10。為了保證算法穩定運行，兩種算法的適應度函數中，權值系數1，1，0001。

3.3 點到點運動仿真

仿真的計算機配置：操作系統為64 位Window 10 企業版，處理器為AMD Ryzen 5 2600X 3.60 GHz，內存8.0 GB。

機器人初始狀態時選取各關節角度為［0 0 0 0 0 0］，目標位置選取工作空間內的一組關節角，根據正向運動學公式（13）求出末端執行器的位姿矩陣， 分別使用適應度函數和， 在CMA－ES 算法上獨立運行1 000 次，求逆運動學的解。 隨機選取機器人的一組關節角度為［0.412 0.365 0.254 0.121 0.454 0.235］。

兩種適應度函數的算法單獨運行1 000 次的結果見表3，結果表明改進后適應度函數代表位置誤差的適應度函數的平均值比原適應度函數中的平均值小9 個數量級；任意選取1 000 組結果中的一組的位置誤差結果見表4，可以看出改進適應度函數的位置誤差值比未改進的適應度函數求得的位置精度高10數量級。

表3 CMA－ES 獨立運行1 000 次的適應度函數f1的值Tab.3 The value of the fitness function f1 for CMA－ES running independently for 1 000 times

表4 點對點運動位置誤差Tab.4 Position error of point－to－point motion

3.4 連續軌跡跟蹤仿真

在工作空間內的軌跡方程為式（16）：

其中，0≤≤2π，步長為π／60（rad），長度單位為m，為了滿足實際工作需求，保證末端執行器關節軸在工作過程中始終平行于世界坐標系軸正方向，運動軌跡的每一個點的矢量姿態均為［1；0；0］。

使用原適應度函數和改進適應度函數對選取的空間內軌跡方程（16）進行連續軌跡跟蹤，適應度函數和中代表位置誤差的適應度函數的結果見表5，連續求解過程中適應度函數值的波動情況如圖4 所示。 仿真結果表明，改進的適應度函數中的平均值比原適應度函數中的平均值少13個數量級，同時對軌跡上所有點進行軌跡跟蹤，軌跡跟蹤以及軌跡仿真如圖5 所示，可以看到能完全實現對給定曲線的跟蹤，且運動過程平穩。

表5 連續軌跡跟蹤的適應度函數f1的值Tab.5 The value of the fitness function f1 for continuous trajectory tracking

圖4 適應度函數f1的波動Fig.4 Fluctuation of fitness function f1

圖5 軌跡仿真與跟蹤Fig.5 Trajectory simulation and tracking

兩種適應度函數使用CMA－ES 算法求解軌跡的關節角度變化如圖6 所示。 結果表明，在保持原適應度函數的特點上，基于加權最小二乘法“最佳柔順性”規則改進的適應度函數求解的結果唯一且更光滑，關節運動距離更小，能源消耗更少，運行過程平穩。

圖6 關節角變化Fig.6 Changes of joint angle

在兩種適應度函數求解下，笛卡爾空間中目標位置（，，） 和仿真位置（，，） 之間的誤差變化，如表6、表7 和圖7 表示。 可以看出，兩者求解的位置精度相差較大，采用新適應度函數求解得到的平均位置誤差值穩定在10m 數量級，最小值誤差為0 m，且代表位置誤差穩定度的標準差值也穩定在10m 數量級，在軌跡跟蹤精度和軌跡跟蹤穩定度方面明顯優于使用原適應度函數解得的結果。

表6 原適應度函數軌跡跟蹤的位置誤差Tab.6 Position error of original fitness function for trajectory tracking

表7 改進適應度函數軌跡跟蹤的位置誤差Tab.7 Position error of improved fitness function for trajectory tracking

圖7 空間位置誤差波動Fig.7 Fluctuation of spatial position error

4 結束語

本文提出改進的適應度函數，將加權最小二乘法的“最佳柔順性”規則和位姿誤差結合，形成改進的適應度函數，使用CMA－ES 算法求逆運動學解，使其解具有唯一性；在單點求得的逆解中， 改進適應度函數中代表位置誤差的適應度函數的平均值比原適應度函數中的平均值少10 個數量級；在連續的軌跡求解中，改進適應度函數中的平均值比原適應度函數逆解的結果提升13 個數量級，且使用改進適應度函數求解得到的平均位置誤差值穩定在10m。

基于CMA－ES 算法將機器人關節角范圍作為算法約束條件，改進的適應度函數求解精度更高，且解出的各關節角位移平滑，路徑更短，遵循“多運動下臂，少運動上臂” 的原則，使得機器人能耗最少，提高了工業機器人工作效率。