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

黃振宇,姜勇

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

0 引言

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

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

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

1 基于POE運動學建模

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

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

R=[x,y,z]

(1)

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

RRT=RTR=I,detR=1

(2)

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

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

(3)

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

(4)

(5)

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

(6)

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

圖1 機械臂模型

圖2 CSSA算法流程

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

(7)

2 基于CSSA逆運動學求解

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

發(fā)現(xiàn)者位置更新:

(8)

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

跟隨者位置更新:

(9)

A+=AT(AAT)-1

(10)

為了計算機械臂實際末端位姿和預(yù)期位姿之間的誤差,根據(jù)位置矢量p和接近矢量a進行加權(quán)來構(gòu)造適應(yīng)度函數(shù)。

(11)

式中:α、β、γ是適應(yīng)度函數(shù)的權(quán)重值;θinit為初始關(guān)節(jié)角;f2為關(guān)節(jié)角最小變化值。

3 仿真實驗

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

圖3 適應(yīng)度曲線

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

圖4 機械臂仿真圖

圖5 機械臂實驗圖

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

圖6 位姿誤差

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

4 結(jié)語

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