《动手学机器人学》第四节(下):一文搞懂坐标变换

本系列教程作者：小鱼
公众号：鱼香ROS
QQ交流群：139707339
教学视频地址：小鱼的B站
完整文档地址：鱼香ROS官网
版权声明：如非允许禁止转载与商业用途。

目录

• 《动手学机器人学》第一节：数学基础–矩阵
• 《动手学机器人学》第二节：MiniConda、JupyterNotebook与ROS2
• 《动手学机器人学》第三节：动手学数学基础，使用NumPy操作矩阵
• 《动手学机器人学》第四节(上):位姿描述
• 《动手学机器人学》第四节(下):一文搞懂坐标变换
• 《动手学机器人学》第五节（上）：动手学空间姿态描述与变换
• 《动手学机器人学》第五节（中）：使用ROS2的TF2进行坐标变换
• 《动手学机器人学》第五节（下）：使用Python操作TF
• 更新中…欢迎关注小鱼公众号鱼香ROS，第一时间获取更新

正文

5.坐标变换

位姿是相对的，同一个物体在不同的参考坐标系下的位姿数据肯定是不同的。在后续的学习和使用当中，我们会经常需要求同一个点在不同坐标系的位姿表示，这就要求我们掌握坐标变换的方法了。

比如在手眼系统中，我们可以通过视觉算法获取到工件坐标系P鱼相机坐标系C之间的关系，我们想要控制机械臂的末端运动到工件坐标系P进行夹取，那么我们就要知道工件坐标系P在机器人基坐标系B下的位姿，如何获取呢？这就需要坐标进行变换了。

5.1 平移坐标变换

如图，坐标系{A}、{B}、{C}的姿态是相同的，其之间的姿态对应的旋转矩阵都是单位矩阵。在我们已知:

• {A}为参考坐标系，{B}的位置矢量${}_B^{A}P=[1,1,1{]}^T$
• {B}为参考坐标系，{C}的位置矢量${}_C^{B}P=[3,0,1{]}^T$

求：

• {A}为参考坐标系，{C}的位置矢量${}_C^{A}P$

大家可以利用简单的几何知识手算一下，再继续往下看

正确的答案应该是：
$${}_C^{A}P=[4,1,2{]}^T$$
算法也很简单，直接将${}_B^{A}P$鱼${}_C^{B}P$相加即可
$${}_C^{A}P={}_B^{A}P+{}_C^{B}P=[1,1,1{]}^T+[3,0,1{]}^T=[4,1,2{]}^T$$
由此我们可以得出平移坐标变换可以求出同一个点（{P}）在相同姿态不同位置坐标系({A}鱼{B})下的不同表示,也可以得到坐标的平移方程
$${}_C^{A}P={}_B^{A}P+{}_C^{B}P\text{\hspace{1em}(坐标平移方程)}$$
小鱼说需要注意的是：在上述例子中，{A},{B},{C}三个坐标系的姿态都是相同的，所以${}_C^{A}R={}_B^{A}R={}_C^{B}R$，也都是是单位旋转矩阵

5.2 旋转坐标变换

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

上图中，坐标系{A}鱼坐标系{B}原点重合（连名字都连一块了），我们已知：

• {B}坐标系绕{A}的z轴即$A_z$旋转了$45^{\circ }$
• {B}鱼{C}姿态相同，{B}为参考坐标系，{C}的位置矢量为${}_C^{B}P=[3,0,1{]}^T$

求

• {A}为参考坐标系,{C}的位置矢量${}_C^{A}P$
• {A}为参考坐标系,{C}的旋转矩阵${}_C^{A}R$

这次相对来说有一些困难，不过我们依然可以手推出来,接着和小鱼的思路一起走一下

5.2.1 求${}_C^{A}R$

因为{B}鱼{C}姿态相同，所以{A}参考坐标系下{C}的姿态和{A}参考坐标系下{B}的姿态是相同的，即：
$${}_B^{A}R={}_C^{A}R$$
接着只需要求${}_B^{A}R$即可，因为{B}坐标系绕{A}的z轴即$A_z$旋转了$45^{\circ }$,我们可以一一计算旋转矩阵各个元素的值
$${}_B^{A}R={}_C^{A}R=\left[\begin{array}{ccc}{B}_x\cdot A_x& B_y\cdot A_x& B_z\cdot A_x\\ B_x\cdot A_y& B_y\cdot A_y& B_z\cdot A_y\\ B_x\cdot A_z& B_y\cdot A_z& B_z\cdot A_z\end{array}\right]=\left[\begin{array}{ccc}cos(45)& cos(90+45)& cos(90)\\ cos(90-45)& cos(45)& cos(90)\\ cos(90)& cos(90)& cos(0)\end{array}\right]=\left[\begin{array}{ccc}cos(45)& cos(90+45)& 0\\ cos(90-45)& cos(45)& 0\\ 0& 0& 1\end{array}\right]$$
将结果在出来，再利用工具来算一下，看看最终结果对不对

到此我们求出了${}_C^{A}P$鱼${}_C^{A}R$，但聪明的鱼粉肯定不会就此罢休，我们接着来举一反三

{B}绕着{A}的z轴旋转了45度我们计算出来了，那如果{B}绕着{A}旋转$\theta$角度该如何呢?相信聪明的鱼粉心中已经有了答案
$$R(z,\theta )=\left[\begin{array}{ccc}cos\theta & -sin\theta & 0\\ sin\theta & cos\theta & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(小鱼提示：重要方程1)}$$
那如果不是绕着z轴旋转，而是绕着x轴呢？
$$R(x,\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -sin\theta \\ 0& sin\theta & cos\theta \end{array}\right]$$
绕着y轴呢？
$$R(y,\theta )=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]$$

5.2.2 求${}_C^{A}P$

现在我们已知${}_C^{B}P$和${}_B^{A}R$,那如何求${}_C^{A}P$呢，我们可以使用下面这个方程
$${}_C^{A}P={}_B^{A}R{}_C^{B}P\text{\hspace{1em}(坐标旋转方程)}$$
关于这个方程，其实是将${}_C^{B}P$在{B}上各轴的分量变换到了{A}的各轴上

${}_B^{A}R$是一个$3\ast 3$的矩阵，${}_C^{B}P$是$3\ast 1$的矩阵，其相乘结果还是一个$3\ast 1$的矩阵，利用前几节学习的矩阵乘法手算或者使用numpy进行计算，得到最终结果为：
$${}_C^{A}P={}_B^{A}R{}_C^{B}P=[2.12,2.12,1{]}^T$$

小思考:{C}的参考坐标系从{B}变成了{A},{C}在空间中的位置发生了变化了吗？

5.3平移旋转复合变换

在我们平时搞机（器人）时，一般情况下两个坐标系原点不重合姿态也不相同。我们将坐标变换拆分成先绕参考坐标系旋转，再绕参考坐标系平移两步，这样我们就得到了坐标的复合变换方程
$${}_C^{A}P={}_B^{A}R{}_C^{B}P{+}_B^{A}P$$
这个公式其实也很好理解，${}_B^{A}R{}_C^{B}P$就是将{C}在{B}上的各轴分量转换到{A}上，再和原来在{A}上各轴的分量再相加。不理解的同学也没事，后面我们使用最多的还是齐次变换矩阵，根据方程直接摝代码就行了。

6.左手还是右手

最后小鱼要说的是左手坐标系和右手坐标系，一般情况下我们都是使用右手坐标系搞机。

用右手确定的坐标系就是右手坐标系了，跟着小鱼一起做：

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

如果你换左手，做上述动作，你会发现y轴是朝手心里的。

再放张图给肢体不协调的同学：

7.练习

光说不练假把式，我们来做个手眼转换题

如图示，已知：

1.相机坐标系{C}为参考坐标系，工具坐标系{P}的位置矢量在相机坐标系{C}x,y,z各轴投影为$2,1,2$，并且工具坐标系和相机坐标系姿态相同。

2.机器人基坐标系{B}为参考坐标系，相机坐标系{C}在的位置矢量在{B}各轴的投影为$0,0,3$,坐标系{C}和绕着坐标系{B}的x轴转了180度

可以参考下图看题目

求：

{B}为参考坐标系，坐标系{P}的位置矢量和旋转矩阵

答案：

位置矢量：$[2,-1,1{]}^T$

旋转矩阵：$\left[\begin{array}{ccc}1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$

参考文档

• 机器人学导论
• 机器人学基础

---

技术交流&&问题求助：

• 微信公众号及交流群：鱼香ROS
• 小鱼微信：AiIotRobot
• QQ交流群：139707339
• 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划

作者介绍：

我是小鱼，机器人领域资深玩家，现深圳某独脚兽机器人算法工程师一枚
初中学习编程，高中开始接触机器人，大学期间打机器人相关比赛实现月入2W+（比赛奖金）
目前在输出机器人学习指南、论文注解、工作经验，欢迎大家关注小鱼，一起交流技术，学习机器人