pi六轴算法_Mahony姿态解算算法笔记（二）

本文使用 Zhihu On VSCode 创作并发布

在Mahony姿态解算算法笔记（一）中，我们介绍了融合加速度计与陀螺仪共六轴数据的姿态解算算法，该篇将介绍融合加速度计、陀螺仪和磁力计共九轴数据的姿态解算算法。

该算法可到其网站下载源码https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/

由于笔者水平有限，文中难免存在一些不足和错误之处，诚请各位批评指正。

1 引入磁力计数据的目的

在六轴数据融合姿态解算算法中，我们通过理论和实际重力加速度向量来补偿陀螺仪误差。但重力加速度向量垂直于大地坐标系的

平面，即平行于Yaw轴转轴，故无法用于修正Yaw轴的角度数据。因此我们引入磁力计数据，由于地球磁场方向在中低纬度地区与地面大致平行，因此通过磁力计数据我们可以有效的修正Yaw轴的角度数据。

但在一部分情况中，复杂的磁场环境会导致九轴数据融合的结果并不能远好于六轴数据，甚至劣于六轴数据融合的结果，比如RoboMaster机器人云台在某些情况下不使用磁力计数据。原因是云台复杂多变的磁场环境会给磁力计引入很大的噪声。综上所述，具体选择九轴还是六轴数据的融合还需要根据具体环境来决定。

2 椭球拟合

由于

三轴的单位存在差异，测量数据的向量顶点会落在椭球面上而非理想状态下的球面上。另外，电路板产生的电磁场会使磁力计测量数据出现偏移，这则会导致椭球的中心不在原点。因此，在磁力计进行椭球拟合是很有必要的。具体椭球拟合算法可以参考

https:// blog.csdn.net/shenshike xmu/article/details/70143455

3 融合磁力计数据

九轴数据融合于六轴数据融合算法的思路与框架是完全一致的，只需在六轴数据融合算法的基础上加入磁力计修正部分即可。与加速度计补偿方法大致相同，唯一的区别在于标准重力加速度在地球表面任何地方始终固定不变，可直接从地理坐标系变换到机体坐标系。但地磁的大小与方向并非固定不变，**因此需要磁力计测量值本身的数据确定理论地磁向量，由于磁力计测得的磁场向量在机体坐标系中，该向量还需通过

变换到地理坐标系中，经过一定处理后，再通过

变换回机体坐标系，从而得到理论地磁向量。**经过两次四元数确定的坐标转换矩阵即可使理论地磁向量与实际地磁向量的偏差反应角速度误差，就好比从白汤中取出肉片在红汤中蘸一蘸便有了辣味。

3.1 将磁场信息表示为向量

地球磁场可以分解为（投影到）水平和竖直两个分量。其中竖直分量与水平分量的比值由磁场与大地的倾角决定，而这个倾角在不同纬度地区会有所不同，在赤道地区磁场方向与大地几乎平行，而在磁极处则垂直于大地，因此在磁极处磁力计无法用于校正航向。我们将由测量和姿态矩阵转换得到的理论磁场向量

与仅通过测量得到的

实际磁场向量

表示为࿱