从头手搓一台ros2复合机器人（带机械臂）

一.前言

        从今天开始跟着小鱼老师学习了机器人运动的入门篇并记录了自己的学习过程，这一部分主要是学习空间坐标系中位姿之间的关系，第一遍学习时还是有点懵逼，但我将学习的内容加入了自己的见解，理解起来可能会相对容易一些，课后有一道练习题，大家记得看一下。好的，话不多说，马上开始！

二.ros2学习

         前四小节内容可以订阅往期博客。

5.空间坐标描述

首先，我们要明白无论是在二维空间还是在三维空间，我们想要描述一个物体的位置和姿态第一步就是确定一个参考坐标系，物体的位置和姿态描述我们都是以这个坐标系作为参考的。（注意：不要混淆位置和姿态的概念）

参考坐标系，这一点很重要，因为没有绝对的坐标，只有相对的坐标。

假如我们确定了一个空间直角坐标系A,我们就可以使用x,y,z来确定A坐标系中任意一点P的位置，可以记作：

再比如：

所以参考坐标系{A}中一点P也可以写作：

好，位置的表示大概就是以上内容，接下来介绍一下什么是姿态。

我们已经知道坐标系A中的一个点P的位置，我们如何描述P点在{A}坐标系下的姿态呢？

上图是使用RVIZ2绘制的，红色代表x轴，绿色代表y轴，蓝色代表z轴。

从图中可以看出，坐标系{P}的每一个轴和参考坐标系的每一个轴之间都有一个角度，比如Px轴和Ax,Ay,Az三个轴之间存在三个角度，通过这三个角度我们就可以确定Px轴和参考坐标系{A}之间的关系，以此类推，我们也可以确定Py轴和Pz轴分别与{A}之间的关系。

每个轴的相对姿态关系确定了，坐标系之间的姿态也就确定了，点的姿态也就确定了。

我们将上述坐标系{P}的三个轴相对于参考坐标系{A}三个轴的共九个角度的余弦值，组成一个3*3的矩阵，该矩阵就是旋转矩阵，因该矩阵是{P}相对于{A}的姿态关系的表示，故记作：

两个向量的点乘为两个向量的长度与它们夹角余弦的积,所以可以表示为单位向量Px​与Ax​的点积，旋转矩阵就可以写为下面的形式

注意：

• 当两个坐标系之间姿态没有变化，即坐标系间x,y,z轴方向对应重合，则旋转矩阵为单位矩阵
• 从矩阵的角度看，矩阵的逆等于矩阵的转置，则该矩阵为正交矩阵，显而易见，旋转矩阵是正交矩阵

 okok，接下来就是个人感觉最难理解的部分啦，希望能挨个字认真理解

        通过位置矢量我们可以描述一个点在特定参考坐标系下的位置，通过旋转矩阵可以描述一个点在特定参考坐标系下的姿态。

        在机器人当中位置和姿态一般会一起出现，所以我们将其组合就叫做位姿。（位姿=位置+姿态）。书面性的概念我就不过多介绍了，接下来根据具体应用例子来理解。

        位姿是相对的，同一个物体在不同的参考坐标系下的位姿数据肯定是不同的。比如在手眼系统中，我们可以通过视觉算法获取到工件坐标系P与相机坐标系C之间的关系，我们想要控制机械臂的末端运动到工件坐标系P进行夹取，那么我们就要知道工件坐标系P在机器人基坐标系B下的位姿，这就需要进行坐标变换了。

5.1 平移坐标变换

如图，坐标系{A}、{B}、{C}的姿态是相同的，其之间的姿态对应的旋转矩阵都是单位矩阵。（图片

中一个格子长度为1）

坐标平移方程：

5.2 旋转坐标变换

如果坐标系之间姿态不同，同一个点的位姿在不同参考姿态下的位置和姿态也会不一样。

上图中，坐标系{A}鱼坐标系{B}原点重合

这个问题就稍微有点复杂了，我们一步步来说：
1.因为{B}鱼{C}姿态相同，所以{A}参考坐标系下{C}的姿态和{A}参考坐标系下{B}的姿态是相同的，即：

3D Rotation Converter我们可以使用这个工具进行验证：

我们接着来举一反三

{B}绕着{A}的z轴旋转了45度我们计算出来了，那如果{B}绕着{A}的z轴旋转θ角度该如何呢?

如果不是绕着z轴旋转，而是绕着x轴或y轴呢？依葫芦画瓢都能算出来

注意：{C}的参考坐标系从{B}变成了{A},{C}在空间中的位置并没有发生变化

5.3 平移旋转复合变换

        在我们平时搞机器人时，一般情况下两个坐标系原点不重合姿态也不相同。

        我们将坐标变换拆分成先绕参考坐标系旋转，再绕参考坐标系平移两步，这样我们就得到了坐标的复合变换方程

        说白了，我们可以先假想一个坐标系A'（姿态与A相同）与坐标系B原点重合，然后先用旋转坐标变换把坐标系C的分量变换到A'上，最后再通过平移坐标变换把A'还原到A

5.4 右手坐标系

用右手确定的坐标系就是右手坐标系

拿起右手，先给竖起大拇指，然后打开手掌，将大拇指的方向朝向下图中的蓝色z轴，让剩下的四根手指朝向红色的x轴，此时朝向手心外的就是绿绿的y轴的方向了。

5.5 课后练习

        算旋转矩阵时，第一列的三个值分别是用所求坐标的x轴与参考系的x，y，z轴夹角的余弦值，第二列的三个值分别是用所求坐标的y轴与参考系的x，y，z轴夹角的余弦值，第三列的三个值分别是用所求坐标的z轴与参考系的x，y，z轴夹角的余弦值