六自由度机械臂逆运动学求解

郭志强，崔天时，吕信超，张　帧，李格伦，刘斯达

(东北农业大学，哈尔滨　150030)



六自由度机械臂逆运动学求解

郭志强，崔天时，吕信超，张帧，李格伦，刘斯达

(东北农业大学，哈尔滨150030)

摘要：对六自由度机械臂进行了正运动学分析与求解，并提出了一套解决六自由度机械臂逆运动学问题的算法，同时使能耗达到最少。首先从机械臂的结构特点出发，建立D-H坐标系，得到正运动学模型；然后通过对正运动学模型的可解性进行分析，采用矩阵逆乘的方法来得到机械臂逆运动学的完整析解；再通过求极值的方法来算出机械臂在运动过程中哪种运动轨迹耗能最少；最后用求解实例的方法验证正运动学模型和逆运动学求解的正确性。

关键词：六自由度机械臂；正运动学；逆运动学

0引言

我国是农业大国，每年收获所耗费的劳动力非常多，机械臂就成为了替代人类工作的一种很好的工具。机械臂是模仿人类手臂而设计的一种能够自动运动的装置，用来实现抓取、收获等工作。它能够在人类无法工作的环境下代替人类工作，并能够保证稳定性，对提高生产效率和降低劳动成本都起到了非常重要的作用，目前已经被应用在各个领域当中。

六自由度机械臂逆运动学求解问题是近几年来国内外主要研究的热点之一。逆运动学求解的困难程度直接与机械臂的结构有非常大的关系，许多学者对六自由度机械臂逆运动学求解做出了非常大的贡献。在求解逆运动学问题中，迭代法只能求出一组解；解析法虽然可以得到全部解，但是计算复杂；人工神经网络、遗传法等只是在理论上进行了研究，无法保证解的精度和稳定性，很少用于机械臂的运动控制当中。

为此，本文提出了一种能够有效地解决六自由度机械臂逆运动学问题的实时算法，在分析机械臂运动特性的基础上，建立了D-H坐标系来研究机械臂的运动学问题。首先建立机械臂正运动学模型，然后采用矩阵逆乘的方法来求解逆运动学问题，最后进行仿真实验，验证了机械臂正运动学模型和逆运动学求解的正确性。

1正运动学模型

机械臂正运动学求解是已知机械臂各个关节的角度，来求取机械臂末端操作器的期望位置，主要是利用D-H坐标系。一般思路是：首先，在各个关节建立参考坐标系；然后确定每两个相邻的坐标系之间的关系；最后，得出机械臂总变换矩阵。

1.1　D-H坐标系的建立

本文以六自由度机械臂为研究对象，如图1所示。其中，每个舵机都可以单独工作。根据机械臂的结构特点，使用D-H方法建立基础坐标系，x0-z0表示机械臂的底座，再依次建立关节2-6的坐标系，x6-z6表示机械臂的末端操作器，如图2所示。

图1　六自由度机械臂

1.2　正运动学求解

在D-H坐标系建立后，根据相邻连杆之间的位置关系，确定D-H参数。机械臂各个关节的参数如表1所示。其中，θ为连杆转角；d为z轴上两条相邻公垂线的距离；α为每一条公垂线的长度；∂为关节轴线之间的夹角。

图2　六自由度机械臂坐标系的建立

相邻的两个坐标系θda∂0-1θ10a190°1-2θ20a202-3θ30a303-4θ4-d4a490°4-5θ5d5090°5-6θ6000

机械臂相邻相邻两个关节坐标系的齐次变换矩阵通式为

(1)

其中，Ai+1表示坐标系i+1相对于i的关系，i=1，…，6。

将D-H参数代入该公式，得到正运动学方程。其末端操作器相对于基座的变换方程为

nz=S234C5C6-C234S6

oz=-S234C5S6-C234C6

ax=C1C234S5-S1C5

ay=S1C234S5+C1C5

az=S234S5

px=C1S234d5-S1d4+C1C234a4+C1C23a3+C1C2a2+

C1a1

py=S1S234d5+C1d4+S1C234a4+S1C23a3+S1C2a2+S1a1pz=-C234d5+S234a4+S23a3+S2a2

(2)

其中，正余弦预算符号在书写上做了简化，如S1=sinθ1，C1=cosθ1，S234=sin(θ2+θ3+θ4)。

2逆运动学求解及最优控制

逆运动学求解是根基机械臂末端操作器预计位置，求出每个关节变量平移和旋转地数值，是正运动学的反向求解过程。正运动学求解比较简单而且解唯一；逆运动学求解相对复杂可能存在多解、唯一和无解的情况。本文介绍了一种根据运动学正解，采用矩阵逆乘来求解逆运动的方法。

2.1　求解关节角θ1、θ2和θ3

(3)

让方程两边矩阵的第3行第4列对应相等，可得

S1px-C1py=-d4

(4)

(5)

(6)

(7)

最后求得

(8)

让方程两端的第1行第3列对应相等，第2行第3列对应相等，可得到

在式(9)两边同时乘以S234，在式(10)两边同时乘以C234，可以得到

(C1ax+S1ay)S234=C234az

(11)

化简得

(12)

让方程第1行第4列与第 2行第4列分 别对应相等，可知

移项化简为

C1px+S1py-a1-S234d5-C234a4=C23a3+C2a2(15)

(16)

令

X1=C1px+S1py-a1-S234d5-C234a4

(17)

X2=pz+C234d5-S234a4

(18)

所以，X1和X2是已知量，对两边同时平方相加得

2a2a3(C23C2+S23S2)

(19)

最后可得

(20)

对式(20)继续化简可得

(21)

2.2　求解关节角θ4、θ5和θ6

因为θ2和θ3已经知道，所以得

θ4=θ234-θ2-θ3

对式子RTH两边连续乘以A1-A4的逆矩阵，可得

(22)

让式(22)中第1行第3列与第2行第3列分别对应相等，可知

(23)

(24)

2.3　最优控制

机械臂逆运动学的解不是唯一确定的，在计算出所有解之后，由于关节运动范围限制要将其中一部分解舍去，在剩下的解当中通常选取一个最理想的接近当前机械臂的解，而选取最理想解的方法通常选用最优控制理论。最优控制就是在给定条件下对给定的受控系统确定一种控制率，使该系统相应于预先设定的性能指标具有最优值。控制系统最优化问题包括性能指标的合理选择及最优化控制系统的设计，而性能指标在很大程度上决定了最优控制性能和最优控制形成。下面先对最优控制理论做一个简单的介绍。

假定由下面的微分方程构成动态系统，则

x(t)=f(t，x(t)，u(t))t∈[0,T]

(25)

其中，x(t)=[x1(t),...,xr(t)]T为t时刻的状态向量；x(t)=dx(t)/dt;u(t)=[u1(t),...,un(t)]T为t时刻的控制控制向量；f=[f1,...,fr]T为给定的连续可微的函数。

本方程表示系统从t=0时刻开始工作，一直到t=T，T为末端时间。通过这个方程，根据能量关系，可以得到下面的目标函数，即

(26)

最优控制理论其实就是极值原理和最优原理。以机械臂底座为坐标原点，通过正运动学分析，可以知道末端操作器相对于原点的位置方程为

(27)

(28)

求出满足方程式(28)的极小值的各个角度，即为机械耗能最少时每个舵机应转动的角度。

3求解实例分析

与正运动学相比，逆运动学求解更难，但是对机械臂的运动却更加重要，它是轨迹规划的一个非常重要的环节。所以，必须对其进行具体求解实例分析，具体过程如下：

用MatLab可以绘制出当前机械臂的位姿，如图3所示。

图3　已知各个关节角的机械臂的位姿

假设机械臂当前各个舵机的角度都为0，要使末端操作器到达满足上述的终点位置，将终点矩阵方程代入逆运动学方程中，可以得到8组解，如表2所示。

表2　逆运动学的解

由表2可知：第1、3、5、6组解是存在的，并且其中第3组解和之前已经知道的6个角度非常接近，也满足式(28)，可以作为最优解。这说明了前面所提出的逆运动学求解方法与最优控制算法的正确性，还说明了从关节空间到笛卡尔坐标系之间是一种复映射关系，即多对一的映射关系。第2、4、7、8组解无解说明了机械臂的结构不能满足条件，适当选取齐次矩阵，这几组解将有可能存在。由于逆运动学具有多解的可能性，在日程生产应用当中，还要考虑机械臂的工作环境、空间及初始位姿等条件来选择合适的解。

4结论

首先按照机械臂的结构特点建立了运动学模型。然后，通过对正运动学方程的分析，提出了矩阵逆乘的方法来求解逆运动学方程。在已知末端操作器位姿的情况下求出每个关节的旋转角度，并通过求极值的方法求出机械臂运动的最优路径。最后，通过具体的角度计算证明了这种方法的准确性，并且充分说明了关节空间到笛卡尔坐标系之间的多对一关系。本研究为机械臂轨迹规划算法的研究提供了非常重要的理论依据。

参考文献：

[1]姜铭,易红,李鹭扬. RGRR-Ι构造混联6R机器人[J].机械工程学报,2010,46(1):30-36.

[2]钱东海,王新峰,赵伟.基于旋量理论Paden-Kaha子问题的6自由度机器人逆解算法[J].机械工程学报,2009,45(9):72-76.

[3]张波涛,刘士荣,史先鹏.多连杆冗余移动机械臂建模与运动规划[J].科技通报,2010,26(2):200-205.

[4]李宪,郭永存,张军华.模块化六自由度机械臂逆运动学解算与验证[J].农业机械学报,2013,(44)4:246-251.

[5]董云,杨涛,李文.基于解析法和遗传算法的机械手运动学逆解[J].计算机仿真,2012,29(3):239-243.

[6]Kalra P，Mahapatra P B，Aggarwal D K. An evolutionary approach for solving the multimodal inverse kinematics problem of industrial robots[J].Mechanism and Machine Theory,2006,41(10):1213-1229..

[7]徐文福,刘宇,强文义.自由漂浮空间机器人的笛卡尔连续路径规划[J].控制与决策,2008,23(3):278-282.

[8]Husty M L, Pfurner M, Schrocker H P.A new and efficientalgorithm for the inverse kinematics of a general serial 6R manipula-tor[J].Mechanism and Machine Theory,2007,42(1):66-81.

[9]于艳秋,廖启征.基于有理数运算的一般6R机器人位置逆解算法[J].机械工程学报,2005,41(3):229-233.

The Solution to Inverse Kinematics About Six Degrees of Freedom Mechanical Arm

Guo Zhiqiang， Cui Tianshi， Lv Xinchao， Zhang Zhen， Li Gelun， Li Sida

(Northeast Agricultural University, Harbin 150030, China)

Abstract：At present, the application of mechanical arm is more and more widely. Forward kinematics of six degrees of freedom mechanical arm is analyzed and solved in this paper, and a set of algorithms to solve the problem of inverse kinematics of six degrees of freedom mechanical arm is proposed, at the same time to achieve a minimum energy consumption. First of all, the d-h coordinate system is established according to the structure characteristics of mechanical arm, then forward kinematics mode is achieved. Second the solvability of forward kinematics model is analyzed, then the complete analysis solution of inverse kinematics of mechanical arm is get based on method of inverse matrix. Again by the method of calculating extremum, which movement locus consumes energy least in the process of movement can be calculated. Finally through the method of solving instances, the correctness of the forward kinematics model and the solution of inverse kinematics is validated.

Key words：six degrees of freedom mechanical arm; forward kinematics; inverse kinematics

中图分类号：S24；TP24

文献标识码：A

文章编号：1003-188X(2016)12-0051-05

作者简介：郭志强(1991-)，男，黑龙江双鸭山人，硕士研究生，(E-mail)764845828@qq.com。通讯作者：崔天时(1967-)，男，哈尔滨人，副教授，硕士生导师，博士。

基金项目：国家“863计划”项目(810028)

收稿日期：2015-11-03