基于多目標(biāo)粒子群算法的機(jī)器人逆運(yùn)動(dòng)學(xué)求解方法

劉洋

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

0 引言

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

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

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

1 機(jī)器人逆運(yùn)動(dòng)學(xué)

1.1 機(jī)器人運(yùn)動(dòng)學(xué)模型

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

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

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

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

1.2 機(jī)器人逆運(yùn)動(dòng)學(xué)求解

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

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

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

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

公式（3）表明逆運(yùn)動(dòng)學(xué)求解需要滿足三方面的目標(biāo)要求，即位置精度最高、姿態(tài)精度最高以及關(guān)節(jié)角度值變化最小。針對(duì)這一多目標(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ù)對(duì)所得的解集進(jìn)行多解優(yōu)化，從而完成機(jī)器人逆運(yùn)動(dòng)學(xué)的求解。

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

2.1 基本定義

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

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

2.2 算法流程

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

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

圖2 算法流程圖

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

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

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

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

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

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

根據(jù)公式（3）的描述，需要同時(shí)滿足位置、姿態(tài)精度最高和關(guān)節(jié)變化幅度最小三個(gè)要求。其中，關(guān)節(jié)變化幅度最小是在已經(jīng)實(shí)現(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對(duì)應(yīng)的實(shí)際位置信息。

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

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

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

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

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

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

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

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

2.6 多解優(yōu)化

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

多解優(yōu)化的評(píng)價(jià)函數(shù)為：

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

3 算法仿真

算法仿真采用的參數(shù)如下：粒子數(shù)100個(gè)、迭代次數(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)的逆運(yùn)動(dòng)學(xué)求解均得以實(shí)現(xiàn)，且具有很高的位置精度和姿態(tài)精度。第三組的實(shí)際關(guān)節(jié)角度與目標(biāo)關(guān)節(jié)角度差異很大，這主要是由于機(jī)器人逆運(yùn)動(dòng)學(xué)求解的不唯一性。相比于目標(biāo)關(guān)節(jié)角度，求解得到的實(shí)際關(guān)節(jié)角度的關(guān)節(jié)轉(zhuǎn)動(dòng)幅度更小，更符合實(shí)際應(yīng)用的需求。

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

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

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

表2 仿真結(jié)果表

4 結(jié)語

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