姿态的多种表示方式

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

1. 旋转矩阵-位姿描述中
2. 坐标轴旋转-绕xyz轴旋转角度
3. 四元数-ROS2的TF2中的姿态描述

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

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

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

常用的坐标转换包括：

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


## 1.旋转矩阵

关于旋转矩阵我们在前几节教程中已经介绍了，旋转矩阵采用旋转后的坐标系每一个轴在原坐标系各轴的夹角余弦值组成3*3的矩阵。

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

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

### 1.1 旋转矩阵的描述

![rviz的位姿显示](7.2.1空间坐标描述/imgs/image-20211102153515882.png)


如图，描述`坐标系{P}`和`参考坐标系{A}`之间的姿态关系来表示${^A_P}R$点的姿态。


$$
{^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{旋转矩阵}
$$

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

$$
{^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}
$$



### 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.坐标轴旋转

旋转矩阵是一个冗余的（九个值之间存在约束关系），可以只需要三个参数来表示旋转矩阵。结合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`

接着我们考虑旋转时所绕的轴的不同，坐标系B鱼坐标系A初始姿态相同
- 坐标系{B}绕坐标系A的x轴`Ax`旋转$\alpha$
- 接着坐标系{B}绕着A的y轴`Ay`旋转$\beta$
- 接着绕`Az`旋转$\gamma$

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

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


### 2.1 固定旋转轴的固定角

绕着某个固定参考坐标系的得到的姿态表示方式为固定角方式。

根据旋转顺序不同，固定角有12种旋转方式
- `xyz`,`xyx`, `xzy`
- `xzx`,`yzx`, `yzy`
- `yxz`, `yxy` , `zxy`
- `zxz`, `zyx`, `zyz`

我们以最常用的XYZ顺序为例，推出固定角转旋转矩阵的公式。

$$
R(x,\alpha)R(y,\beta)R(z,\gamma)=\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\gamma} & -sin\gamma & 0\\
{sin\gamma} & cos\gamma & 0\\
{0} &0 &1\\
\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}
$$

其他旋转顺序对应的旋转矩阵可以尝试自行推导.

### 2.2 非固定旋转轴的欧拉角

非固定旋转轴，即每次旋转是绕着自身的坐标轴进行旋转。




技术交流&&问题求助：

- **微信公众号及交流群：鱼香ROS**
- **小鱼微信：AiIotRobot**
- **QQ交流群：139707339**

- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划