加速度计与陀螺仪融合解算姿态角

1. 从手机屏幕旋转到无人机飞行为什么需要姿态角不知道你有没有注意过当你把手机从竖屏切换到横屏时屏幕内容会自动旋转这背后就是姿态角在起作用。姿态角专业点说叫欧拉角其实就是描述一个物体在三维空间里歪成什么样的三个角度。就像你用手机拍照时手稍微倾斜一点照片就会跟着转个方向。在无人机和机器人领域姿态角更是关键中的关键。想象一下无人机在空中飞行时的场景它需要知道自己是平飞、爬升还是俯冲俯仰角pitch是向左倾斜还是向右倾斜横滚角roll以及机头指向哪个方向航向角yaw。这三个角度合起来就是完整的姿态信息。但这里有个问题单独使用加速度计或者陀螺仪都很难准确测量这些角度。加速度计就像个水平仪能告诉你哪个方向朝下但动态情况下会被运动干扰陀螺仪像个旋转计数器能感知转动速度但时间一长就会累积误差。这就像你闭着眼睛转圈——刚开始还能记得转了几圈转久了就晕头转向了。2. 传感器单独作战的局限性2.1 加速度计静态准动态懵加速度计测量的是物体受到的加速度包括重力加速度。在静止状态下它确实是个测量姿态角的好手。比如你的手机平放在桌面上时Z轴测到1g重力加速度X/Y轴接近0这时候计算姿态角非常准。但一旦动起来就麻烦了。假设无人机正在加速上升除了重力加速度还有向上的运动加速度。这时候加速度计测到的下方向就不是真正的重力方向了算出来的姿态角自然也不准。我做过一个实验用手快速晃动装有加速度计的开发板测得的俯仰角误差能达到20度以上。2.2 陀螺仪短期稳长期飘陀螺仪测量的是角速度也就是转动的快慢。通过对角速度积分理论上可以得到姿态角的变化。这个方法在短时间内非常精确我用MPU6050模块测试过在1秒内的姿态跟踪误差不到1度。但积分会累积误差。就像你用步数计算走了多远如果步长测量有误差走得越远偏差越大。陀螺仪的零偏不转动时也有微小输出会导致积分结果随时间不断偏离真实值。实测中10分钟后姿态角误差可能超过30度完全不能用了。3. 传感器融合112的解决方案3.1 互补滤波简单实用的入门方法既然两个传感器各有优缺点很自然想到把它们结合起来。互补滤波就是最直观的解决方案用加速度计修正陀螺仪的长期漂移用陀螺仪弥补加速度计的动态误差。具体实现可以这么理解把加速度计测得的角度当作慢响应但准确的参考陀螺仪积分得到快响应但会飘的角度然后用一个加权系数α通常取0.01-0.1来混合两者当前角度 α * 加速度计角度 (1-α) * (上一角度 陀螺仪角速度 * Δt)我在STM32上实现过这个算法代码核心部分大概是这样float complementary_filter(float acc_angle, float gyro_rate, float prev_angle, float alpha) { return alpha * acc_angle (1-alpha) * (prev_angle gyro_rate * DT); }这个方法的优点是计算量小在8位单片机上都能跑。实测在四轴飞行器上姿态角误差能控制在3度以内足够完成基础飞行控制。3.2 卡尔曼滤波更智能的融合方式想要更高精度卡尔曼滤波是更专业的解决方案。它不像互补滤波那样简单加权而是建立了系统的状态方程和观测方程通过概率统计的方式最优地结合两个传感器的信息。卡尔曼滤波分为预测和更新两个步骤预测用陀螺仪数据预测当前姿态更新用加速度计测量值修正预测值我最近用Python实现了一个简化版的卡尔曼滤波器class SimpleKalman: def __init__(self): self.angle 0 self.bias 0 self.P [[0,0],[0,0]] # 误差协方差矩阵 def update(self, acc_angle, gyro_rate, dt): # 预测步骤 self.angle (gyro_rate - self.bias) * dt self.P[0][0] dt * (dt*self.P[1][1] - self.P[0][1] - self.P[1][0] 0.003) # 过程噪声 self.P[0][1] - dt * self.P[1][1] self.P[1][0] - dt * self.P[1][1] self.P[1][1] 0.001 * dt # 陀螺仪零偏噪声 # 更新步骤 y acc_angle - self.angle # 测量残差 S self.P[0][0] 0.05 # 测量噪声 K [self.P[0][0]/S, self.P[1][0]/S] # 卡尔曼增益 self.angle K[0] * y self.bias K[1] * y # 更新协方差 self.P[0][0] - K[0] * self.P[0][0] self.P[0][1] - K[0] * self.P[0][1] self.P[1][0] - K[1] * self.P[0][0] self.P[1][1] - K[1] * self.P[0][1] return self.angle这个算法在树莓派上测试时姿态角误差能控制在1度以内即使剧烈晃动也能快速收敛。不过计算量比互补滤波大不少需要至少ARM Cortex-M4级别的处理器才能流畅运行。4. 实际应用中的坑与技巧4.1 传感器校准精度从何而来无论算法多高级如果传感器本身数据不准结果肯定好不了。加速度计和陀螺仪都需要校准我总结了一套简单实用的校准方法加速度计校准将设备六个面前、后、左、右、上、下依次朝下放置记录每个位置的三轴输出计算各轴的偏移量和比例因子陀螺仪校准保持设备完全静止采集100-200个样本计算各轴的平均值作为零偏值校准后的传感器性能会有显著提升。我曾经测试过校准后的MPU6050陀螺仪零偏稳定性能提高10倍以上。4.2 动态条件下的特殊处理在无人机实际飞行中会遇到很多特殊情况需要处理剧烈加速度情况当检测到较大的非重力加速度如无人机快速爬升时应该暂时降低加速度计的权重甚至完全依赖陀螺仪。可以通过计算加速度矢量的模来判断float acc_magnitude sqrt(ax*ax ay*ay az*az); if(fabs(acc_magnitude - 1.0) 0.2) { // 非重力加速度超过0.2g // 降低加速度计权重 }磁力计辅助在需要绝对航向的场合如无人机导航可以引入磁力计来修正yaw角的漂移。不过要注意避开电机等强磁场干扰源。5. 从理论到实践一个完整的实现案例5.1 硬件选型建议根据我的项目经验推荐几款性价比高的IMU模块型号加速度计量程陀螺仪量程接口适用场景MPU6050±16g±2000°/sI2C入门级无人机BMI160±16g±2000°/sSPI/I2C中端机器人ICM-20602±16g±2000°/sSPI高端飞控ADXL345±16g-I2C纯加速度计应用5.2 软件实现步骤以STM32MPU6050为例分享我的实现流程初始化硬件配置I2C接口设置MPU6050采样率建议500Hz启用DLPF数字低通滤波传感器校准静止状态下采集100组数据计算各轴零偏数据读取线程void IMU_Thread(void const *argument) { while(1) { MPU6050_Read_Data(acc, gyro); osDelay(2); // 500Hz采样 } }姿态解算线程void Attitude_Thread(void const *argument) { float roll0, pitch0; while(1) { // 加速度计角度 float acc_roll atan2(acc.y, acc.z); float acc_pitch atan2(-acc.x, sqrt(acc.y*acc.y acc.z*acc.z)); // 互补滤波 roll 0.02*acc_roll 0.98*(roll gyro.x*0.002); pitch 0.02*acc_pitch 0.98*(pitch gyro.y*0.002); osDelay(2); } }测试与调参静态测试检查零偏稳定性动态测试快速旋转检查响应速度调整滤波参数直到达到最佳效果这套方案在我参与的四轴飞行器项目中表现良好飞行姿态稳定即使做翻滚动作也能快速恢复水平。