崔洪新 李煥良 韓金華 李 沛

解放軍理工大學(xué)野戰(zhàn)工程學(xué)院，南京，210007

?

一種六自由度機(jī)械手奇異回避算法

崔洪新 李煥良 韓金華 李 沛

解放軍理工大學(xué)野戰(zhàn)工程學(xué)院，南京，210007

針對傳統(tǒng)機(jī)械手奇異回避算法存在的計算量大、實時性差的問題，提出了基于“奇異分離+指數(shù)級阻尼倒數(shù)”的奇異回避算法。首先，在對機(jī)械手運動學(xué)建模分析的基礎(chǔ)上，通過Jacobian矩陣計算將導(dǎo)致奇異的參數(shù)分離出來；然后，介紹了指數(shù)級阻尼倒數(shù)算法的原理，并對采用指數(shù)級阻尼倒數(shù)進(jìn)行奇異回避導(dǎo)致的誤差進(jìn)行了研究。仿真與實驗結(jié)果表明，該算法可實現(xiàn)關(guān)節(jié)角速度在奇異域的平穩(wěn)光滑過渡，且僅犧牲了機(jī)械臂末端在部分方向上的精度，由此算法的有效性和實用性得到驗證。

六自由度機(jī)械手；奇異回避；奇異分離；指數(shù)級阻尼倒數(shù)

0 引言

奇異性是關(guān)節(jié)型機(jī)器人的固有特性，當(dāng)機(jī)械臂運動到奇異位形附近時，雅可比矩陣的逆是病態(tài)的且趨于無窮大，從而需要無窮大的關(guān)節(jié)角速度和角加速度，這會導(dǎo)致機(jī)械臂在運動過程中出現(xiàn)振動，嚴(yán)重影響機(jī)器人的軌跡跟蹤精度。因此，回避奇異的軌跡規(guī)劃和控制策略研究一直是機(jī)器人研究的熱點[1]。

常用的奇異回避算法是阻尼最小方差法[2-3]，雖然該算法保證了關(guān)節(jié)角速度在奇異位形附近的連續(xù)性和有限性，但存在機(jī)械臂末端在各個方向上的跟蹤精度較差的問題；文獻(xiàn)[4]通過將阻尼系數(shù)只加在最小奇異值上,對阻尼最小方差法的軌跡跟蹤性能進(jìn)行了改進(jìn)。文獻(xiàn)[5]針對研磨機(jī)器人的特點，提出了以可操作性為準(zhǔn)則[6]的奇異回避方法，提高了機(jī)器人的研磨精度。文獻(xiàn)[7-8]針對PUMA類型機(jī)械臂的結(jié)構(gòu)特點，分別提出了“奇異分離+緊密二次型規(guī)劃”法和“奇異分離+阻尼倒數(shù)法”的PUMA機(jī)器人奇異回避算法。其他的奇異回避方法還有約束平面法[9]、幾何代數(shù)法[10]、虛擬驅(qū)動法[11]和變速度控制法[12]等。但采用上述方法進(jìn)行奇異回避計算時，需要對Jacobian矩陣進(jìn)行實時的奇異值分解，或者需要實時估計Jacobian矩陣的最小奇異值，運算量較大。

本文針對六自由度機(jī)械手有三個關(guān)節(jié)的軸線相交于一點的特點，將機(jī)械臂分成前臂和腕部兩個部分。首先建立了機(jī)械臂的運動學(xué)模型，進(jìn)而推導(dǎo)出了以腕部為參考點的雅可比矩陣，并將六自由度的奇異回避問題分解為兩個三自由度的奇異回避子問題；然后，將機(jī)械臂的奇異分成內(nèi)部奇異、邊界奇異和腕部奇異，并計算出相應(yīng)的奇異影響因子，采用指數(shù)級阻尼倒數(shù)代替一般倒數(shù)的方法進(jìn)行奇異回避；最后，通過數(shù)值仿真和實驗對提出的“奇異分離+指數(shù)級阻尼倒數(shù)”的奇異回避算法進(jìn)行了驗證。

1 運動學(xué)與雅可比矩陣建模

1.1 運動學(xué)正解

采用D-H坐標(biāo)系法建立的六自由度機(jī)械手各連桿坐標(biāo)系如圖1所示，相應(yīng)的連桿結(jié)構(gòu)參數(shù)如表1所示。其中，ai為沿Xi軸從Zi軸移動到Zi+1軸的距離；αi為繞Xi軸從Zi軸旋轉(zhuǎn)到Zi+1軸的角度；di為沿Zi軸從Xi-1軸移動到Xi軸的距離；θi為繞Zi軸從Xi-1軸旋轉(zhuǎn)到Xi軸的角度。

圖1 機(jī)械手D-H坐標(biāo)系及參數(shù)Fig.1 D-H coordinate system and parameters

表1 機(jī)械手D-H參數(shù)表

(1)

將表1中的參數(shù)代入式(1)，并逐次相乘得到機(jī)械臂末端到全局坐標(biāo)系的齊次變換矩陣為

(2)

r11=s1(c4s6+s4c5c6)-c1[c6s23s5+

c23(s4s6-c4c5c6)]

r12=s1(c4c6-s4c5s6)+c1[s23s5s6-

c23(s4c6+c4c5s6)]

r13=-s1s4s5-c1(s23c5+c23c4s5)

r21=s1[c23(c4c5c6-s4s6)-s23s5c6]-

c1(c4s6+s4c5c6)

r22=s1[s23s5s6-c23(c4c5s6+s4c6)]-

c1(c4c6-s4c5s6)

r23=-s1(s23c5+c23c4s5)+c1s4s5

r31=s23(s4s6-c4c5c6)-c23s5c6

r32=s23(s4c6+c4c5s6)+c23s5s6

r33=s23c4s5-c23c5

si=sinθisij=sin (θi+θj)

cj=cosθjcij=cos (θi+θj)

機(jī)械臂末端在全局坐標(biāo)系中的坐標(biāo)分量為

(3)

機(jī)械臂末端的姿態(tài)角為

(4)

1.2 運動學(xué)逆解

根據(jù)六自由度機(jī)械手運動學(xué)逆解的存在性判斷條件，即機(jī)械臂后3個關(guān)節(jié)的軸線相交于一點則必存在逆運動學(xué)解，可得運動學(xué)逆解計算公式為

(5)

L2=(a2s3-d4)(c1Px+s1Py-a1)-(a3+a2c3)Pz

L3=(a2c3-a3)(c1Px+s1Py-a1)-(d4-a2s3)Pz

s5=-r13(c1c23c4+s1s4)-r23(s1c23c4-c1s4)+r33s23c4

c5=-r13c1s23-r23s1s23-r33c23s6=-r11(c1c23s4-s1c4)-r21(s1c23s4+c1c4)+r31s23s4

c6=r11[(c1c23c4+s1s4)c5-c1s23s5]-r31(s23c4c5+c23s5)+r21[(s1c23c4-c1s4)c5-s1s23s5]

六自由度機(jī)械手運動學(xué)逆解計算方程組有多個解，在實際運動規(guī)劃過程中基于各關(guān)節(jié)的運動范圍約束和角度變化量最小的原則取出最優(yōu)解。

1.3 雅可比矩陣建模

(6)

其中,Jli和Jai分別表示第i關(guān)節(jié)的線速度和角速度傳遞比。機(jī)械手的雅可比矩陣為

J1x=-(c4s6+s4c5c6)(a1+a2c2+a3c23-d4s23)

J1y=-(c4c6-s4c5s6)(a1+a2c2+a3c23-d4s23)

J1z=s4s5(a1+a2c2+a3c23-d4s23)

J2x=a2[s3(c4c5c6-s4s6)+c3s5c6]+a3s5c6-d4(c4c5c6-s4s6)

J2y=-a2[s3(c4c5s6+s4c6)+c3s5s6]-a3s5s6+d4(c4c5s6+s4c6)

J2z=a2(c3c5-s3c4s5)+a3c5+c4s5d4

J3x=d4(s4s6-c4c5c6)+a3s5c6

J3y=d4(c4c5s6+s4c6)-a3s5s6

J3z=a3c5+d4c4s5

2 奇異位形分析

由于機(jī)械臂后三個關(guān)節(jié)的軸線相交于一點，因此將機(jī)械臂分成前臂和腕部，采用奇異分離法進(jìn)行奇異位形分析。

2.1 奇異位形分析

由于機(jī)械臂末端和腕部的位置相對固定，因此建立以腕部為參考點的雅可比矩陣Jw，其計算公式為

(7)

其中,zi是繞關(guān)節(jié)i轉(zhuǎn)動的單位向量在基坐標(biāo)系中的表示，其計算公式為

zi=0R1…i-1Riz0

(8)

Pi表示基坐標(biāo)系相對于{i}坐標(biāo)原點的位置矢量，其計算公式為

(9)

Pw表示腕部在基坐標(biāo)系中的位置矢量，即Pw=P4。

由于機(jī)械臂后三個關(guān)節(jié)的軸線相交，所以Pw=Pn-1=Pn-2=Pn-3。因此Jnw、J(n-1)w和J(n-2)w全為零矢量。

最終，可得機(jī)械臂腕部雅可比矩陣為

(10)

其中，J11、J21、J22為腕部雅可比矩陣的分塊矩陣，其計算公式如下：

M=a1+a2c2+a3c23-d4s23

N=a2s2+a3s23+d4c23

G=a3s23+d4c23

(11)

因此，腕部線速度和角速度可分別表達(dá)為

(12)

進(jìn)而將運動學(xué)逆問題分解為兩個問題：

(13)

由腕部雅可比矩陣表達(dá)式計算可得

(14)

(1)當(dāng)a3s3+d4c3=0，即θ3=arctan(-d4/a3)時奇異位形稱為邊界奇異；

(2)當(dāng)a1+a2c2+a3c23-d4s23=0時，奇異位形稱為內(nèi)部奇異；

(3)當(dāng)s5=0，即θ5=0時奇異位形稱為腕部奇異，此時Z4軸和Z6軸共線。

2.2 前臂奇異分離

由于機(jī)械臂末端的線速度與腕部的線速度相同，為便于分析將腕部在全局坐標(biāo)系中的線速度轉(zhuǎn)換為坐標(biāo)系{3}中的線速度：

(15)

將J11轉(zhuǎn)換到坐標(biāo)系{3}中表示的計算公式為3J11=(0R3)-1J11，因而可得

(16)

(17)

(18)

當(dāng)k1=0時，關(guān)節(jié)1的角速度為無窮大；當(dāng)k2=0時，關(guān)節(jié)2和關(guān)節(jié)3的角速度為無窮大。k1、k2分別為內(nèi)部奇異因子和邊界奇異因子。

2.3 腕部奇異分離

同理，將J22轉(zhuǎn)換到坐標(biāo)系{5}中表示的計算公式為5J22=(0R5)-1J22，可得

(19)

(20)

k3=sinθ5

式中，k3為腕部奇異因子。

將機(jī)械臂末端的角速度轉(zhuǎn)換到坐標(biāo)系{5}中的計算公式為

(21)

當(dāng)k3=0時，關(guān)節(jié)4和關(guān)節(jié)6的角速度將為無窮大，此時稱為腕部奇異。

3 奇異回避算法

前文中通過奇異分離將機(jī)械手的奇異位形分成內(nèi)部奇異、邊界奇異和腕部奇異三種，并分別計算了相應(yīng)的奇異影響因子k1、k2和k3。本節(jié)引入指數(shù)級阻尼倒數(shù)算法進(jìn)行奇異回避分析。

3.1 指數(shù)級阻尼倒數(shù)算法原理

可得以下關(guān)系式：

(22)

從式(22)可以看出，當(dāng)奇異因子的絕對值|ki|遠(yuǎn)大于阻尼系數(shù)λi時，阻尼系數(shù)產(chǎn)生的影響可忽略不計；當(dāng)奇異因子的絕對值|ki|遠(yuǎn)小于阻尼系數(shù)λi時，在奇異位形中阻尼系數(shù)起主要作用，保證關(guān)節(jié)角速度的連續(xù)光滑。

在實際的機(jī)械手軌跡規(guī)劃中，奇異并不僅發(fā)生在奇異因子等于0的點，而是奇異位形附近的一定區(qū)域,因此，設(shè)定閾值εi作為判斷奇異的條件，即當(dāng)|ki|≤εi時機(jī)械臂處于奇異域，阻尼系數(shù)開始作用，而在奇異域外阻尼系數(shù)不起作用。因而得到阻尼系數(shù)的計算公式為

(23)

式中，λ0為標(biāo)準(zhǔn)阻尼系數(shù)。

根據(jù)式(15)、式(17)、式(20)和式(21)可得各關(guān)節(jié)的角速度計算公式為

(24)

3.2 奇異誤差分析

(25)

相應(yīng)的速度誤差為

(26)

4 仿真與實驗

在對機(jī)械手奇異位形和指數(shù)級阻尼倒數(shù)算法分析的基礎(chǔ)上，通過數(shù)值仿真與實驗對提出的算法進(jìn)行驗證。

4.1 數(shù)值仿真

圖2 誤差系數(shù)變化曲線Fig.2 Curve of error coefficient

設(shè)x={α,β,γ,Px,Py,Pz}表示機(jī)械臂末端在位姿空間的坐標(biāo)，其中{α,β,γ}表示末端姿態(tài)的Z-Y-X歐拉角，單位為弧度；{Px,Py,Pz}表示末端的位置，單位為mm。設(shè)機(jī)械臂末端直線插補(bǔ)的起點x0和終點xe分別為

采用梯形法規(guī)劃機(jī)械臂末端的運動速度，并設(shè)運動總時間tf=20 s，加速和減速時間相等且ts=2 s。計算得到前臂奇異因子和腕部奇異因子隨時間變化曲線如圖3所示。從圖3中可以看出：當(dāng)t1=3.3 s時，k2=0；當(dāng)t2=15.4 s時，k3=0，因此機(jī)械臂在運動過程中發(fā)生邊界奇異和腕部奇異。

指數(shù)級阻尼倒數(shù)相關(guān)參數(shù)分別設(shè)為：λ0=0.5，ε2=1.5，ε3=0.05。分別利用直接求逆法、阻尼倒數(shù)法和本文提出的指數(shù)級阻尼倒數(shù)法計算各關(guān)節(jié)的角速度。其中第i關(guān)節(jié)用直接求逆法計算的角速度表示為ωij，用阻尼倒數(shù)法計算的角速度表示為ωid，用指數(shù)級阻尼倒數(shù)法計算的角速度表示為ωie。各關(guān)節(jié)角速度變化曲線如圖4所示。圖中，“ωid+0.1”表示ωid曲線向上平移0.1個單位，其他類此。

(a)前臂奇異因子變化曲線

(b)腕部奇異因子變化曲線圖3 奇異因子值隨時間變化曲線Fig.3 Time-varying curves of singularity factors

從圖4中可以看出：在奇異域外三條曲線重合，角速度值相同；在奇異域，采用直接求逆法計算的部分關(guān)節(jié)角速度發(fā)生奇異突變，即關(guān)節(jié)2、3、5的角速度在邊界奇異域發(fā)生突變，關(guān)節(jié)4和6的角速度在邊界奇異和腕部奇異域均發(fā)生突變；采用阻尼倒數(shù)法和指數(shù)級阻尼倒數(shù)法計算的各關(guān)節(jié)角速度變化曲線均能平穩(wěn)光滑地通過奇異域，計算結(jié)果與式(24)計算結(jié)果一致，說明這兩種方法均能很好地抑制由奇異導(dǎo)致的速度突變。

4.2 實驗驗證

實驗系統(tǒng)是一個六自由度PUMA型機(jī)械手，通過視覺傳感系統(tǒng)對機(jī)械手末端的運動進(jìn)行測量以實現(xiàn)閉環(huán)控制，實驗系統(tǒng)如圖5所示。

(a)關(guān)節(jié)1 (b)關(guān)節(jié)2 (c)關(guān)節(jié)3

(d)關(guān)節(jié)4 (e)關(guān)節(jié)5 (f)關(guān)節(jié)6圖4 機(jī)械臂各關(guān)節(jié)角速度變化曲線Fig.4 Angular velocity curves of manipulator joints

圖5 實驗系統(tǒng)圖Fig.5 Experimental schematic diagram

利用該實驗系統(tǒng)對本文提出的奇異回避方法進(jìn)行驗證，機(jī)械臂運動的初始位姿和期望位姿分別設(shè)為

末端運動速度按照梯形規(guī)劃法進(jìn)行規(guī)劃，運動過程中發(fā)生前臂奇異和腕部奇異。實驗結(jié)果如圖6和圖7所示，其中，圖6為機(jī)械臂各關(guān)節(jié)的實際角速度變化曲線，圖7為機(jī)械臂末端的位置和姿態(tài)誤差曲線。

(a)前臂

(b)腕部圖6 各關(guān)節(jié)實際運動角速度Fig.6 Actual angular velocities of manipulator’s joints

(a)位置誤差

(b)姿態(tài)誤差圖7 機(jī)械臂末端跟蹤誤差Fig.7 Tracking errors of end-effector

從圖6中可以看出，各關(guān)節(jié)角速度變化曲線平穩(wěn)光滑，在奇異位形關(guān)節(jié)角速度沒有發(fā)生突變。從圖7中可以看出機(jī)械臂末端的位置和姿態(tài)誤差均較小，滿足工程應(yīng)用的要求。因而，該實驗結(jié)果證明了本文提出的“奇異分離+指數(shù)級阻尼倒數(shù)法”的有效性和實用性，能夠回避奇異位形對速度和加速度帶來的影響，高精度地跟蹤預(yù)定軌跡完成任務(wù)。

5 結(jié)語

本文研究了六自由度機(jī)械手的運動學(xué)奇異問題。針對機(jī)械臂后三個關(guān)節(jié)的軸線相交于一點的特點，將六自由度的奇異問題分解成2個三自由度的奇異問題，并計算了相應(yīng)的奇異因子；提出了“奇異分離+指數(shù)級阻尼倒數(shù)”的奇異域回避算法。該算法不需要對Jacobian矩陣進(jìn)行奇異值分解，也不需要實時估算其最小奇異值；只需將奇異域內(nèi)的奇異因子倒數(shù)換成指數(shù)級阻尼倒數(shù)即可實現(xiàn)奇異回避。因而運算量小、實時性好，且只犧牲了末端在部分方向上的速度精度。誤差分析結(jié)果表明，指數(shù)級阻尼倒數(shù)產(chǎn)生的誤差與實際值成比例，當(dāng)相應(yīng)的關(guān)節(jié)速度在奇異域內(nèi)接近于0時，可進(jìn)一步減小運動誤差。數(shù)值仿真和實驗結(jié)果證明了本文所提出的奇異回避算法的可行性和有效性。本文的研究方法也可應(yīng)用于其他類型機(jī)械臂的奇異回避研究中。

[1] 劉海濤. 工業(yè)機(jī)器人的高速高精度控制方法研究[D]. 廣州：華南理工大學(xué)，2012. LIU Haitao. Research on High-speed and High-precision Control of Industrial Robots [D]. Guangzhou: South China University of Technology,2012.

[2] CHIAVERINI S, SICILIANO B，EGELAND O. Review of Damped Least-squares Inverse Kinematics with Experiments on an Industrial Robot Manipulator[J]. IEEE Transactions on Control Systems Technology,1994,2(2):123-134.

[3] CHENOWETH S K M, SORIA J, OOI A. A Singularity-avoiding Moving Least Squares Scheme for Two-dimensional Unstructured Meshes[J]. Journal of Computational Physics,2009,228(15):5592-5619.

[4] CHIAVERINI S. Singularity-robust Task-priority Redundancy Resolution for Real-time Kinematic Control of Robot Manipulators[J]. IEEE Transactions on Robotics and Automation,1997,13(3):398-410.

[5] XIAO Wenlei, JI Huan. Redundancy and Optimization of a 6R Robot for Five-axis Milling Applications: Singularity, Joint Limits and Collision[J]. Production Engineering,2012,6(3):287-296.

[6] 劉瑋, 常思勤. 一種新型并聯(lián)機(jī)器人的奇異性與工作空間研究[J]. 中國機(jī)械工程, 2012, 23(7): 786-789. LIU Wei, CHANG Siqin. Study on Singularity and Workspace of a Novel Parallel Manipulator[J]. China Mechanical Engineering,2012,23(7):786-789.

[7] CHENG F T, HOUR T L, SUN Y Y, et al. Study and Resolution of Singularities for a 6-DOF PUMA Manipulator[J]. IEEE Transactions on System, Man and Cybernetics, Part B: Cybernetics,1997,27(2):332-343.

[8] 徐文福,梁斌,劉宇,等. 一種新的PUMA類型機(jī)器人奇異回避算法[J]. 自動化學(xué)報, 2008,34(6): 670-675. XU Wenfu, LIANG Bin, LIU Yu, et al. A Novel Approach to Avoid Singularities of PUMA-type Manipulators[J]. Acta Automatica Sinica,2008,34(6):670-675.

[9] PENDAR H, MAHNAMA M, ZOHOOR H. Singularity Analysis of Parallel Manipulators Using Constraint Plane Method[J]. Mechanism and Machine Theory,2011,46(1):33-43.

[10] LI Qinchuan, XIANG J, CHAI X, et al. Singularity Analysis of a 3-RPS Parallel Manipulator Using Geometric Algebra[J]. Chinese Journal of Mechanical Engineering,2015,28(6):1204-1212.

[11] PARK J O, IM Y D. Singularity Avoidance of CMGs by Virtual Actuators[J]. International Journal of Control Automation and Systems,2010,8(4):891-895.

[12] MCMAHON J, SCHAUB H. Simplified Singularity Avoidance Using Variable-speed Control Moment Gyroscope Null Motion[J]. Journal of Guidance Control and Dynamics, 2009,32(6):1938-1943.

(編輯 王艷麗)

A Novel Avoid Singularity Algorithm for 6-DOF Manipulators

CUI Hongxin LI Huanliang HAN Jinhua LI Pei

College of Field Engineering，PLA University of Science and Technology，Nanjing,210007

Aiming at the problems of the heavier calculation burden and worse real-time of traditional singularity avoidance algorithms, a novel approach (named “singularity separation plus exponential order damped reciprocal” algorithm) was proposed for 6-DOF manipulators. Firstly, the singularity configurations were analyzed and the singularity parameters were separated based on the kinematics calculation. Then, the principle of exponential order damped reciprocal algorithm was introduced and error coefficient caused by the exponential order damped reciprocal algorithm was analyzed. The simulation and experimental results show that the proposed algorithm may achieve smooth transition of joint angular velocities and only part velocities accuracy of the end-effector are sacrificed, the effectiveness and practicability of the proposed algorithm are proved.

6-DOF manipulator; singularity avoidance; singularity separation; exponential order damped reciprocal

2016-07-15

全軍軍事類研究生資助課題(2015JY138)

TP242.2

10.3969/j.issn.1004-132X.2017.11.010

崔洪新，男，1987年生。解放軍理工大學(xué)野戰(zhàn)工程學(xué)院博士研究生。主要研究方向為工程裝備無人化。發(fā)表論文10余篇。E-mail：chx503207@163.com。李煥良，男，1977年生。解放軍理工大學(xué)野戰(zhàn)工程學(xué)院副教授。韓金華，男，1989年生。解放軍理工大學(xué)野戰(zhàn)工程學(xué)院博士研究生。李 沛，男，1991年生。解放軍理工大學(xué)野戰(zhàn)工程學(xué)院博士研究生。