将机械手与CodeSys中的运动学模型绑定

1.背景介绍

最近搞到了一台工业机械手，虽然这个机械手有自己的控制程序，但是我们还是想用CodeSys来进行运动控制，测试一下它的运动控制功能是否可用。
下面介绍一下如何将CodeSys中的运动学模型与现实的机械手进行绑定（映射、对应，使用你喜欢的术语）。

2.选定运动学模型

上图左侧为实物六轴机械手的机械结构及各个轴的旋转方向。
上图右侧为CodeSys中的运动学模型：TRAFO.Kin_ArticulatedRobot_6DOF。

可以看到，物理机械手与运动学模型的每个关节的相对转动方向都是能够对应上的。也就是说，CodeSys中的运动学模型是能够用于实物六轴机械手的运动学控制的。

3.机械手各尺寸的对应

假如我们需要在CodeSys中使用六轴机器人模型，则必须先将下面的7个参数配置好：d1、a1、a2、d3、a3、d4、d6。只有这7个参数与现实世界的机械手对应上，才能对现实的机械手进行准确、精确的控制。

那么这七个参数d1、a1、a2、d3、a3、d4、d6分别代表的是啥？我们来看一下CodeSys的示意图

从上图可以看到，这七个参数代表的意义为：（世界坐标，轴的颜色RGB与轴的对应XYZ）
d1：机械坐标系的XY平面到关节2（J2）轴线的Z方向距离。
a1：关节2（J2）轴线到关节1（J1）轴线的X方向距离。
a2：关节2（J2）轴线到关节3（J3）轴线的Z方向距离
d3：关节4（J4）轴线到关节1（J1）轴线的Y方向距离
a3：关节3（J3）轴线到关节4（J4）轴线的Z方向距离
d4：关节3（J3）轴线到关节5（J5）轴线的X方向距离
d6：法兰盘平面与关节5（J5）轴线的X方向距离
结合目前我们实际机械手的尺寸及结构,
这7个参数应该为：
d1: 345
a1: 20
a2: 260
d3: 0
a3: 20
d4: 280
d6: 78
d3为0是因为我们的物理机械手的J4、J1的轴线在同一个平面内。

4.总结

4.1.选择正确的运动学模型

可能运动学模型有些结构一眼看上去和实际的物理机械手结构不一样，但只是距离为0而已。一定要确保能够对应上才能进行后面的操作。

4.2.注意各个关节旋转的正方向。

有些机械手在出厂时会在机械手上标注好各个关节旋转的正方向，但是请忘记这些标记的方向。因为现在我们用的是自己的运动学模型，因此必须要按照我们运动学模型指定的旋转方向来规范各个轴。
轴的旋转正方向可以根据示意图的箭头，按照右手螺旋法则确定。（是的，和电磁感应的右手法则方向是一致的）

4.3.编码器零点与机械零点的偏移修正。

由于安装的原因，机械手的各个轴的编码器在走到零点时，可能实际上还没走到机械上的零点（两个半键槽对齐，如下图所示）。

也就是说，编码器零点与机械零点没对齐。由于我们的运动学模型要求各个轴的零点必须要处于模型指定的位置（各个轴与机械坐标系的某个轴平行）。否则，由于各个轴的空间关系，各个轴的偏移误差会产生非线性累加，导致最终的运动学求解结果与实际位置相差甚大。因此，必须要保证各个轴的模型零点与机械零点保持一致。
这个时候其实只要在软件轴上进行偏差修正就行。也就是各对各个轴的fOffsetPosition赋值就可以了。这个补偿的正负号可以通过测试确定，感觉直接填机械零点时轴的读数即可。