劉洋

（四川大學(xué)計算機學(xué)院，成都 610065）

0 引言

已知機器人末端執(zhí)行器的位置姿態(tài)信息，求解機器人各關(guān)節(jié)角度值的過程稱為機器人的逆運動學(xué)求解。機器人逆運動學(xué)是機器人領(lǐng)域的基礎(chǔ)問題，在工程實踐中有著十分廣泛的應(yīng)用需求。

機器人逆運動學(xué)的求解方法很多，主要可以分為解析方法和數(shù)值方法兩類。由于解析方法[1]具有很大的局限性且對自由度較高的機器人效果不佳，目前主流的研究工作均集中在數(shù)值方法上。數(shù)值方法的本質(zhì)就是將機器人逆運動學(xué)問題轉(zhuǎn)換成一個優(yōu)化問題，再利用相關(guān)優(yōu)化方法進(jìn)行求解。目前應(yīng)用較多的數(shù)值方法主要分為神經(jīng)網(wǎng)絡(luò)和群體智能方法兩類。通過神經(jīng)網(wǎng)絡(luò)的方法[2]，可以很好地實現(xiàn)對機器人的逆運動學(xué)求解。但是神經(jīng)網(wǎng)絡(luò)精度的高低完全取決于所使用訓(xùn)練集質(zhì)量的好壞，而不同結(jié)構(gòu)的機器人之間無法共享同一訓(xùn)練集；因此必須針對特定的機器人結(jié)構(gòu)單獨提取高質(zhì)量的訓(xùn)練集，這無疑大幅增加了神經(jīng)網(wǎng)絡(luò)方法的使用成本。相比于神經(jīng)網(wǎng)絡(luò)方法，群體智能方法在通用性上有著巨大的優(yōu)勢。常用的群體智能方法，如粒子群算法[3]、螢火蟲算法[4]、遺傳算法[5]等，在機器人逆運動學(xué)求解中均取得了不錯的效果，實現(xiàn)了很高的位置精度。但在工程實踐中，很多應(yīng)用場景對姿態(tài)精度也有很高的要求，上述的單目標(biāo)優(yōu)化方法無法同時解決這兩方面的需求。

針對機器人逆運動學(xué)求解中對位置精度和姿態(tài)精度兩方面的需求，文章將其轉(zhuǎn)化為多目標(biāo)優(yōu)化問題，并通過多目標(biāo)粒子群算法加以實現(xiàn)。

1 機器人逆運動學(xué)

1.1 機器人運動學(xué)模型

以自由度為6的機器人為例，根據(jù)D-H方法[6]建立機器人各關(guān)節(jié)坐標(biāo)系Oxiyizi，如圖1所示。Ox0y0z0

表示世界坐標(biāo)系，Ox6y6z6表示工具坐標(biāo)系。如何構(gòu)建起兩個坐標(biāo)系之間的轉(zhuǎn)換關(guān)系正是機器人運動學(xué)研究的內(nèi)容。

圖1 機器人關(guān)節(jié)坐標(biāo)系

其中，p=[px,py,pz]T表示機器人工具坐標(biāo)系的原點在世界坐標(biāo)系中的坐標(biāo)，即機器人末端控制器的位置信息；[n,s,a]表示機器人工具坐標(biāo)系各坐標(biāo)軸單位向量在世界坐標(biāo)系中的坐標(biāo)，即機器人末端控制器的姿態(tài)信息，用歐拉角形式表示為

1.2 機器人逆運動學(xué)求解

相比于正運動學(xué)求解，機器人逆運動學(xué)求解過程無疑要困難很多。在已知機器人各關(guān)節(jié)角度值的情況下，可以利用公式（1）求解出末端執(zhí)行器的位置和姿態(tài)信息：

其中，p、e為機器人末端執(zhí)行器的位置和姿態(tài)信息；Fposition()、Forientation()表示運用機器人正運動學(xué)模型對機器人末端執(zhí)行器的位置和姿態(tài)信息進(jìn)行求解的過程。

利用公式（2），可以將機器人逆運動學(xué)表述為多目標(biāo)優(yōu)化問題：

其中，ptar、etar為機器人末端控制器的目標(biāo)位置和姿態(tài)；qtar為待求解的機器人關(guān)節(jié)轉(zhuǎn)角值；qinit為機器人關(guān)節(jié)轉(zhuǎn)角初始值為機器人關(guān)節(jié)轉(zhuǎn)角θi的限制范圍。

公式（3）表明逆運動學(xué)求解需要滿足三方面的目標(biāo)要求，即位置精度最高、姿態(tài)精度最高以及關(guān)節(jié)角度值變化最小。針對這一多目標(biāo)優(yōu)化問題，文章根據(jù)位置精度和姿態(tài)精度的目標(biāo)函數(shù)構(gòu)造相應(yīng)的適應(yīng)度函數(shù)并采用多目標(biāo)粒子群算法[7]進(jìn)行求解；在此基礎(chǔ)之上，利用關(guān)節(jié)角度值變化的目標(biāo)函數(shù)對所得的解集進(jìn)行多解優(yōu)化，從而完成機器人逆運動學(xué)的求解。

2 多目標(biāo)粒子群算法

2.1 基本定義

由于多個適應(yīng)度函數(shù)的存在，在多目標(biāo)粒子群算法中需要引入帕累托支配的概念：對于向量u=[u1,u2,…,un] 和v=[v1,v2,…,vn]，當(dāng)且僅當(dāng)u部分小于v時，u支配v。即：

當(dāng)兩組適應(yīng)度之間存在帕累托支配關(guān)系時，可以根據(jù)支配關(guān)系進(jìn)行比較評價優(yōu)劣。但很多時候兩組適應(yīng)度之間并不存在帕累托支配關(guān)系，無法比較評價優(yōu)劣，此時兩組解均應(yīng)當(dāng)進(jìn)行保留。因此，將所有相互之間沒有支配關(guān)系的解構(gòu)成一個集合，即帕累托最優(yōu)解集：

2.2 算法流程

多目標(biāo)粒子群算法中的基本搜索單位是粒子，具有位置和速度兩種屬性。在逆運動學(xué)求解過程中，每個粒子的位置都是一組關(guān)節(jié)角度值，表示逆運動學(xué)問題的一組可能解；每個粒子的速度表示該粒子在空間內(nèi)搜索時的方向和幅度，并隨著搜索的進(jìn)行不斷進(jìn)行調(diào)整。

針對逆運動學(xué)問題的多目標(biāo)粒子群優(yōu)化算法流程如圖2所示。

圖2 算法流程圖

粒子群初始化：粒子群初始化主要包括粒子位置和速度的初始化。粒子位置的初始化通過在各關(guān)節(jié)角度限制范圍內(nèi)隨機生成來實現(xiàn)。粒子速度的初始化通過在各關(guān)節(jié)搜索速度范圍內(nèi)隨機生成來實現(xiàn)。

適應(yīng)度計算：根據(jù)公式（2）計算粒子位置代表的關(guān)節(jié)角度值對應(yīng)的末端執(zhí)行器位置姿態(tài)。根據(jù)所得的末端執(zhí)行器位置姿態(tài)分別計算粒子對應(yīng)的多個適應(yīng)度函數(shù)的值，從而完成對粒子適應(yīng)度的計算。

歷史最優(yōu)解提取：包括帕累托最優(yōu)解集的更新、粒子的局部最優(yōu)解Pd的更新以及粒子群的全局最優(yōu)解Pg的更新。局部最優(yōu)解Pd的更新根據(jù)帕累托支配關(guān)系即可確定。帕累托最優(yōu)解集的更新也通過帕累托支配關(guān)系判定，同時要盡可能的保持解集中解的多樣性。全局最優(yōu)解Pg的更新則需要在帕累托最優(yōu)解集更新完成的基礎(chǔ)上進(jìn)行。

粒子信息更新：當(dāng)全局最優(yōu)解Pg不滿足精度要求并且迭代次數(shù)沒達(dá)到上限時，需要對粒子的位置和速度信息進(jìn)行更新，從而開始新一輪的迭代：

多解優(yōu)化：當(dāng)全局最優(yōu)解Pg滿足精度要求或者迭代次數(shù)達(dá)到上限時，算法停止迭代，需要進(jìn)一步對帕累托最優(yōu)解集進(jìn)行多解優(yōu)化，從帕累托最優(yōu)解集中選取出最適合的一個作為逆運動學(xué)問題的最優(yōu)解。

2.3 適應(yīng)度函數(shù)的構(gòu)造

根據(jù)公式（3）的描述，需要同時滿足位置、姿態(tài)精度最高和關(guān)節(jié)變化幅度最小三個要求。其中，關(guān)節(jié)變化幅度最小是在已經(jīng)實現(xiàn)位置、姿態(tài)精度最高基礎(chǔ)上的進(jìn)一步要求。因此，在帕累托最優(yōu)解集的優(yōu)化過程中，僅需要考慮位置精度和姿態(tài)精度的適應(yīng)度函數(shù)。

位置精度的適應(yīng)度函數(shù)為：

其中，ptar=[px_tar,py_tar,pz_tar]T為目標(biāo)位置信息；pcur=[px_cur,py_cur,pz_cur]T為通過公式（2）得到的關(guān)節(jié)轉(zhuǎn)角qcur對應(yīng)的實際位置信息。

姿態(tài)精度的適應(yīng)度函數(shù)為：

2.4 帕累托最優(yōu)解集的更新

隨著粒子的不斷搜索，帕勒托最優(yōu)解集也在不斷進(jìn)行更新。這一更新通過帕累托支配關(guān)系來實現(xiàn)。對于當(dāng)前解，若其與帕累托最優(yōu)解集中的解沒有支配或被支配的關(guān)系，則將其加入帕勒托最優(yōu)解集；若其與帕累托最優(yōu)解集中的解具有支配關(guān)系，則將其加入帕累托最優(yōu)解集并刪除解集中所有被其支配的解；若其與帕勒托最優(yōu)解集中的解具有被支配關(guān)系，則帕累托最優(yōu)解集保持不變。

隨著迭代次數(shù)的增加，帕累托最優(yōu)解集的規(guī)模也在不斷增大。這不僅會對存儲能力提出很高的要求，而且會嚴(yán)重影響算法的計算速度。因此，需要對集合中解的數(shù)量設(shè)定一個上限，并決定帕累托最優(yōu)解淘汰的策略。文章采用自適應(yīng)網(wǎng)格方法，將適應(yīng)度空間按網(wǎng)格等分為許多子空間，并將每個子空間內(nèi)的解的個數(shù)作為該子空間內(nèi)解的密度值。為了保持解集中解的多樣性，讓集合內(nèi)的解在空間中分布的更為均勻，選擇密度值最大的子空間內(nèi)的解隨機進(jìn)行淘汰。

2.5 歷史最優(yōu)解的提取

歷史最優(yōu)解是粒子群算法中的核心參數(shù)，包括每個粒子的局部最優(yōu)解Pd和整個粒子群的全局最優(yōu)解Pg。它們也需要隨著算法的迭代不斷進(jìn)行更新。

粒子的局部最優(yōu)解Pd的更新較為簡單，僅根據(jù)帕勒托支配關(guān)系即可確定。若當(dāng)前解支配上一輪迭代的局部最優(yōu)解，則將當(dāng)前解確定為局部最優(yōu)解；若當(dāng)前解被上一輪迭代的局部最優(yōu)解支配，則局部最優(yōu)解保持不變；若二者不存在支配關(guān)系，則隨機選擇一個作為局部最優(yōu)解。

粒子群的全局最優(yōu)解Pg的更新要從帕累托最優(yōu)解集中選取。粒子群算法每進(jìn)行一次迭代，均需要對帕累托最優(yōu)解集進(jìn)行更新。在此基礎(chǔ)之上，采用輪盤賭算法選擇一個子空間，子空間被選中的概率與其密度值成反比。選中子空間后，在子空間中隨機選擇一個解作為粒子群的全局最優(yōu)解Pg進(jìn)行更新。

2.6 多解優(yōu)化

經(jīng)過多目標(biāo)粒子群算法的搜索，可以得到機器人逆運動學(xué)問題的帕累托最優(yōu)解集。要從中選出唯一的一個作為問題的最優(yōu)解，需要進(jìn)行多解優(yōu)化。由于帕累托最優(yōu)解集中的解均具有較高的位置精度和姿態(tài)精度，在評價體系中添加對關(guān)節(jié)角度值變化的評價。

多解優(yōu)化的評價函數(shù)為：

其中，ftotal為多解優(yōu)化的評價函數(shù)。帕累托最優(yōu)解集中使得ftotal最小的解，即為機器人逆運動學(xué)問題的最優(yōu)解。

3 算法仿真

算法仿真采用的參數(shù)如下：粒子數(shù)100個、迭代次數(shù) 1000次、慣性權(quán)重w=0.5、加速常數(shù)c1=1.2、c2=1.2。

根據(jù)目標(biāo)位置姿態(tài)進(jìn)行仿真，結(jié)果如表2所示。通過多目標(biāo)粒子群算法的優(yōu)化，三組目標(biāo)位置姿態(tài)的逆運動學(xué)求解均得以實現(xiàn)，且具有很高的位置精度和姿態(tài)精度。第三組的實際關(guān)節(jié)角度與目標(biāo)關(guān)節(jié)角度差異很大，這主要是由于機器人逆運動學(xué)求解的不唯一性。相比于目標(biāo)關(guān)節(jié)角度，求解得到的實際關(guān)節(jié)角度的關(guān)節(jié)轉(zhuǎn)動幅度更小，更符合實際應(yīng)用的需求。

圖3以第一組目標(biāo)位置姿態(tài)的求解為例，展示了多目標(biāo)粒子群算法搜索的過程。圖中平面坐標(biāo)系的坐標(biāo)軸分別為粒子對應(yīng)的位置精度和姿態(tài)精度的適應(yīng)度。開始搜索時，由于粒子在搜索空間中隨機均勻的進(jìn)行搜索，粒子的適應(yīng)度分布的也較為均勻。隨著迭代次數(shù)的增加，粒子在適應(yīng)度空間內(nèi)逐漸向原點方向靠攏，粒子搜索的范圍越來越集中；此時的帕累托最優(yōu)解集的數(shù)量很大，這些帕累托最優(yōu)解共同構(gòu)成了帕累托前沿，成折線形分布。最終，當(dāng)?shù)瓿蓵r，進(jìn)行搜索的粒子匯聚在一起，幾乎不再運動；帕累托最優(yōu)解集的數(shù)量也大幅減少，且均滿足位置精度和姿態(tài)精度的要求。根據(jù)多解優(yōu)化，即可從中選出最優(yōu)解。

圖3 不同迭代次數(shù)的粒子適應(yīng)度分布圖

表1 目標(biāo)位置姿態(tài)信息表

表2 仿真結(jié)果表

4 結(jié)語

本文將機器人逆運動學(xué)問題轉(zhuǎn)化為多目標(biāo)優(yōu)化問題，并通過多目標(biāo)粒子群算法進(jìn)行了實現(xiàn)。在此基礎(chǔ)之上，通過計算機仿真對算法性能進(jìn)行了測試。仿真測試表明，多目標(biāo)粒子群算法在同時滿足目標(biāo)位置精度和姿態(tài)精度的情況下，實現(xiàn)了對機器人逆運動學(xué)問題的求解。但是多目標(biāo)粒子群算法對計算能力的要求很高，耗費的時間較長。要實現(xiàn)實時處理，必須采用并行計算且對GPU的性能要求較高。在未來的研究中，可以對算法進(jìn)行進(jìn)一步的改進(jìn)，以提高算法求解速度。