基于POE和CSSA的冗余機械臂逆運動學求解方法

黃振宇,姜勇

(1. 沈陽理工大學 自動化與電氣工程學院,遼寧 沈陽 110006;2. 中國科學院 a. 網絡化控制系統重點實驗室; b. 沈陽自動化研究所,遼寧 沈陽 110016;3. 中國科學院 機器人與智能制造創新研究院,遼寧 沈陽 110169)

0 引言

傳統串聯機械臂通常采用D-H參數法[1]來建立機械臂的運動學模型。D-H參數法需要在機械臂各個關節處建立關節坐標系,然后得到每一個關節坐標系相對于前一個關節坐標系的齊次變換矩陣,將得到的齊次變換矩陣按照關節的順序連乘,即得到機械臂的正向運動學方程。趙志亮等[2]利用D-H法建立7自由度仿人機械臂正運動學方程對其工作空間進行分析。哈佛大學教授ROGER Brockett最先開始將機械臂的運動使用旋量的幾何特性映射來進行描述,進而形成了一種機械臂基于指數積的建模方法,通常也將這種方法稱為指數積公式。隨著大量學者使用指數積公式在機器人領域進行廣泛應用和深入研究,該方法也越來越成熟。吳亮等[3]采用旋量法建立機械臂正運動學方程進行運動學分析。相較于傳統的D-H參數建模法,使用旋量指數積公式建模法來描述機器人的運動具有兩個方面的優勢:一是該方法在建模中只用到基礎坐標系和工具坐標系這兩個坐標系,并且能夠對轉動關節和移動關節利用相同參數進行統一描述,具有很好的通用性;二是運動螺旋能夠將機械臂運動的幾何意義完整且清楚地描述出來,有利于避免機械臂出現奇異性問題。

相較于傳統6自由度機械臂來說,冗余機械臂因其自由度的增加使得計算運動學反解的難度也隨著增加。文獻[4]中使用Paden-Kahan子問題法對工業機械臂進行逆運動學求解。該方法是一種運動學逆解問題的幾何算法,其利用正向運動學解映射的指數積公式進行構造。使用這種方法通?？梢缘玫揭粋€或兩個精度較高的封閉解,可是面對冗余度機械臂逆解的無窮性子問題法將遭遇較大的困難。冗余機械臂由于其具有復雜的耦合性,為了求解機械臂的逆解,文獻[5]采用牛頓拉普森數值迭代法求解近似逆解。對于特定結構的仿人機械臂手臂,趙志亮等提出了一種7自由度機械手模型的參數化逆運動學解,討論了如何表示機械臂的冗余,然后進行運動學分析,推導出機械臂的參數化逆運動學解。針對傳統方法在機械臂逆運動學求解問題中的不足,許多學者也試著將一些智能算法[5-10]應用到逆運動學求解中,其優點在于求解不受機械臂構型的限制,具有較強的通用性。

本文以具有S-R-S構型的7自由度冗余機械臂為研究對象,提出一種7自由度仿人機械臂的逆運動學求解方法。首先基于旋量指數積法建立機械臂正運動學方程;然后,采用一種改進的混沌麻雀搜索算法[11]求解運動學逆解,該算法引入高斯變異和混沌序列使其搜索精度提高并且防止陷入局部最優解;最后在MATLAB上編程實現反解算法進行驗證,將求出的逆解利用旋量指數積建模法畫出的機械臂位姿進行驗證。重復進行實驗,統計機械臂實際末端位姿與預期位姿之間的誤差并進行分析。

1 基于POE運動學建模

在三維空間中,為了描述剛體的運動,不僅需要描述剛體的位置,還要確定其姿態。在旋量理論中,剛體在空間中的所有運動都可以通過繞某個旋轉軸的轉動加上沿平行于該軸線的移動得到。

假設{B}為固連在機械臂的坐標系,稱其為本體坐標系,{A}為空間坐標系,稱其為基礎坐標系,且都是笛卡兒空間坐標系。定義3×3的矩陣

R=[x,y,z]

(1)

式中:x,y,z∈R3,是本體坐標系相對于基礎坐標系的方向矢量;R是一個旋轉矩陣,用來描述本體坐標系與基礎坐標系的相對姿態變化。R∈SO(3),屬于李群元素,具有如下性質:

RRT=RTR=I,detR=1

(2)

式中I為3×3的單位矩陣。

向量P∈R3為一個三維向量,位置矢量用來描述本體坐標系與基礎坐標系的相對位置變化。結合姿態矩陣和位置矢量可以組成一個位姿矩陣T=(R,P)∈SE(3),寫為齊次矩陣形式如下:

(3)

機械臂各關節運動均可看作旋量運動,取各關節軸向矢量為ω∈R3,取各關節軸線上一點r∈R3,則其運動旋量ξ為

(4)

(5)

剛體的旋量運動若以矩陣指數的形式來表示,則有

(6)

本文研究的7自由度機械臂具有S-R-S構型。S-R-S機械臂也稱為擬人機械臂,通常用作仿人手臂,因為該機械臂的結構與人類手臂相似。如圖1所示,7個旋轉關節被布置成與人類手臂類似的肩部、肘部和腕部。肩關節(1、2和3)可以視為虛擬球形關節,因為這些關節軸在一個點相交。腕關節(5、6和7)也具有相同的結構。此外,兩個相鄰的關節軸垂直放置。

圖1 機械臂模型

圖2 CSSA算法流程

將坐標系建立在機械臂末端,則工具坐標系T相對于基礎坐標系B的位姿矩陣可以由gbt表示,機械臂初始位姿為gbt(0),機械臂的正運動學為

(7)

2 基于CSSA逆運動學求解

機械臂逆運動學求解是已知末端位姿gbt求解關節角θ=(θ1,θ2,θ3,θ4,θ5,θ6,θ7)的過程。本文采用一種改進混沌麻雀搜索算法求解7自由度冗余機械臂的逆解。算法首先設置初始化參數,其中需要初始化的參數有麻雀種群規模N,發現者和跟隨者數量比例,目標函數gbt(θ)的維度,最大進化迭代數和求解精度,在關節角度范圍內利用佳點集均勻分布的特性初始化種群,生成N個取值范圍在(-π,π)內的七維向量Zi。計算種群中每只麻雀的適應度值fi,選出當前最優適應度值和最壞適應度值以及所對應的位置。挑選適應度值較優的前P個個體作為發現者,剩余的麻雀作為追隨者,根據公式更新發現者和追隨者的位置。一次迭代完成后,重新計算每只麻雀的適應度值fi和麻雀種群的平均適應度值favg。根據當前種群的狀態,更新麻雀個體所迭代出的最優和最壞適應度值以及對應的位置。最后判斷算法運行是否到達設置的程序退出條件,如果滿足其中之一,則循環結束并輸出尋優結果,否則繼續更新發現者和跟隨者的位置進行迭代,直至滿足循環結束條件。

發現者位置更新:

(8)

式中:t是當前迭代次數;T是最大迭代次數;α為(0,1]的均勻隨機數;Q為標準正態分布的隨機數;L為元素均為1的d維向量;R2和ST為閾值和安全值。

跟隨者位置更新:

(9)

A+=AT(AAT)-1

(10)

為了計算機械臂實際末端位姿和預期位姿之間的誤差,根據位置矢量p和接近矢量a進行加權來構造適應度函數。

(11)

式中:α、β、γ是適應度函數的權重值;θinit為初始關節角;f2為關節角最小變化值。

3 仿真實驗

為了檢驗該算法在求解冗余機械臂逆解的可行性,在MATLAB中進行仿真實驗,設置麻雀種群大小都為POP_size=100,最大進化迭代次數MAX_gen=100,適應度函數的權值分別為α=0.001,β=1,γ=0.5。在經過100次迭代之后,其中一組實驗適應度曲線如圖3所示。

圖3 適應度曲線

該次仿真實驗中,算法迭代了100次,根據每次計算之后得到最后的適應度,輸出最優種群結果即一組關節角值,將S-R-S機械臂配置成肩肘腕的模型,在MATLAB中繪出機械臂的位姿狀態(圖4)。為了驗證其正確性,還將對應的逆解值輸入實驗室機械臂控制器中,使其點動達到實驗的期望位姿,如圖5所示。

圖4 機械臂仿真圖

圖5 機械臂實驗圖

此外,本文進行了100次實驗,統計了100次實驗中機械臂期望位姿與實際位姿的誤差,如圖6所示。

圖6 位姿誤差

從圖6中可以看出,在100次實驗中,實驗的位姿誤差均為10-4數量級,且有91次實驗的位姿誤差小于2×10-4。

4 結語

本文運用旋量指數積的方法對一種7自由度冗余機械臂進行正運動學建模,并且進行正運動學分析,這種方法能夠清晰地描述機械臂整個系統的運動,簡化了機械臂運動學分析的過程,而且它從整體上就可以描述機械臂的運動,從而避免在使用傳統方法為機械臂建模時可能產生機械臂關節的奇異性。同時運動旋量與力旋量存在對偶關系,所以有關運動旋量的理論也適用于力旋量,本文建模方法對于后續機械臂動力學分析提供了完備的研究基礎。然后本文采用一種改進混沌麻雀搜索算法來對機械臂進行逆運動學分析,求解出機械臂的逆解,這種算法求逆解不需要考慮冗余機械臂的高度耦合特性,并且其計算出的結果經過MATLAB中的仿真驗證后,所得到的結果誤差可以維持在10-4數量級。