開(kāi)鏈?zhǔn)蕉噙B桿機(jī)構(gòu)的逆運(yùn)動(dòng)學(xué)求解算法

康凱 劉婷婷 王二建 李紅艷 朱國(guó)權(quán) 周建新

摘要：在D-H坐標(biāo)系下，建立普適的開(kāi)鏈?zhǔn)蕉噙B桿機(jī)構(gòu)的正運(yùn)動(dòng)學(xué)模型較易，但建立通用的逆運(yùn)動(dòng)學(xué)模型卻相當(dāng)困難，并且其復(fù)雜程度隨著關(guān)節(jié)個(gè)數(shù)的增多而顯著增大。為此，結(jié)合粒子群優(yōu)化算法（PSO），通過(guò)迭代的方式求解逆運(yùn)動(dòng)學(xué)問(wèn)題。針對(duì)PSO算法求解時(shí)的精度不高、易陷入局部極值的缺點(diǎn)，提出局部搜索策略，提高解的精度，并采用逃逸策略保證種群多樣性，使算法在全局中搜索。經(jīng)驗(yàn)證，改進(jìn)后的PSO算法是一種精確、有效的求解逆運(yùn)動(dòng)學(xué)的算法。

關(guān)鍵詞：逆運(yùn)動(dòng)學(xué);粒子群優(yōu)化算法;局部搜索;逃逸策略

中圖分類(lèi)號(hào)：G242? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2019）23-0209-04

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

1 引言

開(kāi)鏈?zhǔn)蕉噙B桿機(jī)構(gòu)的逆運(yùn)動(dòng)學(xué)是指在給定末端操作器位姿的條件下求解各個(gè)關(guān)節(jié)變量的取值。逆運(yùn)動(dòng)學(xué)作為機(jī)器人軌跡規(guī)劃和軌跡控制的基礎(chǔ)，是機(jī)器人學(xué)的關(guān)鍵部分。求解逆運(yùn)動(dòng)學(xué)問(wèn)題常采用封閉解法，其特點(diǎn)是編程實(shí)現(xiàn)簡(jiǎn)單，計(jì)算速度快，物理意義明確，并且可以找到所有可能的解[1]。但其在很大程度上受到機(jī)械結(jié)構(gòu)的限制，需要針對(duì)不同類(lèi)型結(jié)構(gòu)建立不同的模型。

粒子群優(yōu)化（Particle Swarm Optimization， PSO）是一種模擬鳥(niǎo)類(lèi)覓食行為的全局優(yōu)化算法，由Kennedy和Eberhart于1995年提出[2]。基于PSO算法的逆運(yùn)動(dòng)學(xué)求解，意在關(guān)節(jié)變量空間中搜索出最優(yōu)解。搜索過(guò)程僅依賴(lài)于正運(yùn)動(dòng)學(xué)，而正運(yùn)動(dòng)學(xué)的計(jì)算是不受機(jī)構(gòu)的結(jié)構(gòu)限制。因此，PSO算法在求解逆運(yùn)動(dòng)學(xué)問(wèn)題上體現(xiàn)出較高的普適性[3]。

但是，PSO算法的結(jié)果不能達(dá)到理想的精度，需對(duì)每一代的最優(yōu)解增添一項(xiàng)局部搜索。此外，PSO算法對(duì)多峰值復(fù)雜函數(shù)搜索的效果不佳，易陷入局部最優(yōu)[4]。可引入逃逸策略后，增強(qiáng)了種群的多樣性，及時(shí)地跳出局部最優(yōu)。

2 問(wèn)題描述

逆運(yùn)動(dòng)學(xué)問(wèn)題的求解可轉(zhuǎn)化成式（1）的方式進(jìn)行描述：

[minfθθ∈SS=xlk≤θk≤uk，θ∈Rn，lk∈R，uk∈R，k=1，2，3，...，n] （1）

其中[S]是[n]維空間中的一個(gè)非空集合，[n]代表關(guān)節(jié)變量的個(gè)數(shù)。函數(shù)[fθ]是定義在[S]上的目標(biāo)函數(shù)。

如圖1所示，在笛卡爾坐標(biāo)系中，末端操作器位姿矩陣[W]表示為：

[W=NxOxAxPxNyOyAyPyNzOzAzPz0001=NOAP0001]? ? ? ? ? ? （2）

其中，[NOA]表示姿態(tài)，[P]表示位置，目標(biāo)位姿矩陣設(shè)為[W?]，則位置誤差為：

[εP=Pθ-P?Pθ-P?T]? ? ? ? ? ? ? ? ? ?（3）

姿態(tài)誤差為：

[εN=NθN?+OθO?+AθA?-12]? ? ? ? ? ? ? （4）

目標(biāo)函數(shù)寫(xiě)成：

[fθ=εP+εN]? ? ? ? ? ? ? ? ? ? ? ? ? （5）

[Pθ]、[Nθ]、[Oθ]、[Aθ]由正運(yùn)動(dòng)學(xué)方程求得。

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

3.1 基本粒子群算法

PSO算法源于對(duì)鳥(niǎo)群覓食行為的模擬，鳥(niǎo)群中的個(gè)體通過(guò)共享信息保持鳥(niǎo)群飛行的一致性。個(gè)體間的信息共享機(jī)制確保群體的運(yùn)動(dòng)產(chǎn)生從無(wú)序到有序的演變，這是PSO算法的基礎(chǔ)[4]。

在PSO算法中，每個(gè)粒子代表鳥(niǎo)群中的個(gè)體，其包含位置和速度兩種信息。每個(gè)粒子的位置代表著優(yōu)化問(wèn)題的一個(gè)解。粒子位置坐標(biāo)對(duì)應(yīng)目標(biāo)函數(shù)的值作為該粒子的適應(yīng)度，其決定粒子性能的優(yōu)劣。每個(gè)粒子的速度控制其飛行的方向和距離。在每次迭代中，粒子將當(dāng)前位置與自身發(fā)現(xiàn)的最好位置（[pbest]）和所有粒子發(fā)現(xiàn)的最好位置（[gbest]）做比較，更新自身的位置和速度[5]。

在逆運(yùn)動(dòng)學(xué)問(wèn)題中，設(shè)搜索空間[S]是[N]維，粒子的群體規(guī)模為[M]，第[i]（[i=1，2，...，M]）個(gè)粒子的位置為[θi=θi1，θi2，...，θiN]，其經(jīng)歷最好的位置為[pbesti=pbesti1，pbesti2，...，pbestiN]，速度為[vi=vi1，vi2，...，viN]，所有粒子發(fā)現(xiàn)的最好位置記為[gbest=gbest1，gbest2，...，gbestN]。

PSO算法的進(jìn)化方程為：

[vijt+1=wvijt+c1r1pbestij-θijt+c2r2gbestj-θijtθijt+1=θijt+vijt+1]? ? （6）

其中，[i=1，2，...，M][j=1，2，...，N]，[t]表示迭代次數(shù)，[w]為慣性權(quán)重決定繼承當(dāng)前速度的能力，[c1]、[c2]為學(xué)習(xí)因子決定粒子自我總結(jié)和向優(yōu)良個(gè)體學(xué)習(xí)的能力，[r1]、[r2]為[0，1]之間均勻分布的隨機(jī)數(shù)。

基本粒子群的算法步驟如下：

①隨機(jī)初始各粒子的位置和速度。

②計(jì)算各粒子的適應(yīng)度，將粒子[i]的位置存入[pbesti]中，將[pbest]的最好位置存儲(chǔ)在[gbest]中。

③采用（6）式更新粒子的位置和速度。

④計(jì)算各粒子的適應(yīng)度，若當(dāng)前粒子[i]的適應(yīng)度優(yōu)于[pbesti]，更新[pbesti]。

⑤將[pbest]的最好位置存儲(chǔ)在[gbest]中。

⑥若[gbest]滿(mǎn)足精度要求或達(dá)到最大迭代次數(shù)，算法結(jié)束，否則，返回③。

實(shí)際在更新粒子的位置時(shí)，還應(yīng)考慮是否越界的問(wèn)題，即[θj]是否超出了第[j]維坐標(biāo)的范圍。若超出范圍，則將邊界值賦給[θj]。

3.2 改進(jìn)的粒子群算法

3.2.1 局部搜索

受慣性權(quán)重、學(xué)習(xí)因子等參數(shù)的影響粒子移動(dòng)步長(zhǎng)具有隨機(jī)性。因而，即便某個(gè)粒子進(jìn)入最優(yōu)解的勢(shì)力范圍內(nèi)，也未必能夠精確地落在最優(yōu)解的位置，進(jìn)而影響到最優(yōu)解的精度。局部搜索用來(lái)改善當(dāng)前粒子的位置，即搜索粒子鄰域內(nèi)的更優(yōu)解。若存在更優(yōu)的位置，則用更優(yōu)的位置代替當(dāng)前的位置。

局部搜索的對(duì)象可以是整個(gè)種群，也可以只對(duì)當(dāng)前最優(yōu)粒子進(jìn)行局部搜索。在相同的種群規(guī)模和迭代次數(shù)的條件下，前者求解精度更高，而后者計(jì)算量小一些。考慮算法對(duì)時(shí)間的要求，本文采用后者的方式。

若在粒子鄰域內(nèi)盲目的搜索，勢(shì)必會(huì)影響算法的效率。沿著負(fù)梯度方向進(jìn)行搜索可以快速找到粒子鄰域內(nèi)極值點(diǎn)，確保了算法的效率[6]。

下面討論目標(biāo)函數(shù)梯度的求解，在D-H坐標(biāo)中[7]，末端操作器的位姿矩陣由變換矩陣[Ai]連乘得到如式（8）所示，變換矩陣[Ai]表示成如下的形式：

[Ai=cosθi-sinθicosαisinθisinαiaicosθisinθicosθicosαi-cosθisinαiaisinθi0sinαicosαidi0001]（7）

[Wθ=A1A2...An]? ? ? ? ? ? ? ? ? ? ? （8）

在矩陣[Ai]中，[ai]、[αi]、[θi]、[di]分別是關(guān)節(jié)偏移、關(guān)節(jié)扭轉(zhuǎn)、旋轉(zhuǎn)關(guān)節(jié)的關(guān)節(jié)變量、滑動(dòng)關(guān)節(jié)的關(guān)節(jié)變量，對(duì)于具體的機(jī)器人來(lái)說(shuō)，[ai]、[αi]是固定不變的。

現(xiàn)只考慮關(guān)節(jié)變量是旋轉(zhuǎn)變量的情形，矩陣[Ai]對(duì)[θi]求偏導(dǎo)，得：

[?Ai?θi=-sinθi-cosθicosαicosθisinαi-aisinθicosθi-sinθicosαisinθisinαiaicosθi00000000]

[=0-100100000000000Ai=RiAi]? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（9）

由式（9）可以推出末端操作器的位姿矩陣[Wθ]對(duì)[θi]的偏導(dǎo)數(shù)：

[?Wθ?θi=?Nθ?θi?Oθ?θi?Aθ?θi?Pθ?θi0000=A1A2..RiAi.An] （10）

因而推出[fθ]對(duì)[θi]的偏導(dǎo)數(shù)：

[?fθ?θi=2Pθ-P??Pθ?θi+2NθN?+OθO?+AθA?-1?Nθ?θiN?+?Oθ?θiO?+?Aθ?θiA?]（11）

用[?fθ]表示[fθ]的梯度，由式（11）得：

[?fθ=?fθ?θ1?fθ?θ2...?fθ?θN]? ? ? ? ? ?（12）

在沿著負(fù)梯度方向進(jìn)行搜索時(shí)，還應(yīng)考慮搜索步長(zhǎng)[λ]。若搜索步長(zhǎng)設(shè)置的過(guò)長(zhǎng)，雖然搜索速度快，但算法易發(fā)散。若搜索步長(zhǎng)設(shè)置過(guò)短，雖然搜索速度慢，但算法收斂性能好。既然是局部搜索，考慮到粒子本身已經(jīng)接近最優(yōu)值，因而采取小步長(zhǎng)的方式進(jìn)行搜索是最佳的[8]。

若將關(guān)節(jié)變量[θ]采用二維向量的方式進(jìn)行表示，沿著負(fù)梯度方向上的搜索如圖2所示，圖中的同一曲線上的點(diǎn)具有相同的適應(yīng)度。每步搜索可寫(xiě)成式（13）的形式。

[θ=θ-λ?fθ]? ? ? ? ? ? ? ? ? ? ?（13）

設(shè)最大搜索步數(shù)為[MS]，在一代中粒子[i]的適應(yīng)度最優(yōu)，對(duì)粒子[i]做局部搜索。每一步搜索，倘若其適應(yīng)度優(yōu)于[pbesti]則將其值賦給[pbesti]。搜索完[MS]步后，將[pbesti]的值存入到[gbest]中。

3.2.2 逃逸策略

PSO算法迭代若干次后，所有的粒子都會(huì)趨于[gbest]。如果當(dāng)前[gbest]是一個(gè)局部極值，那么一旦所有粒子都收斂于這一點(diǎn)之后，這些粒子就很難跳出這個(gè)局極值點(diǎn)。為改善粒子生存密度，在PSO 算法的基礎(chǔ)上提出粒子逃逸策略，獲得更大的生存空間，從而提高算法的收斂速度和精度[9]。當(dāng)算法運(yùn)行過(guò)程中[gbest]連續(xù)[G]代無(wú)變化，需采用逃逸策略來(lái)產(chǎn)生新一代的粒子群。將原始一半種群保留，另一半采取式（14）的方式進(jìn)行逃逸[10]。

[θijt+1=lj+ruj-lj]? ? ? ? ? ? ? ? ? （14）

式中，[r]為[0，1]區(qū)間上的隨機(jī)數(shù)，[θj∈lj，uj]。

3.3.3 改進(jìn)的PSO算法的流程圖

基于局部搜索和逃逸策略的PSO算法流程圖如圖3所示：

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

為驗(yàn)證基于PSO算法求解逆運(yùn)動(dòng)學(xué)問(wèn)題性能的好壞，以及改進(jìn)策略是否提升PSO算法的性能，選取如圖4所示的開(kāi)鏈?zhǔn)竭B桿結(jié)構(gòu)的機(jī)器人進(jìn)行測(cè)試，其位姿矩陣如式（15）所示的。

[W=-0.4698-0.3420-0.8138-0.5383-0.17100.9397-0.29620.01690.86600.0000-0.50002.06600001]? ? ? ? ? ?（15）

機(jī)器人的結(jié)構(gòu)參數(shù)如表1所示，表2給出了算法改進(jìn)前后的實(shí)驗(yàn)結(jié)果，圖5分別顯示目標(biāo)函數(shù)在算法改進(jìn)前后的收斂情況。

在兩種算法中，粒子群規(guī)模[M=40]，維數(shù)[N=5]，學(xué)習(xí)因子[c1=c2=1.5]，最大迭代次數(shù)設(shè)為100，最小誤差設(shè)為0.02，[θi∈-180?180]（[i=1，2，...，5]）。原始算法中的慣性權(quán)重[w=0.7]。在改進(jìn)的算法中，慣性權(quán)重[w=0.9]，搜索步長(zhǎng)[λ=0.2]，最大搜索步數(shù)[MS=4]，逃逸代數(shù)[G=5]。

表2的數(shù)據(jù)表明，PSO算法可以完成對(duì)逆運(yùn)動(dòng)學(xué)問(wèn)題的求解。在加入局部搜索環(huán)節(jié)和逃逸策略后，解的精度得到很大的提升，并且加快了收斂速度。圖5展示了兩種算法的整個(gè)收斂過(guò)程。很明顯，改進(jìn)算法的曲線優(yōu)于原始算法。

5 結(jié)論

本文利用PSO算法避開(kāi)直接求解逆運(yùn)動(dòng)學(xué)問(wèn)題，針對(duì)PSO算法中的不足提出改進(jìn)策略。結(jié)果表明，改進(jìn)后的PSO算法能夠加快收斂速度改善計(jì)算精度，保證機(jī)器人逆運(yùn)動(dòng)學(xué)問(wèn)題的實(shí)時(shí)性和準(zhǔn)確性。

參考文獻(xiàn)：

[1] Manseur R， Doty KL. Structural Kinematics of 6-Revolute-axis Robot Manipulators[J]. Mechanism and Machine Theory， 1996， 31（5）： 6472657.

[2] Eberhart R， Kennedy J. A new optimizer using particle swarm theory[C]. Proc Int Sym Micro Machine and Human Science： Nago2ya， Japan， Oct. 1995， 139- 43.

[3] 劉洪霞. 粒子群算法改進(jìn)及應(yīng)用[D]. 南寧： 廣西民族大學(xué)， 2011：4-5.

[4] 郝武偉， 曾建潮. 基于聚類(lèi)分析的隨機(jī)微粒群算法[J].計(jì)算機(jī)工程與應(yīng)用， 2010， 46（8）： 40-44.

[5] 龔純， 王正林. 精通MATLAB最優(yōu)化計(jì)算[M].北京：電子工業(yè)出版社， 2009.

[6] 陳寶林. 最優(yōu)化理論與算法[M].北京： 清華大學(xué)出版社， 2005.

[7] （美）Saeed B. Niku. 機(jī)器人學(xué)導(dǎo)論——分析、系統(tǒng)及應(yīng)用[D].北京： 電子工業(yè)出版社， 2004 .

[8] 任玉杰. 數(shù)值分析及其MATLAB實(shí)現(xiàn)[M]. 北京： 高等教育出版社， 2007.

[9] 謝錚桂， 鐘少丹， 韋玉科. 改進(jìn)的粒子群算法及收斂性分析[J].計(jì)算機(jī)工程與應(yīng)用，2011， 47（1）： 46-49.

[10] 王志. 粒子群優(yōu)化算法及其改進(jìn)[D].重慶： 重慶大學(xué)， 2011： 9-10.

【通聯(lián)編輯：唐一東】