提示：文章完成后，目录可以自动生成，如何生成可以参考右边的帮助文档
文章目录
- 测量角度和角速
-
-
- 1.角度和角速度获取
- 2 互补滤波
- 3卡尔曼滤波
- 4 卡尔一级低通滤波
- 5 实现卡尔曼滤波算法
-
测量角度和角速
1.角度和角速度获取
??通过角度和角速反馈进行直立控制，因此角度和角速的测量非常重要。本系统使用 MPU6050 集成加速度传感器和陀螺仪作为姿态传感器，可输出三轴的加速度和角速度。角速度的获取可以通过陀螺仪直接读取，角度的获取可以用两种方法来测量：一种是通过加速度计的加速度重量来计算，另一种是通过陀螺仪输出的角速度来获得积分。 ??MPU6050 坐标系定义如图1-1 。 
??当仅绕Y 轴旋转的原理是一样的。当绕组时Z 轴旋转时，由于重力加速度固定Z 轴的方向，所以在X 与Y 轴没有加速度重量，只有通过加速度计才能得到绕Z轴的角度。Z 轴角只能通过角速积分获得，但由于偏差，一段时间后将不再具有参考意义。 ??物体的旋转运动是三轴旋转角度的叠加。我们读取加速度计的数据，并根据公式获得相应的姿态角度。因此，汽车绕着X 轴与Y 以下公式可以计算轴的角度：
??在完全静止的情况下，可以通过加速度计获得所需的角度，但在实际应用中，车身摆动会产生加速度，叠加在测量信号加在测量信号上，无法准确反映1-3 所示。
??那么，通过陀螺仪的角速度得到积分的角度呢？如果测量的角速度有轻微的误差或漂移，则在积分操作后形成积累误差。随着时间的增加，角度信息将不再准确，这种方法也不是很可行。 ??我们可以综合加速度计和陀螺仪的角度，进行滤波和平滑处理，以获得准确的角度。该程序提供了三种角度准确的算法：1.DMP 算法2.互补滤波算法3.卡尔曼滤波算法。 ??DMP 算法是MPU6050 一种自带的滤波方法，只要有一些初始化，就可以用官方的库函数读取四元数，姿态角可以根据公式计算。
2 互补滤波
??加速度计和陀螺仪获得的角度有一定的缺点。从长远来看，加速度计获得的角度更准确，但波动很大，可以认为它与高频噪声混合；陀螺仪获得的角度在短时间内更准确，但存在积分误差，可以认为它与低频噪声混合。我们可以让他们通过低通过滤器和高通过滤器叠加在一起，这是一种互补的过滤算法。
对应我们的代码是： angle = K1 * angle_m (1-K1) * (angle gyro_m * dt); 可以看出，互补滤波是通过加速度计获得的角度校准陀螺仪积分的角度，从而逐渐跟踪加速度传感器获得的角度。K1 与1-K1 从这两个角度取不同的权重，可以表明我们对不同数据的信任。
3卡尔曼滤波
??传感器测量的数据总是有很多不确定性，比如很多噪音，大部分都符合高斯分布。对于我们的汽车来说，输入是角速，输出是角度，这是一个线性系统。如果系统是线性系统，这些不确定性符合高斯分布，我们可以使用卡尔曼滤波算法进行最佳估计。卡尔曼滤波器的思想是利用系统的状态方程来预测当前值，并使用传感器测量的观测值来修正预测值。与互补滤波器一样，可以选择不同的权重来实现，但这种权重是动态变化的。传感器的状态方程和测量方程如下：
4 卡尔一级低通滤波
/****************************** BEFIN ******************************** ** **@Name : Complementary_Filter_x **@Brief : 一阶互补滤波 **@Param angle_m: 计算加速度的角度 ** gyro_m: 陀螺仪的角速 **@Return : None **@Author : @mayuxin **@Data : 2022-06-04 ******************************** END *********************************/ float Complementary_Filter_x(float angle_m, float gyro_m) {
static float angle; float K1 =0.02; angle = K1 * angle_m (1-K1) * (angle gyro_m * dt); return angle; } 5 实现卡尔曼滤波算法
/***************************** BEFIN ******************************** ** **@Name : Kalman_Filter_x **@Brief : 获取x轴角度简易卡尔曼滤波 **@Param Accel: 加速度算出的角度 ** Gyro: 陀螺仪的角速度 **@Return : None **@Author : @mayuxin **@Data : 2022-06-04 ******************************** END *********************************/
float dt=0.005; //每5ms进行一次滤波
float Kalman_Filter_x(float Accel,float Gyro)
{
static float angle_dot;
static float angle;
float Q_angle=0.001; // 过程噪声的协方差
float Q_gyro=0.003; //0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.5; // 测量噪声的协方差 既测量偏差
char C_0 = 1;
static float Q_bias, Angle_err;
static float PCt_0, PCt_1, E;
static float K_0, K_1, t_0, t_1;
static float Pdot[4] ={
0,0,0,0};
static float PP[2][2] = {
{
1, 0 },{
0, 1 } };
angle+=(Gyro - Q_bias) * dt; //先验估计
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]=-PP[1][1];
Pdot[2]=-PP[1][1];
Pdot[3]=Q_gyro;
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
Angle_err = Accel - angle; //zk-先验估计
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][0];
E = R_angle + C_0 * PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = C_0 * PP[0][1];
PP[0][0] -= K_0 * t_0; //后验估计误差协方差
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
angle += K_0 * Angle_err; //后验估计
Q_bias += K_1 * Angle_err; //后验估计
angle_dot = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
return angle;
}