[原创] STM32测姿态——基本知识，旋转矩阵、欧拉角、四元数

wbhb2011 2016-9-6 21:12 楼主

这部分这几天在看，主要参考买的机器人书和秦永元——惯性导航，这本老书，堪称经典。机器人书在介绍到四元数的时候非常简单的带过了，惯性导航这本书后面有详细的介绍。实际上，我也没看懂太多，想着先记住结论，后面实验验证吧。毕竟线性代数忘光了（看来有必要在书堆里面翻出大学的线性代数）。贴图片太麻烦了，写到后面就省略了一些内容（数学公式太多...）.

向量：向量表示点相对于某个坐标系的位移。

位姿：坐标系的位置和方向总称为位姿。

下面介绍旋转矩阵：

看右面的图，最终目的是用旋转矩阵和B坐标系（｛B｝）的点积来表示V坐标系（｛V｝）。这个过程包含5个步骤：

1、点P用｛V｝中定义的单位向量表示：

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image002.pngfile:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image004.png

2、点P用｛B｝中定义的单位向量表示：

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image005.pngfile:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image007.png

3、｛V｝的单位向量表示｛B｝的单位向量：

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image008.pngfile:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image010.pngfile:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image012.png

4、｛V｝的单位向量转换表示｛B｝中的P点

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image014.png

5、得到｛V｝与｛B｝坐标系变换的旋转矩阵VRB

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image016.png

也就是：

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image018.png

然后我们拓展到三维下的旋转矩阵，像Figure 2。初始坐标系A，和变换后的坐标系B。

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image020.png

旋转矩阵表示是：

file:///C:/Users/hm7212/AppData/Local/Temp/msohtmlclip1/01/clip_image022.png

另外我们可以用分别绕x,y,z轴旋转的矩阵表示法：

用这种表示法是采用了欧拉旋转定理，绕三轴依次旋转。旋转次序也有说法：欧拉式，卡尔丹式。欧拉式是XYX XZX YXY YZY ZXZ ZYZ 这样，绕一个轴重复旋转但是不连续。卡尔丹式是绕三个不同的轴旋转，XYZ..这样。

后面，咱们开始介绍四元数...

图片的编辑实在太麻烦了，引用个博客给大家直接看吧：

点这里

结论我给大家总结一下：因为旋转矩阵的计算量大，还存在万向节锁的问题，因此只能用在旋转在-90°~+90°范围内的运动。而四元数不存在这个问题。所以咱们采用四元数。

四元数这个东西用数学的话说是超复数，是四维空间。怎么表示旋转，先构造一个四元数

然后记住结论点p旋转到p' 过程可用：

来表示。q-1是矩阵q的逆，不记得的话翻看线性代数吧，或者就当没看见这句话。。。

掌握了这些知识，虽说是冰山一角，不过至少已经啃完一角了。

还没完...

惯性导航这本书，介绍了一些别的概念——方向余弦法，旋转矢量法。这两种方法就不具体介绍了。有空（等我彻底理解了）在补上。这本书比较了几种方法：欧拉角算法、方向余弦法、四元数法、旋转矢量法。前两种容易理解，但数学运算量大，不适合工程应用。后两种实际都是四元数法，计算量小，效率高。其中旋转矢量法比四元数法更适合高动态的运载体，适合频繁抖动的姿态解算。

了解姿态的更新算法是非常重要，逻辑是这样的：通过加速度计+陀螺仪数据滤波完了融合——进行姿态阵解算得出航向等机体的姿态信息（当前）——根据现在的姿态与即将要变化的导航坐标系做姿态阵变换解算（也就是怎么旋转）得出要控制的位置和速度——施加给机体——回到第一步。

下一章节，咱们介绍读出数据，开始实际操作的第一步。