c++求矩阵的秩_（番外）姿态与旋转矩阵（III）

（番外）姿态与旋转矩阵（III）

又拖更了好久好久啊，本意在这个番外里面只用选择矩阵的，但是思考了下后面要聊的内容，觉得还是需要引入别的姿态表达方式。好在在旋转矩阵的基础上，四元数和旋转矢量（轴-角表示）都比较容易定义。

1. 旋转矩阵、轴-角、四元数

前面说了姿态是指两个坐标系之间的关系，也就是描述一个坐标系怎么旋转才能和另一个坐标系重合。那么欧拉角所采用的方法就是分别沿三个轴转动三个角度；要真的对一个向量实施这个旋转操作，还是需要将欧拉角转换为相应的旋转矩阵，因此可以把欧拉角称为姿态的“描述”，而旋转矩阵则既可以对姿态进行描述，也可以直接用来处理两个坐标系中的矢量。

类似的，轴-角也更像是一种姿态的“描述”，因为它既不直观，也不利于运算（欧拉角在小扰动时还是可以用来近似计算的），而四元数则是一种兼顾描述和计算的表示方法。下面来给出它们的定义以及和旋转矩阵间的关系。

1.1 轴-角

一个坐标系既然可以通过绕三轴的三次转动与另一坐标系重合，那当然也可以直接绕一个空间矢量旋转一次来与另一坐标系重合。这个空间矢量就是所谓的“轴”，定义为单位矢量，而转动的角度就是“角”，定义为标量。 通常用

表示轴，用

表示角，我们就可以这样来描述两个坐标系的关系：世界坐标系

沿

向量旋转

角度（依旧是从原点出发方向，正角度为顺时针旋转），就与机体坐标系

重合了。有几点需要注意一下：

• 旋转轴矢量在原坐标系和新坐标系中的投影是不变的。比如
  系绕
  矢量转动了
  度来到了
  系（忽略这个轴不是单位矢量哈），这条旋转轴在
  系中也仍然是
  ，换句话说，我们可以让
  系绕
  矢量转动
  度来到了
  系。
• 矢量
  就是旋转矢量。
• 轴-角不太容易用于矢量计算，所以一般还是要把它转成旋转矩阵，它们之间的关系是

其中

符号代表取矢量的反对称矩阵。

• 现在给大家提个问题，如果我们知道
  系绕
  矢量转动了
  度来到了
  系，那么
  是谁到谁的旋转矩阵呢？答案是从
  系到
  系的，也就是说按照上面定义的轴-角，得到的是
  ，大家可以用绕坐标轴的旋转验证一下，这里就不赘述了。
• 其他几个性质：

这给出了如何从旋转矩阵计算轴角。也能够明显的看出旋转矩阵的秩和旋转角之间的关系。

OK，这样一来，轴角的意义以及和旋转矩阵间的相互转换就清楚了，下面我们来看四元数。

1.2 四元数

根据轴-角直接给出四元数的定义，令标量

，矢量

，那么显然

我们记下面的矢量为四元数

若

系绕

矢量转动了

度来到了

系，按照上式定义的四元数与旋转矩阵间的关系为

利用

，另一种表示法为

同样的四元数定义，还可以直接进行增广矢量的计算

其中

为共轭四元数，为四元数向量部分取负号，

为四元数左乘矩阵：

同样可以定义四元数右乘矩阵：

有

因此可以把上面的公式代换一下，得到

这就是四元数的一些基本情况了，重要的还是已知一个旋转（绕某矢量转一个角度），然后如何写出这个旋转的四元数和想要的旋转矩阵。

2. 姿态推算

看下上篇留下的两个问题中的第一个：

• 已知无人机
  时刻的姿态，机载陀螺测量到了此时机体坐标系的角速度
  ，经过
  时间段后来到了
  时刻，求
  时刻无人机的姿态？

这就是无人机姿态估计利用陀螺进行姿态推算的环节。

2.1 离散化方法

实际上将上节得到的结论

进行一阶（欧拉）离散化，可以得到

2.2 链式法则方法

此外还可以用旋转矩阵的链式法则来看这件事情。问题是求

时刻无人机的姿态，也就是

，我们已知了

，那么根据链式法则有

回想下前面关于轴角的概念

其中

和

是如此定义的：我们从

开始，无人机绕

矢量转动了

角，达到了

。那么只要确定了这一对轴角，问题就解决了。

大家再想一想，陀螺的测量是什么？无人机三轴的角速度，我们可以理解为也就是无人机绕空间某轴转动角速度在机体坐标系三个轴上的投影，那么经过

时间段陀螺测得无人机在三轴上的角增量

就是我们要求的轴角在机体坐标系三轴上的投影。于是我们就可以得到

带回上面的式子，得到

然后利用指数矩阵的展开

，可得

可见，一阶离散法对应的是矩阵指数

的一阶近似（保留前两项）。

2.3 姿态推算的精度

• 我们进行上面计算的前提是假设
  时间段内无人机的角速度没有变化，但实际并不是这样，因此
  的大小对姿态推算的精度有影响，目前开源飞控的姿态推算在400Hz或以上，一些大学实验室甚至做到了1kHz以上，
  越小，该时间段内无人机的角速度没有变化的假设就越接近真实（然而永远是近似，但以足够）。
• 的大小同样影响着圆锥误差和划摇误差，但由于本专栏讨论的MEMS器件精度都不高，所以在
  足够小的情况下，也省略了这两个误差的补偿。
• 姿态推算算法的阶次，实际上就是使用矩阵指数
  的几阶近似来推算姿态，可以看到，当
  足够小时，高阶项会变得非常微弱，因此也更有理由采用计算量小的一阶算法。D. H. Titterton的《捷联惯性导航技术》里面有一张表可以参考（表11.1）。

sigma_max代表角增量的最大值

可见

从0.01s减小到0.005s（100Hz到200Hz），一阶算法的漂移误差减小了3倍多，四阶算法的误差减小了17倍。因此积极的提高姿态推算运行频率好处是非常明显的。

那么下一篇我们就要用轴角和四元数以及旋转矩阵来解决第二个问题啦。