差分粒子群算法在PUMA機(jī)器人逆運(yùn)動(dòng)學(xué)求解中的應(yīng)用

汪院林 袁銳波 袁安華

摘要：傳統(tǒng)逆運(yùn)動(dòng)學(xué)求解主要從逆運(yùn)動(dòng)學(xué)方程出發(fā)，基于一定的數(shù)學(xué)理論推導(dǎo)，不能完全實(shí)現(xiàn)計(jì)算機(jī)程序化，且精度與計(jì)算效率較低：為改善這一缺陷，基于機(jī)器人正向運(yùn)動(dòng)學(xué)方程，借助MATLAB工具，使用蒙特卡洛法仿真分析出PUMA560機(jī)器人的工作空間，任取一點(diǎn)末端執(zhí)行器位姿作為逆運(yùn)動(dòng)學(xué)求解的已知位姿矩陣T，結(jié)合差分粒子群仿生智能算法作為逆運(yùn)動(dòng)學(xué)求解的主要理論算法。將計(jì)算出的旋轉(zhuǎn)關(guān)節(jié)變量θ1-θ6代入正運(yùn)動(dòng)學(xué)方程，得出末端位姿矩陣于;通過計(jì)算分析T與于相關(guān)角度誤差，兩矩陣所對應(yīng)的位置向量與姿態(tài)向量誤差精度為0.001數(shù)量級，完全滿足目前機(jī)器人定位要求?；诓罘至Ｗ尤豪碚摰臋C(jī)器人逆運(yùn)動(dòng)學(xué)求解方法計(jì)算收斂速度更快，能高度實(shí)現(xiàn)計(jì)算機(jī)程序化，誤差精度高，提高計(jì)算效率。

關(guān)鍵詞：正運(yùn)動(dòng)學(xué);差分粒子群算法;收斂速度;位姿矩陣;逆運(yùn)動(dòng)學(xué)解

DOI： 10. 11907/rjdk.191724

開放科學(xué)（資源服務(wù)）標(biāo)識碼（OSID）：

中圖分類號：TP319

文獻(xiàn)標(biāo)識碼：A

文章編號：1672-7800（2020）004-0203-05

0 引言

近年來，機(jī)器人技術(shù)發(fā)展迅速，在工業(yè)等領(lǐng)域占據(jù)重要地位，機(jī)器人運(yùn)動(dòng)學(xué)研究也日益增多。機(jī)器人運(yùn)動(dòng)學(xué)主要由正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)兩部分組成：機(jī)器人正運(yùn)動(dòng)學(xué)主要以基坐標(biāo)為基礎(chǔ)，根據(jù)各關(guān)節(jié)旋轉(zhuǎn)角度確定末端執(zhí)行器的位姿;逆運(yùn)動(dòng)學(xué)與其相反，以末端關(guān)節(jié)為出發(fā)點(diǎn)最終回到基坐標(biāo)。正運(yùn)動(dòng)學(xué)由多個(gè)關(guān)節(jié)的位姿矩陣進(jìn)行連乘運(yùn)算，最終確定末端執(zhí)行器唯一的位姿矩陣，而逆運(yùn)動(dòng)學(xué)末端位姿可以對應(yīng)機(jī)器人系統(tǒng)關(guān)節(jié)的多個(gè)解[1]。機(jī)器人逆運(yùn)動(dòng)學(xué)問題隨著運(yùn)動(dòng)關(guān)節(jié)的增多而更加復(fù)雜，它實(shí)質(zhì)上是解一個(gè)非線性方程問題。實(shí)際應(yīng)用中，機(jī)器人所要執(zhí)行的任務(wù)通常用其基坐標(biāo)系描述[2]。因此，機(jī)器人逆運(yùn)動(dòng)學(xué)求解在控制領(lǐng)域顯得尤為重要。

目前，機(jī)器人逆運(yùn)動(dòng)方程求解方法主要有解析法、幾何法、迭代法、幾何一解析法[3]等。Paul等[4]對PUMA機(jī)器人提出了當(dāng)d6=0時(shí)逆運(yùn)動(dòng)的解析解;Lee等[5]提出應(yīng)用幾何學(xué)方法求解各關(guān)節(jié)變量;王奇志等[6]提出當(dāng)d6≠0時(shí)的一種解析解法;戴齊等[2]對PUMA機(jī)器人逆運(yùn)動(dòng)解利用幾何一解析法進(jìn)行研究?；趥鹘y(tǒng)理論方法雖然模型建立簡單，但計(jì)算效率低下，難于析出機(jī)器人逆運(yùn)動(dòng)學(xué)的所有解，且計(jì)算機(jī)程序的可移植性較差;吳振宇等[7]利用遺傳粒子群算法在解決非線性問題時(shí)的優(yōu)勢，對PUMA機(jī)器人的逆運(yùn)動(dòng)解提出遺傳粒子群算法理論，但該方法需要大量的樣本數(shù)據(jù)進(jìn)行訓(xùn)練，且對目標(biāo)函數(shù)進(jìn)行最優(yōu)搜索時(shí)收斂速度較慢，會(huì)陷入局部最優(yōu)狀態(tài)，存在一定不足。

本文提出一種新的逆運(yùn)動(dòng)求解方法：基于差分進(jìn)化算法（ Differential Evolution Algorithm.DE），該方法結(jié)構(gòu)簡單，收斂速度快，魯棒性較強(qiáng)，是與粒子群優(yōu)化算法（ ParticleSwarm Optimization，PSO）同時(shí)考慮位置與速度兩個(gè)參數(shù)相結(jié)合的一種混合智能算法[8]。從正向運(yùn)動(dòng)學(xué)方程出發(fā)，在各關(guān)節(jié)角變量范圍內(nèi)取出相應(yīng)角度值，建立PUMA機(jī)器人工作空間范圍模型。通過圖像方式驗(yàn)證三維空間尺寸是否與機(jī)器人結(jié)構(gòu)尺寸一致，圖形各投影平面中有沒有明顯的空洞現(xiàn)象?；谒心┒藞?zhí)行器位姿點(diǎn)，取其中某一點(diǎn)位姿，將差分粒子群混合算法作為PUMA機(jī)器人逆運(yùn)動(dòng)求解主要理論，彌補(bǔ)了傳統(tǒng)逆運(yùn)動(dòng)求解方法的不足，提高了計(jì)算效率和精度。

1 正運(yùn)動(dòng)學(xué)模型

1.1 D-H法坐標(biāo)系及幾何參數(shù)

PUMA型機(jī)器人有6個(gè)自由度，且6個(gè)關(guān)節(jié)均為旋轉(zhuǎn)類型，可采用標(biāo)準(zhǔn)D-H法建立桿件坐標(biāo)系描述各關(guān)節(jié)之間的特征參數(shù)和運(yùn)動(dòng)關(guān)系。該矩陣為4x4的齊次變換矩陣，通過該矩陣可得出與PUMA560機(jī)器人幾何尺寸相關(guān)的運(yùn)動(dòng)學(xué)方程。標(biāo)準(zhǔn)D-H法得到的相鄰桿件之間的齊次變換矩陣如下：

各連桿幾何參數(shù)如表1所示。

1.2 運(yùn)動(dòng)學(xué)方程

PUMA560機(jī)器人由多個(gè)旋轉(zhuǎn)關(guān)節(jié)組成，其中包含旋轉(zhuǎn)基座、大小臂、手腕等幾部分[9]。前3個(gè)關(guān)節(jié)用于對末端執(zhí)行器進(jìn)行定位，后3個(gè)關(guān)節(jié)主要用于保證末端執(zhí)行器的姿態(tài)。PUMA機(jī)器人運(yùn)動(dòng)學(xué)正解方程如下：

若給出關(guān)節(jié)旋轉(zhuǎn)角及各連桿幾何參數(shù)，通過式（2）可求出PUMA機(jī)器人末端執(zhí)行器位姿;若僅已知末端關(guān)節(jié)位姿，則需要確定中間各關(guān)節(jié)旋轉(zhuǎn)角，這需要進(jìn)行大量計(jì)算方可求出。

2 PUMA機(jī)器人工作空間仿真

2.1 蒙特卡洛法工作空間分析

目前，確定機(jī)器人工作空間的方法主要有數(shù)值法、解析法及圖解法。圖解法方法較簡單，但受關(guān)節(jié)數(shù)目限制，對多自由度機(jī)器人進(jìn)行工作空間分析時(shí)需采用分組處理[10];解析法主要采用微分幾何學(xué)原理，通過繪制函數(shù)解析式的多次包絡(luò)曲線確定工作空間，表示復(fù)雜，多用于三自由度以下機(jī)器人的工作空間分析[11];數(shù)值法多建立在極值理論及相關(guān)優(yōu)化理論之上，通常計(jì)算量龐大，用于圖形邊界為曲面且凹陷時(shí)，計(jì)算可靠性低[12]。本文提出一種建立于數(shù)值法基礎(chǔ)上且引進(jìn)隨機(jī)分布方法的蒙特卡洛法，它主要依托隨機(jī)抽樣的數(shù)學(xué)理論將工程問題轉(zhuǎn)化為數(shù)學(xué)模型，應(yīng)用范圍較廣。在工程中，主要用于一些隨機(jī)無規(guī)律性的物理問題，而在機(jī)器人領(lǐng)域多用來進(jìn)行誤差分析。然而，機(jī)器人工作空間的仿真計(jì)算也能通過蒙特卡洛法理論完成，可利用計(jì)算機(jī)輔助計(jì)算[13]。

蒙特卡洛法求解機(jī)器人工作空間，主要是在機(jī)器人各關(guān)節(jié)的旋轉(zhuǎn)變量范圍內(nèi)按一定步長隨機(jī)取值，然后把各旋轉(zhuǎn)關(guān)節(jié)的取值代入方程式（2）中，最終計(jì)算出自由端執(zhí)行器的空間坐標(biāo)值，而末端關(guān)節(jié)所有隨機(jī)值的集合則構(gòu)成機(jī)器人工作空間。將末端執(zhí)行器得到的坐標(biāo)點(diǎn)輸出到圖形設(shè)備上，可得出機(jī)器人工作空間的三維仿真圖形。

蒙特卡洛法主要步驟如下[14]：

（5）設(shè)定所需要輸出的點(diǎn)集數(shù)，通過循環(huán)方式重復(fù)步驟（3）、步驟（4），直到滿足條件為止。

上述算法可借助MATLAB軟件程序化完成，最終將末端位置空間點(diǎn)通過三維繪圖函數(shù)plot3輸出。利用蒙特卡洛法進(jìn)行機(jī)器人工作空間仿真步驟如圖2所示。

2.2 PUMA560機(jī)器人工作空間仿真分析

根據(jù)機(jī)器人運(yùn)動(dòng)學(xué)正解方程及蒙特卡洛法理論，將該算法編寫成MATLAB程序進(jìn)行仿真分析。PUMA560機(jī)器人各關(guān)節(jié)角度取值點(diǎn)為50 000個(gè)，最終輸出50 000個(gè)點(diǎn)作為三維工作空間，仿真結(jié)果如圖3-圖6所示。

從以上三維工作空間仿真圖及各平面投影圖分析可得出：仿真分析出的工作空間均未超出各關(guān)節(jié)變量旋轉(zhuǎn)范圍，且各圖形尺寸范圍與PUMA560機(jī)器人結(jié)構(gòu)尺寸相一致。從圖中看出各點(diǎn)集均勻，未出現(xiàn)空洞等現(xiàn)象。因此，該仿真結(jié)果與實(shí)際相符。

3 逆運(yùn)動(dòng)學(xué)求解

粒子群優(yōu)化算法（PSO）是來源于觀察鳥群覓食而誕生的一種仿生算法，廣泛應(yīng)用于非線性問題、多極值和不可微等場合的優(yōu)化，需要設(shè)置的參數(shù)較少[16]。PSO算法在搜索空間范圍內(nèi)尋找最優(yōu)解，主要與粒子適應(yīng)度和群體兩個(gè)參數(shù)有關(guān)，而粒子群的每一個(gè)體可表示某優(yōu)化問題的一個(gè)概率解。每個(gè)粒子主要由速度和位置兩大屬性決定。粒子適應(yīng)度是將粒子位置代入目標(biāo)函數(shù)得到的數(shù)值，粒子適應(yīng)度是判斷粒子優(yōu)劣的標(biāo)準(zhǔn)。在對問題進(jìn)行搜索迭代時(shí)，粒子主要依據(jù)兩個(gè)參數(shù)作出變換：①粒子受自身經(jīng)驗(yàn)影響而得到的最優(yōu)解，即個(gè)體歷史極值pBest;②粒子受全局影響而尋到的最優(yōu)解，即全局歷史極值gBest。用以下公式對粒子位置及速度進(jìn)行更新。

式中，v（t+1）、x（t+1）分別為下一時(shí)刻粒子的速度與位置;C1、c2為學(xué)習(xí)因子，代表個(gè)體經(jīng)驗(yàn)與群體經(jīng)驗(yàn)對粒子速度影響所占權(quán)重;rand（）為隨機(jī)數(shù)，范圍為[0，1];ω為慣性權(quán)重，是與時(shí)間有關(guān)的動(dòng)態(tài)調(diào)整參數(shù)。ω取值對算法影響至關(guān)重要，ω較大能提高全局搜索能力，但收斂速度很慢，較小則易陷入局部搜索。本文對ω的取值隨迭代次數(shù)從0.9線性遞減為0.4[17]，變化公式如下：

差分進(jìn)化算法（DE）是一種并行求解的進(jìn)化算法，與遺傳算法類似，用相同的算術(shù)操作（如：變異、交叉和選擇算子）對初始群體或父代群體進(jìn)行更新。DE首先進(jìn)行變異操作，在搜索范圍內(nèi)任意選取一個(gè)體作為基準(zhǔn)父代個(gè)體，并在父代個(gè)體外隨機(jī)選取兩個(gè)向量進(jìn)行差值計(jì)算，加權(quán)后與父代個(gè)體求和;然后經(jīng)交叉算子在父代個(gè)體與變異個(gè)體間進(jìn)行選擇，產(chǎn)生實(shí)驗(yàn)個(gè)體;最終結(jié)合目標(biāo)函數(shù)對父代個(gè)體和實(shí)驗(yàn)個(gè)體作選擇，生成新的子代個(gè)體[18]。

其中，F(xiàn)（x）為對應(yīng)問題的目標(biāo)函數(shù)值。

本文根據(jù)PUMA560機(jī)器人逆運(yùn)動(dòng)學(xué)方程建立數(shù)學(xué)模型（目標(biāo)函數(shù)）：機(jī)器人末端執(zhí)行器位姿矩陣為T，利用差分粒子群算法優(yōu)化得出第t次搜索時(shí)各關(guān)節(jié)變量所對應(yīng)的矩陣T，數(shù)學(xué)模型可表達(dá)為兩矩陣T與T之間的空間距離，使其為極小佰，計(jì)算如下：

本文結(jié)合DE與PSO各自的優(yōu)點(diǎn)，提出一種差分粒子群算法（DEPSO），用于解決PUMA560機(jī)器人運(yùn)動(dòng)學(xué)問題。該算法以PSO流程為主，通過DE算法增強(qiáng)了粒子群體的多樣性，可避免PSO算法陷入局部最優(yōu)解。DEPSO算法步驟如下：①在粒子群體范圍內(nèi)，初始化粒子位置與速度，設(shè)計(jì)數(shù)器t=0;②計(jì)算粒子適應(yīng)度，用于DE算法選擇粒子，計(jì)數(shù)器t=t+1;③粒子根據(jù)式（8）進(jìn)行DE變異操作并作選擇;④更新局部最優(yōu)解pBest與全局最優(yōu)解，根據(jù)式（5）.式（6）計(jì)算粒子位置與速度;⑤粒子根據(jù)式（9）進(jìn)行DE交叉計(jì)算;⑥計(jì)算粒子適應(yīng)度;⑦選擇新一代粒子;⑧更新局部最優(yōu)解和全局最優(yōu)解;⑨若計(jì)數(shù)器大于給定循環(huán)次數(shù)，則退出計(jì)算，否則，調(diào)至步驟②。

DEPSO算法基本流程如圖7所示。

4 實(shí)驗(yàn)結(jié)果與分析

PUMA560機(jī)器人各桿件參數(shù)如表1所示。根據(jù)蒙特卡洛法機(jī)器人正運(yùn)動(dòng)學(xué)求出所有末端關(guān)節(jié)位姿矩陣，任取其中一點(diǎn)作為逆運(yùn)動(dòng)學(xué)求解的末端位姿矩陣T，如下所示：

根據(jù)矩陣T與于，可從位置與姿態(tài)矩陣分析求解精度。實(shí)驗(yàn)?zāi)┒岁P(guān)節(jié)矩陣T對應(yīng)的位置坐標(biāo)為[ 600.227，622.914，164.221];關(guān)節(jié)變量θ1-θ6所對應(yīng)矩陣T的位置坐標(biāo)為[600.2283，622.9124，164.2217]，計(jì)算得出T與于之間的誤差精度為0.001mm數(shù)量級。

姿態(tài)矩陣所對應(yīng)角度為T和于各自的反余弦值[20]。對T與于進(jìn)行分析可得出：PUMA560機(jī)器人指定末端位姿矩陣法向向量各自的方向角[106.5334°，76.3895°，21.6705°]，所求關(guān)節(jié)角對應(yīng)位姿矩陣的法向向量方向角為[106.5313°，76.3855°，21.6714°];機(jī)器人指定末端位姿矩陣滑動(dòng)向量方向角為[61.0559°，143.2778°，69.4417°]，所求關(guān)節(jié)角對應(yīng)的位姿矩陣滑動(dòng)向量方向角為[61.0585°，143.2788°，69.4397°];PUMA560機(jī)器人指定末端位姿矩陣所對應(yīng)的接近向量方向角各自為[34.1543°，56.6547°，83.4417°]，所求關(guān)節(jié)角對應(yīng)位姿矩陣的接近向量方向角各自為[34.1507°，56.6577°，83.4445°】。通過以上計(jì)算可看出，利用差分粒子群算法計(jì)算PUMA560機(jī)器人得出的關(guān)節(jié)變量所對應(yīng)的末端姿態(tài)矩陣與指定末端姿態(tài)矩陣的誤差精度在0.001°數(shù)量級，由此可知，將本文的差分粒子群算法用于求解PUMA560機(jī)器人逆運(yùn)動(dòng)學(xué)求解完全滿足定位精度要求。

5 結(jié)語

本文從正運(yùn)動(dòng)學(xué)方程出發(fā)，根據(jù)蒙特卡洛法求解出PUMA560機(jī)器人工作空間的末端關(guān)節(jié)位姿矩陣，任取一點(diǎn)用于逆運(yùn)動(dòng)學(xué)求解過程。結(jié)合差分優(yōu)化算法與粒子群算法優(yōu)點(diǎn)，提出一種差分粒子群混合智能算法，將其作為逆運(yùn)動(dòng)學(xué)求解方法。該方法與其它傳統(tǒng)逆運(yùn)動(dòng)學(xué)求解方法相比較，更適合于計(jì)算機(jī)計(jì)算，具有定位精度高、收斂速度快、計(jì)算高效等優(yōu)點(diǎn)。通過實(shí)例驗(yàn)證可知，該方法用于機(jī)器人逆運(yùn)動(dòng)求解問題滿足當(dāng)前機(jī)器人定位誤差要求，是智能算法與逆運(yùn)動(dòng)數(shù)值法相結(jié)合的新理論。

參考文獻(xiàn)：

[1]JOHN J，CRAIG.機(jī)器人學(xué)導(dǎo)論[M].以，贠超，譯.北京：機(jī)械工業(yè)出版社.2006.

[2] 戴齊，姚先啟.一種求解PUMA機(jī)械手運(yùn)動(dòng)學(xué)逆問題的分解法[J].西南交通大學(xué)學(xué)報(bào)，1989（4）：30-35.

[3] 陳寧，焦恩璋.PUMA機(jī)械手逆運(yùn)動(dòng)方程求解新方法[J].南京林業(yè)大學(xué)學(xué)報(bào)（自然科學(xué)版），2003，27（4）：23-26.

[4]PAUL R P.Robot manipulators： mathematics， programming and con-trol： the computer control of robot manipulators[M].Cambridge： MITPress.1981.

[5] FU K S，CONZALEZ R C，LEE C S G.Robotics： control， sensing，vision and intelligence [J]. Me Graw-Hill. 1987（6）：78-82.

[6] 王奇志，徐心和，尹朝萬.PUMA機(jī)械手逆運(yùn)動(dòng)方程新的推導(dǎo)方法及求解[J].機(jī)器人，1998，20（2）：81-87.

[7]吳振宇，姚明江，馮林，等.基于遺傳粒子群的PUMA機(jī)器人逆運(yùn)動(dòng)求解[J].計(jì)算機(jī)仿真，2011，28（ 10）：173-176.

[8]池元成，方杰，蔡國飆.基于差分進(jìn)化和粒子群優(yōu)化算法的混合優(yōu)化算法[J].計(jì)算機(jī)工程與設(shè)計(jì)，2009.30（ 12）：641-647.

[9] 布魯諾·西西里安諾.機(jī)器人學(xué)：建模、規(guī)劃與控制[M].張國良，譯.西安：西安交通大學(xué)出版社，2013.

[10]蔡蒂，謝存禧，張鐵，等.基于蒙特卡洛法的噴涂機(jī)器人工作空間分析及仿真[J].機(jī)械設(shè)計(jì)與制造，2009（3）：161-162.

[11] KOHLI D. HSU M S.The jacobian analysis of workspaces of mechan-ical manipulators[J]. Mechanism& Machine Theory， 1987， 22（3）：265-275.

[12]趙燕江，張永德，姜金剛.基于Matlah的機(jī)器人工作空間求解方法[J].機(jī)械科學(xué)與技術(shù)，2009，28（12）：1657-1661.

[13] RASTEGAR J，PEREL D.Ceneration of manipulator workspaceboundary geometry using the monte carlo method and interactive com-puter graphics[J]. Journal of mechanical design， 1990， 112 （3）：452-454.

[14] 郭明，周國斌.多關(guān)節(jié)機(jī)器人工作空間的分析與評價(jià)方法[J].機(jī)器人，1988，10 （4）：7-12.

[15] 沙漠，鄧子龍.基于MATLAB對于PUMA560機(jī)器人的運(yùn)動(dòng)空間分析研究[J].機(jī)械制造與自動(dòng)化，2016，45（2）：156-159.

[16] KAO Y T，ZAHARA E.A hybrid genetic algorithm and particleswarm optimization for multimodal functions[J]. Applied Soft Com-puting Journal， 2008.8 （2）： 849-857.

[17] 王志，胡小兵，何雪海.一種新的差分與粒子群算法的混合算法[J].計(jì)算機(jī)工程與應(yīng)用，2012，48（6）：46-48.

[18] 欒麗君，譚立靜，牛奔.一種基于粒子群優(yōu)化算法和差分進(jìn)化算法的新型混合全局優(yōu)化算法[J].信息與控制，2007，36（6）： 708-714.

[19]謝宏，禹文科，楊鵬，等，多子群分層差分粒子群算法的逆運(yùn)動(dòng)求解方法[J].電子測量與儀器學(xué)報(bào)，2015，29（10）：95-99.

[20] 芮梃.PUMA機(jī)器人逆運(yùn)動(dòng)模擬退火粒子群求解方法[J].計(jì)算機(jī)工程與應(yīng)用，2010，46（3）：27-29.

（責(zé)任編輯：杜能鋼）

作者簡介：汪院林（1994-），男，昆明理工大學(xué)機(jī)電工程學(xué)院碩士研究生，研究方向?yàn)榱黧w傳動(dòng)與控制;袁銳波（1968-），男，博士，昆明理工大學(xué)機(jī)電工程學(xué)院教授，研究方向?yàn)殡娨罕壤欧刂?、?fù)雜機(jī)電系統(tǒng)集成與控制;袁安華（1991-），男，昆明理工大學(xué)機(jī)電工程學(xué)院碩士研究生，研究方向?yàn)榱黧w傳動(dòng)與控制。本文通訊作者：袁銳波。