崔洪新 李焕良 韩金华 李 沛

解放军理工大学野战工程学院，南京，210007



一种六自由度机械手奇异回避算法

崔洪新 李焕良 韩金华 李 沛

解放军理工大学野战工程学院，南京，210007

针对传统机械手奇异回避算法存在的计算量大、实时性差的问题，提出了基于“奇异分离+指数级阻尼倒数”的奇异回避算法。首先，在对机械手运动学建模分析的基础上，通过Jacobian矩阵计算将导致奇异的参数分离出来；然后，介绍了指数级阻尼倒数算法的原理，并对采用指数级阻尼倒数进行奇异回避导致的误差进行了研究。仿真与实验结果表明，该算法可实现关节角速度在奇异域的平稳光滑过渡，且仅牺牲了机械臂末端在部分方向上的精度，由此算法的有效性和实用性得到验证。

六自由度机械手；奇异回避；奇异分离；指数级阻尼倒数

0 引言

奇异性是关节型机器人的固有特性，当机械臂运动到奇异位形附近时，雅可比矩阵的逆是病态的且趋于无穷大，从而需要无穷大的关节角速度和角加速度，这会导致机械臂在运动过程中出现振动，严重影响机器人的轨迹跟踪精度。因此，回避奇异的轨迹规划和控制策略研究一直是机器人研究的热点[1]。

常用的奇异回避算法是阻尼最小方差法[2-3]，虽然该算法保证了关节角速度在奇异位形附近的连续性和有限性，但存在机械臂末端在各个方向上的跟踪精度较差的问题；文献[4]通过将阻尼系数只加在最小奇异值上,对阻尼最小方差法的轨迹跟踪性能进行了改进。文献[5]针对研磨机器人的特点，提出了以可操作性为准则[6]的奇异回避方法，提高了机器人的研磨精度。文献[7-8]针对PUMA类型机械臂的结构特点，分别提出了“奇异分离+紧密二次型规划”法和“奇异分离+阻尼倒数法”的PUMA机器人奇异回避算法。其他的奇异回避方法还有约束平面法[9]、几何代数法[10]、虚拟驱动法[11]和变速度控制法[12]等。但采用上述方法进行奇异回避计算时，需要对Jacobian矩阵进行实时的奇异值分解，或者需要实时估计Jacobian矩阵的最小奇异值，运算量较大。

本文针对六自由度机械手有三个关节的轴线相交于一点的特点，将机械臂分成前臂和腕部两个部分。首先建立了机械臂的运动学模型，进而推导出了以腕部为参考点的雅可比矩阵，并将六自由度的奇异回避问题分解为两个三自由度的奇异回避子问题；然后，将机械臂的奇异分成内部奇异、边界奇异和腕部奇异，并计算出相应的奇异影响因子，采用指数级阻尼倒数代替一般倒数的方法进行奇异回避；最后，通过数值仿真和实验对提出的“奇异分离+指数级阻尼倒数”的奇异回避算法进行了验证。

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)

机械臂末端的姿态角为

(4)

1.2 运动学逆解

根据六自由度机械手运动学逆解的存在性判断条件，即机械臂后3个关节的轴线相交于一点则必存在逆运动学解，可得运动学逆解计算公式为

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

六自由度机械手运动学逆解计算方程组有多个解，在实际运动规划过程中基于各关节的运动范围约束和角度变化量最小的原则取出最优解。

1.3 雅可比矩阵建模

(6)

其中,Jli和Jai分别表示第i关节的线速度和角速度传递比。机械手的雅可比矩阵为

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

由于机械臂后三个关节的轴线相交于一点，因此将机械臂分成前臂和腕部，采用奇异分离法进行奇异位形分析。

2.1 奇异位形分析

由于机械臂末端和腕部的位置相对固定，因此建立以腕部为参考点的雅可比矩阵Jw，其计算公式为

(7)

其中,zi是绕关节i转动的单位向量在基坐标系中的表示，其计算公式为

zi=0R1…i-1Riz0

(8)

Pi表示基坐标系相对于{i}坐标原点的位置矢量，其计算公式为

(9)

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

由于机械臂后三个关节的轴线相交，所以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时，关节1的角速度为无穷大；当k2=0时，关节2和关节3的角速度为无穷大。k1、k2分别为内部奇异因子和边界奇异因子。

2.3 腕部奇异分离

同理，将J22转换到坐标系{5}中表示的计算公式为5J22=(0R5)-1J22，可得

(19)

(20)

k3=sinθ5

式中，k3为腕部奇异因子。

将机械臂末端的角速度转换到坐标系{5}中的计算公式为

(21)

当k3=0时，关节4和关节6的角速度将为无穷大，此时称为腕部奇异。

3 奇异回避算法

前文中通过奇异分离将机械手的奇异位形分成内部奇异、边界奇异和腕部奇异三种，并分别计算了相应的奇异影响因子k1、k2和k3。本节引入指数级阻尼倒数算法进行奇异回避分析。

3.1 指数级阻尼倒数算法原理

可得以下关系式：

(22)

从式(22)可以看出，当奇异因子的绝对值|ki|远大于阻尼系数λi时，阻尼系数产生的影响可忽略不计；当奇异因子的绝对值|ki|远小于阻尼系数λi时，在奇异位形中阻尼系数起主要作用，保证关节角速度的连续光滑。

在实际的机械手轨迹规划中，奇异并不仅发生在奇异因子等于0的点，而是奇异位形附近的一定区域,因此，设定阈值εi作为判断奇异的条件，即当|ki|≤εi时机械臂处于奇异域，阻尼系数开始作用，而在奇异域外阻尼系数不起作用。因而得到阻尼系数的计算公式为

(23)

式中，λ0为标准阻尼系数。

根据式(15)、式(17)、式(20)和式(21)可得各关节的角速度计算公式为

(24)

3.2 奇异误差分析

(25)

相应的速度误差为

(26)

4 仿真与实验

在对机械手奇异位形和指数级阻尼倒数算法分析的基础上，通过数值仿真与实验对提出的算法进行验证。

4.1 数值仿真

图2 误差系数变化曲线Fig.2 Curve of error coefficient

设x={α,β,γ,Px,Py,Pz}表示机械臂末端在位姿空间的坐标，其中{α,β,γ}表示末端姿态的Z-Y-X欧拉角，单位为弧度；{Px,Py,Pz}表示末端的位置，单位为mm。设机械臂末端直线插补的起点x0和终点xe分别为

采用梯形法规划机械臂末端的运动速度，并设运动总时间tf=20 s，加速和减速时间相等且ts=2 s。计算得到前臂奇异因子和腕部奇异因子随时间变化曲线如图3所示。从图3中可以看出：当t1=3.3 s时，k2=0；当t2=15.4 s时，k3=0，因此机械臂在运动过程中发生边界奇异和腕部奇异。

指数级阻尼倒数相关参数分别设为：λ0=0.5，ε2=1.5，ε3=0.05。分别利用直接求逆法、阻尼倒数法和本文提出的指数级阻尼倒数法计算各关节的角速度。其中第i关节用直接求逆法计算的角速度表示为ωij，用阻尼倒数法计算的角速度表示为ωid，用指数级阻尼倒数法计算的角速度表示为ωie。各关节角速度变化曲线如图4所示。图中，“ωid+0.1”表示ωid曲线向上平移0.1个单位，其他类此。

(a)前臂奇异因子变化曲线

(b)腕部奇异因子变化曲线图3 奇异因子值随时间变化曲线Fig.3 Time-varying curves of singularity factors

从图4中可以看出：在奇异域外三条曲线重合，角速度值相同；在奇异域，采用直接求逆法计算的部分关节角速度发生奇异突变，即关节2、3、5的角速度在边界奇异域发生突变，关节4和6的角速度在边界奇异和腕部奇异域均发生突变；采用阻尼倒数法和指数级阻尼倒数法计算的各关节角速度变化曲线均能平稳光滑地通过奇异域，计算结果与式(24)计算结果一致，说明这两种方法均能很好地抑制由奇异导致的速度突变。

4.2 实验验证

实验系统是一个六自由度PUMA型机械手，通过视觉传感系统对机械手末端的运动进行测量以实现闭环控制，实验系统如图5所示。

(a)关节1 (b)关节2 (c)关节3

(d)关节4 (e)关节5 (f)关节6图4 机械臂各关节角速度变化曲线Fig.4 Angular velocity curves of manipulator joints

图5 实验系统图Fig.5 Experimental schematic diagram

利用该实验系统对本文提出的奇异回避方法进行验证，机械臂运动的初始位姿和期望位姿分别设为

末端运动速度按照梯形规划法进行规划，运动过程中发生前臂奇异和腕部奇异。实验结果如图6和图7所示，其中，图6为机械臂各关节的实际角速度变化曲线，图7为机械臂末端的位置和姿态误差曲线。

(a)前臂

(b)腕部图6 各关节实际运动角速度Fig.6 Actual angular velocities of manipulator’s joints

(a)位置误差

(b)姿态误差图7 机械臂末端跟踪误差Fig.7 Tracking errors of end-effector

从图6中可以看出，各关节角速度变化曲线平稳光滑，在奇异位形关节角速度没有发生突变。从图7中可以看出机械臂末端的位置和姿态误差均较小，满足工程应用的要求。因而，该实验结果证明了本文提出的“奇异分离+指数级阻尼倒数法”的有效性和实用性，能够回避奇异位形对速度和加速度带来的影响，高精度地跟踪预定轨迹完成任务。

5 结语

本文研究了六自由度机械手的运动学奇异问题。针对机械臂后三个关节的轴线相交于一点的特点，将六自由度的奇异问题分解成2个三自由度的奇异问题，并计算了相应的奇异因子；提出了“奇异分离+指数级阻尼倒数”的奇异域回避算法。该算法不需要对Jacobian矩阵进行奇异值分解，也不需要实时估算其最小奇异值；只需将奇异域内的奇异因子倒数换成指数级阻尼倒数即可实现奇异回避。因而运算量小、实时性好，且只牺牲了末端在部分方向上的速度精度。误差分析结果表明，指数级阻尼倒数产生的误差与实际值成比例，当相应的关节速度在奇异域内接近于0时，可进一步减小运动误差。数值仿真和实验结果证明了本文所提出的奇异回避算法的可行性和有效性。本文的研究方法也可应用于其他类型机械臂的奇异回避研究中。

[1] 刘海涛. 工业机器人的高速高精度控制方法研究[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年生。解放军理工大学野战工程学院博士研究生。主要研究方向为工程装备无人化。发表论文10余篇。E-mail：chx503207@163.com。李焕良，男，1977年生。解放军理工大学野战工程学院副教授。韩金华，男，1989年生。解放军理工大学野战工程学院博士研究生。李 沛，男，1991年生。解放军理工大学野战工程学院博士研究生。