d2l-ros2/docs/chapt7/7.2.3姿态的多种表示.md

姿态的不同表示

大家好，我是小鱼，本节课，我们来学习姿态的多种表示方式。在前面的课程中，我们一共接触了三种姿态的表示方式：

1. 旋转矩阵-在位姿描述一节中
2. 坐标轴旋转-绕xyz轴旋转不同的角度(欧拉角)
3. 四元数-ROS2的TF2中的姿态描述

本节小鱼将对以上的三种姿态描述进行归类与介绍，并对他们之间的转换方法进行讲解，下一节小鱼带你一起通过代码直观的观察和操作姿态变换。

小鱼将常用的坐标描述分为三类，共五种。这五种也是小鱼在平时工作中所接触到的几乎所有姿态描述方法，三类共五种方法如下：

• 旋转矩阵-旋转矩阵
• 坐标轴旋转-固定轴欧拉角,非固定轴欧拉角
• 任意轴旋转-等效轴角,四元数

常用的坐标转换包括：

• 固定角与四元数互转
• 固定角与旋转矩阵互转
• 四元数与旋转矩阵互转

1.旋转矩阵

关于旋转矩阵我们在前几节教程中已经介绍了，旋转矩阵采用的是旋转后的坐标系三个轴分别与原坐标系三个轴的夹角余弦值共九个数字组成3*3的矩阵。

旋转矩阵一般记作记作$R$

若两个坐标系姿态相同，其旋转矩阵为单位矩阵。

1.1 旋转矩阵的描述

如图，描述坐标系{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} \tag{旋转矩阵} $$

两个向量的点乘=两个向量的长度(1)与它们夹角余弦的积,所以$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} $$

1.2 绕某一轴旋转$\theta$转旋转矩阵

新的坐标系绕原坐标系某一坐标轴旋转任意角度得到的旋转矩阵有如下等式。

绕x轴旋转$\theta$后姿态矩阵

$$ R(x,\theta)= \begin{bmatrix} 1 & 0 & 0\ 0 & {cos\theta} & -sin\theta \ 0&{sin\theta} & cos\theta \ \end{bmatrix} $$

绕y轴旋转$\theta$后姿态矩阵

$$ R(y,\theta)= \begin{bmatrix} {cos\theta} & 0 & {sin\theta}\ 0 &1 &0\ {-sin\theta} & 0 &cos\theta \ \end{bmatrix} $$

绕z轴旋转$\theta$后姿态矩阵

$$ R(z,\theta)= \begin{bmatrix} {cos\theta} & -sin\theta & 0\ {sin\theta} & cos\theta & 0\ {0} &0 &1\ \end{bmatrix} $$

2.欧拉角-绕坐标轴的旋转

2.1 12种旋转顺序

旋转矩阵是一个冗余的（九个值之间存在约束关系），可以只需要三个参数来表示旋转矩阵。聪明的鱼粉肯定会想到，假如知道坐标系绕分别绕X、Y、Z轴的旋转角度，不就同样可以表示旋转了吗？

这个猜想是对的，结合1.2中的绕三个轴的旋转的三个$\theta$，按照特定的顺序将对应的旋转矩阵乘起来就可以确定一个旋转矩阵。

但需要注意的是，矩阵的乘法不具备交换性，所以旋转顺序不同会造成不同的结果。

比方说若是先绕自身$x$轴旋转$\alpha$，再绕自身$y$轴旋转$\beta$：

$$ R(x,\alpha)R(y,\beta)= \begin{bmatrix} 1 & 0 & 0\ 0 & {cos\alpha} & -sin\alpha \ 0&{sin\alpha} & cos\alpha \ \end{bmatrix} \begin{bmatrix} {cos\beta} & 0 & {sin\beta}\ 0 &1 &0\ {-sin\beta} & 0 &cos\beta \end{bmatrix}= \begin{bmatrix} {cos\beta} & 0 & {sin\beta}\ sin\alpha sin\beta &cos\alpha & -sin\alpha cos\beta\ {-cos\alpha sin\beta} & sin\alpha &cos\alpha cos\beta \ \end{bmatrix} $$

比方说若是，先绕自身$y$轴旋转$\beta$，绕自身$x$轴旋转$\alpha$：

$$ R(y,\beta)R(x,\alpha)= \begin{bmatrix} {cos\beta} & 0 & {sin\beta}\ 0 &1 &0\ {-sin\beta} & 0 &cos\beta \ \end{bmatrix} \begin{bmatrix} 1 & 0 & 0\ 0 & {cos\alpha} & -sin\alpha \ 0&{sin\alpha} & cos\alpha \ \end{bmatrix}= \begin{bmatrix} cos\beta & sin\alpha sin\beta & {sin\beta}\ 0 & {cos\alpha} & -sin\alpha \ {-sin\beta} & sin\alpha &cos\alpha sin\beta \ \end{bmatrix} $$

所以我们对旋转顺序做排列组合，可以得到12种旋转顺序:

• xyz,xyx, xzy
• xzx,yzx, yzy
• yxz, yxy , zxy
• zxz, zyx, zyz

2.2 两种参考坐标系

除了要考虑旋转时所绕轴的顺序，还要考虑参考坐标系（坐标轴）的不同。

2.2.1 参考固定的坐标系

假设坐标系B鱼坐标系A初始姿态相同

• 坐标系{B}绕坐标系A的x轴Ax旋转$\alpha$
• 接着坐标系{B}绕着A的y轴Ay旋转$\beta$
• 接着绕Az旋转$\gamma$

上述三次旋转中，旋转时都是绕着A坐标系的xyz轴为参考坐标系，该旋转方式成为固定旋转轴的旋转，称之为固定角欧拉角或固定轴旋转。

2.2.2 参考自身坐标系

我们也可以不沿着坐标系A的各轴旋转，而是绕旋转之后B的某一轴再次旋转，我们称之为非固定旋转轴的欧拉角。

小鱼说:无论是参考自身坐标系还是参考固定的坐标系，都有12种旋转方式，所以欧拉角有12*2=24种旋转方式，后面的计算中我们也直观的感受到24种旋转方式的不同。

2.3 固定转轴欧拉角 转 旋转矩阵

首先我们来考虑绕固定的坐标系旋转如何转换成旋转矩阵

我们以顺序XYZ来举例说明，其他旋转顺序类似

现在假设A、B两个坐标系重合，B坐标系绕A坐标系的X轴旋转45度，绕A的Z轴旋转90度.

求旋转之后A为参考坐标系，B坐标系的姿态$^ABR{XYZ(45,0,90)}$

小鱼先告诉你最终的结果： $$ ^ABR{XYZ(45,0,90)}=R{Z(90)}R{Y(0)}R{X(45)}=R{Z(90)}R_{X(45)}=\begin{bmatrix} 0.&-0.70710678 & 0.70710678\ -1&0&0\ 0&0.70710678&0.70710678\ \end{bmatrix} $$

为什么结果是将绕Z轴的旋转矩阵乘绕X轴的旋转矩阵呢？

这里引用林沛群老师的解释：

我们可以假设一个向量v固定在B坐标系上，那我们让B坐标系绕着A坐标系的三个轴做旋转，就可以认为是让向量v绕着坐标系A的三个轴做旋转，那先转的肯定先乘，所以我们先让向量v乘上Rx(45),再让其乘上Rz(90)，即： $$ v' = R{Z(90)}(R{X(45)}v) $$ 根据矩阵乘法的结合律，括号可以去掉： $$ v' = R{Z(90)}R{X(45)}v $$

所以我们可以得到，绕固定轴XYZ旋转的欧拉角转旋转矩阵方法: $$ R{XYZ(\gamma,\beta,\alpha)}=R{Z(\alpha)}R{Y(\beta)}R{X(\gamma)} $$ 最终结果：

根据旋转顺序不同，固定角有12种旋转方式，这里我们给出了绕固定轴以XYZ顺序旋转欧拉角转旋转矩阵的等式，其他旋转顺序对应的旋转矩阵可以尝试自行推导.

2.2 非固定旋转轴的欧拉角

非固定旋转轴，即每次旋转是绕着自身的坐标轴进行旋转，其旋转动图如2.2.2节所示。

非固定旋转的欧拉角转旋转矩阵推导也很简单，我们以旋转顺序ZYX为例子分析

2.2.1 ZYX

因为每次旋转都是绕着自身进行的，我们可以将每次的旋转进行拆解

$$ ^ABR={^A{B'}R}{^{B'}{B''}R}{^{B''}{B}R} $$

等式右边的三次旋转按照顺序Z-Y-X进行的，所以最终B坐标系在A坐标系下的姿态为：

$$ ^ABR{Z'Y'X'}=R{Z(\alpha)}R{Y(\beta)}R_{X(\gamma)} $$

最终结果太难敲，小鱼直接截图啦

旋转矩阵转欧拉角的方法需要使用双参变量的反正切函数，我们后面在程序当中直接调用对应函数即可实现，这里对原理就不再进行推导了

3.轴角

在介绍四元数之前，我们先来说说等效角度轴线，这种表示姿态的方式。

上一节欧拉角中无论是绕着自身的某个轴旋转，还是绕着固定的坐标系的某个轴进行旋转，旋转时参考的轴都是坐标系的主轴

假如我们参考的轴不是主轴，那么任何姿态都可以通过选择适当的轴和角度得到，换句话说，两个坐标系之间的任何姿态都可以通过绕某一个特定的轴(矢量)旋转特定的角度得到。

说到这里相信你已经理解了轴角的意义，接着我们给出轴角和旋转矩阵之间的转换关系

轴角转旋转矩阵

假设坐标系B和参考坐标系A宠儿，将B绕着A坐标系下的矢量$^AK$按右手定则旋转$\theta$角度，旋转之后B坐标系在A坐标系下的姿态可以用 $$ ^A_BR(K,\theta) $$ 表示，注意矢量K为单位矢量(模场为1)，K为一个3*1的矢量 $$ ^AK=[k_x,k_y,k_z]^T $$

在已知矢量K和$\theta$的情况下，我们如何得到旋转矩阵呢?

有等式： 其中

$$ c\theta=cos\theta \ s\theta=sin\theta \ v\theta=1-cos\theta $$

$\theta$的符号由右手定则确定，右手大拇指指向矢量K的方向.

旋转矩阵转轴角需要根据情况讨论，该部分转换我们直接调用相应函数实现，这里对其原理不再叙述，感兴趣的同学可以参考：https://en.wikipedia.org/wiki/Rotation_matrix#Axis_of_a_rotation

四元数

除了轴角这种方式

四元数是简单的超复数。 复数是由实数加上虚数单位 i 组成，其中i²= -1。 相似地，四元数都是由实数加上三个虚数单位 i、j和k 组成，而且它们有如下的关系： i² = j² = k² = -1， iº = jº = kº = 1 , 每个四元数都是 1、i、j 和 k 的线性组合，即是四元数一般可表示为a + bi+ cj + dk，其中a、b、c 、d是实数。 对于i、j和k本身的几何意义可以理解为一种旋转，其中i旋转代表Z轴与Y轴相交平面中Z轴正向向Y轴正向的旋转，j旋转代表X轴与Z轴相交平面中X轴正向向Z轴正向的旋转，k旋转代表Y轴与X轴相交平面中Y轴正向向X轴正向的旋转，-i、-j、-k分别代表i、j、k旋转的反向旋转。