基于实时逆运动学算法的6R 机器人三维仿真*

黎柏春，杨建宇，耿 磊，于天彪，王宛山

(东北大学 机械工程与自动化学院，沈阳 110819)

0 引言

近年来机器人逆运动学一直是人们研究的重点。然而机器人运动学的逆解具有一定的难度，特别是针对6R 机器人的逆解，许多学者在这方面已做了大量的工作［1，5］。目前机器人运动学逆解的方法主要有两种:解析法和数值法。解析法运算速度快，实时性好，但只有机器人结构满足一定的几何条件才可求得相应的逆运动学解析解。数值法适用于6R 机器人的大多数情况，但无法得到全部解，而且有迭代不收敛的危险。因此工业上广泛应用的6R 机器人其几何结构均满足求得逆运动学解析解的条件［4］。由于机器人运动学逆解存在多解的情况，因此在求得其逆解后需根据实际情况，以无碰撞、轨迹连续和行程最小等原则选取最优解，并进行后续的轨迹规划等研究。为了直观地选取机器人的逆解，分析机器人的工作过程，机器人三维仿真已成为机器人研究的重要手段。例如文献［6］开发了机器人仿真工具箱;文献［7］对五自由度的机械臂进行了建模和仿真研究;文献［8］基于OpenGL 对机器人进行了三维仿真研究;文献［9］介绍了一种新颖的构建机器人3D 仿真环境的方法。但在这些研究文献中，机器人的逆运动学求解基本采用数值解法，计算效率较低，难以求解得到逆解的所有解［10-11］，而且未结合轨迹规划和实际应用进行仿真。

1 6R 机器人运动学模型和运动学逆解

1.1 6R 机器人运动学模型

本文研究的六自由度工业机器人，其结构如图1所示。根据机器人结构和D-H 参数法为机器人建立坐标系如图2 所示，第n个关节的坐标系为Xn-1Yn-1Zn-1，X6Y6Z6为机器人末端执行器坐标系。为机器人建立好关节坐标系后，根据机器人的几何参数(如图2 所示)可得到D-H 参数表，如表1 所示。由于关节坐标系是在图1 所示的机器人姿态，为保证θ1，θ2…θ6的实际意义与机器人控制系统中的一致，因此参数表中θn有初始角和正负情况。

图1 六自由度工业机器人的三维模型

图2 机器人关节坐标系

相邻关节坐标系的四步变换都是相对于当前坐标系的，因此所有的矩阵都是右乘，从而可得到两个相邻关节坐标系的变换矩阵式(1)。为了简化表达，本文将sin(θi)记为Si，将cos(θi)记为Ci。根据表1 中的参数和式(1)可得到机器人的总变换矩阵，即机器人正运动学模型，式(2)。

表1 机器人的D-H 参数表

续表

1.2 机器人运动学逆解

机器人逆运动学是进行机器人轨迹规划的基础，更是进行机器人仿真和控制的前提。机器人逆运动学求解是根据机器人末端执行器的期望位姿，求出机器人各个关节的角度。根据机器人末端执行器的期望位置可求得等式(2)中右边的矩阵，由式(2)两边的矩阵元素相等可得到12 个方程。由于等式(2)中右边矩阵中的旋转矩阵(同时在前三行和前三列的部分)的各个分量线性相关，即九个方程实际为三个方程。因此实际为六个方程求解六个未知数，理论上可求得各关节变量。对于本文研究的6R 机器人而言，属于末端三个腕关节的轴线相交于一点的垂直关节型6R 机器人(如图1 所示)，根据文献［12］可知，满足可求得解析解的条件，最多存在8 组逆解。虽然由上述的12个方程通过逐步消元法可求解得到逆解，但求解过程复杂，容易出现增根和漏根的情况。因此本文引入复指数，通过矩阵变换求解运动学逆解。

用矩阵Mi表示Ti(di)Ti(ai)Ri(αi)，Ri表示Ri(θi)，由式(1)和(2)可得

引入矩阵P

对式(16)作如下构造，可对角化变换矩阵Ri

式(3)通过上面的变换，左右两边为4 ×4 阶的复数矩阵，利用左右两边矩阵的元素相等可求解得到运动学逆解如下。

式中，k为整数，六个式中k的取值可不同，但必须使θ1，θ2…θ6的值在关节角的转动范围内，其中:

1.3 运动学模型和运动学逆解的验证

对于机器人运动学模型的验证可借助于三维建模软件。在工业机器人三维模型环境下为其各个关节设定好初始角度(如表2 所示)，然后选择空间任意一点P，分别测量得到该点在X0Y0Z0坐标系和X6Y6Z6坐标系下的坐标如图3 所示。将设定的关节角度代入机械人正运动学模型(式(2))，可将P点在X6Y6Z6坐标系下的坐标变换为X0Y0Z0坐标系下的坐标，如式(4)所示。通过正运动学模型计算得到的坐标与测量得到的坐标基本相等，由此可验证机器人正运动学模型的正确性。

表2 机器人各关节的初始化角度

图3 空间点坐标测量图

对于机器人逆运动学的验证，同样取表2 中的关节角，通过机器人运动学模型得到机器人期望姿态即A，代入逆运动学解析解中可求得所有该期望位姿所对应的关节角如表3 所示。由此可验证运动学逆解的正确性。

表3 期望位姿的全部逆解

2 多项式插值轨迹规划算法

由于本文研究的6R 机器人其主要工作任务是为机床自动装卸工件，只需控制机器人末端执行器起点、终点以及中间几个点的位置和姿态，因此关节空间的轨迹规划即可满足要求。本文采用应用最多的多项式插值规划算法来保证关节位置、速度和加速度在时域的连续性和稳定性。

式(5)即为用于插值的多项式，t为机器人工作过程中的时间。式中i值(i次多项式)由约束条件决定。分别设机器人起点和终点的关节位置为θn(t0)和θn(tf)，为防止机器人在启动和停止时由于速度和加速度突变产生振动，保证机器人精确抓取和安放工件，因此起点和终点位置速度和加速度均为零，即˙θn(t0)=0 ，˙θn(tf)=0 ，¨θn(t0)=0 和¨θn(tf)=0 。六个约束，即采用五次多项式插值。

将起点和终点位置的约束条件带入式(6)中可求得a0，a1…a5再带回式(6)中就可得到机器人关节在任意时间的位置，速度和加速度，即运动规律。

3 机器人三维仿真平台构建

基于前面的运动学模型，运动学逆解和五次多项式的轨迹规划算法，利用MATLAB 的Simulink 3D Animation 构建集成机器人运动学和轨迹规划模型的实时三维仿真平台，其效果如图4 所示。该平台由机器人控制面板和三维可视化窗口组成，通过控制面板可实现与机器人的实时交互仿真，三维可视化窗口则可实时显示机器人的仿真状态。

图4 机器人实时三维仿真平台

4 应用实例

以机器人抓取和安装工件为实例在构建的三维仿真平台上进行仿真。机器人工作过程中需要控制的关键位置如图5 所示。根据图5 的机器人期望位姿通过三维仿真平台可得到相应的关节角如表4 所示(由于逆解存在多解，本文根据图5 的位姿选择相应的逆解)。

图5 机器人的期望位姿

表4 期望位姿的逆解

机器人抓取和安装工件过程可分为三段路径，设三段路径所花时间均为5s，由此通过仿真平台可得到机器人末端执行器的轨迹、关节轨迹曲线、角速度曲线和角加速度曲线如图6 和图7 所示(由于篇幅所限，本文只列出第二关节的相关曲线)。由图7 的曲线可以看出关节轨迹曲线平滑，角速度曲线和角加速度曲线连续，从而避免机器人的振动，保证机器人在工作中的稳定性，有利于机器人精确到达预定位置。

图6 末端执行器轨迹

图7 第二关节的关节轨迹、角速度和角加速度曲线

5 结论

(1)在机器人逆运动学求解过程中，引入复指数，从而简化了逆运动学的求解过程，减少了计算量，求解过程既没有增根也没有漏根，得到的逆运动学解析解可保证机器人三维仿真平台的实时性。

(2)构建的具有较强交互性的机器人实时三维仿真平台，使机器人的仿真更为直观有效。而且可实时计算得到机器人的运动学、逆运动学和轨迹规划结果，对机器人的进一步研究具有重要意义。

(3)由应用实例可得出构建的三维仿真平台可靠有效，五次多项式的轨迹规划算法也可使机器人在自动抓取和安装工件过程中运行稳定，可避免机器人振动，有利于机器人精确到达预定位置。

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

［2］Ali T，Hasan，A.M.S. Hamouda etc. An adaptive -learning algorithm to solve the inverse kinematics problem of a 6 D.O.F serial robot manipulator［J］. Advances in Engineering Software，2006(37):432 –438.

［3］刘达，王田苗. 一种解析和数值相结合的机器人逆解算法［J］. 北京航空航天大学学报，2007，33(6):727 -730.

［4］倪振松，廖启征，魏世民，等. 空间6R 机器人位置反解的对偶四元数法［J］. 机械工程学报，2009，45(11):25 -29.

［5］王其军，杜建军. MOTOMAN 机器人逆运动学新分析［J］. 2010，42(3):451 -454.

［6］Kucuk S，Bingul Z. An off-line robot simulation toolbox［J］. Computer Applications in Engineering Education，2010，18(1):41 -52.

［7］Mohammed Abu Q，Iyad A，Hatem E. Modeling and simulation of 5 DOF education robot arm［C］// Advanced Computer Control (ICACC ). Shenyang: IEEE， 2010:569 -574.

［8］陈素丽，任福深. 基于OpenGL 的弧焊机器人仿真设计［J］. 科学技术与工程，2012，12(3):543 -550.

［9］甘亚辉，戴先中. 一种高效的开放式关节型机器人3D 仿真环境构建方法［J］. 机器人，2012，34(5):628 -633.

［10］Zhang Pei-yan，Lu Tian-sheng，Song Li-bo. RBF networksbased inverse kinematics of 6R manipulator［J］. Advanced Manufacture Technology，2005(26):144 -147.

［11］Ali T，Hasan，A.M.S. Hamouda etc. An adaptive learning algorithm to solve the inverse kinematics problem of a 6 D.O.F serial robot manipulator［J］. Advances in Engineering Software，2006(37):432 -438.

［12］周刚. 垂直关节型6 自由度机器人笛卡尔空间轨迹规划的研究［D］.广州:华南理工大学，2011.