朱森光

摘 要：使用OpenGL技术建立机械臂的三维模型，通过C++编程技术计算机械臂上的机械手末端作直线或曲线运动时对应的机械臂各关节角度的变化数据，使用这些角度数据去动态绘制机械臂三维模型实现机械臂运动的动画效果，机械臂运动时将轨迹数据和对应的关节角度数据记录下来供后续分析为机械臂设计和现场应用中的轨迹规划提供理论依据从而达到了运动学仿真的目的。

关键词：机械臂;机械手;运动学;工具中心点;OpenGL;仿真

中图分类号：TP242

文献标志码：A

文章编号：1007-757X（2020）11-0118-05

Abstract：OpenGL technology is used to establish the 3D model of the robotic arm. The C++ programming technique is used to calculate the changing data of the joint angle of the arm when the manipulator end on the arm is used for linear or curved motion. These angle data are used to dynamically draw the 3D model of the robotic arm， and output animation effect of the robot arm movement. The trajectory data and the corresponding joint angle data are recorded. They can be used for the subsequent analysis and provide a theoretical basis for the trajectory planning in the robot arm design and field application. Thereby it achieves the purpose of kinematics simulation.

Key words：robotic arm;manipulator;kinematics;TCP;OpenGL;simulation

0 引言

随着工业4.0时代的到来，越来越多的工厂使用机械臂代替人进行枯燥和重复性的劳动。机械臂实体结构复杂、体积庞大且价格昂贵，由于机械臂是运动装置使用不当可能还会带来一定的危险。借助于计算机图形仿真技术，通过电脑仿真研究机械臂成为一种既经济又安全的方法，本研究基于OpenGL编程技术[1-2]实现了一种机械臂运动学仿真的方案，可以让用户无需购买昂贵的机械臂实体，仅凭一台普通的电脑即可达到分析机械臂运动学特征和论证项目可行性的目的。

1 机械臂建模

建立机械臂模型是实现对机械臂运动学进行仿真研究的重要前提，本研究将以典型的六轴联动机械臂为例进行介绍，使用OpenGL技术建立的一个机械臂简单模型，如图1所示。

在图1中，在关节1处建立一个大地坐标系XYZ，关节1可以绕Y轴旋转;在关节2处建立坐标系X1Y1Z1，关节2可以绕Z1轴旋转;在关节3处建立坐标系X2Y2Z2，关节3可以绕Z2轴旋转;在关节4处建立坐标系X3Y3Z3，关节4可以绕X3轴旋转;在关节5处建立坐标系X4Y4Z4，关节5可以绕Z4轴旋转;在关节6处建立坐标系X5Y5Z5，关节6可以绕X5轴旋转;点O5是机械手在机械臂上的安装位置，在机械手末端建立坐标系X6Y6Z6。图1中椭圆圈住的区域就是机械手的示意模型。

2 运动学分析

2.1 齐次变换矩阵

齐次变换矩阵是机器人技术里最重要的数学分析工具之一，关于齐次变换矩阵的原理在很多文献[3-6]中已经描述过在此不再详述，这里仅描述使用齐次变换矩阵技术生成图1中机械臂末端点O5位姿矩阵的过程。

首先定义一些变量符号，在图1中，关节1绕Y轴旋转的角度定义为θ0，当θ0=0时，O1点在坐标系XYZ内的坐标是（x0，y0，0）;关节2绕Z1轴旋转的角度定义为θ1，图1中的θ1当前位置值为+90度;定义O1O2两点距离为x1， 关节3绕Z2轴旋转的角度定义为θ2，图1中的θ2当前位置值为-90度;定义O2O3两点距离为x2，关节4绕X3轴旋转的角度定义为θ3， 图1中的θ3当前位置值为0度;定义O3O4两点距离为x3，关节5绕Z4轴旋转的角度定义为θ4， 图1中的θ4当前位置值为-60度;定义O4O5两点距离为x4，关节6绕X5轴旋转的角度定义为θ5， 图1中的θ5当前位置值为0度。以上描述中的角度正负值定义符合右手法则，即当右手除大拇指以外的其余四个手指握住旋转轴并且四个手指指向旋转方向时如果此时大拇指指向旋转轴的正方向则旋转角度就是正值，反之就是负值。

下面根据以上定义的变量符号描述齐次变换矩阵。

根据以上奇次变换矩阵可以写出机械臂末端点O5的位姿矩阵C，如式（1）。

式中，Nx，Ox，Ax可以理解为末端点O5姿态的X轴投影向量;Ny，Oy，Ay可以理解为末端点O5姿态的Y轴投影向量;Nz，Oz，Az可以理解為末端点O5姿态的Z轴投影向量。

2.2 正运动学求解

正运动学求解就是求出式（1）中矩阵C的解析表达式，将式（1）展开后得到如下。

如果机械手末端点O6相对于坐标系X5Y5Z5的相对坐标为U（a，b，c），那么机械手末端点O6在大地坐标系XYZ中的坐标，如式（2）。

以上即为机器人正运动学的求解过程，Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz表达式中的x0、y0、x1、x2、x3、x4为机械固有尺寸，c0、c1、c2、c3、c4、c5分别为cosθ0、cosθ1、cosθ2、cosθ3、cosθ4、cosθ5的简写，s0、s1、s2、s3、s4、s5分别为sinθ0、sinθ1、sinθ2、sinθ3、sinθ4、sinθ5的简写。实际上C的子矩阵M=NxOxAxNyOyAyNzOzAz就是坐标系X5Y5Z5在大地坐标系XYZ下的姿态矩阵，（Px，Py，Pz）就是点O5在大地坐标系XYZ下的绝对坐标。很多应用还需要再定义一个机械手在坐标系X5Y5Z5下的相对姿态矩阵N=Nx′Ox′Ax′Ny′Oy′Ay′Nz′Oz′Az′，姿态矩阵N就是图1中坐标系X6Y6Z6在坐标系X5Y5Z5下的相对姿态，工具坐标U（a，b，c）和姿态矩阵N一般通过现场标定的方法[7-9]间接计算求得

机械手末端点O6的位姿矩阵D，如式（3）。

2.3 逆运动学求解

机器人逆运动学求解是根据位姿矩阵C反求六个关节的旋转角θ0、θ1、θ2、θ3、θ4、θ5的问题。为了便于求解，对式（1）进行变换，令矩阵S0=R0*T0，然后将式（1）等号两边同时左乘矩阵S0的逆矩阵S0-1，如式（4）。

式（4）展开后等式左边矩阵里的12个元素跟等式右边矩阵里的12个元素互相相等于是就得到如下12个化简后的方程组成的方程组如下。

机器人逆运动求解就是根据以上12个方程求出θ0、θ1、θ2、θ3、θ4、θ5，由于三角函数存在非单调性和周期性的特点，因此会导致存在多组解，此时可以根据机械手的运动连续性条件滤除多余的解，求出θ0、θ1、θ2、θ3、θ4、θ5后就可以用这些角度去编程控制机械手三维模型作动画运动实现运动学仿真了。

2.4 姿态控制

2.4.1 欧拉角法

机械手末端的姿态可以用欧拉角来表示，欧拉角可以描述一个坐标系相对于另一个坐标系的姿态，机械手末端坐标系X6Y6Z6相对于大地坐标系XYZ的姿态就可以用欧拉角来表示。欧拉角有三个角分别是俯仰角α、偏航角β和横滚角γ，在本研究建立的机械臂模型的坐标系中，欧拉角与前文所述的矩阵M和矩阵N的关系，如式（5）。

由于矩阵M是矩阵C的子矩阵，所以改变欧拉角就能间接改变矩阵C，根据矩阵C进行逆运动学求解就可以得出各关节的角度了，如图2所示。

图2示意了机械手的两种姿态，两种姿态之间渐变时保持机械手末端顶点的位置不变，仅仅改变机械手的俯仰角α，那么这种机械手本体绕末端顶点作俯仰动作的运动就是一种姿态控制运动，如图3所示。

图3则示意了一种平移运动，此时姿态角α、β、γ均保持不变，仅改变Px的值。

2.4.2 四元数法

除了可以用欧拉角法来控制姿态外还可以用四元数法控制姿态，欧拉角法在两个或两个以上的欧拉角同时变化时不能保证姿态渐变的线性平滑度，而四元数法则没有这个缺陷。

四元数表达式为Q=Qx*i+Qy*j+Qz*k+Qw，它有三个虚部和一个实部，假设A、B两个坐标系刚开始重合或平行，现在让坐标系B绕坐标系A中的单位矢量轴u=ai+bj+ck旋转一个角度θ，则坐标系B在坐标系A中的姿态可用四元数Q表示，如式（6）。

四元数Q与前文所述的矩阵M和矩阵N的关系，如式（7）。

3 运动仿真

运动仿真的对象是机械臂上安装的机械手，不同的应用场合需要安装不同形状和功能的机械手，很多资料上也将机械手称之为工具[9-10]，所以才有前文中的工具坐标U（a，b，c）这一描述。机械手的运动参考点也分好几种，有以机械手的质心为参考点的，有以机械手的形心为参考点的，也有以机械手上某一关键作用点为参考点的，运动参考点也称之为工具中心点（Tool center point，TCP），在本研究建立的机械模型中将以机械手末端点O6为运动参考点来作运动仿真。

3.1 直线运动仿真

设机械手末端点O6作直线运动时起始点在XYZ坐标系下的坐标为A（srcX，srcY，srcZ），终止点在XYZ坐标系下的坐标为B（dstX，dstY，dstZ），则直线上任一点的坐标可表示为P（（dstX-srcX）*k/N，（dstY-srcY）*k/N，（dstZ-srcZ）*k/N），其中N为总分割点数，k的范围为0至N。对每个分割点进行逆运动学求解得到N+1组关节角度θ0、θ1、θ2、θ3、θ4、θ5的值，按照时间序列依次调用OpenGL的API函数glRotated动态绘制机械臂三维模型形成机械手末端顶点直线运动的仿真效果，如图4所示。

3.2 圆弧运动仿真

假设已知圆弧上三点的坐标，首先根据三点确定三点所在平面的方程，联立三点到圆心的距离相等可以求得圆心坐标A（Cx，Cy，Cz）和半径r，同时也可以求得圆所在平面内从圆心出发的一对正交单位向量U和V。设P（x，y，z）为圆上任意一点，圆的参数方程，如式（9）—式（11）。

对式（9）-式（11）中的θ进行采样产生圆弧上一组点的坐标，对这些点的坐标进行逆运动学求解得到一组关节角度θ0、θ1、θ2、θ3、θ4、θ5的值，按照时间序列依次调用OpenGL的API函数glRotated动态绘制机械臂三维模型形成机械手末端頂点圆弧运动的仿真效果，如图5所示。

4 编程实现

通过前面的分析发现机械臂运动学方程是一组非线性方程组，由于各关节之间的正交性，所以存在关节角度θ0、θ1、θ2、θ3、θ4、θ5的解析表达式，可以用C++编程技术算出关节角度，使用关节角度编程控制机械臂三维模型动态刷新视图形成动画效果达到仿真的目的，如图6所示。

图6是使用C++开发工具开发的仿真软件，实现了以下功能。

（1） 通过界面输入六个关节角度进行正运动学求解得到机械臂末端的位姿矩阵C。

（2） 通过界面输入机械臂末端的位姿矩阵C进行逆运动学求解得到六个关节角度。

（3） 通过界面控制末端机械手的俯仰动作。

（4） 通过界面控制末端机械手做上下平移运动。

（5） 通过界面控制末端机械手做前后平移运动。

（6） 通过界面控制末端机械手做左右平移运动。

（7） 通过界面控制末端机械手的欧拉角。

（8） 固定姿态和渐变姿态作直线运动。

（9） 三点确定空间圆弧作圆弧运动。

5 总结

本研究通过建立合适的机械臂模型和相对坐标系，借助于齐次变换矩阵、欧拉角和四元数技术演算得出了机械臂正逆运动学的一系列公式，以机械手末端作直线和圆弧运动为例计算轨迹数据和对应的关节角度数据，使用这些数据去动态刷新机械臂三维模型形成动画效果从而达到了机械臂运动学仿真的目的。本方案具备详细的数据记录功能、良好的交互性和可扩展的界面框架，为后续更深层次的机械臂技术研究奠定了基础。

参考文献

[1] 乔林，费广正. OpenGL程序设计[M]. 北京：清华大学出版社，2000.

[2] 约翰·克赛尼希. OpenGL编程指南[M].北京：机械工业出版社，2017.

[3] 熊友伦，丁漢，刘恩沧. 机器人学[M]. 武汉：华中科技大学出版社，1993.

[4] 蔡自兴. 机器人学[M]. 北京：清华大学出版社，2000.

[5] John J Craig. 机器人导论[M]. 北京：机械工业出版社，2006.

[6] 杨静宇，李德昌. 机器人学[M]. 北京：中国科学技术出版社，1989.

[7] 李德钊，邓华. 基于改进型RLM算法的六轴机械臂运动学标定实验[J]. 测控技术，2019， 38（1）：42-48.

[8] 李祥云，王同合，范百兴，向民志. 基于激光跟踪仪的机器人工具坐标系标定[J]. 测绘科学与工程，2017（2）：19-22.

[9] 李宏胜，汪允鹤. 六关节工业机器人工具坐标系的标定[J]. 自动化技术与应用，2016， 35（8）：101-104.

[10] 陈欣欣. 工业机器人工具坐标系建立的教学策略浅谈[J]. 福建轻纺，2019（1）：47-50.

（收稿日期：2019.10.21）