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

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

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

中圖分類號：G242? ? ? ? 文獻標(biāo)識碼：A

文章編號：1009-3044（2019）23-0209-04

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

1 引言

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

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

但是，PSO算法的結(jié)果不能達到理想的精度，需對每一代的最優(yōu)解增添一項局部搜索。此外，PSO算法對多峰值復(fù)雜函數(shù)搜索的效果不佳，易陷入局部最優(yōu)[4]?？梢胩右莶呗院?，增強了種群的多樣性，及時地跳出局部最優(yōu)。

2 問題描述

逆運動學(xué)問題的求解可轉(zhuǎn)化成式（1）的方式進行描述：

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

其中[S]是[n]維空間中的一個非空集合，[n]代表關(guān)節(jié)變量的個數(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ù)寫成：

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

[Pθ]、[Nθ]、[Oθ]、[Aθ]由正運動學(xué)方程求得。

3 逆運動學(xué)求解

3.1 基本粒子群算法

PSO算法源于對鳥群覓食行為的模擬，鳥群中的個體通過共享信息保持鳥群飛行的一致性。個體間的信息共享機制確保群體的運動產(chǎn)生從無序到有序的演變，這是PSO算法的基礎(chǔ)[4]。

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

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

PSO算法的進化方程為：

[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)良個體學(xué)習(xí)的能力，[r1]、[r2]為[0，1]之間均勻分布的隨機數(shù)。

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

①隨機初始各粒子的位置和速度。

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

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

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

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

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

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

3.2 改進的粒子群算法

3.2.1 局部搜索

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

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

若在粒子鄰域內(nèi)盲目的搜索，勢必會影響算法的效率。沿著負梯度方向進行搜索可以快速找到粒子鄰域內(nèi)極值點，確保了算法的效率[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é)變量、滑動關(guān)節(jié)的關(guān)節(jié)變量，對于具體的機器人來說，[ai]、[αi]是固定不變的。

現(xiàn)只考慮關(guān)節(jié)變量是旋轉(zhuǎn)變量的情形，矩陣[Ai]對[θ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θ]對[θi]的偏導(dǎo)數(shù)：

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

因而推出[fθ]對[θ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）

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

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

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

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

3.2.2 逃逸策略

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

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

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

3.3.3 改進的PSO算法的流程圖

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

4 實驗結(jié)果及分析

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

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

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

在兩種算法中，粒子群規(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]。在改進的算法中，慣性權(quán)重[w=0.9]，搜索步長[λ=0.2]，最大搜索步數(shù)[MS=4]，逃逸代數(shù)[G=5]。

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

5 結(jié)論

本文利用PSO算法避開直接求解逆運動學(xué)問題，針對PSO算法中的不足提出改進策略。結(jié)果表明，改進后的PSO算法能夠加快收斂速度改善計算精度，保證機器人逆運動學(xué)問題的實時性和準(zhǔ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] 劉洪霞. 粒子群算法改進及應(yīng)用[D]. 南寧： 廣西民族大學(xué)， 2011：4-5.

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

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

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

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

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

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

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

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