改進(jìn)適應(yīng)度函數(shù)的CMA－ES 算法在機(jī)器人逆運(yùn)動(dòng)學(xué)中的應(yīng)用

譚薪興， 李 光， 薛晨慷， 易 靜， 于權(quán)偉

（湖南工業(yè)大學(xué) 機(jī)械工程學(xué)院， 湖南 株洲 412007）

0 引 言

機(jī)器人運(yùn)動(dòng)學(xué)包括正向運(yùn)動(dòng)學(xué)和逆向運(yùn)動(dòng)學(xué)，正向運(yùn)動(dòng)學(xué)是由一組關(guān)節(jié)角度確定機(jī)器人末端執(zhí)行器一個(gè)姿態(tài)，逆向運(yùn)動(dòng)學(xué)是由目標(biāo)姿態(tài)求解各關(guān)節(jié)角度，逆向運(yùn)動(dòng)學(xué)求解復(fù)雜且存在多解。

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

近年來(lái)，利用智能算法求解機(jī)器人逆向運(yùn)動(dòng)學(xué)的方法取得一定的成果。 神經(jīng)網(wǎng)絡(luò)算法的計(jì)算代價(jià)高，計(jì)算能力很大程度取決于數(shù)據(jù)的大小，同時(shí)也取決于網(wǎng)絡(luò)的深度和復(fù)雜程度；遺傳算法全局搜索能力強(qiáng)，局部搜索能力較弱，容易誤入局部最優(yōu)解；蟻群算法和魚(yú)群算法參數(shù)設(shè)置復(fù)雜，如果參數(shù)設(shè)置不當(dāng)，容易偏離最優(yōu)解；粒子群算法操作簡(jiǎn)單，收斂速度快，在眾多領(lǐng)域得到廣泛應(yīng)用，但存在提前收斂、維數(shù)災(zāi)難等問(wèn)題。

自適應(yīng)協(xié)方差矩陣進(jìn)化策略（Covariance MatrixAdaptation Evolution Strategy， CMA－ES）是在進(jìn)化策略（Evolution Strategy ，ES）算法的基礎(chǔ)上發(fā)展而來(lái)的無(wú)約束優(yōu)化算法，在全局優(yōu)化、多峰優(yōu)化、多目標(biāo)優(yōu)化、大規(guī)模優(yōu)化和結(jié)構(gòu)性工程等領(lǐng)域得到了大量應(yīng)用，該算法的搜索性能較高，并且能夠達(dá)到比較高的搜索精度。 文獻(xiàn)［6］提出了改進(jìn)的CMA－ES 算法，在保留原算法的優(yōu)點(diǎn)上，可以實(shí)時(shí)且高精度地求出逆向運(yùn)動(dòng)學(xué)解；文獻(xiàn)［7］引入1／5 成功理論，調(diào)整CMA－ES 算法的規(guī)模和步長(zhǎng)，提高了算法求解效率；上述智能算法提高了機(jī)器人逆向運(yùn)動(dòng)學(xué)求解的效率，但是忽略了機(jī)器人在實(shí)際運(yùn)動(dòng)過(guò)程中的平穩(wěn)性和能耗。

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

1 CMA－ES 算法

1.1 算法原理

CMA－ES 算法是一種進(jìn)化策略類(lèi)算法。 經(jīng)典ES 算法尋找最優(yōu)解主要依靠突變，但是如何調(diào)整突變的方向沒(méi)有成熟的理論支撐，需要依據(jù)經(jīng)驗(yàn)設(shè)置，因此該方法會(huì)導(dǎo)致無(wú)效的突變。 為克服經(jīng)典ES 的不足，CMA－ES 采用正態(tài)分布（，） 生成一定數(shù)量的搜索種群，（，） 表示搜索分布均值為，以協(xié)方差矩陣為的多元正態(tài)分布，且，其中，的列向量由的特征向量正交基組成，為 對(duì)角矩陣，化簡(jiǎn)多元正態(tài)分布（，） 得式（1）。

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

由式（1）的逆序可知，確定等概率密度圓球面，通過(guò)、的變化可以確定的橢球面；球面的分布尺度由中的對(duì)角元素決定，主軸的方向由確定，變化、實(shí)現(xiàn)球面的分布旋轉(zhuǎn)。

1.2 算法步驟

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

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

突變。 突變的目的是使個(gè)體之間產(chǎn)生差異，CMA－ES 算法的突變過(guò)程是以當(dāng)代的均值為中心，產(chǎn)生下一代種群，式（2）。

選擇和重組。 經(jīng)典ES 算法的選擇策略有兩種：一種是（） 策略，適應(yīng)度排名由上一代的父代與當(dāng)前子代一起參與，同時(shí)選擇優(yōu)秀的個(gè)體；另一種是（，） 策略，即父代不參與當(dāng)前代的適應(yīng)度排名競(jìng)爭(zhēng)，優(yōu)秀個(gè)體只從當(dāng)前子代中選擇。 CMA －ES 采用的是第二種選擇策略，對(duì)個(gè)子代進(jìn)行適應(yīng)度評(píng)價(jià)，根據(jù)適應(yīng)能力由高到低排序，選取適應(yīng)能力強(qiáng)的前個(gè)個(gè)體，作為下一代種群的父代，更新策略參數(shù)、、，以此來(lái)傳遞優(yōu)良的基因。

（1） 均值移動(dòng)：下一代起始搜索點(diǎn)，即新的均值是從樣本，…，中選擇出來(lái)的個(gè)最優(yōu)子群的加權(quán)平均值，式（3） 和式（4）。

（2）協(xié)方差矩陣自適應(yīng)調(diào)整（CMA），式（5） ～式（9）。

其中，c是p的更新學(xué)習(xí)速率；h為赫維賽德階躍（）函數(shù)，用于調(diào)整‖p的過(guò)大增長(zhǎng)；μ為方差有效選擇質(zhì)量，且1 ＜μ≤，和c分別為的“秩1” 和“秩” 的更新學(xué)習(xí)速率。

（3）全局步長(zhǎng)控制（CSA），式（10）和式（11）。

其中，可看作步長(zhǎng)變化的伸縮因子；c為p的更新學(xué)習(xí)率；d為接近于1 的阻尼系數(shù)；‖（0，）‖為歸一化進(jìn)化路徑在隨機(jī)選擇下的期望長(zhǎng)度。

判斷是否達(dá)到最大迭代次數(shù)或精度，若是，則停止，輸出最優(yōu)解和最優(yōu)決策向量，否則返回Step 2。

2 逆運(yùn)動(dòng)學(xué)計(jì)算

2.1 正向運(yùn)動(dòng)學(xué)數(shù)學(xué)模型

采用標(biāo)準(zhǔn)D－H 法建立的REBot－V－6R－6500型六自由度機(jī)器人結(jié)構(gòu)簡(jiǎn)圖，如圖2 所示。 目前，普遍認(rèn)為D－H 法是對(duì)機(jī)器人運(yùn)動(dòng)學(xué)建模最好的方法，可以確定機(jī)器人的關(guān)節(jié)參數(shù)和關(guān)節(jié)變量，與機(jī)器人結(jié)構(gòu)順序和復(fù)雜程度無(wú)關(guān)，通過(guò)齊次變換矩陣描述兩個(gè)相鄰坐標(biāo)系間的空間位姿關(guān)系。

圖2 REBot－V－6R－6500 結(jié)構(gòu)簡(jiǎn)圖Fig.2 Structure diagram of REBot－V－6R－6500

首先，建立連桿坐標(biāo)系，通過(guò)4 次旋轉(zhuǎn)平移變換，根據(jù)DH 參數(shù)，第連桿的參數(shù)定義為： a為連桿長(zhǎng)度；α為相鄰兩關(guān)節(jié)軸線的夾角；d為第根連桿和第1 連桿的偏置距離；θ為第連桿的旋轉(zhuǎn)角。 變換過(guò)程：繞z 軸旋轉(zhuǎn)θ角，沿z 軸平移d，沿x軸平移a， 繞x軸旋轉(zhuǎn)α，可以得到機(jī)器人連桿坐標(biāo)系相對(duì)于連桿1 坐標(biāo)系的變換矩陣（矩陣）即式（12）， REBot－V－6R－6500 機(jī)器人關(guān)節(jié)參數(shù)，見(jiàn)表1。

表1 REBot－V－6R－6500 機(jī)器人關(guān)節(jié)參數(shù)Tab.1 Joint parameters of REBot－V－6R－6500 robot

根據(jù)式（12）和表（1）中的參數(shù)，得到該型號(hào)六自由度機(jī)器人正向運(yùn)動(dòng)學(xué)數(shù)學(xué)模型式（13），通過(guò)正向運(yùn)動(dòng)學(xué)控制機(jī)器人末端執(zhí)行器到達(dá)指定位置。

2.2 適應(yīng)度函數(shù)

用機(jī)器人末端執(zhí)行器的接近矢量和位置矢量建立適應(yīng)度函數(shù)和，式（14）。 適應(yīng)度函數(shù)為位置誤差范數(shù)，表示目標(biāo)位置P和實(shí)際位置P之間的誤差；適應(yīng)度函數(shù)為姿態(tài)誤差范數(shù)，表示目標(biāo)位置a和實(shí)際位置a之間的誤差；適應(yīng)度函數(shù)表示位置誤差范數(shù)和姿態(tài)誤差范數(shù)之和。 系數(shù)和是為方便調(diào)整求解過(guò)程中位置和姿態(tài)誤差而設(shè)置的權(quán)重，以便求得最合適的逆解，本文中1，1。

六自由度機(jī)器人逆運(yùn)動(dòng)學(xué)存在多解，為了獲得唯一的逆解，本文基于加權(quán)最小二乘法，結(jié)合“最佳柔順性”規(guī)則，通過(guò)附加約束建立目標(biāo)函數(shù)，使用CMA－ES 算法求解適應(yīng)度函數(shù)最小值，獲得最優(yōu)解，最佳柔順性的含義為式（15）：

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

3 仿真與結(jié)果分析

3.1 求解工作空間

機(jī)器人的關(guān)節(jié)角取值范圍參考該型號(hào)機(jī)器人說(shuō)明書(shū)，見(jiàn)表2。 求解機(jī)器人工作空間方法有：解析法、幾何法、數(shù)值法等，但這幾種計(jì)算復(fù)雜、效率低，本文在機(jī)器人各關(guān)節(jié)允許的角度值限制內(nèi)，以蒙特卡洛法為基礎(chǔ)，用隨機(jī)函數(shù)給各關(guān)節(jié)角賦值，得到10 000組關(guān)節(jié)角，計(jì)算機(jī)器人可達(dá)工作空間；利用正向運(yùn)動(dòng)學(xué)公式（13）解出末端執(zhí)行器在笛卡爾空間中離散點(diǎn)圖，從而得到機(jī)器人三維工作空間，如圖3所示。

表2 關(guān)節(jié)角取值范圍Tab.2 Range of joint angle

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

3.2 參數(shù)設(shè)置

CMA－ES 算法的種群數(shù)量100，優(yōu)秀個(gè)體50，初始步長(zhǎng)05，初始均值在0～1 內(nèi)隨機(jī)產(chǎn)生，求解過(guò)程中，前一個(gè)逆向運(yùn)動(dòng)學(xué)解作為當(dāng)前初始點(diǎn)均值，初始均值縮放系數(shù)0.65，算法的停止條件為進(jìn)化次數(shù)200 或者適應(yīng)度函數(shù)≤10。為了保證算法穩(wěn)定運(yùn)行，兩種算法的適應(yīng)度函數(shù)中，權(quán)值系數(shù)1，1，0001。

3.3 點(diǎn)到點(diǎn)運(yùn)動(dòng)仿真

仿真的計(jì)算機(jī)配置：操作系統(tǒng)為64 位Window 10 企業(yè)版，處理器為AMD Ryzen 5 2600X 3.60 GHz，內(nèi)存8.0 GB。

機(jī)器人初始狀態(tài)時(shí)選取各關(guān)節(jié)角度為［0 0 0 0 0 0］，目標(biāo)位置選取工作空間內(nèi)的一組關(guān)節(jié)角，根據(jù)正向運(yùn)動(dòng)學(xué)公式（13）求出末端執(zhí)行器的位姿矩陣， 分別使用適應(yīng)度函數(shù)和， 在CMA－ES 算法上獨(dú)立運(yùn)行1 000 次，求逆運(yùn)動(dòng)學(xué)的解。 隨機(jī)選取機(jī)器人的一組關(guān)節(jié)角度為［0.412 0.365 0.254 0.121 0.454 0.235］。

兩種適應(yīng)度函數(shù)的算法單獨(dú)運(yùn)行1 000 次的結(jié)果見(jiàn)表3，結(jié)果表明改進(jìn)后適應(yīng)度函數(shù)代表位置誤差的適應(yīng)度函數(shù)的平均值比原適應(yīng)度函數(shù)中的平均值小9 個(gè)數(shù)量級(jí)；任意選取1 000 組結(jié)果中的一組的位置誤差結(jié)果見(jiàn)表4，可以看出改進(jìn)適應(yīng)度函數(shù)的位置誤差值比未改進(jìn)的適應(yīng)度函數(shù)求得的位置精度高10數(shù)量級(jí)。

表3 CMA－ES 獨(dú)立運(yùn)行1 000 次的適應(yīng)度函數(shù)f1的值Tab.3 The value of the fitness function f1 for CMA－ES running independently for 1 000 times

表4 點(diǎn)對(duì)點(diǎn)運(yùn)動(dòng)位置誤差Tab.4 Position error of point－to－point motion

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

在工作空間內(nèi)的軌跡方程為式（16）：

其中，0≤≤2π，步長(zhǎng)為π／60（rad），長(zhǎng)度單位為m，為了滿(mǎn)足實(shí)際工作需求，保證末端執(zhí)行器關(guān)節(jié)軸在工作過(guò)程中始終平行于世界坐標(biāo)系軸正方向，運(yùn)動(dòng)軌跡的每一個(gè)點(diǎn)的矢量姿態(tài)均為［1；0；0］。

使用原適應(yīng)度函數(shù)和改進(jìn)適應(yīng)度函數(shù)對(duì)選取的空間內(nèi)軌跡方程（16）進(jìn)行連續(xù)軌跡跟蹤，適應(yīng)度函數(shù)和中代表位置誤差的適應(yīng)度函數(shù)的結(jié)果見(jiàn)表5，連續(xù)求解過(guò)程中適應(yīng)度函數(shù)值的波動(dòng)情況如圖4 所示。 仿真結(jié)果表明，改進(jìn)的適應(yīng)度函數(shù)中的平均值比原適應(yīng)度函數(shù)中的平均值少13個(gè)數(shù)量級(jí)，同時(shí)對(duì)軌跡上所有點(diǎn)進(jìn)行軌跡跟蹤，軌跡跟蹤以及軌跡仿真如圖5 所示，可以看到能完全實(shí)現(xiàn)對(duì)給定曲線的跟蹤，且運(yùn)動(dòng)過(guò)程平穩(wěn)。

表5 連續(xù)軌跡跟蹤的適應(yīng)度函數(shù)f1的值Tab.5 The value of the fitness function f1 for continuous trajectory tracking

圖4 適應(yīng)度函數(shù)f1的波動(dòng)Fig.4 Fluctuation of fitness function f1

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

兩種適應(yīng)度函數(shù)使用CMA－ES 算法求解軌跡的關(guān)節(jié)角度變化如圖6 所示。 結(jié)果表明，在保持原適應(yīng)度函數(shù)的特點(diǎn)上，基于加權(quán)最小二乘法“最佳柔順性”規(guī)則改進(jìn)的適應(yīng)度函數(shù)求解的結(jié)果唯一且更光滑，關(guān)節(jié)運(yùn)動(dòng)距離更小，能源消耗更少，運(yùn)行過(guò)程平穩(wěn)。

圖6 關(guān)節(jié)角變化Fig.6 Changes of joint angle

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

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

表7 改進(jìn)適應(yīng)度函數(shù)軌跡跟蹤的位置誤差Tab.7 Position error of improved fitness function for trajectory tracking

圖7 空間位置誤差波動(dòng)Fig.7 Fluctuation of spatial position error

4 結(jié)束語(yǔ)

本文提出改進(jìn)的適應(yīng)度函數(shù)，將加權(quán)最小二乘法的“最佳柔順性”規(guī)則和位姿誤差結(jié)合，形成改進(jìn)的適應(yīng)度函數(shù)，使用CMA－ES 算法求逆運(yùn)動(dòng)學(xué)解，使其解具有唯一性；在單點(diǎn)求得的逆解中， 改進(jìn)適應(yīng)度函數(shù)中代表位置誤差的適應(yīng)度函數(shù)的平均值比原適應(yīng)度函數(shù)中的平均值少10 個(gè)數(shù)量級(jí)；在連續(xù)的軌跡求解中，改進(jìn)適應(yīng)度函數(shù)中的平均值比原適應(yīng)度函數(shù)逆解的結(jié)果提升13 個(gè)數(shù)量級(jí)，且使用改進(jìn)適應(yīng)度函數(shù)求解得到的平均位置誤差值穩(wěn)定在10m。

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