王本亮 高山 孫宏偉 史東華

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

引言

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

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

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

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

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

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

1 機械臂末端避障的回轉(zhuǎn)力方法

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

假設(shè)控制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)的一種設(shè)計方法:

(5)

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

(6)

(7)

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

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

ω×(ΔRTωg)+

(8)

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

2 運動學(xué)逆解的阻尼最小二乘法

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

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

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

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

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)

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

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

Algorithm I Reaction Planning

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

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

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

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

4 仿真研究

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

機器人運動學(xué)正解由指數(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)得到機械臂正運動學(xué)映射. 在實驗環(huán)境為Intel i5/2.5GHZ/2G的計算機上, 給定3組機械臂末端初始狀態(tài)及可達目標(biāo)狀態(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 由回轉(zhuǎn)力控制所生成的避障軌跡Fig.4 Collision-free trajectories generated by the gyroscopic force controller

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

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

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

5 小結(jié)

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