手眼标定中RT矩阵的欧拉角和Halcon中pose的类型之间的关系

Posted 2021-05-24 苏州程序大白

手眼标定中RT矩阵的欧拉角和Halcon中pose的类型之间的关系

1、Halcon做3D相关的应用：

使用到halcon的CreatePose算子，生成不同的Pose，并且可以将pose通过算子pose_to_hom_mat3d转换成4*4的RT矩阵。每个pose的生成，都包括TX,TY,TZ,RX,RY,RZ和一个旋转顺序type决定。Halcon的描述中，type可以定义位’gba’,'abg’等常用模式，但是我们实际于机械手配合做项目时，不同厂家的机械手所对应的RX,RY,RZ或A,B,C的值都不一样，并且和halcon的描述类型也无法直接对应，那如何解决这个问题呢

RT矩阵，是包含了平移和旋转两个方面，其中3*3的R矩阵，就是旋转矩阵，也就是欧拉角RX,RY,RZ或A,B,C展开成矩阵形式的数值，这些数值都是由角度的sin或cos相乘相加得到的。

欧拉角就是空间三维旋转的角度，一般会有很多种表达方式，如XYZ,ZYZ,ZYX,XYX,XZY等，这些字母表示绕着该轴旋转某个角度，然后按照字母的顺序，把三次的二位旋转矩阵相乘，最后得到一个三维的旋转矩阵。理论上这些不同的type之间的角度都是可以转换的，从欧拉角到旋转矩阵R的转换无非就是把针对每个旋转轴进行旋转的角度和顺序不一致而已。如图所示，按照不同的字母顺序，将这些矩阵分别相乘，即可得到对应的旋转矩阵。这样我们就得到了旋转矩阵跟具体旋转角度之间的关系。

Halcon的算子pose_to_hom_mat3d也是同样的原理，将pose的RX,RY,RZ通过不同的顺序相乘，得到矩阵。那么最重要的如何确认项目中的机械手坐标值和halcon的pose类型之间的关系呢？下面以我个人目前的项目经验给大家总结一下：
常用的6轴机械手的坐标系（个人目前遇到的，希望其他朋友碰到能补充更多）为三种：
XYZ;
ZYX;
ZYZ。
XYZ对应Halcon的gba，ZYX对用Halcon的abg，ZYZ暂时没有Halcon直接对应的pose类型，但是有固定公式可以转换，稍后可以直接给出代码。
XYZ和ZYX两种类型只是矩阵相乘的顺序不同。当你知道自己使用的6轴机械手是XYZ时，你就可以在获取机械手坐标的时候，可以直接根据坐标值创建Halcon的pose变量，这样在做手眼标定或引导抓取的时候就可以一一对应而不会出现数值差别巨大的问题了。
PS：目前市面上的大品牌机械手，我有用过的ABB机械手对应的欧拉角为ZYZ，Denso机械手对应的欧拉角为ZYX，APE机械手对应的欧拉角为ZYZ。如何判断，其实可以根据机械手本身坐标的叫法，一般叫做”RX,RY,RZ“这种可能是ZYX或XYZ；叫做”A,B,C“的可能是ZYZ。

ZYZ模式下的角度转换思路，以及不同欧拉角模式下的角度转换公式，现在分享给大家：

在上面中我们曾经说过：欧拉角转换成旋转矩阵的过程，实际上是将每一步旋转得出的二维旋转矩阵相乘，得到一个33的三维旋转矩阵，其重点在于计算出每一步绕的旋转轴得出的二维旋转矩阵。而所谓的ZYZ旋转模式，坐标系在旋转的时候，先绕着Z轴旋转角度A，再绕着新生成的Y轴旋转角度B，再绕着新生成的Z轴旋转角度C。最终将三个二维旋转矩阵相乘，得到三维旋转矩阵。在这里的计算顺序是RARBRC.
在这里可能细心的朋友会发现，按照顺序来进行矩阵相乘的话，应该是RA左乘RB，再左乘RC，也就是RCRBRA才合理，为什么我们这里给出的公式是反过来的呢？这里涉及到另外一个很重要的技巧，就是在这个计算公式中左乘和右乘的区别：

对于RARB*RC这个公式而言，从右往左看这个公式的时候，所有的旋转轴都是原始坐标系的旋转轴：先绕着Z轴旋转角度C，再绕着初始的Y轴旋转角度B，再绕着初始的Z轴旋转角度A；从左往右看这个公式的时候，所有的旋转轴都是新生成的坐标系的旋转轴：先绕着Z轴旋转角度A，再绕着新生成的Y轴旋转角度B，再绕着新生成的Z轴旋转角度C。这个矩阵相乘的左右乘法原则，可以引申到任何坐标系的转换中，未来如果有机会分享3D引导抓取或者示教反面的技术细节将会再次提到这个点，方便理解。

因此通过上面的分析我们可以得到ZYZ模式下的欧拉角转换成旋转矩阵的方式，同时就可以解决之前大家提到的另外一个问题，如何将ZYZ的转角和XYZ的转角进行互换？最简单的原则就是通过旋转矩阵来进行中转，因为旋转矩阵是保持不变的，变化的只是旋转的模式和角度，因此将ZYZ角度转换成旋转矩阵，再将旋转矩阵转换成XYZ的角度即可（这一步可以参考我的上面公式）。

最后，大家可以根据上面那张旋转顺序图，自己理解一下，是不是按照我上面说的左乘和右乘的两种顺序模式都可以到获得同一个旋转结果。

最关心的ZYZ模式下如何转成halcon的pose，这个在halcon的例程中是有的，大家可以直接打开那个例程去看里面的内容，并且我会直接贴上使用C#调用的函数：

halcon例程：从机械手的pose转换成halcon的pose：hand_eye_create_robot_poses.hdev运行程序后先根据提示选择选择类型，halcon已经直接给出了这三种类型，而转换函数是input_pose (PoseType, WindowHandle, Pose)，大家直接点进去即可看到。
        public void Get_pose_zyz(HTuple x, HTuple y, HTuple z, HTuple a, HTuple b, HTuple c, out HTuple Pose)
        {
            HTuple hv_HomMat3DIdentity = new HTuple();
            HTuple hv_HomMat3DTranslate = new HTuple();
            HTuple hv_HomMat3DT_Rl = new HTuple();
            HTuple hv_HomMat3DT_Rl_Rm = new HTuple();
            HTuple hv_HomMat3D = new HTuple();
            HTuple hv_pose1 = new HTuple();
            HOperatorSet.HomMat3dIdentity(out hv_HomMat3DIdentity);
            HOperatorSet.HomMat3dTranslate(hv_HomMat3DIdentity, x.D, y.D, z.D, out hv_HomMat3DTranslate);
            HOperatorSet.HomMat3dRotateLocal(hv_HomMat3DTranslate, a.TupleRad(), new HTuple("z"), out hv_HomMat3DT_Rl);
            HOperatorSet.HomMat3dRotateLocal(hv_HomMat3DT_Rl, b.TupleRad(), new HTuple("y"), out hv_HomMat3DT_Rl_Rm);
            HOperatorSet.HomMat3dRotateLocal(hv_HomMat3DT_Rl_Rm, c.TupleRad(), new HTuple("z"), out  hv_HomMat3D);
            HOperatorSet.HomMat3dToPose(hv_HomMat3D, out hv_pose1);
            Pose = hv_pose1.Clone();
        }

最重要的，如果不适用halcon的代码， 要自己实现ZYZ到XYZ角度的转换，那还是要使用我上面说的先从角度转换成矩阵再转换成角度的方式，这种方式原理和逻辑很简单，但是计算代码比较复杂，是纯粹的数学公式解方程而已，这里就不贴了，相信各位高手自己都可以解决。

关注苏州程序大白，持续更新技术分享。谢谢大家支持