RT-Thread-姿态解算（MPU9250 & Motion Driver 6）RT-Thread问答社区

姿态解算（MPU9250 & Motion Driver 6）

发布于 2019-08-10 15:31:30 浏览：3968 订阅该版

* 本帖最后由 sogw 于 2019-8-10 15:31 编辑 *

*本文的主要目的是讲解如何使用 MPU9250 和官方motion driver 6.12 库实现姿态解算。*

基础知识：

**坐标系**
（1）地心惯性坐标系（简称 i系）
坐标系的原点O为地球的中心， 且与地球的自转无关。z轴与地球的自转轴重合，其正向指向北极。x轴、y轴在地球的赤道平面内，Ox指向春分点，y轴由右手定则确定。三个坐标轴指向惯性空间固定不动，这个坐标系是惯性仪器测量的参考标准。
（2）地球坐标系（简称 e系）
坐标系的原点O位于地心，z轴为地球自转轴，x轴、y轴在地球的赤道平面内，x轴指向零度子午线，y轴则指向东经90°方向。 e系与i系的相对运动为地球的自转。
（3）地理坐标系（简称 g系）
地理坐标系是指原点位于运载体所在的地球表面，其中一轴与地理垂线重合的右手直角坐标系。地理坐标系的原点O选取在载体重心处，对于地理坐标系的坐标轴有不同的取法，如东北天、北西天、北东地等，这些只是坐标轴的正向方向不同，不影响导航解算。[align=center]![dili.png](/uploads/201908/10/152835z44zx4zky44f4jfo.png)
（4）载体坐标系（简称 b系）
与载体固连在一起，坐标系原点O位于载体的质心，Ox沿载体横轴向右，Oz沿载体垂直轴向上，Oy符合右手定则沿纵轴向前。载体的姿态角信息可由地理坐标系与载体坐标系的角度确定。[align=center]![zaiti.png](/uploads/201908/10/152835z213mut44u2rtt00.png)
（5）导航坐标系（简称 n系）
惯导系统中求解导航参数时所采用的坐标系。
**欧拉角**
莱昂哈德·欧拉用欧拉角来描述刚体在三维欧几里得空间的取向。对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。![FastAdmin](http://www.crazepony.com/assets/img/euler-angles.png) ![200px-Euler2a.gif](/uploads/201908/10/152835lz9fcn0fw000isfc.gif)摘自[维基百科-欧拉角](http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)。
**姿态解算的目的即是为了获取姿态信息，姿态由多种表示方式，常见的是四元数，欧拉角，矩阵和轴角。他们各自有其自身的优点，在不同的领域使用不同的表示方式。通常我们使用欧拉角表述姿态。 ****在姿态解算前，一般需要进行传感器标定等，此不是本文的目的，不再赘述。**
**姿态解算需要一些算法，常见的有 非线性互补滤波算法、非线性互补滤波算法、Mahony互补滤波算法。算法的比较可以参考这里：**[http://www.olliw.eu/2013/imu-data-fusing/](http://www.olliw.eu/2013/imu-data-fusing/)
**鉴于使用的是MPU9250模块，内含DMP，且其官方 InvenSense 的 Motion Driver 6.12（内含姿态解算算法等）支持九轴数据融合。所以直接使用 MD6 库获取姿态信息。不再自行解算。**
**MD6**
（1）获取
MD6可以从x893大神的github获取：[https://github.com/x893/MotionDriver6](https://github.com/x893/MotionDriver6)。MD6对arm支持很好，文档也很详细, 移植相对简单，基本看着文档就可已完成移植和使用。（2）移植
参考文档 “App Note 3- Motion Driver 6.12 Porting Guide”。
首先将源代码添加进工程。源代码即为 “core” 下的所有文件，如果想使用针对性优化过的二进制库，需要替换掉 “core” 的二进制库。然后实现类似如下的底层接口（注意返回值）：![port.png](/uploads/201908/10/152835mxjj53xay4a3gz85.png)最后添加预编译选项：![CPPDEFINES.png](/uploads/201908/10/152835tssjzngrz1r45j7g.png)编译成功即可
（3）使用添加 MD6 中的 “main.c” 到工程中，删减掉不需要的输入输出等，解决编译错误。然后可以创建线程来运行其中的 “main” ，就可以获得欧拉角了。（注意定点数的转化）
[align=center]![app.png](/uploads/201908/10/152835awz6h366zr6mm3me.png)