ardupilot开发 --- 坐标变换 篇

0. 一些概念

• 相关概念：旋转矩阵、平移矩阵、单应矩阵、齐次变换矩阵。
• 坐标变换包括旋转变换和平移变换。
• 为什么要坐标变换？引入坐标变换可以解决哪些问题？应用场景？
  • 已知一点p在坐标系A中的坐标，计算p在坐标系B中的坐标。
  • 已知一向量v在坐标系A中的值，计算v在坐标系B中的值。
  • 描述坐标系A和坐标系B之间的位姿关系（姿态和位置）。
    用于描述三维空间中刚体的姿态。
  • 一向量v绕坐标系A的xyz轴旋转$\varphi \theta \psi$角度并进行一定平移后在坐标系A中的新坐标。
  • 在数学建模过程中，往往需要将不同的物量量表示在同一个坐标系内才能列出等式。
  • 刚体运动学。
  • 图像的投影。

1. 坐标系的旋转

描述坐标系的旋转常用的方法包括：

• 轴角法
• 旋转矩阵
• 欧拉角
• 四元数

这几种旋转表示方法有各自的优缺点和应用场景，这里不作赘叙。

1.1 轴角法

不常用，略…

1.2 四元素

待续…

1.3 基于欧拉角的旋转矩阵

欧拉角形式的方向余弦矩阵。
轴角法和四元数，这两个可以归纳为用一次旋转来表示两个坐标系间的姿态关系。欧拉角则是用三次旋转来表示姿态。

欧拉角指：横滚角$\varphi$，俯仰角$\theta$，偏航角$\psi$。
$\varphi ,\theta ,\psi$在不同的领域有不同的定义规则：主要的区别是旋转顺序的不同，绕原始(固定)坐标轴轴旋转还是绕新(运动)坐标轴的不同（也称外旋或内旋）。
在航天航空领域的欧拉角：

• 内旋
  绕运动轴旋转得到新坐标系。
• $Z\to Y\to X$
  原始坐标系经过 $Z\to Y\to X$ 顺序旋转一定的角度得到新坐标系。
• 右手系
  涉及的坐标系都遵循右手定则。

1.3.1 单轴旋转矩阵

前提：右手系
这里只给出结论，不作推导。
坐标系A分别单独绕$x,y,z$轴旋转$\varphi ,\theta ,\psi$角度，欧拉旋转矩阵表达式如下：
$$\begin{gathered} T_B^A=R_x(\varphi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\varphi & -sin\varphi \\ 0& sin\varphi & cos\varphi \end{array}\right]（式1） \\ {T}_B^A=R_y(\theta )=\left[\begin{array}{ccc}cos\theta & 0& sin\theta \\ 0& 1& 0\\ -sin\theta & 0& cos\theta \end{array}\right]（式2） \\ {T}_B^A=R_z(\psi )=\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]（式3） \end{gathered}$$
这些旋转矩阵的意义是：新坐标系到原始坐标系的变换矩阵，即：
$$\left[\begin{array}{c}{x}_A\\ y_A\\ z_A\end{array}\right]=T_B^A\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\end{array}\right]$$
注意：

• $\varphi ,\theta ,\psi$是原始坐标系A旋转运动到新坐标系B的旋转量，但是得到的旋转矩阵$R_x,R_y,R_z$却是新坐标系B到原始坐标系A的坐标变换矩阵！
  即：
  $$\begin{gathered} \varphi ,\theta ,\psi ：A\to B的旋转量 \\ {R}_x(\varphi ),R_y(\theta ),R_z(\psi )：B\to A的旋转矩阵 \end{gathered}$$
• $\varphi ,\theta ,\psi$满足右手定则，绕右手坐标系的正方向旋转时为正。
• 系B到系A的变换矩阵可以有多种符号表示方式，通常有：$T_B^A{，}^A{T}_B，T_{AB}$都表示 B ~> A 的变换关系矩阵！注意上下标的位置！!
• 旋转矩阵是正交的，因此有：
  $$\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\end{array}\right]=T_A^B\left[\begin{array}{c}{x}_A\\ y_A\\ z_A\end{array}\right]=(T_B^A{)}^{-1}\left[\begin{array}{c}{x}_A\\ y_A\\ z_A\end{array}\right]=(T_B^A{)}^T\left[\begin{array}{c}{x}_A\\ y_A\\ z_A\end{array}\right]$$
  即：$T_A^B=(T_B^A{)}^{-1}=(T_B^A{)}^T$.

1.3.2 多轴旋转矩阵

12种常用的内旋欧拉角又可分为两个类别：

• 常规欧拉角
• 泰特 - 布赖恩角

注意： 下表中如 X