基于四元數(shù)的機械手姿態(tài)定向控制

黃水華,江 沛,韋 巍,項 基,彭勇剛

(浙江大學 電氣工程學院,浙江 杭州 310027)

?

基于四元數(shù)的機械手姿態(tài)定向控制

黃水華,江 沛,韋 巍,項 基,彭勇剛

(浙江大學 電氣工程學院,浙江 杭州 310027)

為了有效利用機械手關節(jié)自由度,提出機械手的姿態(tài)定向控制方法,即保持機械手末端朝向,而允許工具繞某一朝向軸的旋轉自運動.四元數(shù)法在姿態(tài)控制上不存在奇異點,因而采用四元數(shù)的方法,實現(xiàn)機械手的姿態(tài)定向控制.通過構建姿態(tài)定向誤差的四元數(shù)表述形式,引入姿態(tài)定向誤差反饋,構造李雅普諾夫函數(shù),證明了其全局穩(wěn)定性.采用定向控制后,機械手由原先的六維度控制任務轉變?yōu)槲寰S,從而多出一個冗余度,可以用于提高機械手在避免關節(jié)限位、奇異點以及提升可操作度等方面的能力.實驗證明,采用該方法能夠良好地實現(xiàn)姿態(tài)定向誤差收斂,驗證了理論上的穩(wěn)定性證明,獲得比姿態(tài)定位控制更好的運行效果.

定向控制；姿態(tài)控制；四元數(shù)；機器人運動學

工業(yè)機械手關節(jié)自由度一般不小于6,姿態(tài)控制功能是該類機械手所必須具備的.在傳統(tǒng)的機械手姿態(tài)控制中,要求機械手的工具姿態(tài)完全確定,即姿態(tài)定位控制.在一些機械手應用場合中,機械手的姿態(tài)控制不需要實現(xiàn)定位控制,如工業(yè)機械手焊接允許焊槍繞其軸線旋轉運動,乒乓球機械手擊球允許球拍面的軸向旋轉運動.該類姿態(tài)控制為定向控制,即要求機械手工具的某一軸向跟隨規(guī)劃方向運動.定向控制任務的維數(shù)為五維(三維位置以及兩維姿態(tài)).對于關節(jié)冗余的機械手,一般可以采用冗余控制算法實現(xiàn)機械手的運動規(guī)劃和運行控制.

姿態(tài)的表述方法有很多種,包括特殊正交群SO(3)、歐拉角、修正羅德里格參數(shù)、四元數(shù)法等.Luh等[1]將特殊正交群用于機械手姿態(tài)控制[1],然而在Lin[2]的工作中,姿態(tài)誤差形式[1]被證明其不具備全局穩(wěn)定性.修正羅德里格參數(shù)表達的姿態(tài)運動范圍受限[3],較少應用于機械手姿態(tài)控制,部分工作見文獻[4,5].歐拉角采用最少的3個參數(shù)表達姿態(tài)[6-8].采用歐拉角表述的姿態(tài)反饋存在奇異情況,無法保證全局穩(wěn)定性[9].歐拉角計算復雜,三參數(shù)的姿態(tài)表述必須解決奇異問題.四元數(shù)法采用冗余四參數(shù)表述姿態(tài),能夠實現(xiàn)采用連續(xù)參數(shù)描述姿態(tài)的變化過程,并且克服了奇異問題,計算復雜度低,被廣泛用于表述姿態(tài)誤差,引入反饋控制[10-14].此外,四元數(shù)被廣泛應用于機械手的姿態(tài)規(guī)劃[15-16].

本文引入四元數(shù)法,實現(xiàn)機械手的姿態(tài)定向控制.通過姿態(tài)定向控制,機械手原先的六維任務(三維位置、三維姿態(tài)),轉化為五維.采用姿態(tài)定向控制,可實現(xiàn)任務的姿態(tài)降維,從而增加機械手的冗余自由度,有效地提高機械手靈活性,有助于實現(xiàn)關節(jié)限位、碰撞避障、奇異點避免等約束.本文通過構建李雅普諾夫函數(shù)與姿態(tài)定向誤差的表述形式,引入控制律,實現(xiàn)了姿態(tài)定向控制的全局穩(wěn)定.實驗證明,采用該方法能夠有效地實現(xiàn)姿態(tài)定向誤差收斂,計算復雜度低,便于實時控制.

1 四元數(shù)基礎

(1)

式(1)等價于

(2)

式中：四元數(shù)q表示旋轉運動可以等價繞軸向K旋轉角度ψ.旋轉矩陣R與q的關系可以表示為

R=I3+2q0S(qv)+2S(qv)S(qv).

(3)

式中：對于任意的向量a=[a1,a2,a3]T,S(a)是對應的反對稱矩陣,

(4)

將式(1)代入式 (3),可得

(5)

根據(jù)式(5)可知,從R中計算單位四元數(shù)q的公式如下：

(6)

(7)

式中：E(q)為雅克比矩陣，

(8)

E(q)具有如下性質：

E(q)TE(q)=I3， E(q)Tq=0.

(9)

(10)

2 機械手運動控制

機械手的機械結構提供了關節(jié)空間到笛卡爾空間的映射關系：

x(t)=f(θ(t)).

(11)

(12)

式中：p表示位置信息,q為姿態(tài)的四元數(shù)向量, Jp、Jq分別為位置與四元數(shù)的雅克比矩陣.將式(10)代入式(12),可得

(13)

(14)

在定向控制中,繞終端坐標系下的某一軸向旋轉對末端最終的執(zhí)行效果無影響.通過工具坐標系的選取,可以將繞終端Z軸旋轉ωez定為無效旋轉,則運動學等式可以簡化成

(15)

(16)

3 誤差反饋與穩(wěn)定性

在一般情況下,位置誤差表示為p(t)-pd(t).姿態(tài)誤差的表示方法根據(jù)不同的姿態(tài)表示方法,有不同的表示.對于旋轉矩陣,誤差形式為

(17)

采用四元數(shù)法,姿態(tài)誤差可以表示為

(18)

當末端姿態(tài)與期望姿態(tài)重合時,RdR*=I3,相對應四元數(shù),則表示為q=qd且δq=[1,0,0,0]T.根據(jù)四元數(shù)法的幾何意義可知,R*Rd與δq等價,即可以通過式(5)、(6)相互轉化.對于定向問題(假設末端坐標系的Z軸為允許旋轉運行的軸向),不需要使得姿態(tài)嚴格滿足Rd=R,僅需使得a=ad,其中a為末端Z軸在基坐標系下的坐標值.

定理1 滿足末端Z軸定向控制,即a=ad,只需使得δq1=0以及δq2=0.其中,δq1、δq2為式(18)中δq的分量.

證明：當a=ad時,根據(jù)式(5)、(17),可得

根據(jù)定理1可知,末端X軸定向控制須滿足δq2=0及δq3=0,末端Y軸定向控制須滿足δq1=0及δq3=0.

在機械手運動控制的一階微分模型下,位置誤差反饋引入ep=p-pd.假設姿態(tài)定向誤差反饋引入ea=[ea1,ea2,ea3]T,其中ea為相對于末端坐標系下的姿態(tài)誤差,則系統(tǒng)輸入變量為

(19)

定義如下李雅普諾夫函數(shù)：

(20)

(21)式中：δq表示以末端坐標為參考坐標系,末端坐標系到期望坐標系的姿態(tài)誤差.根據(jù)式(7)、(14),可知

(22)

(23)

式中：?表示繞Z軸旋轉運動ωez,由于采用五維度控制,該項為不可控項,其值未知.將式(23)代入式(22),可得

(24)

根據(jù)式(8)、(24),可得

(25)

將式(25)代入式(21),可得

(26)

(27)

4 姿態(tài)定向誤差構建

在姿態(tài)定位控制中,R唯一確定,因而姿態(tài)誤差的表述形式是唯一的,見式(17)、(18).在姿態(tài)定向控制中,目標任務僅需確保某一軸向的方向一致,繞其軸向的旋轉運動為自由項.此時,在任務規(guī)劃中,矩陣Rd無法唯一確定,從而定向姿態(tài)誤差的四元數(shù)表述δq存在不確定項.

圖1 姿態(tài)定向誤差的通用描述示意圖Fig.1 Expression of attitude pointing error

(28)

不同于姿態(tài)定位控制,在定向控制中,ψ′的動態(tài)變化對姿態(tài)定向誤差沒有影響.引入姿態(tài)定向誤差的表述形式：

(29)

等價于忽略可能存在的軸向旋轉運動項,即ψ′=0.引入式(29)作為誤差反饋,根據(jù)式(27)可知,當δq1=0和δq2=0時,δq=[1,0,0,0]T.此時,定向姿態(tài)重合,誤差為零.由式(29),可知

(30)

(31)

L∈[0,1].根據(jù)非線性系統(tǒng)的相平面分析可知,系統(tǒng)(31)中,除了L=1的平衡點外,其他區(qū)域都將收斂至穩(wěn)定平衡點L=0.

(32)

(33)

同理,可以獲取允許末端X軸向運動的姿態(tài)定向誤差表述與反饋角速度：

(34)

以及允許末端Y軸向運動的姿態(tài)定向誤差表述與誤差反饋角速度：

(35)

5 實驗結果

為了驗證姿態(tài)定向控制的穩(wěn)定性和提高機械手工作性能的特點,設計了2個實驗：在姿態(tài)定位控制與姿態(tài)定向控制下,實現(xiàn)機械手擊打乒乓球的任務.在實驗中,7自由度機械手的DH參數(shù)α、a、d如表1所示.表中，θ為關節(jié)角.機械手運動控制采用文獻[17]的鉗位加權最小范數(shù)方法,基本方程為

(36)

式中：In、Im為對應關節(jié)和任務維數(shù)的單位方陣,Hw為關節(jié)逆權重對角矩陣,J為機械手雅克比,λ

表1 乒乓球機械手DH參數(shù)

為阻尼項,φ(θ)為關節(jié)鉗位項,x為機械手實際運行軌跡,xd為期望軌跡.采用該方法,機械手將在關節(jié)約束下實現(xiàn)位置與姿態(tài)的跟蹤.在實驗中,關節(jié)伺服驅動器工作在速度運行模式下,采用實時控制的方式實現(xiàn)關節(jié)速度指令輸出.

機械手打球的某一規(guī)劃任務為從初始位姿

運行至終端位姿：

運行時間為5 s.若為姿態(tài)定向控制,則終端目標的軸向任務為o=[0.500 0,0.500 0,0.707 1]T,位置與Tf相同.為了驗證2種不同控制方法的誤差收斂情況,設定機械手的初始關節(jié)位置為θ(0)=[-100.0,120.0,90.0,200.0,180.0,220.0,-30.0]T.在實驗中,誤差反饋增益設為kp=5,ka=5.

如圖2、3所示為乒乓球機械手擊球時采用姿態(tài)定位控制的運行結果.其中,定位控制方法參照文獻[10].采用姿態(tài)定位控制方式,機械手的控制任務為六維,機械手關節(jié)數(shù)為七維,僅有一個冗余度.如圖2(a)、(b)所示分別為機械手的關節(jié)運行速度與位置.圖2(b)給出關節(jié)的歸一化曲線.圖中,θmax與θmin分別為容許的最大、最小關節(jié)位置.如圖2(c)所示為機械手誤差反饋后輸出的控制指令,見式(19),機械手任務誤差見圖2(d),可知機械手的誤差沒有完全收斂到零.采用控制律(36),由于機械手在運行過程中,當t=3.5 s時,關節(jié)2、5、7接近于限位.為了避免關節(jié)限位約束,會引入阻尼項(λ>0),從而導致JJ+≠Im.式(23)中的條件不成立,誤差全局收斂的前提條件被破壞.采用控制律(36)實現(xiàn)姿態(tài)定位控制,由于要保證關節(jié)限位約束,從而影響任務的執(zhí)行,任務的位置與姿態(tài)跟蹤誤差較大,見圖2(d).對于機械手,即使末端位置是可達的,末端位置與姿態(tài)也未必可達.在該實驗中,末端位置與姿態(tài)未完全到位,且3個關節(jié)已接近限位,因而對應的末端位姿不一定可達.

圖2 姿態(tài)定位控制下的機械手運行軌跡Fig.2 Results of attitude control using quaternion

圖3 姿態(tài)定位控制運行結果Fig.3 Snaps of manipulator under attitude control

圖4 姿態(tài)定向控制下的機械手運行軌跡Fig.4 Results of attitude pointing control using quaternion

如圖4所示為乒乓球機械手擊球時的采用姿態(tài)定向控制的運行結果.采用姿態(tài)定向控制方式,七關節(jié)的機械手的控制任務為5維,剩余2個冗余度,有利于機械手關節(jié)限位避障.由于5、7號電機負載小,低速時,在減速器、摩擦力等的影響下,速度的跟蹤效果較差.如圖4(d)所示為任務誤差,在5、7號關節(jié)電機低速跟蹤存在較大抖動的情況(見圖4(a))下,采用定向姿態(tài)控制,能夠良好地跟蹤任務,實現(xiàn)位姿的誤差收斂.由圖4(b)可見,關節(jié)限位沒有觸發(fā),機械手的權重雅克比處于良好的狀態(tài).此時,在機械手運動過程中,阻尼項λ一直為零,滿足JJ+=Im,從而滿足位姿收斂條件,使得任務能夠良好跟蹤.對比圖3(a)、4(a)可以發(fā)現(xiàn),采用姿態(tài)定向控制,機械手關節(jié)速度的二范數(shù)和小于姿態(tài)定位控制,機械手在消耗較少的能量下,更加精確地跟蹤主任務.其中,圖4(c)中的ωex、ωez與圖4(d)中的ea1、ea2依賴于機械手末端當前的姿態(tài).位置控制與姿態(tài)定位控制規(guī)劃的控制指令不依賴于機械手的當前狀態(tài),定向控制的控制指令給定與誤差形式都依賴于當前的機械手姿態(tài).在采用同一控制律(36)的情況下,姿態(tài)定向控制能夠更好地執(zhí)行任務,有利于乒乓機械手的回球動作執(zhí)行,且本身球拍面的軸向運動不影響機械手的接球任務.對應于機械手,末端的位置與姿態(tài)是否可達,轉化為位置與軸向是否可達,從而擴大機械手可達區(qū)間,有利于乒乓機械手的運行及功能提升.

兩種方法對應的機械手運行過程如圖3、5所示.機械手末端的球拍面法線方向o分別為[0.495 5,0.502 2,0.708 7]T與[0.500 2,0.500 1,0.706 9]T.在運動過程中,球拍面軸向方向與規(guī)劃方向的誤差如圖6所示.圖中，φ為o與期望的球拍面軸向方向的夾角.對比2種方法可知,采用姿態(tài)定向控制機械手更加靈活,能夠有效地避免關節(jié)限位,并且提升機械手雅克比最小奇異值,有利于關節(jié)速度的降低,從而保證伺服能夠良好地跟蹤速度指令.此外,采用姿態(tài)定向控制可以降低控制維度,擴大機械手姿態(tài)定向的可達區(qū)域.

圖5 姿態(tài)定向控制運行結果Fig.5 Snaps of manipulator under attitude pointing control

圖6 姿態(tài)定位控制和定向控制的球拍軸向誤差Fig.6 Angle between axis of pingpang pat and desired direction

6 結 語

本文引入機械手的姿態(tài)定向控制方法,并且構造定向姿態(tài)誤差的表述方法,實現(xiàn)機械手姿態(tài)定向控制的全局穩(wěn)定性.通過乒乓機械手擊球的實驗對比,有效地證明了姿態(tài)定向控制相對于姿態(tài)定位控制的優(yōu)勢.一般機械手使用參數(shù),提供機械手的空間位置運動范圍,未考慮機械手在位置運動范圍內的姿態(tài)范圍.采用機械手實現(xiàn)位置和姿態(tài)控制時,如果機械手的定向姿態(tài)控制能夠等效地完成相關功能,可以采用機械手姿態(tài)定向的方法,有利于機械手在關節(jié)限位約束的條件下,實現(xiàn)姿態(tài)和位置的良好跟蹤.

[1] LUH J Y S, WALKER M W, PAUL R P C. Resolved-acceleration control of mechanical manipulators [J]. IEEE Transactions on Automatic Control, 1980, 25(3): 468-474.

[2]LIN S K. Singularity of nonlinear feedback control scheme for robots [J]. IEEE Transactions on Systems, Man and Cybernetics, 1989, 19(1): 134-139.

[3]CRASSIDIS J L, MARKIEY F L. Attitude estimation using modified Rodrigues parameters [J]. 1996: 71-83．

[4]DIMAROGONAS D V, TSIOTRAS P, KYRIAKOPOULOS K J. Laplacian cooperative attitude control of multiple rigid bodies [C]∥ IEEE International Symposium on Intelligent Control. [S. l.]: IEEE, 2006: 3064-3069.

[5]REN W. Distributed cooperative attitude synchronization and tracking for multiple rigid bodies [J]. IEEE Transactions on Control Systems Technology, 2010, 18(2): 383-392.

[6]LIN S K. Euler parameters in robot Cartesian control [C]∥ IEEE International Conference on Robotics and Automation. [S. l.]: IEEE, 1988: 1676-1681．

[7]KREUTZ K. On manipulator control by exact linearization [J]. IEEE Transactions on Automatic Control, 1989, 34(7): 763-767.

[8]CACCAVALE F, N NATALE C, SICILLIANO B, et al. Resolved-acceleration control of robot manipulators: a critical review with experiments [J]. Robotica, 1998, 16(05): 565-573．

[9]LIN S K. Singularity of nonlinear feedback control scheme for robots [J]. IEEE Transactions on Systems, Man and Cybernetics, 1989, 19(1): 134-139．[10]LIN S K. Singularity of nonlinear feedback control scheme for robots [J]. IEEE Transactions on Systems, Man and Cybernetics, 1989, 19(1): 134-139.

[11]WEN J T Y, KREUTZ-DELGADO K. The attitude control problem [J]. IEEE Transactions on Automatic Control, 1991, 36(10): 1148-1162．

[12]LIZARRALDE F, WEN J T. Attitude control without angular velocity measurement: a passivity approach [J]. IEEE Transactions on Automatic Control, 1996, 41(3): 468-472．

[13]CACCAVALE F, SICILLIANO B. Quaternion-based kinematic control of redundant spacecraft/manipulator systems [C]∥ IEEE International Conference on Robotics and Automation. [S. l.]: IEEE, 2001: 435-440.

[14]XIAN B, DE UEIROZ M S, DAWSON D, et al. Task-space tracking control of robot manipulators via quaternion feedback [J]. IEEE Transactions on Robotics and Automation, 2004, 20(1): 160-167.

[15]TAYLOR R H. Planning and execution of straight line manipulator trajectories [J]. IBM Journal of Research and Development, 1979, 23(4): 424-436．

[16]劉松國, 朱世強, 王宣銀, 等. 基于四元數(shù)和 B 樣條的機械手平滑姿態(tài)規(guī)劃器[J]. 浙江大學學報: 工學版, 2009, 43(7): 1192-1196. LIU Song-guo, ZHU Shi-qiang, WANG Xuan-yin,et al．Smooth orientation planner for manipulators based on quaternion and B-spline [J]. Journal of Zhejiang University: Engineering Science, 2009, 43(7): 1192-1196．

[17]HUANG S, PENG Y, WEI W, et al. Clamping weighted least-norm method for the manipulator kinematic control: avoiding joint limits [C]∥Control Conference (CCC). [S. l.]: IEEE, 2014: 8309-8314.

Attitude pointing control of manipulator based on quaternion

HUANG Shui-hua, JIANG Pei,WEI Wei, XIANG Ji, PENG Yong-gang

(CollegeofElectricalEngineering,ZhejiangUniversity,Hangzhou310027,China)

A general framework for the kinematic control of manipulator with attitude pointing task was presented. Attitude pointing task is keeping the direction of end-effector instead of its attitude following the command. The nonsingular unit quaternion was adopted in the manipulator kinematic control. The expression of attitude pointing error using quaternion was introduced, and Lyapunov function was constructed to prove the global stability of proposed method used in the closed-loop inverse kinematic control. By using attitude pointing control, the manipulator would get an extra degree of freedom compared to the attitude control. The manipulator would have better dexterity and manipulability. The ability of avoiding joint limits and obstacles was improved. The experiments implemented on a 7-DOF manipulator verified the theoretical results and good performance of attitude pointing control. The comparison with attitude control was given to show the advantages of proposed method．

attitude pointing; attitude control; quaternion; robot kinematic

2015-05-18. 浙江大學學報(工學版)網(wǎng)址： www.journals.zju.edu.cn/eng

國家自然科學基金資助項目(61374174)；浙江省自然科學基金資助項目(LY13F030001)；杭州市重大科技創(chuàng)新產(chǎn)業(yè)鏈資助項目(20132111A04).

黃水華(1990-),男,博士生,從事機械手運動控制、視覺伺服研究.ORCID: 0000-0001-6928-6404. E-mail: eehuangsh@gmail.com 通信聯(lián)系人：韋巍,男,教授,博導. ORCID: 0000-0002-7021-2792. E-mail: wwei@zju.edu.cn

10.3785/j.issn.1008-973X.2016.01.025

TP 241

A

1008-973X(2016)01-0173-07