MPU6050实现Mahony互补滤波（程序使用江协科技模板）

超氧的歧化反应

已于 2024-04-28 13:51:38 修改

阅读量867

点赞数 10

文章标签：算法嵌入式硬件线性代数

于 2024-04-14 10:17:37 首次发布

本文链接：https://blog.csdn.net/m0_73885374/article/details/137734376

版权

欧拉角分为三轴偏转角度，分别是俯仰角Pitch、滚转角Roll、偏航角Yaw。

这三个角度能表示物体此时的姿态。

MPU6050能够测量出三轴角速度和加速度，但是单靠陀螺仪和角加速度计都不能获得精确且稳定的欧拉角，原因如下：

3轴加速度计(Accelerometer)本质上是三个互相垂直的测力计，如果只用加速度计测角度，小车向前加速和小车停在斜坡上受力相同。因此加速度计具有静态稳定性而不具有动态稳定性。

3轴陀螺仪传感器(Gyroscope)用于测量角速度，如果只用角速度积分测角度，当物体静止时，角速度值会因为微小噪声无法完全归零，这样通过积分，微小噪声不断累积，会导致计算出来的角度产生较大偏差。因此陀螺仪具有动态稳定性而不具有静态稳定性。

因此要进行数据融合，把陀螺仪和角加速度计的数据结合起来，综合考虑、取长补短。

常见的数据融合方法有硬件DMP解算四元数、Kalman滤波、Mahony互补滤波、Madgwick滤波、和神经网络等。

Mahony 互补滤波是一种常用于融合加速度计和陀螺仪数据的滤波算法，由美国加州大学伯克利分校机械工程系教授 Robert Mahony 在 2008 年提出。该方法将加速度和陀螺仪的数据进行互补融合，得到更加准确的姿态估计结果。Mahony 互补滤波算法的优点在于实现简单，运算速度快，且具有良好的实时性能，广泛应用于飞行器、机器人、无人驾驶车辆等领域中的导航和姿态控制等方面。

Mahony互补滤波的核心思想是将大地坐标系（即 s 系）中归一化的重力加速度转换到载体坐标系（即 b 系）后与归一化的 b 系加速度计输出做向量积，求得偏差后使用 PI 控制器来校正陀螺仪。

下面介绍 Mahoney 互补滤波实现姿态解算的详细过程。

1.获取加速度计的值(为物体坐标系下对应的值)，对其归一化(归一化的原因是因为姿态变化矩阵中的四元数是规范四元数，利用陀螺仪去更新的四元数也要归一化，所以加速度计获得的值也需归一化才能是两者对应)。记从加速度计获得的值为ax、ay、az(分别对应x、y、z轴的值)，其归一化方法如下:

$a_x=\frac{a_x}{\sqrt{a_{x}^{2}+a_{y}^{2}+a_{z}^{2}}}\\ a_y=\frac{a_y}{\sqrt{a_{x}^{2}+a_{y}^{2}+a_{z}^{2}}}\\ a_z=\frac{a_z}{\sqrt{a_{x}^{2}+a_{y}^{2}+a_{z}^{2}}}$

2.获取陀螺仪算出的姿态矩阵中的重力分量(因为加速度计是测得的物体坐标系下的值，所以我们也要提取利用角速度算出的姿态矩阵中的物体坐标系下的重力分量)。重力分量记为Vx、Vy、Vz，具体计算方法如下:通过四元数计算的从E系(地理坐标系)变换到b系(物体坐标系)姿态矩阵为:

$c_{E}^{b}=\left( \begin{matrix} 1-2q_{2}^{2}-2q_{3}^{2}& 2q_1q_2-2q_0q_3& 2q_0q_2+2q_1q_3\\ 2q_1q_2+2q_0q_3& 1-2q_{1}^{2}-2q_{3}^{2}& 2q_2q_3-2q_0q_1\\ 2q_1q_3-2q_0q_2& 2q_0q_1+2q_2q_3& 1-2q_{1}^{2}-2q_{2}^{2}\\ \end{matrix} \right)$

则提取b系（物体坐标系）下重力分量为

$\left( \begin{array}{c} V_x\\ V_y\\ V_z\\ \end{array} \right) =c_{E}^{b}\left( \begin{array}{c} 0\\ 0\\ 1\\ \end{array} \right) =\left( \begin{array}{c} 2q_1q_3-2q_0q_2\\ 2q_2q_3+2q_0q_1\\ 1-2q_{1}^{2}-2q_{2}^{2}\\ \end{array} \right)$

3.将加速度计获得的重力向量归一化后的值与提取的姿态矩阵的重力向量叉乘获取姿态误差，根据叉乘定义 $\overrightarrow{A}\times \overrightarrow{B}=\left( y_1z_2-y_2z_1,x_1z_2-x_2z_1,x_1y_2-x_2y_1 \right)$ ，得出姿态误差向量，其中ex、ey、ez为误差向量对应的x、y、z的三个元素：

$\left( \begin{array}{c} e_x\\ e_y\\ e_z\\ \end{array} \right) =\left( \begin{array}{c} a_x\\ a_y\\ a_z\\ \end{array} \right) \times \left( \begin{array}{c} V_x\\ V_y\\ V_z\\ \end{array} \right) =\left( \begin{array}{c} a_yV_z-a_zV_y\\ a_zV_x-a_xV_z\\ a_xV_y-a_yV_x\\ \end{array} \right)$

4.互补滤波

将误差输入Pid控制器与本次姿态更新中陀螺仪测得的角速度相加，得到一个修正的角速度值，获得的修正的角速度值去更新四元素，从而获得准确的姿态角信息。

使用PI控制器消除漂移误差，只要存在误差，控制器便会持续作用，直至误差为零。

控制的效果取决于P和I参数，分别对应比例控制和积分控制的参数。对误差进行比例积分运算，更新陀螺仪角速度值。

设 gx、gy、gz 为陀螺仪测得的三个轴的角速度及滤波后的角速度修正值，Kp为互补滤波系数，Ki为积分系数，对两个重力分量叉乘后的误差进行积分，结果得到三轴角速度值，则修正角速度计算方法如下

$g_x\left( k \right) =g_x\left( k-1 \right) +K_p\cdot e_x\left( k \right) +K_i\cdot \sum_0^k{e_x}\\ g_y\left( k \right) =g_y\left( k-1 \right) +K_p\cdot e_y\left( k \right) +K_i\cdot \sum_0^k{e_y} \\g_z\left( k \right) =g_z\left( k-1 \right) +K_p\cdot e_z\left( k \right) +K_i\cdot \sum_0^k{e_z}$

5.将修正后的角速度代入四元数微分方程，更新四元数:

根据

$\frac{\text{d}\overrightarrow{Q}}{\text{d}t}=\Omega _b\overrightarrow{Q}$

即

$\left( \begin{array}{c} \dot{q}_0\\ \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \end{array} \right) =\frac{1}{2}\left( \begin{matrix} 0& -g_x& -g_y& -g_z\\ g_x& 0& g_z& -g_y\\ g_y& -g_z& 0& g_x\\ g_z& g_y& -g_x& 0\\ \end{matrix} \right) \left( \begin{array}{c} \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3\\ \end{array}\\ \end{array} \right)$

根据一阶龙格库塔法，可以得到这个微分方程的数值解

$\left( \begin{array}{c} \dot{q}_0\\ \dot{q}_1\\ \dot{q}_2\\ \dot{q}_3\\ \end{array} \right) \cdot \Delta t=\left( \begin{array}{c} \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3\\ \end{array}\\ \end{array} \right) _k-\left( \begin{array}{c} \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3\\ \end{array}\\ \end{array} \right) _{k-1}$

$\left( \begin{array}{c} \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3\\ \end{array}\\ \end{array} \right) _k=\left( \begin{array}{c} \begin{array}{c} q_0\\ q_1\\ q_2\\ q_3\\ \end{array}\\ \end{array} \right) _{k-1}+\frac{\Delta t}{2}\left( \begin{array}{c} -g_xq_1-g_yq_2-g_zq_3\\ g_xq_0-g_yq_3+g_zq_2\\ g_xq_3+g_yq_0-g_zq_1\\ -g_xq_2+g_yq_1+g_zq_0\\ \end{array} \right)$

6.最后将四元数转化为角度制欧拉角：

$\left( \begin{array}{c} \text{Roll}\\ \text{Pitch}\\ \text{Yaw}\\ \end{array} \right) =\left( \begin{array}{c} \arctan \frac{2\left( q_0q_1+q_2q_3 \right)}{1-2\left( q_{1}^{2}+q_{2}^{2} \right)}\\ \arcsin \left[ 2\left( q_0q_2-q_3q_1 \right) \right]\\ \arctan \frac{2\left( q_0q_3+q_1q_2 \right)}{1-2\left( q_{2}^{2}+q_{3}^{2} \right)}\\ \end{array} \right)$

接下来就可以使用C语言对以上6个步骤进行算法上的实现。

IMU.h文件：

#ifndef __IMU_H
#define __IMU_H

#include "math.h"

typedef struct{
		float AX;
		float AY;
		float AZ;
		float GX;
		float GY;
		float GZ;
}param_imu;

typedef struct{
		float Pitch;
		float Roll;
		float Yaw;
}param_Angle;

extern param_imu imu_data;
extern param_Angle imu_Angle;
void IMU_getEuleranAngles(void);

#endif

IMU.c文件：

#include "stm32f10x.h"                  // Device header
#include "IMU.h"
#include "MPU6050.h"

#define Mohony_Kp      20.00f
#define Mohony_Ki      0.001f
#define cycle_T        0.005f      //200Hz
#define half_T         0.0025f
#define pi             3.1415926f

int16_t Ax, Ay, Az, Gx, Gy, Gz;
param_imu imu_data;
param_Angle imu_Angle;

float q[4] = {1.0,0.0,0.0,0.0};
float exInt = 0.0,eyInt = 0.0,ezInt = 0.0;

float fast_sqrt(float x)
{
	float halfx = 0.5f * x;
	float y = x;
	long i = *(long *) &y;
	i = 0x5f3759df - (i>>1);
	y = *(float *) &i;
	y = y * (1.5f-(halfx * y * y));
	return y;
}

void IMU_GetValues(void)
{
	MPU6050_GetData(&Ax,&Ay,&Az,&Gx,&Gy,&Gz);
	//Full Scale +-16 g
	imu_data.AX = ((float)Ax)*32/65536;
	imu_data.AY = ((float)Ay)*32/65536;
	imu_data.AZ = ((float)Az)*32/65536;
	//Full Scale +-2000 degree/s
	imu_data.GX = ((float)Gx)*4000/65536/180*pi;
	imu_data.GY = ((float)Gy)*4000/65536/180*pi;
	imu_data.GZ = ((float)Gz)*4000/65536/180*pi;
}

void IMU_AHRSupdate(param_imu* imu_temp)
{
	float ax,ay,az;
	float gx,gy,gz;
	float vx,vy,vz;
	float ex,ey,ez;
	ax = imu_temp->AX;
	ay = imu_temp->AY;
	az = imu_temp->AZ;
	gx = imu_temp->GX;
	gy = imu_temp->GY;
	gz = imu_temp->GZ;
	float q0 = q[0];
	float q1 = q[1];
	float q2 = q[2];
	float q3 = q[3];
	float norm = fast_sqrt(ax*ax+ay*ay+az*az);
	ax = ax * norm;
	ay = ay * norm;
	az = az * norm;
	vx = 2 * (q1*q3 - q0*q2);
	vy = 2 * (q2*q3 + q0*q1);
	vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
	ex = ay*vz - az*vy;
	ey = az*vx - ax*vz;
	ez = ax*vy - ay*vx;
	exInt += Mohony_Ki * ex;
	eyInt += Mohony_Ki * ey;
	ezInt += Mohony_Ki * ez;
	gx += Mohony_Kp * ex + exInt;
	gy += Mohony_Kp * ey + eyInt;
	gz += Mohony_Kp * ez + ezInt;
	q0 += (-q1*gx-q2*gy-q3*gz) * half_T;
	q1 += ( q0*gx+q2*gz-q3*gy) * half_T;
	q2 += ( q0*gy-q1*gz+q3*gx) * half_T;
	q3 += ( q0*gz+q1*gy-q2*gx) * half_T;
	norm = fast_sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
	q[0] = q0 * norm;
	q[1] = q1 * norm;
	q[2] = q2 * norm;
	q[3] = q3 * norm;
}

void IMU_getEuleranAngles(void)
{
	IMU_GetValues();
	IMU_AHRSupdate(&imu_data);
	imu_Angle.Pitch = asin(-2*q[1]*q[3]+2*q[0]*q[2])*180/pi;
	imu_Angle.Roll  = atan2(2*q[2]*q[3]+2*q[0]*q[1],-2*q[1]*q[1]-2*q[2]*q[2]+1)*180/pi;
	//If "imu_data.GZ" is too small, it is considered to be noise interference.(1 degree/s)
	if((imu_data.GZ*180/pi > 1.0f) || (imu_data.GZ*180/pi < -1.0f))
	{
		imu_Angle.Yaw  += imu_data.GZ*cycle_T*4*180/pi;
	}
}

最后开启定时器中断，每隔5ms进行一次解算，即可得到准确的俯仰角和横滚角。

但是偏航角的零点漂移现象仍然存在，只能在较短时间内维持准确。要想获得稳定且准确的偏航角，需要使用搭载磁场传感器的高级姿态传感器。

最后附上完整的stm32程序，使用自己编写的OLED显示浮点数函数对三轴角度进行实时显示。

gitee链接

该程序可以移植到ICM20602或IMU660RA等更高精度的姿态传感器上，以获得更好地解算效果。

参考资料：

知乎@Ned还在写报告

bilibili@L1nHao