朱齐丹 王欣璐

（哈尔滨工程大学，哈尔滨，150001）

六自由度机械臂逆运动学算法

朱齐丹 王欣璐

（哈尔滨工程大学，哈尔滨，150001）

根据D-H参数法确定六自由度机械臂的运动学方程，结合平面几何法和欧拉角变换法将机械臂的逆运动学求解问题分为两部分，一通过平面几何法确定机械臂腕部点的坐标与前三个关节角的关系，二通过欧拉角变换法确定机械臂末端姿态与后三个关节角的关系，根据逆运动解的选取原则从八组解中选取最优解；利用MATLAB中的Robotics Toolbox建立机械臂的正运动学模型，通过多组位姿下的正逆运动解对比验证逆运动学求解算法的准确性；利用VC++中的QueryPerformanceCounter函数和MATLAB中tic-toc语句得到不同算法所消耗的平均时间，通过消耗时间的对比说明该算法的快速性；利用VC++编程实现机械臂写字的过程，通过对比输入字的形状与机械臂末端的实际运动轨迹，进一步验证该算法是一种快速而准确的逆运动学求解算法。

机器人，六自由度，机械臂，逆运动解，平面几何法，欧拉角变换法

0 引言

机械臂被广泛应用于机械制造、航空航天、医疗和原子能等领域，机械臂的逆运动学问题是其轨迹规划与控制的重要基础，逆运动学求解是否快速准确将直接影响到机械臂轨迹规划与控制的精度，因此针对工业中常用的六自由度机械臂，设计一种快速准确的逆运动学求解方法是十分重要的。

目前，机械臂逆运动学的求解方法主要有：迭代法、解析法和几何法。迭代法虽然在大多数情况下是可行的，但却无法得到全部解；解析法计算较为复杂，但可以得到全部根；几何法针对机械臂的某些特殊结构进行简化，再进行求解，虽然对于一般机械臂不通用，但是其形式简单，求解所需的计算量远远小于迭代法和解析法。Paul等[1]于1981年提出的解析算法对后来的机械臂逆运动学问题研究有着指导性意义。Regnier[2]于1997年提出一种基于迭代法和分布式的算法，能够求出多种结构的六自由度机械臂的位置逆解，但相应的计算时间也会变长。Jun等[3]于2009年提出将工作区速度输入的控制问题转化为求解机器人逆运动学问题，使机械臂的雅克比矩阵可以通过逆运动解快速生成，提高了水下远程操作机器人系统的工作效率。Rolland等[4]于2009年针对并联机械臂提出了基于遗传算法的优化方法，将非线性方程组求解问题转化为逐个优化的过程，建立了逆运动学模型。国内有很多学者在机械臂逆运动学问题上也做出了相应的贡献[5-13]。

对于机械臂的逆运动学求解问题，还有很多新兴的方法，例如人工神经网络[14]、Groebner基法[15]等，这些方法虽然在理论上满足了相应的要求，但所消耗的计算时间一般较长，不能满足工业机械臂控制中对于快速性的要求，因此在实际的工业机械臂控制中，很少会用到这些方法。

本文在逆运动解的求取上选择了将几何法和欧拉角变换法相结合，将六自由度机械臂逆运动解的求取分为两部分：第一部分通过已知的机械臂末端位姿求出机械臂腕部点的坐标，利用几何法和机械臂腕部点的坐标求出前三个关节转过的角度；第二部分通过已经求得的前三个关节轴转过的角度和机械臂末端的姿态，利用欧拉角变换法求出后三个关节轴转过的角度。该方法的计算过程与单独使用解析法或者几何法相比更简便，实现该算法所需的代码长度也小于其他方法所需的代码长度，因此该算法所消耗的计算时间相对较短。本文最后通过VC6.0实现了六自由度机械臂在给定平面上写字的过程，从而验证了本文所提出的逆运动学求解算法的准确性和快速性。

1 运动学模型的建立

利用D-H方法对机械臂进行运动学建模已成为机械臂运动学问题研究的标准方法。其基本思想为：对每个关节轴指定一个参考坐标系；确定任意两个相邻坐标系的关系；得出由机械臂末端执行器至基坐标系的总变换矩阵。

1.1 机械臂关节坐标系的建立

六自由度机械臂如图1所示。根据机械臂的结构特点，使用D-H表示法确定各个旋转关节坐标系，定义机械臂的初始位置，然后建立基坐标系以及各个关节轴的连杆坐标系。由D-H表示法建立的坐标系如图2所示。

图1 机械臂结构图

1.2 机械臂关节变换矩阵与正运动学方程

建立连杆坐标系之后，根据相邻连杆坐标系确定机械臂的连杆参数表，机械臂的连杆参数与运动范围如表1所示。表中，表示相邻连杆绕公共轴线旋转的夹角，表示从公垂线与关节轴的交点的有向距离，表示关节轴和关节轴之间公垂线的长度，表示相邻关节坐标系之间的扭转角，各个关节角均以逆时针旋转方向为正。表中和的单位均为毫米。

表1 ：机械臂的连杆参数表

图2 各个关节轴的连杆坐标系示意图

根据相邻连杆关节坐标系间的齐次变换矩阵，可求出机械臂末端坐标系相对于基坐标系的变换矩阵。正运动学公式表示为

其中：

2 逆运动解的求解过程

逆运动学求解是根据给定的机械臂末端位置和姿态，求出与该位姿对应的六个关节轴转过的角度。该机械臂的后三个关节轴相交于一点，满足Pieper准则[16]，因此存在封闭解。

可以将机械臂逆运动学的求解分为两部分：第一部分是根据机械臂腕部点的坐标求解前三个关节角；第二部分通过已经求出的前三个关节角和给定的机械臂末端姿态矩阵求出后三个关节角。前三个关节角的求取采用平面几何法，后三个关节角的求取采用欧拉角变换法。

2.1 前三个关节角的求取

由图1所示的机械臂可知，此类机械臂在结构上有特殊性，即第一个关节轴在水平面内旋转，而第二个和第三个关节轴都是在竖直平面内旋转，因此，可以将机械臂腕部点分别投影到水平面和竖直平面上，利用平面几何的方法找出前三个关节角与机械臂腕部坐标的关系。

将机械臂分别向垂直于第二、三个关节轴的竖直平面和水平面投影，垂直于第二、三个关节轴的竖直平面投影图如图3所示，水平面投影图如图4所示。

由此可得到如下关系：

由式（4）可得

图3 机械臂的竖直平面投影图

图4 机械臂的水平面投影图

将式（2）和（3）进行平方和可得

由式（6）可得

其中

2.2 后三个关节角的求取

后三个关节角的求取选择欧拉角变换的方法，由已经求得的前三个关节角可以得到，机械臂末端姿态矩阵与其他旋转矩阵有如下关系：

由式（8）可得

其逆矩阵为本身，由此可得

2.3 逆运动学验证

利用MATLAB中的Robotics Toolbox建立六自由度机械臂的正运动学模型。根据机械臂的D-H连杆参数建立机械臂的正运动学方程，并绘制出机械臂的三维示意图，如图5所示。

图5 Robotics Toolbox中的机械臂三维示意图

表2：机械臂的正逆运动解

图5所示的机械臂位姿为六自由度机械臂的初始位姿，通过Robotics Toolbox建立机械臂的运动学模型后，就可以验证逆运动解的正确性了。

任意选取几组关节角，将每组关节角输入到机械臂的正运动学方程中，可以得到相应的机械臂位姿；将机械臂的位姿输入到VC6.0环境下的逆运动学算法中，可以得到各个位姿所对应的六个关节角；将这些关节角与输入到机械臂正运动学方程的关节角进行对比，即可验证逆运动解的正确性。

表2左侧为多组关节角下机械臂的运动学正解，表2右侧为相对应的末端位置和姿态下机械臂的运动学逆解。表2中没有给出各组关节角所对应的机械臂末端姿态矩阵，但实际上运动学正解会得到机械臂末端的姿态矩阵，运动学逆解的求解也需要该姿态矩阵，在此没有列出。

通过对比表2的左侧和右侧的关节角可得，在误差允许范围内，机械臂的逆运动学求解算法是可靠而准确的。

3 机械臂写字过程的实现

机械臂写字的过程是通过VC6.0编程实现的，如图6所示。程序定时读取在书写过程中所经过点的坐标，并将这些坐标数据存入到一个文本中；机械臂在运动时，对该文本中的数据进行处理，使其作为机械臂末端轨迹中间点的坐标；在这些中间点间进行多项式插值形成各个关节角的期望曲线，将曲线上的点定时发送到机械臂的运动控制器中，使机械臂末端按照期望轨迹进行运动。

图6 写字界面

机械臂的运动控制界面如图7所示。首先连接运动控制器；连接成功后，按下读取数据按钮，此时程序将从文本中读取屏幕上字体的坐标，并经过处理后作为机械臂末端轨迹的中间点。在运动前还要输入速度和姿态矩阵，速度是指机械臂末端的矢量速度，姿态矩阵是指机械臂末端在运动过程中所要保持的姿态。输入速度和姿态矩阵后按下轨迹运动按钮，机械臂末端将在一个指定的平面上按照屏幕上的字体轨迹运动。

在整个运动过程中，机械臂各个关节角度、角速度和末端的直角坐标将时时发生变化，程序实时读取机械臂各个关节的角度和角速度，机械臂末端的直角坐标通过其正运动学方程得到。

图7 机械臂的控制界面

在本文实验中，速度设置为0.2（机械臂末端的矢量速度），姿态矩阵选择单位阵，即在整个写字过程中保持机械臂末端是竖直向下的。设置完成后，按下轨迹运动按钮，机械臂在水平面上写出屏幕上的字，整个运动过程如图8所示。

将该算法的计算过程与单独应用几何法或代数法相对比，应用几何法的VC代码长度约为260行，而该算法所需的代码长度约为80行，通过QueryPerformanceCounter函数，获得高分辨率性能计数器的当前值；通过算法程序末端的计数器值与算法程序初始的计数器值相减，得到不同算法的计算过程所消耗的时间；测试多组位姿下不同算法所消耗的时间，最终得到：应用几何法所消耗的平均时间为0.49ms，应用本文中的算法所消耗的平均时间为0.27ms。在MATLAB中，利用tic-toc语句得到应用解析法所消耗的平均时间为0.33s。通过对比各个算法所消耗的平均时间，可以看出，本文提出的算法在快速性上要优于其他算法。

通过对比输入字体的形状与机械臂的实际运动轨迹，说明该逆运动学求解算法在准确性和快速性上都满足了相应的要求，能够应用于工业机械臂的实时控制中。

4 结论

针对一类具有典型结构的六自由度机械臂，在已知机械臂结构参数的基础上，采用D-H方法建立机械臂的连杆坐标系，得到六个关节的坐标变换矩阵，通过坐标变换矩阵连乘得到机械臂的正运动学方程；采用几何法和欧拉角变换法相结合的方法对机械臂逆运动学问题进行求解，通过逆解的选取原则选出最优解；利用MATLAB中的RoboticsToolbox验证逆运动学求解算法的正确性；最后，通过VC6.0编写机械臂写字程序，实现机械臂写字的过程。

图8 机械臂的运动轨迹

本文提出的逆运动学求解算法结合了几何法和欧拉角变换法，相比于单独使用几何法或代数法，该方法能使逆运动学的求解过程得到简化，通过VC中的QueryPerformanceCounter函数和MATLAB中的tic-toc语句得到不同算法所消耗的平均时间，经过对比所消耗的平均时间可以得出：该算法是一种较为快速简便的逆运动学求解算法，能够应用于工业机械臂的实时控制中。

[1] Paul R P, Shimano B E, Mayer G.Kinemaics control equations for simple manipulators [J]. IEEE Transactions on Systems, Man and Cybernetics, 1981, 11(6): 449-445.

[2] Regnier S, Ouezdou F B, Bidaud P. Distributed method for inveres kinematies of all serial manipulators [J]. Mechanism and Machine Theory, 1997, 32(7): 855- 867.

[3] Jun B H, Shim H W, Lee P M, et al. Workspace control system of underwater teleoperated manipulators on RO Vs [C]. Proceedings of OCEANS 2009 EUROPE. Piscataway, NJ: IEEE, 2009: 1-6.

[4] Rolland L, Chandra R. Forward kinematics of the 6-6 general parallel manipulator using real coded genetic algorithms [C]. Proceedings of IEEE/ ASME International Conference on Advanced Intelligent Mechatronics. Piscataway, NJ: IEEE, 2009: 1637-1642.

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

[6] 姜宏超, 刘士荣, 张波涛. 六自由度模块化机械臂的逆运动学分析[J]. 浙江大学学报, 2010, 44(7): 1348-1354.

[7] 付荣, 居鹤华. 高精度解耦六自由度机械臂逆运动学解法[J]. 计算机测量与控制, 2010, 18(7): 1637-1640.

[8] 刘金存, 鲁守银, 姜振廷,等. 一种液压机械臂逆运动学求解的新算法[J]. 制造业自动化, 2013, 35(6): 39-42.

[9] 廖锋. 六自由度机械臂逆运动控制方法[J]. 计算机工程与科学, 2013, 35(6): 174-179.

[10] 谢志江, 李诚, 刘楠, 等. 6自由度装校机械人逆解的确定[J]. 吉林大学学报（工学版）, 2012, 42(6): 1563-1568.

[11] 吕世增, 张大卫, 刘海年. 基于吴方法的6R机器人逆运动学旋量方程求解[J]. 机械工程学报, 2010, 46(17): 35-41.

[12] 焦有宙, 丁攀, 赵大旭. 温室3P3R机械臂系统动力学建模与分析[J]. 农业机械学报, 2012, 43(5): 179-183.

[13] 刘松国, 朱世强, 王宣银. 基于矩阵分解的一般6R机器人实时高精度逆运动学算法[J]. 机械工程学报, 2008, 44(11): 304-309.

[14] Jovanovic V T, Kazerounian K. Using chaos to obtain global solutions in computational kinematics[J]. Journal of Mechanical, 1998, 120: 299-304.

[15] 杭鲁滨, 王彦. 基于Groebner基法的一般串联6R机器人机构逆运动学分析[J]. 上海交通大学学报, 2004, 38(6): 853-856.

[16] Craig J J. Introduction to Robotics [M]. 北京:机械工业出版社, 2006.

本项目获国家自然科学基金资助，项目编号：61175089