王本亮 高山 孫宏偉 史東華

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

引言

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

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

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

幾何力學(xué)以微分幾何與對稱性方法[8]為工具來研究力學(xué)系統(tǒng)的動(dò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].

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

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

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

考慮機(jī)械臂末端剛體, 其質(zhì)量為m, 轉(zhuǎn)動(dòng)慣量為, 位形空間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, 受控剛體運(yùn)動(dòng)方程可以寫為：

(1)

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

(2)

(3)

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

(4)

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

(5)

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

(6)

(7)

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

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

ω×(ΔRTωg)+

(8)

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

2 運(yùn)動(dòng)學(xué)逆解的阻尼最小二乘法

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

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

阻尼最小二乘法[14]綜合了Newton法以及梯度下降法的優(yōu)點(diǎn), 并改善了兩者的不足. 為應(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)為誤差項(xiàng),λ∈為非零阻尼系數(shù). 阻尼最小二乘解為:

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

(13)

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

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

Algorithm I Reaction Planning

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

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

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

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

4 仿真研究

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

機(jī)器人運(yùn)動(dòng)學(xué)正解由指數(shù)積公式[19]給出：

(14)

其中機(jī)器人初始位姿：

(15)

運(yùn)動(dòng)旋量ξi：

(16)

其中ωi∈3是運(yùn)動(dòng)旋量軸線方向上的單位矢量, 定義為:

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

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

ω6=(0,1,0)T

(17)

qi∈3為軸線上的任一點(diǎn), 取為:

(18)

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

圖2 六軸開鏈機(jī)械臂模型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方法比較, 在相同場景下, 考慮開鏈機(jī)械臂末端實(shí)現(xiàn)避障, 取機(jī)械臂末端質(zhì)量m6=3.92kg, 轉(zhuǎn)動(dòng)慣量=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算法運(yùn)行時(shí)間長、采樣效率低, 回轉(zhuǎn)力與阻尼最小二乘法的結(jié)合可達(dá)到快速避障的效果, 且無需用樣條函數(shù)進(jìn)行光滑化處理, 適用于一些末端追蹤過程中快速避障的場景.

5 小結(jié)

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