《动手学机器人学》第四节(上):位姿描述

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

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

正文

1.何为位姿

1.1 引言

在书籍《天才在左，疯子在右》中有这样一个故事，有一位十七岁的少年，他对量子力学有着独特的见解和远超同龄人的知识储备，他说四维生物突破了时空的界限，在它的观察中，人类的一生的活动连线起来，就像一条长长的虫子，它可以看到人的过去，也可以看到人的未来。

我们对四维空间不了解，但对三维立体和二维的平面每天都有接触。一张铺平的中国地图是二维平面的，拔地而起的高楼大厦是三维立体的。

那我们如何用数字表示高楼在地图中的位姿呢？又如何用数字表示某个高楼中你的位姿呢？

1.2 我们先来看地图中高楼的位姿

在地图上，我们可以利用经纬度坐标系来描述位姿，在经纬度坐标系中，我们可以使用经度值和纬度值表示地图上的某一个大楼的坐标，比如天安门的经纬度经度：116.38 ，纬度：39.90

经纬度坐标系是以经纬度原点（几内亚湾）建立二维平面直角坐标系，我们就可以使用x,y来描述该坐标系中的任意一个点的位置。

经度的起点就是指0度经线，位于英国格林尼治天文台旧址。纬度的起点就是指0度纬线，就是指赤道。而经度0和纬度0的交点，大致位于非洲西部的几内亚湾，位于海上。

通过经纬度可以定位到地图中的建筑物的位置，但我们依然无法得知这个建筑物的朝向（东南西北），所以除了描述位置的经纬度外，我们还需要增加一个theta(θ\thetaθ),表示朝向。这样我们就得到了一个物体在二维平面中用数学描述的三个值，经度（x）、维度（y）和朝向（theta）。

1.3 高楼中你的位姿

现实世界是三维的，除了经纬度还有海拔高度。所以采用经度（x）、维度（y）并不能完整的描述出空间中物体的位置，只有经纬度无法表示你所处的楼层的海拔高度，这样就没办法确定你的位置。

我们同样可以在经纬度原点建立一个三维空间直角坐标系，采用经度（x）、维度（y）和海拔高度（z）三个值来描述三维空间中任意一点的位置。

和二维空间中类似，知道了你在某一个大楼中的位置还不够，并不能判断出你的姿态（躺平的、站着的还是侧卧的），那我们该如何描述三维空间中的姿态呢?

答案是旋转矩阵，旋转矩阵是什么？我们接着往下看。

1.4 参考坐标系

无论是在二维空间还是在三维空间，我们想要描述一个物体的位置和姿态第一步就是确定一个参考坐标系，物体的位置和姿态描述我们都是以这个坐标系作为参考的。

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

2.位置的表示

从第一节背景中可以得知，二维平面中的位置可以用x,yx,yx,y表示，三维空间中的位置的表示可以用x,y,zx,y,zx,y,z来表示。

无论是在三维空间还是在二维平面，我们都可以使用x,y,zx,y,zx,y,z来表示位置，只不过对于二维空间来说，z的值是默认的一个固定不变的值，比如000

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

AP=[xyz]{^A}P= \begin{bmatrix} x \\ y \\ z \\ \end{bmatrix} AP=⎣⎡xyz⎦⎤

2.1 位置矢量

直角坐标系{A}其实可以看作由一个三个互相正交（两两垂直）的单位矢量组成的。那么在坐标系{A}中的一点P也可以写作矢量形式，其矢量形式由其在三个单位矢量上的分量组成。

所以参考坐标系{A}中一点P也可以写作
AP=[xpypzp]=[xp，yp,zp]T(位置矢量){^A}P= \begin{bmatrix} {^x}p \\ {^y}p \\ {^z}p \\ \end{bmatrix} = [{^x}p，{^y}p , {^z}p ]^T \tag{位置矢量} AP=⎣⎡xpypzp⎦⎤=[xp，yp,zp]T(位置矢量)

如果说位置矢量不太好理解，可以简单的认为就是坐标轴的x,y,zx,y,zx,y,z

[xyz]\begin{bmatrix}{x}\\{y}\\{z}\\\end{bmatrix} ⎣⎡xyz⎦⎤

3.姿态的表示

在背景中小鱼提到了除了位置，坐标描述还有另外一个非常重要的组成部分——姿态。

接着上面的说，我们已经直到坐标系A中的一个点P的位置，我们如何描述P点在{A}坐标系下的姿态呢？

我们可以以P点为原点再建立一个坐标系(也可以认为该坐标系固定在物体P点上)，这样我们就可以通过描述**新坐标系{P}和参考坐标系{A}**之间的姿态关系来表示AP{^A}PAP点的姿态。

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

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

每个轴的相对姿态关系确定了，坐标系之间的姿态也就确定了，AP{^A}PAP点的姿态也就确定了。

3.1 旋转矩阵

我们将上述坐标系{P}的三个轴相对与参考坐标系{A}三个轴的共九个角度的余弦值，组成3*3的矩阵，该矩阵就是旋转矩阵,因该矩阵是{P}相对于{A}之间的姿态关系的表示，故记作PAR{^A_P}RPAR
PAR=[AxPAyPAzP]=[r11r12r13r21r22r23r31r32r33](旋转矩阵){^A_P}R=[{^A}x_{P} \ {^A}y_{P} \ {^A}z_{P}] = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}\\{r_{21}}&{r_{22}}&{r_{23}}\\{r_{31}}&{r_{32}}&{r_{33}}\\\end{bmatrix} \tag{旋转矩阵} PAR=[AxP AyP AzP]=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤(旋转矩阵)

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

PAR=[Px⋅AxPy⋅AxPz⋅AxPx⋅AyPy⋅AyPz⋅AyPx⋅AzPy⋅AzPz⋅Az]{^A_P}R = \begin{bmatrix} {P_{x}\cdot A_x} & {P_{y}\cdot A_x} & {P_{z}\cdot A_x}\\ {P_{x}\cdot A_y} & {P_{y}\cdot A_y} & {P_{z}\cdot A_y}\\ {P_{x}\cdot A_z} & {P_{y}\cdot A_z} & {P_{z}\cdot A_z}\\ \end{bmatrix} PAR=⎣⎡Px⋅AxPx⋅AyPx⋅AzPy⋅AxPy⋅AyPy⋅AzPz⋅AxPz⋅AyPz⋅Az⎦⎤

将PAR{^A_P}RPAR进行转置可得PART{^A_P}R^TPART

PART=[Px⋅AxPx⋅AyPx⋅AzPy⋅AxPy⋅AyPy⋅AzPz⋅AxPz⋅AyPz⋅Az]{^A_P}R^T = \begin{bmatrix} {P_{x}\cdot A_x} & {P_{x}\cdot A_y} & {P_{x}\cdot A_z}\\ {P_{y}\cdot A_x} & {P_{y}\cdot A_y} & {P_{y}\cdot A_z}\\ {P_{z}\cdot A_x} & {P_{z}\cdot A_y} & {P_{z}\cdot A_z}\\ \end{bmatrix} PART=⎣⎡Px⋅AxPy⋅AxPz⋅AxPx⋅AyPy⋅AyPz⋅AyPx⋅AzPy⋅AzPz⋅Az⎦⎤

可以看出PART{^A_P}R^TPART其实表示坐标系{P}作为参考坐标系下坐标系{A}的姿态，即
PART=APR=PAR−1{^A_P}R^T = {^P_A}R = {^A_P}R^{-1} PART=APR=PAR−1

需要注意的是：

当两个坐标系之间姿态没有变化，即坐标系间x,y,zx,y,zx,y,z轴方向对应重合，则旋转矩阵为单位矩阵，这个很好求得，有兴趣的同学可以算一下
从矩阵的角度看，矩阵的逆等与矩阵的转置，则该矩阵为正交矩阵，显而易见，旋转矩阵是正交矩阵

4.位置+姿态

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

在机器人当中位置和姿态一般会一起出现，所以我们将其组合就叫做位姿

4.1 位姿描述的多个含义

含义1：表示特定参考坐标系下某个物体（点）的位置和姿态，比如我们描述参考坐标系{A}中物体（点）P的位置和姿态
：两个物体之间的关系，我们通常把坐标系固定在物体上，这样就可以表示两个物体之间的位姿关系，比如自行车前轮和后轮的关系

小鱼说：学会了位置和姿态描述，三维空间坐标关系描述相信已经难不倒你了

技术交流&&问题求助：

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

作者介绍：

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