【先楫HPM5361EVK开发板试用体验】06-基于MPU9250的姿态解算

物体的姿态即物体在三维空间中的朝向和旋转状态。这包括了物体相对于某个参考坐标系的方向，通常通过旋转角度或者其他合适的参数来表示。姿态可以用于描述物体的姿势、方向或者整体的朝向，在三维坐标系中，通常采用欧拉角、四元数或旋转矩阵等方式来表示物体的姿态，欧拉角会更加直观一点。

而欧拉角是一种用于描述物体在三维空间中姿态的表示方法，它通过三个旋转角度来定义物体相对于某个参考坐标系的旋转状态。这三个角度分别是：

l 滚转角（Roll） ： 指物体绕X轴的旋转，在飞行器中，滚转角影响机身左右倾斜的程度。

l 俯仰角（Pitch） ：指物体绕Y轴的旋转，在飞行器中，俯仰角影响机身前后倾斜的程度。

l 偏航角（Yaw） ： 指物体绕Z轴的旋转，在飞行器中，偏航角影响飞行器左右方向的旋转。

姿态解算是从IMU测量的原始数据中推导出设备在三维空间中的方向和位置的过程。由于IMU测量的是加速度和角速度，而非直接的姿态信息，因此需要进行姿态解算以获取设备的准确姿态。姿态解算常用的方法有互补滤波和卡尔曼滤波。

1、卡尔曼滤波简介

卡尔曼滤波是一种利用状态估计和观测数据的最优估计滤波方法。其优点在于能够处理系统噪声和不确定性，并提供较为精确的姿态解算。然而，卡尔曼滤波的实现相对较为复杂，包括状态空间模型、协方差矩阵等概念，对计算资源要求较高。在资源受限的嵌入式系统中，可能不太适合使用卡尔曼滤波。

2、互补滤波简介

互补滤波是一种简单而有效的姿态解算方法。它通过加权融合加速度计和陀螺仪的测量数据，以减小姿态解算的漂移误差。互补滤波具有实现简单、计算开销小的优点，适用于实时性要求高的应用。然而，对于动态环境或快速运动的场景，互补滤波可能会受到积累误差的影响，导致姿态解算的不稳定。

卡尔曼滤波和互补滤波的优缺点对比如下图所示。

3、姿态算法

目前开源的飞控李默认都是使用基于mahony的互补滤波算法进行姿态解算的，基于上面互补滤波和卡尔曼滤波的优缺点对比，这里也是先选择用的互补滤波算法来实现姿态的解算。

姿态解算中会经常涉及到坐标系的概念，如大地坐标系（地理）、机体坐标系等。

大地坐标系是大地测量中以参考椭球面为基准面建立起来的坐标系。地面点的位置用大地经度L、大地纬度B和大地高度H表示。大地坐标系的确立包括选择一个椭球、对椭球进行定位和确定大地起算数据。 一个形状、大小和定位、定向都已确定的地球椭球叫参考椭球。参考椭球一旦确定，则标志着大地坐标系已经建立。

机体坐标系，又名体轴系，是指固定在飞行器或者飞机上的遵循右手法则的三维正交直角坐标系。在机体坐标系下，飞机本身是静止的。在飞机的机体坐标系下，可以定义三个坐标轴：X轴指向飞机的前进方向，Y轴指向飞机的右侧方向，Z轴指向飞机的下方方向

姿态解算中的姿态实际上指的是机体坐标轴与地理坐标系的旋转关系。

更多技术细节可以自行搜索或参考以下出处：

基于AHRS的三类姿态解算算法对比(含代码)-基于手机端惯性传感器的航迹推算算法(下) - 知乎 (zhihu.com)

IMU基于互补滤波的姿态解算探究 (qq.com)

4、测试

由上一篇评测报告可知MPU9250模块和HPM5361的连接如下图所示，这里MPU9250使用的是现成的模块，其引脚定义在丝印上有，HPM5361开发板用的是开发板的SPI1，具体引脚为PA26、PA27、PA28、PA29。

通过上图可知MPU9250定义了一个X/Y/Z方向的坐标系，这个坐标系为ENU坐标系，具体相关可以参考以下出处：

无人机运动学控制中的坐标系，及惯性坐标系与机体坐标系之间的矩阵转换欧拉角**_无人机坐标系转化到地理坐标系-CSDN博客**

Z轴向上时姿态角如下图所示。

X轴向上时姿态角如下图所示。

Y轴向上时姿态角如下图所示。