機械手空間圓弧位姿軌跡規劃算法的實現

任秉銀，梁兆東，孔民秀

(哈爾濱工業大學機電工程學院，150001 哈爾濱)

機械手空間圓弧位姿軌跡規劃算法的實現

任秉銀，梁兆東，孔民秀

(哈爾濱工業大學機電工程學院，150001 哈爾濱)

為提高機械手的空間圓弧作業任務的軌跡精度，提出采用齊次矩陣和四元數法分別進行位置和姿態的軌跡規劃方法.對于位置規劃，先對空間圓弧所在平面的圓心角做歸一化處理，再按照平滑角速度曲線規劃位置軌跡上的插值點，并用齊次矩陣表示插補點的位置，保證了插補點的位置始終在所求圓弧上.姿態規劃采用四元數圓弧曲線函數以及分段三次有理插值保形樣條函數實現C2連續的姿態軌跡.算例驗證表明，提出的位姿規劃方法能夠保證機械手末端執行器的軌跡精度，在位置精度及姿態平穩過渡有較高要求的機械手的實際應用方面有推廣價值.

機械手;空間圓弧;位置軌跡規劃;姿態軌跡規劃;四元數曲線

焊接、噴涂、裝配和微操作等生產作業中廣泛使用的機械手的末端執行器經常需要沿特定空間圓弧軌跡進行作業，可見空間圓弧是機械手任務空間作業軌跡的核心元素.高精度及平滑過渡的機械手空間圓弧軌跡的位姿規劃的插補算法是保證機械手的末端空間圓弧軌跡精度的重要前提.對于空間圓弧軌跡的位置規劃，文獻［1］提出在任務空間中采用圓心角的線性插補實現空間圓弧插補的方法;文獻［2］采用帶誤差上限的多段直線來逼近實際的圓弧;文獻［3］采用修正的直線插補法以便于使所有的插補點都能落在圓弧上.上述兩種采用直線逼近圓弧軌跡的位置插補方法理論上均存在偏差.在描述機械手姿態時，歐拉角和旋轉矩陣存在萬向節鎖死、插值困難等缺陷，而四元數不存在這些缺陷，且與旋轉矩陣相比，四元數在計算效率和占用計算機存儲空間方面更有優勢［4］.單位四元數q的集合是R3中的旋轉變換群SO(3)，所以姿態規劃問題可以轉化為在S3空間中構造單位四元數曲線［5］.文獻［6］描述的三次Bézier四元數曲線，文獻［7］采用的 Hermit插值，以及文獻［8］提到的Squad的規劃方法都只滿足C1連續.文獻［9］提出的方法雖滿足任意階連續，但該方法并沒有封閉解，而且會生成2i－1個中間點(i∈N+).文獻［10］中的四元數圓弧能夠克服兩四元數圓弧曲線的連接處產生大的角加速度或者大的轉矩的缺點，使兩圓弧在連接處能夠光滑過渡，使用2k－1次多項式滿足了Ck連續性.

空間圓弧作業任務在實際插補過程中經常需要同時滿足給定點的位置和姿態要求，因此作業任務前需給定空間不共線3點的齊次坐標(包括3點的旋轉矩陣和空間位置坐標).

本文提出對空間圓弧在其所在平面內對應的圓心角做歸一化處理，并采用平滑的S型加減速曲線進行角速度規劃，采用齊次矩陣表示插補點的位置.采用四元數描述機械手末端的姿態，并通過單位四元數圓弧曲線函數以及分段三次有理插值保形函數共同實現C2連續的姿態軌跡，從而有效解決了姿態的平滑調整問題，為機械手高質量完成作業任務奠定了基礎.

1 機械手空間圓弧的位置插補

1.1 空間不共線的3點所在圓弧的圓心和半徑

在基坐標系下給定三維空間中任意不共線的3 點Pe(xe，ye，ze)、Pm和Ps，由該3 點確定的空間平面M1的方程表示為:Ax+By+Cz+D=0.設PePm的垂直平分面為M2，PmPs的垂直平分面為M3，則M1、M2、M3的平面方程及交點P0(x0，y0，z0)均可參照文獻［1］中的方法求得，此交點P0即為Pe、Pm和Ps所確定的空間圓弧的圓心，進而可求出該圓弧的半徑r.

1.2 建立所求圓弧的坐標系

以P0為原點建立圓弧所在平面的坐標系P0-UVW，令 U 軸的方向為，則 U 軸在基坐標系內的方向余弦為

式中:

V軸方向可由右手法則確定，其方向為ο=a × n，令p= ［x0y0z0］T，則空間圓弧自身的坐標系與基坐標系之間的變換矩陣為

由變換矩陣T可將基坐標系內的空間圓弧轉化為P0－UVW坐標系下的UV平面內的圓弧(如圖1).

圖1 空間三點決定的平面圓弧

令基坐標系內的任意一點P的齊次坐標為［x y z1］T，則P點在P0－UVW坐標系的坐標設為［x1y1z11］T，根據文獻［11］，有

1.3 圓弧插補算法

針對平面圓弧所對應的角位移做歸一化處理，且令各插補點對應的圓心角歸一化后的參數為λ(i)，并采用S型加減速曲線對圓弧的角位移進行規劃，確定插補點位置.

1.3.1 插補方向

在實際機械手的空間圓弧的任務操作中，圓弧一般具有確定的插補方向，因此本文約定在P0-UVW坐標系中UV平面內逆時針方向為其插補方向，即由Pe到Pm再到Ps的圓弧始終為逆時針圓弧.

1.3.2 插補角的大小

如圖1所示，設由式(1)計算出空間3點Pe、Pm、Ps在P0-UVW坐標系下的對應點坐標為P1=(x1，y1，0)，P2=(x2，y2，0)，P3=(x3，y3，0).

在P0－UVW坐標系的UV平面內總插補角為

同時可求出p1、p2在P0-UVW坐標系下的平面內夾角為

1.3.3 插補次數的確定

由插補角的大小以及所采用S型加減速曲線確定插補的總時間t.本文考慮到工業機械手的精度及易于實現等要求，采用定時插補且插補周期為Ts，則總的插補次數為:

同理，可由式(3)求得θ12，進而求得由P1到P2的插補次數N12.

1.3.4 插補點的坐標

在P0-UVW坐標系的UV平面內圓弧上各插補點的坐標為

則插補點在基坐標系內的坐標為

2 機械手空間圓弧的姿態插補

2.1 姿態矩陣與單位四元數的對應關系

姿態的旋轉變換可以采用單位四元數描述:機械手末端執行器姿態的三維矢量繞單位軸n1旋轉2β可表示為原姿態所對應的單位四元數乘以單位四元數即

將單位四元數表示為形如 q= ［s，(a，b，c)］的形式，則式中s、a、b、c與q= ［cos β，n1sin β］的對應關系為

同一旋轉變換可用單位四元數或姿態矩陣分別表示，且兩者有明確的對應關系.與單位四元數q= ［s，(a，b，c)］對應的姿態變換矩陣為

設機械手末端姿態矩陣為

與之對應的單位四元數可表示為

其中 s1、a1、b1、c1表達式如下:

2.2 四元數圓弧曲線的構造

2.2.1 四元數圓弧曲線插值函數

利用式(6)將Pe、Pm、Ps這3點給定的姿態矩陣轉換為與其對應的單位四元數 ±q1、±q2、± q3，在八組數據中選取一組 q1、q2、q3［10］.根據文獻［10］設T1為將S3∈R4中的點轉換到R3空間中的3×4矩陣(T1是由q1、q2、q3和R4的原點確定的4×4正交矩陣，將R4中的點投射到R3空間，T1中存在冗余項，故文獻［10］提取T1前3行使T1成為3×4矩陣即可滿足要求，以下同理);為R3空間中的圓轉換為R2平面中圓的2×3矩陣，為R3空間圓的圓心;S為R2平面中圓轉化為R2平面中單位圓的2×2矩陣;設在單位圓上與 q1、q2、q3相對應的點為，按照式(2)和(3)方法計算出、對應圓心角α12，、對應圓心角α13，并設R2中單位圓插值圓心角為α(t)，于是可得S3空間中的四元數圓弧曲線C的表達式為

對式(7)求導(給定鄰近3點的姿態矩陣為定值，對應四元數也為定值，則，S 均為定值)，可得

2.2.2 四元數圓弧曲線參數的有理插值函數

機械手位置插補采用定時插補，為了同時滿足給定點的位置和姿態要求，則位姿插補同步進行.取機械手位置插補的次數N、N12，可得機械手姿態插補的分段時間為

即在時間［0，t1］內完成由0 到α12，在［t1，t2］內完成由α12到α13.機械手在空間圓弧的姿態插補的始末兩點應有一定的速度，設α'(t)為ω(實際機械手控制應將要求的姿態速度轉化為ω)，即求函數α(t)使其滿足以下條件:

為使α(t)滿足以上條件，參照文獻［12］構造分段三次有理插值單調保形函數.

令 α1= α(0)，α2= α(t1)，α3= α(t2)，給定始末兩點的導數值為d1=α'(0)，d3=α'(t2)，滿足單調保形充分條件的d2(α'(t2))由式(8)求得［12］.

其中，ak、bk、ck可由 hk、Δk求得，hk為自變量的間距，Δk為差商;由文獻［12］分析可知其隱含條件為dk＞0，但是文獻中方程(8)并不能保證所求的解為正值.文獻［12］中有理插值函數的C2連續的條件所推得等式為

其中，wk、vk的具體表達式參照文獻［12］.當式(8)所求的解d2為負值時，取其絕對值并不能滿足等式(9)，此時取d2為相對于d1、d3正的無窮小，等式(9)成立，即滿足C2連續的條件，同時也滿足d2＞0.

通過上述方法可以求得分段三次有理插值單調保形函數α(t)，且分段函數［12］表示為:

取ti=i·Ts，i=1，2，3，…，N.將ti代入上述分段函數可以得到 α(ti)，i=1，2，3，…，N.將α(ti)代入式(7)可求得

q(i)為C2連續的單位四元數圓弧曲線表示的姿態插值離散點的函數值.目前大多數機械手在其運動卡的控制模式下的正逆解程序是依照DH法建立的關節坐標系，并采用齊次矩陣表示關節之間的相應變換關系，因此需將上述得到表示插補點姿態的四元數轉化為齊次坐標矩陣，即將q(i)由式(5)轉化為對應的齊次矩陣Trot(i)，并設

3 空間圓弧插補點的齊次坐標計算

將由式(4)得到的P(1∶4，i)和式(10)得到的Trot(i)聯立可得所有插補點的齊次坐標如下:

其中 i=1，2，3，…，N.

大部分的機械手為關節機械手，在實際應用中驅動的是各個關節.因此，將上述方法得到的齊次坐標通過機械手的逆運動學求得逆解，即轉化為相應的關節坐標，然后通過驅動器驅動相應的關節轉到一定的角度使機械手末端到達所要求的位姿.

對于各個關節，其自身插補點之間的插補運動，應依據機械手自身的精度等要求采取三次或者五次樣條曲線連接插補，進而完成機械手任務空間的圓弧規劃.

4 空間圓弧位姿插補仿真及分析

基于MOTORMAN－SK6弧焊機械手的幾何參數，在matlab上進行了仿真實驗.采用本文的機械手圓弧位置規劃方法使得機械手正逆解之后所求得的插補點都在所求圓弧上(如圖(2))，保證其位置的插補精度(圖中3個長三維坐標序列的原點為給定Pe、Pm和Ps的空間位置，坐標序列本身為給定旋轉矩陣對應的動坐標系.圖中實線為Pe、Pm和Ps所確定的圓，三維坐標序列的坐標原點為運行機械手正逆解程序后所求得的插補點位置，三維坐標序列描述了姿態的漸變情況).

圖2 空間圓弧插補位姿軌跡

由給定Pe、Pm、Ps這3點的姿態矩陣轉化后的四元數和位置規劃傳遞給姿態控制參數N、N12，并設定四元數圓弧曲線參數ω為單位速度1，控制機械手姿態的四元數圓弧曲線的參數為角位移α(t)，以及對應的角速度α'(t)和角加速度α″(t)，其曲線仿真結果分別如圖3～5所示.由圖(5)看出α(t)滿足C2連續，由2.2.1小節的結論可知，α(t)滿足C2連續則C滿足C2連續，進而保證姿態插補的C2連續.

圖3 四元數圓弧曲線的參數α(t)曲線

圖4 α'(t)曲線

圖5 α″(t)曲線

5 結論

1)本文的算法不僅能滿足空間圓弧軌跡的位置精度，而且在姿態規劃方面能夠保證機械手姿態的C2平滑過渡，為機械手的控制及驅動提供了有效的算法，具有普遍適用的意義.

2)本文的算法通過修改可以應用在空間任意指定路徑點間位姿兩方面的軌跡規劃，具有推廣價值.

［1］吳鎮煒，談大龍.機械手空間圓弧運動的一種有效軌跡規劃方法［J］.機器人，1999(1):8－11.

［2］陳國良，黃心漢，王敏.機械手圓周運動的軌跡規劃與實現［J］.華中科技大學學報，2005，33(11):63 －66.

［3］葉伯生.機械手空間三點圓弧功能的實現［J］.華中科技大學學報，2007，35(8):5 －8.

［4］FUNDA J，TAYLOR R H.On homogeneous transforms，quaternions and computational efficiency ［J］.IEEE Transactions on Robotics and Automation，1990，6(3):382－388.

［5］GALLIER J.Geometric methods and applications:for computer science and engineering［M］.2nd Edition.New York:Springer Verlag，2011:281 －300.

［6］SHOEMAKE K.Animating rotation with quaternion curves［J］.Computer Graphics，1985，19(3):245 －254.

［7］NAM K W，KIM M S.Hermite interpolation of solid orientations based on a smooth blending of two great circular arcs on SO(3)［C］//Computer Graphics:Development in Virtual Enviroments.lLeeds，UK:［s.n.］，1995:171－183.

［8］DAM E B，KOCH M，LILLHOLM M.Quaternions，Interpolation and Animation［R］.［S.l.］:University of Copenhagen，1998.

［9］PLETINCKX D.Quaternion calculus as a basic tool in computer graphics［J］.Visual Computer，1989，5(1):2－13.

［10］KIM M S，NAM K W.Interpolating solid orientations with circular blending quaternion curve［J］.Computer-Aided Design，1995，27(5):385 －398.

［11］蔡自興.機器人［M］.2版.北京:清華大學出版社，2009:25－27.

［12］SARFRAZ M.A rational cubic spline for the visualization of monotonic data［J］.Computers ＆ Graphics，2000，24(4):509－515.

An algorithm for the interpolation of circular trajectories of manipulators

REN Bing-yin，LIANG Zhao-dong，KONG Min-xiu
(School of Mechatronics Engineering，Harbin Institute of Technology，150001 Harbin，China)

To improve the arc trajectory planning accuracy of manipulators passing the non-collinear threepoints，a new approach for the manipulators’position planning and orientation planning is presented by using homogeneous matrix and quaternion respectively.For the position planning，the central angle of the arc in space is normalized，the positions of interpolation points are planed according to the smooth angular velocity curve and represented by homogeneous matrix，all the interpolation points are on the demanded arc.By using circular blending quaternion curve and a piecewise rational cubic spline together，a C2continuous orientation path which smoothly interpolates the given sequence of the manipulators positions is obtained.Simulation example shows that the presented planning approach for position and orientation can guarantee the end trajectory precision of manipulator，and can be applied to the trajectory planning of the manipulators with high accuracy requirement.

manipulator;arc in space;position trajectory planning;orientation trajectory planning;quaternion curves

TP242

A

0367－6234(2012)07－0027－05

2011－10－19.

國家科技重大專項資助項目(2011ZX04013-012).

任秉銀(1966—)，男，教授，博士生導師.

任秉銀，renby@hit.edu.cn.

(編輯 楊 波)