沈偉清 王本亮 史東華

（北京理工大學(xué)數(shù)學(xué)與統(tǒng)計(jì)學(xué)院，北京 100081）

引言

機(jī)械臂傳統(tǒng)運(yùn)動(dòng)規(guī)劃算法大致可分為路徑規(guī)劃和反應(yīng)規(guī)劃兩種方式.路徑規(guī)劃通常由隨機(jī)采樣生成樹(shù)、最優(yōu)控制等方法得到可行或最優(yōu)的避障路徑，如Kuffner和La Valle等[1]提出的快速隨機(jī)生成樹(shù)法（Rapid Exploring Random Trees， RRT），Junge和Marsden等[2]提出的離散力學(xué)與最優(yōu)控制方法（Discrete Mechanics and Optimal Control，DMOC），以及其他學(xué)者對(duì)上述方法一系列的改進(jìn)，如RRT-connect，RRT，Batch Informed Trees等[3，4].這些算法用于機(jī)械臂路徑規(guī)劃時(shí)存在難以實(shí)時(shí)實(shí)現(xiàn)，計(jì)算復(fù)雜度高等缺陷.反應(yīng)規(guī)劃則通常由操作空間中引入的虛擬勢(shì)場(chǎng)產(chǎn)生的力實(shí)現(xiàn)避障，如Khatib[5]提出的人工勢(shì)場(chǎng)法，Chang和Marsden提出的回轉(zhuǎn)力方法[6]，以及改進(jìn)的人工勢(shì)場(chǎng)法[7]等.這些算法主要應(yīng)用于移動(dòng)機(jī)器人領(lǐng)域.

機(jī)械臂精確高效的反應(yīng)規(guī)劃往往要求解正向動(dòng)力學(xué)問(wèn)題.即在給定機(jī)械臂狀態(tài)變量以及關(guān)節(jié)力或力矩的前提下確定其加速度[8].解此問(wèn)題的多數(shù)方法使用Lagrange形式，并通過(guò)變分原理推導(dǎo)Euler-Lagrange方程，由給定初值的微分方程積分得到任意時(shí)刻的系統(tǒng)狀態(tài).在需長(zhǎng)時(shí)間進(jìn)行高精度控制機(jī)械臂場(chǎng)景中，其控制算法如何長(zhǎng)時(shí)間保持系統(tǒng)守恒量，進(jìn)而能定性準(zhǔn)確地反應(yīng)連續(xù)系統(tǒng)行為仍然是關(guān)鍵挑戰(zhàn).對(duì)于離散時(shí)間仿真，一些先進(jìn)的微分方程數(shù)值算法也會(huì)因誤差積累使系統(tǒng)產(chǎn)生耗散.為了解決這一問(wèn)題，Kane等[9]說(shuō)明了Newmark算法在某種意義上是離散力學(xué)的變分版本，可以保辛結(jié)構(gòu)和動(dòng)量，擁有較為出色的全局能量行為.Marsden和West[10]提出了變分積分子（VI），通過(guò)對(duì) Lagrange函數(shù)離散變分得到的離散Euler-Lagrange（DEL）方程進(jìn)行求解，構(gòu)造了一種應(yīng)用廣泛的保辛算法.在此基礎(chǔ)上出現(xiàn)的李群/李代數(shù)變分積分子已應(yīng)用于諸多動(dòng)力學(xué)建模與控制研究中[11-14].雖然基于DEL方程的變分積分子保辛，但仍然存在一定的計(jì)算復(fù)雜性.變分積分子將動(dòng)力學(xué)方程的積分轉(zhuǎn)化為代數(shù)方程組尋根，計(jì)算復(fù)雜性主要體現(xiàn)在計(jì)算DEL方程和雅可比逆矩陣兩方面.在遞歸動(dòng)力學(xué)的啟發(fā)下，Lee等[15]提出了多體系統(tǒng)的一種李群變分積分子，將DEL的計(jì)算轉(zhuǎn)化為求解離散逆動(dòng)力學(xué)問(wèn)題，并借助鉸接體慣性算法（Articulated Body Inertial Algorithm，ABI）近似計(jì)算雅可比逆矩陣，得到了一種計(jì)算復(fù)雜度只有O（n）的算法.

通常情況下，機(jī)器人最顯著的物理特征由幾何來(lái)自然描述.Brockett[16]基于剛體運(yùn)動(dòng)的李群結(jié)構(gòu)展示了如何用幾何方式來(lái)描述運(yùn)動(dòng)鏈.關(guān)于用微分幾何和李群語(yǔ)言研究機(jī)器人系統(tǒng)的表述參見(jiàn)文獻(xiàn)[8，17].本文使用幾何力學(xué)框架建立開(kāi)鏈機(jī)械臂動(dòng)力學(xué)模型并處理運(yùn)動(dòng)規(guī)劃問(wèn)題，建立了適用于開(kāi)鏈機(jī)械臂多體系統(tǒng)的Hamel變分積分子，并引入反饋控制實(shí)現(xiàn)Hamel積分子尋根計(jì)算，使算法計(jì)算復(fù)雜度為O（n）；此外將上述算法應(yīng)用于反應(yīng)規(guī)劃的正向動(dòng)力學(xué)計(jì)算，提出回轉(zhuǎn)力與人工勢(shì)場(chǎng)法的實(shí)時(shí)避障軌跡規(guī)劃算法，可以克服傳統(tǒng)人工勢(shì)場(chǎng)法中易陷入勢(shì)場(chǎng)局部極小的缺點(diǎn).

本文其余部分安排如下：首先，利用旋量理論及指數(shù)積（Product of Exponentials，POE）公式，對(duì)開(kāi)鏈機(jī)械臂正運(yùn)動(dòng)學(xué)建模，并在Lagrange力學(xué)框架下推導(dǎo)了其遞歸動(dòng)力學(xué)及帶反饋的Hamel變分積分子；其次，將回轉(zhuǎn)力引入人工勢(shì)場(chǎng)法中，通過(guò)回轉(zhuǎn)力改進(jìn)勢(shì)場(chǎng)法中的斥力，實(shí)現(xiàn)機(jī)械臂整臂的實(shí)時(shí)避障軌跡規(guī)劃；最后，通過(guò)兩個(gè)數(shù)值仿真驗(yàn)證了上述所提算法的有效性.

1 基于POE公式的正運(yùn)動(dòng)學(xué)建模

本節(jié)給出機(jī)械臂正運(yùn)動(dòng)學(xué)建模方法.

2 開(kāi)鏈機(jī)械臂的Hamel變分積分子

為模擬開(kāi)鏈機(jī)械臂多體系統(tǒng)，本節(jié)給出開(kāi)鏈機(jī)械臂的Hamel變分積分子，通過(guò)建立在每個(gè)桿上的活動(dòng)標(biāo)架描述運(yùn)動(dòng)，并采用變分原理推導(dǎo)開(kāi)鏈機(jī)械臂Hamel方程，由此得出逆動(dòng)力學(xué)方程.最后將上述過(guò)程離散化，將正向動(dòng)力方程的積分問(wèn)題轉(zhuǎn)化為DEL方程的尋根問(wèn)題，并將文獻(xiàn)[14]中得到的開(kāi)鏈機(jī)械臂Hamel變分積分子進(jìn)一步簡(jiǎn)化更新，應(yīng)用于冗余機(jī)械臂路徑規(guī)劃與反應(yīng)規(guī)劃中.

首先，我們通過(guò)Lagrange力學(xué)框架推導(dǎo)開(kāi)鏈機(jī)械臂的遞歸動(dòng)力學(xué)方程.考慮n連桿，關(guān)節(jié)位形θ=（θ1，…，θn） ∈ Tn.在第 i個(gè)桿的質(zhì)心處建立隨體標(biāo)架{i}，記gi-1，i∈ SE（3）為標(biāo)架 {i}相對(duì)于標(biāo)架{i-1}的位姿表示，g0，i簡(jiǎn)記為gi，則有

圖1 離散積分子網(wǎng)格Fig.1 Discrete-time integrator grids

注 文獻(xiàn)[14]已有上述定理類(lèi)似的陳述和證明，本文利用引理1對(duì)其進(jìn)行了更新和簡(jiǎn)化.

由此，我們給出開(kāi)鏈機(jī)械臂的離散遞歸逆動(dòng)力學(xué)算法Ⅰ.逆動(dòng)力學(xué)方程一般用于最優(yōu)控制問(wèn)題中力矩約束的計(jì)算.

A l g o r i t h mⅠD i s c r e t e R e c u r s i v e I n v e r s e D y n a m i c s I n p u t： M a n i p u l a t o r p a r a m e t e r s S i，T st（0），I i，g i-1，i（0） a n d θ k，θ k+1，t i m e s t e p Δ t O u t p u t：m o m e n t τ 1：f o r i=1→ n d o 2：g k i-1，i=g k i-1，i（0）e A i θ k i 3： ξ k+α i=A d g-k i-1，i ξ k+α i-1+A i·θ k+1 i-θ k i 4：e n d f o r 5：f o r i=n→1 d o 6：f k i← b y f o r m u l a（1 3）7：τ k i← b y f o r m u l a（6）8：Δ t e n d f o r

3 Hamel方程尋根過(guò)程

本節(jié)我們通過(guò)反饋控制實(shí)現(xiàn)Hamel積分子的尋根計(jì)算.

DEL方程定義了系統(tǒng)的離散演化過(guò)程，若已知開(kāi)鏈機(jī)械臂廣義坐標(biāo)為θk，為計(jì)算下一時(shí)刻滿(mǎn)足約束流形（13）的解 θk+1，從控制觀點(diǎn)看[15，19]，可由DEL方程所得的 τe=h（qk，vk+1） （由算法 Ⅰ 定義）作為反饋控制引入機(jī)械臂運(yùn)動(dòng)方程，若‖τe‖小于某收斂閾值ε，則將θk+1視為下一時(shí)刻滿(mǎn)足DEL方程的解，否則可利用τe構(gòu)造迭代策略更新計(jì)算θk+1直到滿(mǎn)足DEL方程.

機(jī)械臂封閉形式的動(dòng)力學(xué)為

此迭代過(guò)程如圖2所示.

圖 2 由反饋 τe（l） 進(jìn)行迭代Fig.2 Iteration with feedback-τe（l）

4 回轉(zhuǎn)力改進(jìn)人工勢(shì)場(chǎng)法

直接在關(guān)節(jié)空間中設(shè)計(jì)控制律并不直觀.操作空間控制的思想是在需要執(zhí)行任務(wù)的操作空間中設(shè)計(jì)控制律f，由雅可比矩陣將其轉(zhuǎn)化為關(guān)節(jié)空間力矩τ.

由Khatib[5]提出的人工勢(shì)場(chǎng)法常用于機(jī)器人操作空間中設(shè)計(jì)反應(yīng)控制律，其關(guān)鍵在于將機(jī)器人放入一個(gè)虛擬的勢(shì)場(chǎng)中，根據(jù)目標(biāo)點(diǎn)產(chǎn)生的引力fatt和障礙物產(chǎn)生的斥力frep綜合作用在機(jī)器人上實(shí)現(xiàn)避障目的.對(duì)機(jī)械臂而言，考慮機(jī)械臂末端位置為xe，目標(biāo)點(diǎn)位置為 xd，kp，kv為控制系數(shù)，則施加在機(jī)械臂末端的引力取為

為簡(jiǎn)單起見(jiàn)，考慮球形障礙物O，當(dāng)機(jī)械臂進(jìn)入避障范圍（|ρ|≤ρ0），假設(shè)機(jī)械臂連桿i距離障礙物最近一點(diǎn)為xi∈R3，則施加在連桿i上的斥力

其中，η為增益項(xiàng)，ρ為xi指向障礙物最近點(diǎn)向量，ρ0為安全距離.

若目標(biāo)點(diǎn)對(duì)機(jī)械臂的引力與障礙物對(duì)機(jī)械臂的斥力合力為0，那么機(jī)械臂將陷入人工勢(shì)場(chǎng)法的局部極小點(diǎn).我們提出一種改進(jìn)策略如下：當(dāng)機(jī)械臂進(jìn)入避障范圍（|ρ|≤ρ0）且ρ·＞0，通過(guò)引入回轉(zhuǎn)力取代式（18）：

可將操作空間中的控制律轉(zhuǎn)化為關(guān)節(jié)空間等效力矩，其中操作空間慣性矩陣，這里Jx為與f在同一個(gè)標(biāo)架下描述的雅可比矩陣.下面給出改進(jìn)人工勢(shì)場(chǎng)法算法Ⅱ：

A l g o r i t h mⅡR e a c t i o n P l a n n i n g I n p u t： M a n i p u l a t o r p a r a m e t e r s S i，θ i，T st（0），I i，g i-1，i（0） ，O b s t a-c l e a v o i d a n c e c o e f f i c i e n t k p，k v，ρ 0，k obs a n d T a r g e t p o i n t x d 1：w h i l e M a n i p u l a t o r e n d-e f f e c t o r p o s i t i o n x e h a s n o t a r r i v e d x d d o 2： τ att=J T xe M xe[-k p（x e-x d）-k v ˙ x e]，τ rep=0 3： i f|ρ|≤ρ 0 a n d ρ·˙ x i＞0 4： c a l c u l a t e f*rep，xi b y f o r m u l a（2 0）5： τ rep=∑n J T x i M x i f*r e p，x i，τ a t t=0 6： τ=τ att+τ rep 7：i=1 e n d w h i l e

5 數(shù)值計(jì)算與分析

本節(jié)我們通過(guò)平面三連桿正向動(dòng)力學(xué)和四連桿反應(yīng)規(guī)劃兩個(gè)例子驗(yàn)證所提算法.

5.1 正向動(dòng)力學(xué)

考慮除重力作用外不施加額外力矩的平面三連桿，其各關(guān)節(jié)為旋轉(zhuǎn)關(guān)節(jié)，每根桿桿長(zhǎng)為0.1m，質(zhì)心取在各桿的幾何中心，慣性矩陣Ii=diag（1，1，1，1，1，1），i=1，2，3.初始位形 θ=（0，0，0）T，重力矢量 e3=[0，0， -9.81，0]T，時(shí)間步長(zhǎng) Δt=0.01s.將標(biāo)架取在關(guān)節(jié)質(zhì)心處描述動(dòng)力學(xué)，采用Hamel變分積分子，式（16）中初值取為 θk，ε取10-8，三連桿運(yùn)動(dòng)100s中各時(shí)間步尋根迭代次數(shù)及反饋誤差力矩 τe分別如圖5（a）和圖5（b）所示，由其可見(jiàn)：Hamel積分子尋根計(jì)算迭代數(shù)小于等于4，且τe一直保持在10-8數(shù)量級(jí)范圍內(nèi).三連桿質(zhì)心軌跡見(jiàn)圖3，末端y軸軌跡如圖4所示，結(jié)果表明反饋Hamel變分積分子與作為基準(zhǔn)算法的半隱式歐拉法[8]所得軌跡軌跡基本一致.

圖3 三連桿各桿質(zhì)心軌跡Fig.3 The trajectory of the center of mass of each link of the three-link manipulator

圖4 三連桿末端y軸軌跡對(duì)比Fig.4 Comparison of y position at end-effector of the three-link manipulator

圖5 Hamel積分子尋根計(jì)算迭代次數(shù)及反饋誤差力矩τeFig.5 The number of iterations of the root-finding of Hamel's integrators and feedback error τe

5.2 改進(jìn)人工勢(shì)場(chǎng)法

考慮基本參數(shù)如表1所示的平面四連桿，第i桿的旋量軸Si：

表1 平面四連桿旋量參數(shù)Table 1 The screw axes and inertia matrix of the planar four-link manipulator

其中，ωi為旋轉(zhuǎn)軸，qi和ci分別為關(guān)節(jié)i及質(zhì)心i的位置向量.

平面四連桿初始位姿為

將標(biāo)架取在關(guān)節(jié)質(zhì)心ci處描述機(jī)械臂動(dòng)力學(xué)，考慮平面四連桿整臂反應(yīng)規(guī)劃，用算法Ⅱ 所得仿真結(jié)果如圖6所示.其中位置為（2.4，2.2，0）、半徑為0.25的內(nèi)側(cè)圓圈表示障礙物，半徑為0.75的外側(cè)虛線圓圈外表示避障的安全區(qū)域，位置（2，3，0）的小圓圈表示目標(biāo)吸引點(diǎn)，曲線為四連桿末端軌跡.當(dāng)平面四連桿靠近障礙物時(shí)，由于傳統(tǒng)人工勢(shì)場(chǎng)法中目標(biāo)點(diǎn)產(chǎn)生的吸引力和障礙物產(chǎn)生的斥力平衡而使平面四連桿陷入靜止.經(jīng)回轉(zhuǎn)力改進(jìn)，可使四連桿運(yùn)動(dòng)方向偏離障礙物而順利避障，其軌跡如圖6所示.結(jié)果表明相比于式（18）中的斥力形式，式（19）在一定程度上可以克服機(jī)械臂陷于勢(shì)場(chǎng)局部極小的缺點(diǎn).

圖6 四連桿避障Fig.6 Avoidance of planar four-link manipulator

6 總結(jié)與展望

本文在Lagrange力學(xué)框架下推導(dǎo)了機(jī)械臂的遞歸動(dòng)力學(xué)及帶反饋的Hamel變分積分子，并將回轉(zhuǎn)力引入傳統(tǒng)人工勢(shì)場(chǎng)法中改進(jìn)斥力設(shè)計(jì)方式，實(shí)現(xiàn)機(jī)械臂整臂的實(shí)時(shí)避障軌跡規(guī)劃.下一步將探究Hamel變分積分子保動(dòng)量、保能量等保結(jié)構(gòu)性質(zhì)，并進(jìn)一步構(gòu)造多體系統(tǒng)的Hamel場(chǎng)變分積分子，利用場(chǎng)積分子分布式計(jì)算特征得到可用于實(shí)時(shí)并行計(jì)算的反應(yīng)規(guī)劃算法.