機(jī)械手空間圓弧位姿軌跡規(guī)劃算法的實(shí)現(xiàn)

任秉銀，梁兆東，孔民秀

(哈爾濱工業(yè)大學(xué)機(jī)電工程學(xué)院，150001 哈爾濱)

機(jī)械手空間圓弧位姿軌跡規(guī)劃算法的實(shí)現(xiàn)

任秉銀，梁兆東，孔民秀

(哈爾濱工業(yè)大學(xué)機(jī)電工程學(xué)院，150001 哈爾濱)

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

機(jī)械手;空間圓弧;位置軌跡規(guī)劃;姿態(tài)軌跡規(guī)劃;四元數(shù)曲線

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

空間圓弧作業(yè)任務(wù)在實(shí)際插補(bǔ)過程中經(jīng)常需要同時滿足給定點(diǎn)的位置和姿態(tài)要求，因此作業(yè)任務(wù)前需給定空間不共線3點(diǎn)的齊次坐標(biāo)(包括3點(diǎn)的旋轉(zhuǎn)矩陣和空間位置坐標(biāo)).

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

1 機(jī)械手空間圓弧的位置插補(bǔ)

1.1 空間不共線的3點(diǎn)所在圓弧的圓心和半徑

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

1.2 建立所求圓弧的坐標(biāo)系

以P0為原點(diǎn)建立圓弧所在平面的坐標(biāo)系P0-UVW，令 U 軸的方向?yàn)?，則 U 軸在基坐標(biāo)系內(nèi)的方向余弦為

式中:

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

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

圖1 空間三點(diǎn)決定的平面圓弧

令基坐標(biāo)系內(nèi)的任意一點(diǎn)P的齊次坐標(biāo)為［x y z1］T，則P點(diǎn)在P0－UVW坐標(biāo)系的坐標(biāo)設(shè)為［x1y1z11］T，根據(jù)文獻(xiàn)［11］，有

1.3 圓弧插補(bǔ)算法

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

1.3.1 插補(bǔ)方向

在實(shí)際機(jī)械手的空間圓弧的任務(wù)操作中，圓弧一般具有確定的插補(bǔ)方向，因此本文約定在P0-UVW坐標(biāo)系中UV平面內(nèi)逆時針方向?yàn)槠洳逖a(bǔ)方向，即由Pe到Pm再到Ps的圓弧始終為逆時針圓弧.

1.3.2 插補(bǔ)角的大小

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

在P0－UVW坐標(biāo)系的UV平面內(nèi)總插補(bǔ)角為

同時可求出p1、p2在P0-UVW坐標(biāo)系下的平面內(nèi)夾角為

1.3.3 插補(bǔ)次數(shù)的確定

由插補(bǔ)角的大小以及所采用S型加減速曲線確定插補(bǔ)的總時間t.本文考慮到工業(yè)機(jī)械手的精度及易于實(shí)現(xiàn)等要求，采用定時插補(bǔ)且插補(bǔ)周期為Ts，則總的插補(bǔ)次數(shù)為:

同理，可由式(3)求得θ12，進(jìn)而求得由P1到P2的插補(bǔ)次數(shù)N12.

1.3.4 插補(bǔ)點(diǎn)的坐標(biāo)

在P0-UVW坐標(biāo)系的UV平面內(nèi)圓弧上各插補(bǔ)點(diǎn)的坐標(biāo)為

則插補(bǔ)點(diǎn)在基坐標(biāo)系內(nèi)的坐標(biāo)為

2 機(jī)械手空間圓弧的姿態(tài)插補(bǔ)

2.1 姿態(tài)矩陣與單位四元數(shù)的對應(yīng)關(guān)系

姿態(tài)的旋轉(zhuǎn)變換可以采用單位四元數(shù)描述:機(jī)械手末端執(zhí)行器姿態(tài)的三維矢量繞單位軸n1旋轉(zhuǎn)2β可表示為原姿態(tài)所對應(yīng)的單位四元數(shù)乘以單位四元數(shù)即

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

同一旋轉(zhuǎn)變換可用單位四元數(shù)或姿態(tài)矩陣分別表示，且兩者有明確的對應(yīng)關(guān)系.與單位四元數(shù)q= ［s，(a，b，c)］對應(yīng)的姿態(tài)變換矩陣為

設(shè)機(jī)械手末端姿態(tài)矩陣為

與之對應(yīng)的單位四元數(shù)可表示為

其中 s1、a1、b1、c1表達(dá)式如下:

2.2 四元數(shù)圓弧曲線的構(gòu)造

2.2.1 四元數(shù)圓弧曲線插值函數(shù)

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

對式(7)求導(dǎo)(給定鄰近3點(diǎn)的姿態(tài)矩陣為定值，對應(yīng)四元數(shù)也為定值，則，S 均為定值)，可得

2.2.2 四元數(shù)圓弧曲線參數(shù)的有理插值函數(shù)

機(jī)械手位置插補(bǔ)采用定時插補(bǔ)，為了同時滿足給定點(diǎn)的位置和姿態(tài)要求，則位姿插補(bǔ)同步進(jìn)行.取機(jī)械手位置插補(bǔ)的次數(shù)N、N12，可得機(jī)械手姿態(tài)插補(bǔ)的分段時間為

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

為使α(t)滿足以上條件，參照文獻(xiàn)［12］構(gòu)造分段三次有理插值單調(diào)保形函數(shù).

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

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

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

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

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

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

3 空間圓弧插補(bǔ)點(diǎn)的齊次坐標(biāo)計算

將由式(4)得到的P(1∶4，i)和式(10)得到的Trot(i)聯(lián)立可得所有插補(bǔ)點(diǎn)的齊次坐標(biāo)如下:

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

大部分的機(jī)械手為關(guān)節(jié)機(jī)械手，在實(shí)際應(yīng)用中驅(qū)動的是各個關(guān)節(jié).因此，將上述方法得到的齊次坐標(biāo)通過機(jī)械手的逆運(yùn)動學(xué)求得逆解，即轉(zhuǎn)化為相應(yīng)的關(guān)節(jié)坐標(biāo)，然后通過驅(qū)動器驅(qū)動相應(yīng)的關(guān)節(jié)轉(zhuǎn)到一定的角度使機(jī)械手末端到達(dá)所要求的位姿.

對于各個關(guān)節(jié)，其自身插補(bǔ)點(diǎn)之間的插補(bǔ)運(yùn)動，應(yīng)依據(jù)機(jī)械手自身的精度等要求采取三次或者五次樣條曲線連接插補(bǔ)，進(jìn)而完成機(jī)械手任務(wù)空間的圓弧規(guī)劃.

4 空間圓弧位姿插補(bǔ)仿真及分析

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

圖2 空間圓弧插補(bǔ)位姿軌跡

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

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

圖4 α'(t)曲線

圖5 α″(t)曲線

5 結(jié)論

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

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

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

［2］陳國良，黃心漢，王敏.機(jī)械手圓周運(yùn)動的軌跡規(guī)劃與實(shí)現(xiàn)［J］.華中科技大學(xué)學(xué)報，2005，33(11):63 －66.

［3］葉伯生.機(jī)械手空間三點(diǎn)圓弧功能的實(shí)現(xiàn)［J］.華中科技大學(xué)學(xué)報，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］蔡自興.機(jī)器人［M］.2版.北京:清華大學(xué)出版社，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.

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

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

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

(編輯 楊 波)