基于坐标系偏移的机器人运动学分析及运动仿真

徐 舟

(库卡机器人制造(上海)有限公司，上海 201616)

0 引言

要控制机器人进行精确的焊接仿真，实现自动化焊接的工业作业，首先要解决机器人运动学建模和逆向运动学的求解问题。

目前主流的运动学建模分析方法有DH建模[1]和旋量理论[2]这两种方法。前者较后者更为常用，更容易上手，后者对数学的理解和掌握更高。DH建模用4个参数表示一个坐标系偏移量，分别是绕z轴旋转θ角，然后沿z轴平移d距离，再沿x轴平移a距离，最后绕x轴旋转α角，虽然只用了4个参数，但是可以看到该方法没有考虑y轴的影响，也就是当相邻的两关节轴平行时，关节轴间的公法线随着轴姿态微小的变化而引起大幅度变化[3]，从而导致在机器人误差辨识中不得不再新增一个绕y轴旋转β角来解决该问题[4]。故本文采用基于坐标系偏移的建模方法实现库卡KR10机器人正向运动学建模和逆向运动学求解。基于坐标系偏移的建模方法虽然需要7个参数，但参数共分为两大类，先是3个平移参数，后是4个旋转参数，易理解、易记忆、易使用，建模效率反而比DH的高。为了直观地进行三维仿真，本文采用C#的3D开源库Helix构建了库卡KR10机器人、焊枪、焊板等三维模型，并将Helix控件嵌入基于WPF开发的仿真软件中，实现了焊接运动仿真。

1 基于坐标系偏移的建模方法

如图1所示，根据齐次坐标变换理论[5]，可得从坐标系i到坐标系i+1的坐标系偏移公式：

图1 坐标系偏移原理

Ti=Trans(xi,yi,zi)Rot(z,Ai)Rot(y,Bi)×
Rot(x,Ci)Rot(?,θi).

(1)

其中:Trans(xi,yi,zi)表示依次平移xi,yi,zi距离;Rot(z,Ai)表示绕z轴旋转Ai;Rot(y,Bi)表示绕y轴旋转Bi;Rot(x,Ci)表示绕x轴旋转Ci;Rot(?,θi)中的旋转轴编号0表示坐标系可绕z轴旋转θi角度，1表示绕y轴旋转θi角度，2表示绕x轴旋转θi角度。公式(1)中共计7个参数，前3个为平移，后4个为旋转。

式(1)中的各项展开如下：

参考文献[1],标准DH建模公式如下：

Ti=Rot(zi-1,θi)Trans(zi-1,di)Trans(xi,ai)Rot(xi,αi).

(2)

其中:Rot表示绕某个轴旋转;Trans表示沿某个轴平移。

对比式(1)和式(2)可看出，基于坐标系偏移的建模方法更易记忆，更有规律，先是xyz，再是ABC，最后是绕某个轴的旋转。

2 正向运动学

根据以上基于坐标系偏移的建模方法可得6轴机器人的建模公式如下:

(3)

结合库卡KR10机器人的SolidWorks 3D图可得如图2(b)所示运动学建模。根据图2可得如表1所示的机器人建模参数，其中旋转轴编号根据Rot(?,θi)得到。

表1 库卡KR10机器人坐标系偏移参数

图2中带箭头的3/4圆表示绕某个轴旋转，这表明该方法不再局限于DH建模中的不能绕y轴旋转。后三个坐标系交汇于一点，满足Pieper原则[6]。

图2 6R机器人的SolidWorks图和坐标系偏移建模

3 逆向运动学

(4)

将表1中平移参数中的非0数值分别表示为x2,z1,z2,z3,z4,z5,z6，联立式(3)和式(4)得：

(5)

并令：

(6)

展开可得：

其中：si和ci分别表示sinθi和cosθi，下文亦然。

(1) 求解θ1。观察L、R矩阵的(2,4)元素，可得到：

pyc1-z6(ayc1-axs1)-pxs1=0.

(7)

推导可得到θ1的两个解：

(8)

(2) 求解θ3、θ2。因矩阵L、R的(1,4)和(3,4)元素分别相等，通过推导首先可得到θ3的两个解：

(9)

继续推导可得θ2的两个解：

θ2=atan2(±I,sqrt(E2+F2-I2))-atan2(F,E).

(10)

(3) 求解θ5、θ4、θ6。因矩阵L、R的(1,3)和(3,3)元素分别对应相等，经推导可得θ5的两个解：

(11)

其中:L1=Jc3-Ks3，J=axc1c2-azs2+ayc2s1,K=azc2+ays1s2+axc1s2。

因矩阵L、R的(2,3)元素对应相等，可得：

s5s4=ayc1-axs1.

(12)

根据式(12)，当θ5=0时，有无穷θ4的解，为统一起见，默认当θ5=0时，θ4=0。故当θ5≠0时有：

(13)

因矩阵L、R的(2,2)元素对应相等，通过推导可得θ6的两个解：

(14)

其中:N=oyc1-oxs1；O=nyc1-nxs1；P=c4。

到此已完成了所有旋转轴θ的求解，并用C#代码实现了以上数学公式的编程和单元测试。

4 仿真软件开发

4.1 WPF和Helix简介

WPF[7]是微软推出以C#语言为基础的基于Windows桌面系统开发的界面框架。Helix[8]是开源的可直接嵌入WPF界面中进行二次开发的3D控件。

4.2 仿真软件框架设计和界面效果

仿真软件的框架基于FluentRibbon、AvalonDock、Prism、MEF等开源库进行二次开发，支持插件化开发，整体架构如图3所示。

图3 仿真软件架构

基于上述框架开发的仿真软件运行效果如图4所示。

图4 仿真软件实际运行效果图

5 运动学验证和焊接仿真

5.1 正向运动学仿真和验证

表2 正运动学轴坐标输入和位姿矩阵关系

5.2 逆向运动学仿真和验证

在仿真软件中，根据表2右侧的位姿矩阵编写单元测试，可获得表3中的8组逆解数据。表3的灰色行表示的一组解与表2轴坐标一致，该计算表明了逆解的正确性。

表3 根据表2右侧的位姿矩阵计算得到运动学逆解 (°)

5.3 最优解的选择

图5 最短行程法计算流程

5.4 焊接仿真验证

为模拟焊枪在焊板上自动作业一条直线,先通过导入界面导入焊枪模型，并附加到机器人法兰上，然后在导入界面导入焊板模型，最后在机器人属性界面中输入模型的焊缝起点(见图6)和终点坐标(见图7)。

图6 示教焊缝起点 图7 示教焊缝终点

每次输入坐标后可在如图8所示界面记录示教坐标。

图8 示教后在作业图界面中记录坐标

将焊缝直线细分100个点后，结合最短行程法，单击仿真按钮可得焊缝仿真效果，如图9所示。

由图9可看到机器人沿焊板边缘直线走出了一条直线，再次表明了本文的正向和逆向运动学分析的正确性。

图9 焊板的焊缝仿真效果

6 结论

(1) 通过开发软件的运动仿真表明：基于坐标系偏移的建模方法是正确的，每一个偏移矩阵虽有7个参数，多于DH的4个参数，但其实也就两大类参数，先平移后旋转，更易理解和使用。

(2) 基于Helix和WPF开发的运动仿真软件，可直观地可视化三维虚拟世界，实现了焊板的焊缝焊接仿真，所见即所得，为后续实现更复杂的自动化焊接等需求奠定了基础。