一種基于幾何力學的機械臂末端規(guī)劃算法

王本亮 高山 孫宏偉 史東華

(1.北京理工大學 數(shù)學與統(tǒng)計學院, 北京 100081) (2.復雜信息數(shù)學表征分析與應用北京市重點實驗室, 北京 100081)(3.中國船舶重工集團公司第七一六研究所, 連云港 222006)

引言

隨著工業(yè)機器人應用范圍的擴展,機器人在導航、搬運、裝配、噴涂與焊接場景中與人交互工作的情況越來越多,如何避免與人及障礙發(fā)生碰撞成為機器人運動規(guī)劃中的一個重要問題.目前運動規(guī)劃中對避障的處理可分為全局規(guī)劃與反應規(guī)劃兩種方式[1].

全局規(guī)劃通常采用概率方法, 盡可能找到一條從運動起點到目標終點的無碰撞軌跡, 再利用樣條函數(shù)將軌跡光滑化. 常用的有Kuffner和LaValle[2]提出的一種基于采樣的路徑規(guī)劃算法—RRT(快速隨機生成樹), 其通過對狀態(tài)空間中的隨機采樣點進行碰撞檢測, 從起始位置快速擴張樹來規(guī)劃路徑. Karaman和Frazzoli等[3]提出了RRT*法, 并證明在一定條件下, 該方法以概率1收斂到最優(yōu)解. 這類基于采樣的規(guī)劃算法通常應用于已知的作業(yè)環(huán)境中, 適合解決高維空間中多自由度、約束復雜的規(guī)劃問題, 但存在實時性與可重復性較差、碰撞檢測頻繁等缺陷. 此外, 還可應用最優(yōu)控制方法來規(guī)劃從起始點到目標點的無碰撞最優(yōu)路徑(如最省時間, 最省能量等), 如Gerdts等[4]應用SQP(序列二次規(guī)劃)法來求解多連桿機器人路徑規(guī)劃的最優(yōu)控制問題, 并引入隱面剔除策略來減少求解過程中障礙帶來的約束. 這類計算方法由于計算復雜度高, 難以實時實現(xiàn).

反應規(guī)劃通常利用機械臂連桿到障礙物的距離信息來實現(xiàn)避障. 由Khatib[5]提出的人工勢場法是機器人反應規(guī)劃最常用的一個方法: 機器人在一個虛擬的力場中運動, 障礙物與其它機器人個體設為斥力極, 目標點設為引力極, 機器人個體根據(jù)力場的綜合作用來實現(xiàn)無碰撞軌跡規(guī)劃, 但由于引力場和斥力場的相互作用, 易使機器人陷于局部極小(零勢場)點處. Luo等[1]利用障礙物與機械臂間的斥力矢量信息構造計算簡單的一階運動方程實現(xiàn)避障, 但所生成的軌跡欠光滑. Chang和Marsden[6]提出了平面質點躲避凸障礙的回轉力控制算法, 與勢場法中的斥力極相比, 回轉力總是垂直于機器人的速度, 該性質保證了由勢場力梯度決定的控制律的收斂性. Garimella等[7]在此基礎上提出了三維空間欠驅動系統(tǒng)避障的回轉力方法.

幾何力學以微分幾何與對稱性方法[8]為工具來研究力學系統(tǒng)的動力學與控制問題. 作為科學最古老分支之一的經(jīng)典力學與數(shù)學中的微分幾何、李群李代數(shù)等分支逐步攜手, 形成了使用微分幾何中的無坐標語言描述、兼具優(yōu)美和廣泛性的現(xiàn)代拉格朗日和哈密頓力學, 為多體系統(tǒng)、流體、場論及幾何控制理論等提供了統(tǒng)一的框架. 我國學者在基于幾何力學的非完整場論及約化、力學系統(tǒng)控制、保結構算法等方面取得了大量原創(chuàng)性的成果[9-12].

利用微分幾何語言對機械臂建模的優(yōu)勢在于: 首先, 可得到描述運動且獨立于坐標選取的最簡潔運動方程; 其次, 能利用系統(tǒng)對稱性進行約化, 并使約化后的系統(tǒng)便于構造理想的控制律; 最后, 還可利用幾何結構的類似, 方便地將有限維連續(xù)系統(tǒng)的結構推廣至無窮維力學系統(tǒng), 從而可應用于軟體機器人的建模仿真控制算法.

為克服上述算法存在的效率低、運動規(guī)劃軌跡欠光滑等缺陷, 本文考慮開鏈機械臂末端避障的路徑規(guī)劃問題, 提出一種基于幾何力學的反應規(guī)劃算法. 本文第一節(jié)利用回轉力實現(xiàn)機械臂末端剛體避障, 同時給出到達指定位姿的控制律, 進而獲得光滑的避障軌跡; 第二節(jié)引入幾何力學框架下的阻尼最小二乘法求解對應的機械臂逆運動學問題; 第三節(jié)將所得算法與特殊歐式群SE(3)中的RRT算法作對比. 最后以六軸開鏈機械臂為例, 給出相應的仿真實驗結果.

1 機械臂末端避障的回轉力方法

考慮機械臂末端剛體, 其質量為m, 轉動慣量為, 位形空間SE(3)=SO(3)3, 位置x=(x,y,z)T∈3, 旋轉姿態(tài)矩陣R∈SO(3), 體角速度為ω=(ωx,ωy,ωz)T∈3, 并記狀態(tài)空間Q=TSE(3)?SE(3)×se(3), 狀態(tài)

假設控制u所在有界控制輸入集為U, 定義控制力矩τ:Q×U→3, 作用力f:Q×U→3, 受控剛體運動方程可以寫為：

(1)

其中·^:3→so(3)為帽子映射:

(2)

(3)

其中Kx,Kv∈3×3為正定矩陣, Δx=x-xg為誤差項,Γ(s)為斜對稱矩陣.

(4)

為使位置控制律具有避障性, Garimella等[7]提出Γ(s)的一種設計方法:

(5)

其中Γi,k1(θi),k2(di),ei(s)為對應障礙物Oi的避障矩陣、角度系數(shù)、距離系數(shù)和旋轉軸, 障礙物可視為一系列圓柱、球及其組合. 為簡單起見, 假設單個障礙物為球形(若單個障礙物為圓柱, 旋轉軸e(s)可取為圓柱的中心軸), 其半徑為r,d為剛體質心位置指向球心的向量,θ為剛體質心運動方向與d之間的夾角,rd為檢測半徑. ?。?/p>

(6)

(7)

同樣的, 為使目標姿態(tài)Rg可達, 可選取姿態(tài)控制律[13]:

τ= -skew(KRΔR)∨-Kω(ω-ΔRTωg)+

ω×(ΔRTωg)+

(8)

結合位置控制律和姿態(tài)控制律, 可獲得剛體在SE(3)中到達指定位姿的無碰撞軌跡c=(R,x):[0,t]→SE(3).

2 運動學逆解的阻尼最小二乘法

從機械的角度看, 對于n連桿開鏈機械臂, 我們可以用一系列通過轉動或移動關節(jié)連接的剛體運動鏈表示. 為簡單起見, 只考慮轉動情形. 此時關節(jié)空間Tn是各獨立關節(jié)角所在空間S1的n重笛卡爾積. 考慮機械臂由關節(jié)空間到工作空間的正運動學映射gst:Tn→SE(3).

逆運動學問題的處理一般可采用分析法或數(shù)值法. 分析法利用機械臂特殊幾何結構得到解析表達式; 數(shù)值法通過迭代收斂得到局部的唯一解, 比較著名的算法有循環(huán)坐標下降法(CCD)、啟發(fā)式FABRIK策略; 還可將逆運動學問題轉化為優(yōu)化問題, 利用Newton法、梯度下降法、阻尼最小二乘法等優(yōu)化算法進行求解(詳見綜述[14]).

阻尼最小二乘法[14]綜合了Newton法以及梯度下降法的優(yōu)點, 并改善了兩者的不足. 為應用歐式空間的阻尼最小二乘法(DLS), 我們利用局部微分同胚ψ:se(3)→SE(3)的逆映射將SE(3)中的逆問題轉換到歐式空間se(3)中.

設cg:[0,t]→SE(3)為期望目標曲線, 令：

F(θ,t)=ψ-1(cg(t)-1gst(θ))

(9)

其中ψ可由指數(shù)映射或作為其近似的Cayley變換給出. 利用SE(3)中的Cayley變換[15]:

ψ-1(M)=(Cay-1(R),(R+I3)-1x)

(10)

其中,M=(R,x)∈SE(3),I3為3階單位矩陣, Cay-1:SO(3)→so(3)為：

Cay-1(R)=(I3+R)-1(I3-R)

(11)

為求解F(θ,t)=0的最佳近似解, 考慮優(yōu)化問題:

(12)

其中e=F(θ,t)為誤差項,λ∈為非零阻尼系數(shù). 阻尼最小二乘解為:

Δθ=JT(JJT+λ2I)-1e

(13)

當參數(shù)λ較小時, 算法效果趨近于Newton算法, 反之, 算法效果趨近于梯度下降法.

下面給出回轉力避障與阻尼最小二乘法相結合的反應規(guī)劃算法(算法I):

Algorithm I Reaction Planning

3 快速隨機生成樹(RRT)算法

為檢驗前面所提規(guī)劃算法的有效性, 我們將其與經(jīng)典的RRT算法進行仿真對比. RRT方法采樣方式可分為關節(jié)空間采樣[16]與工作空間采樣[17]. 由于正運動學映射的非線性, 一般采用工作空間采樣方式. Kuffner[18]介紹了剛體工作空間中常用的采樣策略, 包括SE(3)采樣點間距離的定義、插值方案的選取. 為實現(xiàn)在隨機生成樹中添加新路徑, 即“長樹枝”(圖1)的過程, 我們利用前述阻尼最小二乘法生成采樣點與RRT中父結點的連接路徑.

圖1 快速隨機生成樹算法示意圖Fig.1 Representation of rapid-exploring random tree method (the extend operation)

具體實現(xiàn)過程為: 記初始位姿Q0∈SE(3)為隨機樹T根結點. 預先給定某閾值p(p∈(0,1]). 隨機生成某數(shù)pc(pc∈(0,1)), 若pc≤p, 則在工作空間中隨機采樣, 采樣點記為Qrand; 否則直接取定目標位姿為采樣點Qrand.樹T中距離Qrand最近的結點記為Qfather, 記由Qfather.x指向Qrand.x方向上步長為pstep處的結點為Qnew.x,Qnew.R可利用四元數(shù)與旋轉矩陣的關系插值[18]給出. 其中Q*.x和Q*.R分別表示采樣點Q*位置和姿態(tài). 若機械臂位姿Qnew不與障礙物發(fā)生碰撞, 則將Qnew插入樹T并連接Qnew與Qfather. 重復采樣直至T到達目標位姿, 從而獲得一條由Q0到目標位姿Qd的規(guī)劃路徑.

4 仿真研究

為驗證我們所提出算法的有效性, 以簡單的六軸開鏈機械臂為例(圖2), 其桿長依次為l1,l2,…,l6, 對應質量為m1,m2,…,m6.

機器人運動學正解由指數(shù)積公式[19]給出：

(14)

其中機器人初始位姿：

(15)

運動旋量ξi：

(16)

其中ωi∈3是運動旋量軸線方向上的單位矢量, 定義為:

ω1=ω4=(0,0,1)T

ω2=ω3=ω5=(1,0,0)T

ω6=(0,1,0)T

(17)

qi∈3為軸線上的任一點, 取為:

(18)

將式(15)～式(18)代入式(14)得到機械臂正運動學映射. 在實驗環(huán)境為Intel i5/2.5GHZ/2G的計算機上, 給定3組機械臂末端初始狀態(tài)及可達目標狀態(tài), 分別采用RRT算法測試, 取偏向概率1-p=0.64, 步長pstep=2.8, 經(jīng)樣條函數(shù)光滑化, 得到軌跡如圖3所示.

圖2 六軸開鏈機械臂模型Fig.2 Model of six-axis open chain manipulator

圖3 RRT規(guī)劃算法所生成的避障軌跡Fig.3 Collision-free trajectories generated by the RRT path planning algorithm

圖4 由回轉力控制所生成的避障軌跡Fig.4 Collision-free trajectories generated by the gyroscopic force controller

為將算法I與RRT方法比較, 在相同場景下, 考慮開鏈機械臂末端實現(xiàn)避障, 取機械臂末端質量m6=3.92kg, 轉動慣量=diag(2,2,1), 剛體初始及目標速度、體角速度均設為0, 得到軌跡如圖4所示, 兩種算法規(guī)劃結果比較見表1.

表1 算法I與RRT規(guī)劃比較Table 1 Comparison of algorithm I and RRT planning

由上述結果可知:相比于RRT算法運行時間長、采樣效率低, 回轉力與阻尼最小二乘法的結合可達到快速避障的效果, 且無需用樣條函數(shù)進行光滑化處理, 適用于一些末端追蹤過程中快速避障的場景.

5 小結

本文提出了一種基于幾何力學的開鏈n連桿機械臂末端規(guī)劃算法, 借助末端剛體的自然運動方程, 分別引入勢場力和回轉力進行追蹤與避障, 能夠在工作空間中快速產(chǎn)生光滑的規(guī)劃軌跡; 同時利用阻尼最小二乘法進行運動學逆問題的求解, 得到關節(jié)空間的規(guī)劃路徑, 可以克服傳統(tǒng)算法檢測頻繁, 以及規(guī)劃路徑欠光滑等缺陷, 具有一定的實際應用價值.