7.2.1空间坐标描述.md 8.0 KB
7.2.1 空间坐标描述
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,y$表示，三维空间中的位置的表示可以用$x,y,z$来表示。
无论是在三维空间还是在二维平面，我们都可以使用$x,y,z$来表示位置，只不过对于二维空间来说，z的值是默认的一个固定不变的值，比如$0$
假如我们确定了一个空间直角坐标系A,我们就可以使用$x,y,z$来确定A坐标系中空间中任意一点P的位置，可以记作${^A}P$
$$ {^A}P= \begin{bmatrix} x \ y \ z \ \end{bmatrix} $$
2.1 位置矢量
直角坐标系{A}其实可以看作由一个三个互相正交（两两垂直）的单位矢量组成的。 那么在坐标系{A}中的一点P也可以写作矢量形式，其矢量形式由其在三个单位矢量上的分量组成。
所以参考坐标系{A}中一点P也可以写作 $$ {^A}P= \begin{bmatrix} {^x}p \ {^y}p \ {^z}p \ \end{bmatrix} $$
如果说位置矢量不太好理解，可以简单的认为就是坐标轴的$x,y,z$
$$ \begin{bmatrix}{x}\{y}\{z}\\end{bmatrix} $$
3.姿态的表示
在背景中小鱼提到了除了位置，坐标描述还有另外一个非常重要的组成部分——姿态。
接着上面的说，我们已经直到坐标系A中的一个点P的位置，我们如何描述P点在{A}坐标系下的姿态呢？
我们可以以P点为原点再建立一个坐标系(也可以认为该坐标系固定在物体P点上)，这样我们就可以通过描述新坐标系{P}和参考坐标系{A}之间的姿态关系来表示${^A}P$点的姿态。
上图使用RVIZ2绘制出的，红色代表x轴，绿色代表y轴，蓝色代表z轴。
从图中可以看出，坐标系{P}的每一个轴和参考坐标系的每一个轴之间都有一个角度，比如Px轴和Ax,Ay,Az三个轴之间存在三个角度，通过这三个角度我们就可以确定Px轴和参考坐标系{A}之间的关系，以此类推，我们也可以确定，Py轴和Pz轴和{A}之间的关系。
每个轴的相对姿态关系确定了，坐标系之间的姿态也就确定了，${^A}P$点的姿态也就确定了。
3.1 旋转矩阵
我们将上述坐标系{P}的三个轴相对与参考坐标系{A}三个轴的共九个角度的余弦值，组成3*3的矩阵，该矩阵就是旋转矩阵,因该矩阵是{P}相对于{A}之间的姿态关系的表示，故记作${^A_P}R$ $$ {^AP}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} $$
两个向量的点乘为两个向量的长度与它们夹角余弦的积,所以$r11$可以表示为向量$P{x}$与$A{x}$的点积，旋转矩阵就可以写为下面的形式
$$ {^AP}R = \begin{bmatrix} {P{x}\cdot Ax} & {P{y}\cdot Ax} & {P{z}\cdot Ax}\ {P{x}\cdot Ay} & {P{y}\cdot Ay} & {P{z}\cdot Ay}\ {P{x}\cdot Az} & {P{y}\cdot Az} & {P{z}\cdot A_z}\ \end{bmatrix} $$
将${^A_P}R$进行转置可得${^A_P}R^T$
$$ {^AP}R^T = \begin{bmatrix} {P{x}\cdot Ax} & {P{x}\cdot Ay} & {P{x}\cdot Az}\ {P{y}\cdot Ax} & {P{y}\cdot Ay} & {P{y}\cdot Az}\ {P{z}\cdot Ax} & {P{z}\cdot Ay} & {P{z}\cdot A_z}\ \end{bmatrix} $$
可以看出${^A_P}R^T$其实表示坐标系{P}作为参考坐标系下坐标系{A}的姿态，即
$$
{^A_P}R^T = {^P_A}R = {^A_P}R^{-1}
$$
4.位置+姿态
通过位置矢量我们可以描述一个点在特定参考坐标系下的位置，通过旋转矩阵可以描述一个点在特定参考坐标系下的姿态。
在机器人当中位置和姿态一般会一起出现，所以我们将其组合就叫做位姿
4.1 位姿描述的多个含义
含义1：表示特定参考坐标系下某个物体（点）的位置和姿态，比如我们描述参考坐标系{A}中物体（点）P的位置和姿态
含义2：表示两个坐标系之间的位姿关系，比如位置可以表示坐标系{A}和坐标系{B}原点位置关系，姿态可以表示两个坐标系坐标轴的朝向关系
坐标系之间关系,$^AP_{Bo}$两坐标系原点之间位置矢量 $$ {B} = { ^ABR, {^A}P{Bo} } $$
含义3：两个物体之间的关系，我们通常把坐标系固定在物体上，这样就可以表示两个物体之间的位姿关系，比如自行车前轮和后轮的关系
小鱼说：学会了位置和姿态描述，三维空间坐标关系描述相信已经难不倒你了
5.坐标变换
位姿是相对的，同一个物体在不同的参考坐标系下的位姿数据肯定是不同的，在后续的机器人学习和使用当中我们会经常需要获取同一个点在不同坐标系的位姿表示，这就需要我们掌握坐标变换的方法了。
5.1 平移坐标变换
5.2 旋转坐标变换
6.左手还是右手
参考文档：
- 机器人学导论