開鏈式多連桿機構的逆運動學求解算法

康凱 劉婷婷 王二建 李紅艷 朱國權 周建新

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

關鍵詞：逆運動學;粒子群優化算法;局部搜索;逃逸策略

中圖分類號：G242? ? ? ? 文獻標識碼：A

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

開放科學（資源服務）標識碼（OSID）：

1 引言

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

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

但是，PSO算法的結果不能達到理想的精度，需對每一代的最優解增添一項局部搜索。此外，PSO算法對多峰值復雜函數搜索的效果不佳，易陷入局部最優[4]。可引入逃逸策略后，增強了種群的多樣性，及時地跳出局部最優。

2 問題描述

逆運動學問題的求解可轉化成式（1）的方式進行描述：

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

其中[S]是[n]維空間中的一個非空集合，[n]代表關節變量的個數。函數[fθ]是定義在[S]上的目標函數。

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

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

其中，[NOA]表示姿態，[P]表示位置，目標位姿矩陣設為[W?]，則位置誤差為：

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

姿態誤差為：

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

目標函數寫成：

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

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

3 逆運動學求解

3.1 基本粒子群算法

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

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

在逆運動學問題中，設搜索空間[S]是[N]維，粒子的群體規模為[M]，第[i]（[i=1，2，...，M]）個粒子的位置為[θi=θi1，θi2，...，θiN]，其經歷最好的位置為[pbesti=pbesti1，pbesti2，...，pbestiN]，速度為[vi=vi1，vi2，...，viN]，所有粒子發現的最好位置記為[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]表示迭代次數，[w]為慣性權重決定繼承當前速度的能力，[c1]、[c2]為學習因子決定粒子自我總結和向優良個體學習的能力，[r1]、[r2]為[0，1]之間均勻分布的隨機數。

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

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

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

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

④計算各粒子的適應度，若當前粒子[i]的適應度優于[pbesti]，更新[pbesti]。

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

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

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

3.2 改進的粒子群算法

3.2.1 局部搜索

受慣性權重、學習因子等參數的影響粒子移動步長具有隨機性。因而，即便某個粒子進入最優解的勢力范圍內，也未必能夠精確地落在最優解的位置，進而影響到最優解的精度。局部搜索用來改善當前粒子的位置，即搜索粒子鄰域內的更優解。若存在更優的位置，則用更優的位置代替當前的位置。

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

若在粒子鄰域內盲目的搜索，勢必會影響算法的效率。沿著負梯度方向進行搜索可以快速找到粒子鄰域內極值點，確保了算法的效率[6]。

下面討論目標函數梯度的求解，在D-H坐標中[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]分別是關節偏移、關節扭轉、旋轉關節的關節變量、滑動關節的關節變量，對于具體的機器人來說，[ai]、[αi]是固定不變的。

現只考慮關節變量是旋轉變量的情形，矩陣[Ai]對[θi]求偏導，得：

[?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]的偏導數：

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

因而推出[fθ]對[θi]的偏導數：

[?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）

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

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

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

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

3.2.2 逃逸策略

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

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

式中，[r]為[0，1]區間上的隨機數，[θj∈lj，uj]。

3.3.3 改進的PSO算法的流程圖

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

4 實驗結果及分析

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

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

機器人的結構參數如表1所示，表2給出了算法改進前后的實驗結果，圖5分別顯示目標函數在算法改進前后的收斂情況。

在兩種算法中，粒子群規模[M=40]，維數[N=5]，學習因子[c1=c2=1.5]，最大迭代次數設為100，最小誤差設為0.02，[θi∈-180?180]（[i=1，2，...，5]）。原始算法中的慣性權重[w=0.7]。在改進的算法中，慣性權重[w=0.9]，搜索步長[λ=0.2]，最大搜索步數[MS=4]，逃逸代數[G=5]。

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

5 結論

本文利用PSO算法避開直接求解逆運動學問題，針對PSO算法中的不足提出改進策略。結果表明，改進后的PSO算法能夠加快收斂速度改善計算精度，保證機器人逆運動學問題的實時性和準確性。

參考文獻：

[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] 劉洪霞. 粒子群算法改進及應用[D]. 南寧： 廣西民族大學， 2011：4-5.

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

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

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

[7] （美）Saeed B. Niku. 機器人學導論——分析、系統及應用[D].北京： 電子工業出版社， 2004 .

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

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

[10] 王志. 粒子群優化算法及其改進[D].重慶： 重慶大學， 2011： 9-10.

【通聯編輯：唐一東】