崔洪新 李煥良 韓金華 李 沛

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

?

一種六自由度機械手奇異回避算法

崔洪新 李煥良 韓金華 李 沛

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

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

六自由度機械手；奇異回避；奇異分離；指數級阻尼倒數

0 引言

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

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

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

1 運動學與雅可比矩陣建模

1.1 運動學正解

采用D-H坐標系法建立的六自由度機械手各連桿坐標系如圖1所示，相應的連桿結構參數如表1所示。其中，ai為沿Xi軸從Zi軸移動到Zi+1軸的距離；αi為繞Xi軸從Zi軸旋轉到Zi+1軸的角度；di為沿Zi軸從Xi-1軸移動到Xi軸的距離；θi為繞Zi軸從Xi-1軸旋轉到Xi軸的角度。

圖1 機械手D-H坐標系及參數Fig.1 D-H coordinate system and parameters

表1 機械手D-H參數表

(1)

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

(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)

機械臂末端在全局坐標系中的坐標分量為

(3)

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

(4)

1.2 運動學逆解

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

(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]

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

1.3 雅可比矩陣建模

(6)

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

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 奇異位形分析

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

2.1 奇異位形分析

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

(7)

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

zi=0R1…i-1Riz0

(8)

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

(9)

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

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

最終，可得機械臂腕部雅可比矩陣為

(10)

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

M=a1+a2c2+a3c23-d4s23

N=a2s2+a3s23+d4c23

G=a3s23+d4c23

(11)

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

(12)

進而將運動學逆問題分解為兩個問題：

(13)

由腕部雅可比矩陣表達式計算可得

(14)

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

(2)當a1+a2c2+a3c23-d4s23=0時，奇異位形稱為內部奇異；

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

2.2 前臂奇異分離

由于機械臂末端的線速度與腕部的線速度相同，為便于分析將腕部在全局坐標系中的線速度轉換為坐標系{3}中的線速度：

(15)

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

(16)

(17)

(18)

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

2.3 腕部奇異分離

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

(19)

(20)

k3=sinθ5

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

將機械臂末端的角速度轉換到坐標系{5}中的計算公式為

(21)

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

3 奇異回避算法

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

3.1 指數級阻尼倒數算法原理

可得以下關系式：

(22)

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

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

(23)

式中，λ0為標準阻尼系數。

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

(24)

3.2 奇異誤差分析

(25)

相應的速度誤差為

(26)

4 仿真與實驗

在對機械手奇異位形和指數級阻尼倒數算法分析的基礎上，通過數值仿真與實驗對提出的算法進行驗證。

4.1 數值仿真

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

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

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

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

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

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

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

4.2 實驗驗證

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

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

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

圖5 實驗系統圖Fig.5 Experimental schematic diagram

利用該實驗系統對本文提出的奇異回避方法進行驗證，機械臂運動的初始位姿和期望位姿分別設為

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

(a)前臂

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

(a)位置誤差

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

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

5 結語

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

[1] 劉海濤. 工業(yè)機器人的高速高精度控制方法研究[D]. 廣州：華南理工大學，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] 劉瑋, 常思勤. 一種新型并聯機器人的奇異性與工作空間研究[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]. 自動化學報, 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年生。解放軍理工大學野戰(zhàn)工程學院博士研究生。主要研究方向為工程裝備無人化。發(fā)表論文10余篇。E-mail：chx503207@163.com。李煥良，男，1977年生。解放軍理工大學野戰(zhàn)工程學院副教授。韓金華，男，1989年生。解放軍理工大學野戰(zhàn)工程學院博士研究生。李 沛，男，1991年生。解放軍理工大學野戰(zhàn)工程學院博士研究生。