基于磁力计与MEMS-IMU融合的姿态角解算方法研究
·
目录
概述
针对MEMS惯性测量单元（IMU）单独使用时航向角易受陀螺仪零偏积分漂移影响、磁力计易受环境硬铁干扰导致精度下降的问题，本文提出并实现了一种基于STM32嵌入式平台的9轴传感器融合姿态解算方法。系统以STM32F4微控制器为核心，通过I²C总线采集加速度计、陀螺仪和磁力计数据，采用椭球拟合算法实现磁力计的硬铁与软铁校准，并引入Mahony互补滤波算法将三轴数据融合为四元数，进而解算出横滚、俯仰和航向角。实验结果表明，该校准方法有效降低了环境磁场畸变，融合算法在静态下姿态角波动范围控制在±0.5°以内，动态响应迅速，适用于无人机、机器人及手持设备的姿态测量场景。
关键词： 姿态解算；磁力计校准；Mahony滤波；MEMS-IMU；STM32；嵌入式系统
1 引言
随着微机电系统技术的飞速发展，低成本、低功耗的惯性传感器已广泛应用于无人驾驶、室内导航、姿态控制系统及可穿戴设备等领域。准确、实时地获取载体的三个姿态角（横滚角、俯仰角、航向角）是实现自主导航与稳定控制的核心前提。
在姿态测量系统中，单一的传感器存在固有缺陷：陀螺仪动态性能优良，但长时间工作会产生温度漂移和积分累积误差；加速度计可通过重力矢量解算横滚和俯仰，但无法提供航向信息且受运动加速度干扰严重；磁力计能够感知地磁场方向提供绝对航向，但极易受环境磁场（硬铁和软铁干扰）影响。因此，多传感器数据融合成为提高姿态精度的必然选择。
当前主流融合算法包括扩展卡尔曼滤波、梯度下降法及互补滤波。其中，Mahony互补滤波算法因其计算复杂度低、易于嵌入式实现且能有效抑制陀螺漂移的优点，在工程实践中得到广泛应用。本文基于STM32平台，系统研究了从传感器驱动、磁力计校准到融合解算的完整技术链条，提出了一套高性价比的姿态角解算方案。
2 系统总体方案与硬件设计
2.1 系统总体架构
系统采用模块化设计思想，划分为传感器采集层、数据处理层和融合解算层。硬件平台选用STM32F407ZET6作为主控芯片，其内置浮点运算单元（FPU）可显著提升四元数与三角函数运算效率。传感器部分选用InvenSense MPU6050六轴IMU（集成三轴加速度计和三轴陀螺仪）及Honeywell HMC5883L三轴磁力计。
系统结构框图如图1所示（此处文字描述）：主控芯片通过两个独立的I²C总线接口分别与MPU6050和HMC5883L通信，获取原始传感器数据。原始数据经数字滤波和物理量转换后，进入磁力计校准模块消除硬铁和软铁畸变，最后输入Mahony滤波器与陀螺仪、加速度计数据进行融合，解算并输出稳定的欧拉角。
2.2 传感器数学模型与量程配置
设加速度计输出为重力加速度在载体坐标系下的分量，其标称满量程为±2g，分辨率16384 LSB/g；陀螺仪输出为载体绕各轴的角速度，量程设为±250°/s，分辨率131 LSB/(°/s)。磁力计在±1.3Gauss量程下，分辨率为1090 LSB/Gauss1090。三者数据融合前必须经过归一化和坐标系对齐处理。
3 传感器数据采集与底层驱动实现
本章阐述STM32基于HAL库的I²C底层驱动封装及传感器寄存器级操作实现。该层代码为上层算法提供稳定、阻塞式读取的数据接口。
3.1 I²C通信抽象层
为保证代码可移植性，对HAL库的I²C读写操作进行二次封装，屏蔽寄存器地址字节长度和设备超时参数。关键实现如下：
// i2c_interface.c - 底层通信抽象
#include "stm32f4xx_hal.h"
#define I2C_TIMEOUT 1000
uint8_t I2C_ReadReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *pData, uint16_t len)
{
HAL_StatusTypeDef status;
status = HAL_I2C_Mem_Read(&hi2c1, dev_addr, reg_addr,
I2C_MEMADD_SIZE_8BIT, pData, len, I2C_TIMEOUT);
return (status == HAL_OK) ? 0 : 1;
}
uint8_t I2C_WriteReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *pData, uint16_t len)
{
HAL_StatusTypeDef status;
status = HAL_I2C_Mem_Write(&hi2c1, dev_addr, reg_addr,
I2C_MEMADD_SIZE_8BIT, pData, len, I2C_TIMEOUT);
return (status == HAL_OK) ? 0 : 1;
}3.2 MPU6050驱动实现
MPU6050内部集成数字运动处理器，本设计仅需读取原始数据。初始化流程包括：唤醒芯片（退出睡眠模式）、配置加速度计量程为±2g、陀螺仪量程为±250°/s，并启用内部低通滤波器抑制高频噪声。数据读取函数连续读取14字节寄存器，并按大端格式拼接为16位有符号数。
3.3 HMC5883L驱动与增益配置
HMC5883L支持多种量程配置。为兼顾城市环境地磁场强度（约0.5~0.8Gauss），本文选用±1.3Gauss量程，对应LSB为1.08 mG/LSB。配置寄存器A需设置为连续采样模式、采样率15Hz，以保证与IMU采集频率匹配。
4 磁力计校准算法
磁力计的输出易受载体内部铁磁材料和外部电流产生的磁场干扰影响。干扰可分为两类：硬铁干扰（恒定偏置，表现为球心偏移）和软铁干扰（依赖于方向的畸变，表现为椭球化）。忽略软铁干扰将导致航向角产生周期性的非正弦误差，因此本文采用椭球拟合法进行联合补偿。
4.1 硬铁与软铁畸变模型
4.2 基于椭球拟合的两步校准法
工程实现中，为简化运算并降低内存消耗，本文采用基于极值边界的两步估计策略。第一步（硬铁补偿） ：绕载体做全向“8”字旋转，采集N=200个样本点（NN需足够覆盖全姿态），分别记录三轴的最大值maxi和最小值min，则硬铁偏置估计为：
5 姿态融合算法设计
5.1 基于加速度计的倾角初值提取
在载体静止或匀速运动时，加速度计测量值即为重力场矢量。定义横滚角ϕ和俯仰角θ分别为绕载体坐标系x轴和y轴的旋转角，其解析解为：
该角度用于对磁力计进行倾角补偿，将载体坐标系的磁场矢量投影至水平坐标系。
5.2 倾角补偿下的航向解算
5.3 Mahony互补滤波器设计
Mahony滤波器属于显式互补滤波器，其核心思想是利用加速度计和磁力计的矢量观测值构造误差修正项，通过PI控制环节补偿陀螺仪的零偏和漂移，进而更新表征姿态的四元数
其中Ω为角速度的斜对称矩阵。在离散时间域，采用一阶毕卡逼近更新四元数。
误差函数构造：
将参考重力向量va和参考磁场向量vm从导航坐标系旋转至机体坐标系，与加速度计测量值a和磁力计测量值m进行叉积运算，得到姿态误差：
该误差经比例-积分环节叠加至陀螺仪输出，实现无静差跟踪。滤波器参数Kp决定对加速度/磁力计的信任程度，Ki决定消除陀螺零偏的速度。经调试，本文选取Kp=1.0，Ki=0.001，在抗干扰性和快速性之间取得平衡。
6 实验验证与结果分析
6.1 实验平台与测试条件
为验证本文算法的有效性，搭建了基于STM32F407核心板的实验平台。传感器以100Hz频率输出数据，经过校准与融合后，通过串口以50Hz频率向上位机发送姿态角数据。上位机采用MATLAB进行数据记录和图形化分析。静态测试将平台置于水平转台上；动态测试通过手动旋转和摇摆模拟载体运动。
6.2 磁力计校准效果对比
展示了校准前后的磁力计三维散点分布。校准前，数据云呈现明显的椭球形态且球心偏离原点约60mGauss，导致航向角在旋转过程中出现约8∘的周期性误差。校准后，数据云被映射至以原点为中心的近似球体上，三轴幅值一致性提升至98%以上。
6.3 姿态角静态与动态性能分析
在静态水平测试中，融合算法输出的横滚角和俯仰角在60秒内的标准差分别为0.3∘和0.35∘，航向角标准差为0.6∘。纯陀螺积分模式下航向角在60秒内漂移超过15∘，而融合算法将漂移抑制在1∘以内，充分表明磁力计校准与融合策略有效约束了航向发散。
在动态摇摆测试（频率1Hz，幅值±30∘）中，Mahony滤波器的响应延时约为20ms，能紧密跟踪角度变化，未出现失步或震荡现象，满足实时控制需求。
7 结语
本文系统论述了一种面向STM32平台的磁力计/IMU融合姿态解算方案，完整覆盖了从底层驱动、传感器校准到高阶融合算法的工程实现路径。研究结果表明：（1）基于极值边界估计的磁力计两步校准法能有效消除硬铁和软铁干扰，显著提升航向角精度；（2）Mahony互补滤波算法在STM32F4平台上具备优异的实时性与鲁棒性，能在抑制陀螺漂移的同时保留其动态响应能力。
未来的工作将聚焦于两点：其一，引入基于磁异常检测的自主抗扰机制，在强电磁干扰环境下自动降低磁力计权重以提升系统安全性；其二，利用STM32的DSP指令集对四元数运算进行加速，进一步降低功耗。
附件代码：
1） I2C初始化
以STM32F4系列为例，使用HAL库配置I2C：
// i2c.h
#ifndef I2C_H
#define I2C_H
#include "stm32f4xx_hal.h"
extern I2C_HandleTypeDef hi2c1;
#define I2C_TIMEOUT 1000
// 磁力计常用I2C地址
#define HMC5883L_ADDR_WRITE 0x3C // 0x1E << 1
#define HMC5883L_ADDR_READ 0x3D // 0x1E << 1 | 1
#define MPU9250_ADDR 0x68 // AD0接地时
// I2C读写函数
uint8_t I2C_ReadReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
uint8_t I2C_WriteReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len);
#endif// i2c.c
#include "i2c.h"
uint8_t I2C_ReadReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
HAL_StatusTypeDef ret;
ret = HAL_I2C_Mem_Read(&hi2c1, dev_addr, reg_addr, I2C_MEMADD_SIZE_8BIT,
data, len, I2C_TIMEOUT);
return (ret == HAL_OK) ? 0 : 1;
}
uint8_t I2C_WriteReg(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data, uint16_t len)
{
HAL_StatusTypeDef ret;
ret = HAL_I2C_Mem_Write(&hi2c1, dev_addr, reg_addr, I2C_MEMADD_SIZE_8BIT,
data, len, I2C_TIMEOUT);
return (ret == HAL_OK) ? 0 : 1;
}2） 磁力计驱动（以HMC5883L为例）
HMC5883L内部寄存器包括配置寄存器A（0x00）、配置寄存器B（0x01）和模式寄存器（0x03）
// hmc5883l.h
#ifndef HMC5883L_H
#define HMC5883L_H
#include <stdint.h>
#define HMC5883L_DEV_ADDR 0x1E
// 寄存器地址
#define HMC5883L_REG_CONFIG_A 0x00
#define HMC5883L_REG_CONFIG_B 0x01
#define HMC5883L_REG_MODE 0x02
#define HMC5883L_REG_DATA_X_MSB 0x03
#define HMC5883L_REG_DATA_X_LSB 0x04
#define HMC5883L_REG_DATA_Z_MSB 0x05
#define HMC5883L_REG_DATA_Z_LSB 0x06
#define HMC5883L_REG_DATA_Y_MSB 0x07
#define HMC5883L_REG_DATA_Y_LSB 0x08
#define HMC5883L_REG_STATUS 0x09
#define HMC5883L_REG_ID_A 0x0A
#define HMC5883L_REG_ID_B 0x0B
#define HMC5883L_REG_ID_C 0x0C
// 测量量程配置（配置寄存器B的GN[2:0]）
typedef enum {
HMC5883L_GAIN_0_88 = 0x00, // ±0.88G, LSB=0.73mG
HMC5883L_GAIN_1_3 = 0x01, // ±1.3G, LSB=1.08mG
HMC5883L_GAIN_1_9 = 0x02, // ±1.9G, LSB=1.58mG
HMC5883L_GAIN_2_5 = 0x03, // ±2.5G, LSB=2.08mG
HMC5883L_GAIN_4_0 = 0x04, // ±4.0G, LSB=3.33mG
HMC5883L_GAIN_4_7 = 0x05, // ±4.7G, LSB=3.91mG
HMC5883L_GAIN_5_6 = 0x06 // ±8.1G, LSB=6.75mG
} HMC5883L_Gain_t;
typedef struct {
int16_t x;
int16_t y;
int16_t z;
} HMC5883L_Data_t;
uint8_t HMC5883L_Init(HMC5883L_Gain_t gain);
uint8_t HMC5883L_ReadData(HMC5883L_Data_t *data);
void HMC5883L_GetScaledData(HMC5883L_Data_t *raw, float *mx, float *my, float *mz, float lsb_per_g);
#endif// hmc5883l.c
#include "hmc5883l.h"
#include "i2c.h"
static float g_lsb_per_gauss = 0.73f; // 默认±0.88G量程
uint8_t HMC5883L_Init(HMC5883L_Gain_t gain)
{
uint8_t data;
uint8_t ret;
// 配置寄存器A: 采样率15Hz, 正常测量模式
data = 0x70; // DO[2:0]=111 (15Hz), MS[1:0]=01 (正常)
ret = I2C_WriteReg(HMC5883L_DEV_ADDR, HMC5883L_REG_CONFIG_A, &data, 1);
if (ret) return 1;
// 配置寄存器B: 设置增益
data = gain << 5;
ret = I2C_WriteReg(HMC5883L_DEV_ADDR, HMC5883L_REG_CONFIG_B, &data, 1);
if (ret) return 1;
// 设置LSB系数
switch(gain) {
case HMC5883L_GAIN_0_88: g_lsb_per_gauss = 0.73f; break;
case HMC5883L_GAIN_1_3: g_lsb_per_gauss = 1.08f; break;
case HMC5883L_GAIN_1_9: g_lsb_per_gauss = 1.58f; break;
case HMC5883L_GAIN_2_5: g_lsb_per_gauss = 2.08f; break;
case HMC5883L_GAIN_4_0: g_lsb_per_gauss = 3.33f; break;
case HMC5883L_GAIN_4_7: g_lsb_per_gauss = 3.91f; break;
case HMC5883L_GAIN_5_6: g_lsb_per_gauss = 6.75f; break;
default: g_lsb_per_gauss = 0.73f; break;
}
// 模式寄存器: 连续测量模式
data = 0x00;
ret = I2C_WriteReg(HMC5883L_DEV_ADDR, HMC5883L_REG_MODE, &data, 1);
return ret;
}
uint8_t HMC5883L_ReadData(HMC5883L_Data_t *data)
{
uint8_t buf[6];
uint8_t ret;
ret = I2C_ReadReg(HMC5883L_DEV_ADDR, HMC5883L_REG_DATA_X_MSB, buf, 6);
if (ret) return 1;
// 数据为左对齐的16位有符号补码[reference:4]
data->x = (int16_t)((buf[0] << 8) | buf[1]);
data->z = (int16_t)((buf[2] << 8) | buf[3]);
data->y = (int16_t)((buf[4] << 8) | buf[5]);
return 0;
}
void HMC5883L_GetScaledData(HMC5883L_Data_t *raw, float *mx, float *my, float *mz,
float lsb_per_gauss)
{
// 将LSB转换为高斯(Gauss)
*mx = (float)raw->x * lsb_per_gauss;
*my = (float)raw->y * lsb_per_gauss;
*mz = (float)raw->z * lsb_per_gauss;
}3） 六轴IMU驱动（以MPU6050为例）
// mpu6050.h
#ifndef MPU6050_H
#define MPU6050_H
#include <stdint.h>
#define MPU6050_DEV_ADDR 0x68
// 寄存器地址
#define MPU6050_REG_ACCEL_XOUT_H 0x3B
#define MPU6050_REG_GYRO_XOUT_H 0x43
#define MPU6050_REG_PWR_MGMT_1 0x6B
#define MPU6050_REG_ACCEL_CONFIG 0x1C
#define MPU6050_REG_GYRO_CONFIG 0x1B
typedef struct {
int16_t ax, ay, az;
int16_t gx, gy, gz;
} MPU6050_Data_t;
uint8_t MPU6050_Init(void);
uint8_t MPU6050_ReadAll(MPU6050_Data_t *data);
#endif// mpu6050.c
#include "mpu6050.h"
#include "i2c.h"
uint8_t MPU6050_Init(void)
{
uint8_t data;
// 退出睡眠模式
data = 0x00;
if (I2C_WriteReg(MPU6050_DEV_ADDR, MPU6050_REG_PWR_MGMT_1, &data, 1)) return 1;
// 加速度计量程: ±2g
data = 0x00;
if (I2C_WriteReg(MPU6050_DEV_ADDR, MPU6050_REG_ACCEL_CONFIG, &data, 1)) return 1;
// 陀螺仪量程: ±250°/s
data = 0x00;
if (I2C_WriteReg(MPU6050_DEV_ADDR, MPU6050_REG_GYRO_CONFIG, &data, 1)) return 1;
return 0;
}
uint8_t MPU6050_ReadAll(MPU6050_Data_t *data)
{
uint8_t buf[14];
if (I2C_ReadReg(MPU6050_DEV_ADDR, MPU6050_REG_ACCEL_XOUT_H, buf, 14)) return 1;
data->ax = (int16_t)((buf[0] << 8) | buf[1]);
data->ay = (int16_t)((buf[2] << 8) | buf[3]);
data->az = (int16_t)((buf[4] << 8) | buf[5]);
data->gx = (int16_t)((buf[8] << 8) | buf[9]);
data->gy = (int16_t)((buf[10] << 8) | buf[11]);
data->gz = (int16_t)((buf[12] << 8) | buf[13]);
return 0;
}4） 磁力计校准
磁力计校准是保证航向精度的首要前提。主要包含：
-
硬铁干扰：由设备内部永磁体或电流产生恒定偏置，表现为椭球中心偏离原点
-
软铁干扰：由高导磁材料引起轴间耦合与尺度畸变
4-1）简易硬铁校准（偏移补偿）
// mag_calibration.h
#ifndef MAG_CALIBRATION_H
#define MAG_CALIBRATION_H
#include <stdint.h>
typedef struct {
float offset_x;
float offset_y;
float offset_z;
float scale_x;
float scale_y;
float scale_z;
uint8_t calibrated;
} MagCalib_t;
extern MagCalib_t g_mag_calib;
void MagCalib_Init(void);
void MagCalib_CollectSample(float mx, float my, float mz);
uint8_t MagCalib_IsComplete(void);
void MagCalib_ComputeOffsets(void);
void MagCalib_Apply(float *mx, float *my, float *mz);
#endif// mag_calibration.c
#include "mag_calibration.h"
#include <math.h>
#define CALIB_SAMPLES 200
static float samples_x[CALIB_SAMPLES];
static float samples_y[CALIB_SAMPLES];
static float samples_z[CALIB_SAMPLES];
static uint16_t sample_count = 0;
MagCalib_t g_mag_calib = {0};
void MagCalib_Init(void)
{
sample_count = 0;
g_mag_calib.calibrated = 0;
}
void MagCalib_CollectSample(float mx, float my, float mz)
{
if (sample_count < CALIB_SAMPLES) {
samples_x[sample_count] = mx;
samples_y[sample_count] = my;
samples_z[sample_count] = mz;
sample_count++;
}
}
uint8_t MagCalib_IsComplete(void)
{
return (sample_count >= CALIB_SAMPLES);
}
void MagCalib_ComputeOffsets(void)
{
float min_x = 1000, max_x = -1000;
float min_y = 1000, max_y = -1000;
float min_z = 1000, max_z = -1000;
for (int i = 0; i < sample_count; i++) {
if (samples_x[i] < min_x) min_x = samples_x[i];
if (samples_x[i] > max_x) max_x = samples_x[i];
if (samples_y[i] < min_y) min_y = samples_y[i];
if (samples_y[i] > max_y) max_y = samples_y[i];
if (samples_z[i] < min_z) min_z = samples_z[i];
if (samples_z[i] > max_z) max_z = samples_z[i];
}
// 偏移量为最大值和最小值的平均值
g_mag_calib.offset_x = (max_x + min_x) / 2.0f;
g_mag_calib.offset_y = (max_y + min_y) / 2.0f;
g_mag_calib.offset_z = (max_z + min_z) / 2.0f;
// 尺度归一化（使各轴范围一致）
float range_x = max_x - min_x;
float range_y = max_y - min_y;
float range_z = max_z - min_z;
float avg_range = (range_x + range_y + range_z) / 3.0f;
g_mag_calib.scale_x = avg_range / range_x;
g_mag_calib.scale_y = avg_range / range_y;
g_mag_calib.scale_z = avg_range / range_z;
g_mag_calib.calibrated = 1;
}
void MagCalib_Apply(float *mx, float *my, float *mz)
{
if (!g_mag_calib.calibrated) return;
// 减去偏移
float cx = *mx - g_mag_calib.offset_x;
float cy = *my - g_mag_calib.offset_y;
float cz = *mz - g_mag_calib.offset_z;
// 尺度校正[reference:8]
*mx = cx * g_mag_calib.scale_x;
*my = cy * g_mag_calib.scale_y;
*mz = cz * g_mag_calib.scale_z;
}5） 倾角补偿（Tilt Compensation）
当设备不水平时，磁力计测得的磁场方向与水平面有夹角，必须进行倾角补偿才能得到正确的航向角
5.1） 由加速度计计算俯仰和横滚角
// tilt_compensation.h
#ifndef TILT_COMPENSATION_H
#define TILT_COMPENSATION_H
#include <math.h>
#define RAD_TO_DEG 57.295779513f
#define DEG_TO_RAD 0.01745329252f
void AccelToRollPitch(float ax, float ay, float az, float *roll, float *pitch);
void TiltCompensateMag(float mx, float my, float mz, float roll, float pitch,
float *mx_comp, float *my_comp);
#endif// tilt_compensation.c
#include "tilt_compensation.h"
void AccelToRollPitch(float ax, float ay, float az, float *roll, float *pitch)
{
// 加速度归一化
float norm = sqrtf(ax*ax + ay*ay + az*az);
if (norm < 0.001f) return;
float ax_n = ax / norm;
float ay_n = ay / norm;
float az_n = az / norm;
// 横滚角: 绕X轴旋转
*roll = atan2f(ay_n, az_n);
// 俯仰角: 绕Y轴旋转
*pitch = atan2f(-ax_n, sqrtf(ay_n*ay_n + az_n*az_n));
}
void TiltCompensateMag(float mx, float my, float mz, float roll, float pitch,
float *mx_comp, float *my_comp)
{
float cr = cosf(roll);
float sr = sinf(roll);
float cp = cosf(pitch);
float sp = sinf(pitch);
// 将磁力计数据从机体坐标系旋转到水平坐标系[reference:12]
// 旋转矩阵: R = R_y(-pitch) * R_x(-roll)
*mx_comp = mx * cp + my * sr * sp + mz * cr * sp;
*my_comp = my * cr - mz * sr;
}5.2） 计算航向角
float CalculateHeading(float mx_comp, float my_comp, float declination_deg)
{
// 四象限反正切，自动处理象限判断[reference:13]
float heading = atan2f(my_comp, mx_comp);
// 弧度转角度
float heading_deg = heading * RAD_TO_DEG;
// 归一化到0-360度
if (heading_deg < 0) heading_deg += 360.0f;
// 磁偏角修正：真北航向 = 磁北航向 + 当地磁偏角[reference:14]
heading_deg += declination_deg;
if (heading_deg >= 360.0f) heading_deg -= 360.0f;
if (heading_deg < 0) heading_deg += 360.0f;
return heading_deg;
}6）传感器融合算法
6.1 ）Mahony互补滤波（推荐）
Mahony滤波器是一种计算效率高、适合嵌入式平台的姿态融合算法
// mahony_filter.h
#ifndef MAHONY_FILTER_H
#define MAHONY_FILTER_H
#include <stdint.h>
#include <math.h>
typedef struct {
float q0, q1, q2, q3; // 四元数
float roll, pitch, yaw; // 欧拉角（弧度）
float gyro_bias[3]; // 陀螺仪零偏
float twoKp; // 比例增益
float twoKi; // 积分增益
float integralFBx, integralFBy, integralFBz;
uint8_t initialized;
} MahonyFilter_t;
void Mahony_Init(MahonyFilter_t *filter, float kp, float ki);
void Mahony_Update(MahonyFilter_t *filter,
float gx, float gy, float gz, // 陀螺仪 (rad/s)
float ax, float ay, float az, // 加速度计 (g)
float mx, float my, float mz, // 磁力计 (uT)
float dt);
void Mahony_GetEuler(MahonyFilter_t *filter, float *roll, float *pitch, float *yaw);
#endif// mahony_filter.c
#include "mahony_filter.h"
static float invSqrt(float x)
{
return 1.0f / sqrtf(x);
}
void Mahony_Init(MahonyFilter_t *filter, float kp, float ki)
{
filter->q0 = 1.0f;
filter->q1 = 0.0f;
filter->q2 = 0.0f;
filter->q3 = 0.0f;
filter->twoKp = 2.0f * kp;
filter->twoKi = 2.0f * ki;
filter->integralFBx = 0.0f;
filter->integralFBy = 0.0f;
filter->integralFBz = 0.0f;
filter->initialized = 1;
}
void Mahony_Update(MahonyFilter_t *filter,
float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz,
float dt)
{
float q0 = filter->q0, q1 = filter->q1, q2 = filter->q2, q3 = filter->q3;
float twoKp = filter->twoKp;
float twoKi = filter->twoKi;
float integralFBx = filter->integralFBx;
float integralFBy = filter->integralFBy;
float integralFBz = filter->integralFBz;
float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
float ax_norm, ay_norm, az_norm;
float mx_norm, my_norm, mz_norm;
// 加速度归一化
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax_norm = ax * recipNorm;
ay_norm = ay * recipNorm;
az_norm = az * recipNorm;
// 磁力计归一化
recipNorm = invSqrt(mx*mx + my*my + mz*mz);
mx_norm = mx * recipNorm;
my_norm = my * recipNorm;
mz_norm = mz * recipNorm;
// 四元数元素乘积
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;
// 参考磁场方向（地磁场在导航坐标系中的方向）
hx = mx_norm * q0q0 - my_norm * q0q3 + mz_norm * q0q2
+ mx_norm * q1q1 + my_norm * q1q2 + mz_norm * q1q3
- mx_norm * q2q2 - my_norm * q2q1 + mz_norm * q2q0
+ mx_norm * q3q3 - my_norm * q3q0 - mz_norm * q3q1;
hy = mx_norm * q0q3 + my_norm * q0q0 - mz_norm * q0q1
+ mx_norm * q1q2 - my_norm * q1q1 + mz_norm * q1q0
+ mx_norm * q2q1 + my_norm * q2q2 + mz_norm * q2q3
- mx_norm * q3q0 + my_norm * q3q3 - mz_norm * q3q2;
bx = sqrtf(hx*hx + hy*hy);
bz = -mx_norm * q0q2 + my_norm * q0q1 - mz_norm * q0q0
+ mx_norm * q1q3 - my_norm * q1q0 - mz_norm * q1q1
+ mx_norm * q2q0 + my_norm * q2q3 + mz_norm * q2q2
+ mx_norm * q3q1 + my_norm * q3q2 - mz_norm * q3q3;
// 加速度计估计的重力方向
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;
// 磁力计估计的磁场方向
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
// 误差 = 测量值 - 估计值
float ex = (ay_norm * halfvz - az_norm * halfvy) + (my_norm * halfwz - mz_norm * halfwy);
float ey = (az_norm * halfvx - ax_norm * halfvz) + (mz_norm * halfwx - mx_norm * halfwz);
float ez = (ax_norm * halfvy - ay_norm * halfvx) + (mx_norm * halfwy - my_norm * halfwx);
// 积分误差（消除陀螺仪零偏）
if (twoKi > 0.0f) {
integralFBx += twoKi * ex * dt;
integralFBy += twoKi * ey * dt;
integralFBz += twoKi * ez * dt;
gx += integralFBx;
gy += integralFBy;
gz += integralFBz;
} else {
integralFBx = 0.0f;
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// 应用比例反馈
gx += twoKp * ex;
gy += twoKp * ey;
gz += twoKp * ez;
// 四元数微分方程
gx *= 0.5f * dt;
gy *= 0.5f * dt;
gz *= 0.5f * dt;
float qa = q0;
float qb = q1;
float qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// 归一化四元数
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
filter->q0 = q0 * recipNorm;
filter->q1 = q1 * recipNorm;
filter->q2 = q2 * recipNorm;
filter->q3 = q3 * recipNorm;
filter->integralFBx = integralFBx;
filter->integralFBy = integralFBy;
filter->integralFBz = integralFBz;
}
void Mahony_GetEuler(MahonyFilter_t *filter, float *roll, float *pitch, float *yaw)
{
float q0 = filter->q0, q1 = filter->q1, q2 = filter->q2, q3 = filter->q3;
// 从四元数提取欧拉角
*roll = atan2f(2.0f * (q0*q1 + q2*q3), 1.0f - 2.0f * (q1*q1 + q2*q2));
*pitch = asinf(2.0f * (q0*q2 - q3*q1));
*yaw = atan2f(2.0f * (q0*q3 + q1*q2), 1.0f - 2.0f * (q2*q2 + q3*q3));
}6.2 ）主程序示例
// main.c
#include "stm32f4xx_hal.h"
#include "i2c.h"
#include "hmc5883l.h"
#include "mpu6050.h"
#include "mag_calibration.h"
#include "tilt_compensation.h"
#include "mahony_filter.h"
#include <stdio.h>
#include <math.h>
// 传感器数据
static MPU6050_Data_t imu_data;
static HMC5883L_Data_t mag_raw;
static float ax, ay, az; // 加速度 (g)
static float gx, gy, gz; // 角速度 (rad/s)
static float mx, my, mz; // 磁场 (uT)
// 姿态滤波器
static MahonyFilter_t attitude_filter;
// 时间戳
static uint32_t last_update_time = 0;
// 磁偏角（上海约-5度，需查当地WMM数据）
#define MAGNETIC_DECLINATION -5.0f
// 传感器量程转换系数
#define ACCEL_SCALE_2G (2.0f / 32768.0f)
#define GYRO_SCALE_250DPS (250.0f / 32768.0f * DEG_TO_RAD)
int main(void)
{
HAL_Init();
SystemClock_Config();
MX_I2C1_Init();
MX_USART1_UART_Init();
// 初始化传感器
HMC5883L_Init(HMC5883L_GAIN_1_3);
MPU6050_Init();
// 初始化Mahony滤波器 (Kp=1.0, Ki=0.001)
Mahony_Init(&attitude_filter, 1.0f, 0.001f);
// 磁力计校准（旋转设备做"8"字运动）
printf("Starting magnetometer calibration...\n");
MagCalib_Init();
while (!MagCalib_IsComplete()) {
HMC5883L_ReadData(&mag_raw);
HMC5883L_GetScaledData(&mag_raw, &mx, &my, &mz, 1.08f);
MagCalib_CollectSample(mx, my, mz);
HAL_Delay(10);
}
MagCalib_ComputeOffsets();
printf("Calibration complete!\n");
last_update_time = HAL_GetTick();
while (1) {
uint32_t now = HAL_GetTick();
float dt = (now - last_update_time) / 1000.0f;
last_update_time = now;
if (dt > 0.05f) dt = 0.05f; // 限幅防止异常
// 读取IMU数据
MPU6050_ReadAll(&imu_data);
ax = imu_data.ax * ACCEL_SCALE_2G;
ay = imu_data.ay * ACCEL_SCALE_2G;
az = imu_data.az * ACCEL_SCALE_2G;
gx = imu_data.gx * GYRO_SCALE_250DPS;
gy = imu_data.gy * GYRO_SCALE_250DPS;
gz = imu_data.gz * GYRO_SCALE_250DPS;
// 读取磁力计数据并校准
HMC5883L_ReadData(&mag_raw);
HMC5883L_GetScaledData(&mag_raw, &mx, &my, &mz, 1.08f);
MagCalib_Apply(&mx, &my, &mz);
// Mahony滤波器更新（9轴融合）
Mahony_Update(&attitude_filter, gx, gy, gz, ax, ay, az, mx, my, mz, dt);
// 获取欧拉角
float roll, pitch, yaw;
Mahony_GetEuler(&attitude_filter, &roll, &pitch, &yaw);
// 航向角转角度并修正磁偏角
float heading_deg = yaw * RAD_TO_DEG + MAGNETIC_DECLINATION;
if (heading_deg < 0) heading_deg += 360.0f;
if (heading_deg >= 360.0f) heading_deg -= 360.0f;
// 输出结果
printf("Roll: %.2f, Pitch: %.2f, Yaw: %.2f\n",
roll * RAD_TO_DEG, pitch * RAD_TO_DEG, heading_deg);
HAL_Delay(10);
}
}
智能硬件社区聚焦AI智能硬件技术生态，汇聚嵌入式AI、物联网硬件开发者，打造交流分享平台，同步全国赛事资讯、开展 OPC 核心人才招募，助力技术落地与开发者成长。
更多推荐
6
0- 0
已为社区贡献30条内容
所有评论(0)