mpu6050 姿态解算

1.加速度和陀螺仪的偏移计算

MPU6050每次上电以目前的角度为0度基准,在做平衡车的时候测出的角度都是相对的角度。(通常只对x,y轴做偏移计算)

采集mpu6050静止时的数据。

2.采用旋转矩阵计算欧拉角

2.1  通过加速度计计算欧拉角(注意，由于绕Z旋转时，感受到的重力加速度是不变的，因此加速度计无法计算yaw角)

roll 和 Pitch 计算公式 （注意 pitch角计算公式是否加负号取决于传感器）

具体算式求导过程在知乎上有，下面给出链接

MPU6050姿态解算2-欧拉角&旋转矩阵 - 知乎

个人的理解：用加速度计求欧拉角 简单来看就是  起始点（0，0，g）到终点（ax，ay，az）经过绕ZYX轴来求出欧拉角roll ,pitch。其中起点和终点的加速度都是已知的

2.2  通过陀螺仪来计算欧拉角(只有在角度非常小的时候，角速度的积分才会等于欧拉角)

陀螺仪测量的绕3个轴转动的角速度，因此，对角速度积分，可以得到角度。

1>  设n+1时刻的姿态角为r+Δr、p+Δp、y+Δy，该姿态也是经历了3次旋转。要想计算n+1时刻的姿态，只要在n时刻姿态的基础上，加上对应的姿态角度变化量即可

依次绕ZYX轴旋转

首先绕Z轴转动产生的角速度可以用 Wz = dy/dt (dy表示某一时刻的yaw)来表示，那么此时就产生了一个点{Wx_z Wy_z Wz_z} = {0 ， 0 ，Wz};此时才第一次绕轴转动（绕Z轴），那么再绕Y 和  X轴转动时就可以理解为这个点绕Y轴和X轴转动

那么绕ZYX轴转动完后 绕Z轴的yaw角的角速度：

其中Mx和My 依次向左乘

同理

绕Y轴产生pitch角的角速度

绕Z轴长roll角的角速度

最终可得到

最后可以求出yaw角度：

yaw(n+1) = yaw(n) + ▲yaw = yaw(n) + (dy/dt)▲t

dy/dt = (sinr/cosp)*Gy + (cosr/cosp)*Gz

注意根据这个算式求出的偏航角完全不能够使用，数据会在很短的时间内越积越大（不知道是不是我这个算式错误，yaw的结果只会越来越大）

▲t可以理解为陀螺仪的采样频率  （个人理解）。

一阶互补滤波

float FOCF(float acc_m,float gyro_m,float* last_angle){
    float temp_angle;
    temp_angle=Ka*acc_m+(1-Ka)*(*last_angle+gyro_m*dt);
    *last_angle=temp_angle;
    return temp_angle;
}
Parm ： acc_m ： 根据加速度求出的roll/Pitch角度 gyro_m：根据陀螺仪读取对应轴的角速度（因为我这个陀螺仪pitch是按y轴旋转的）  last_angle：上一次的roll/Pitch 一阶滤波后的角度

roll_FOC = FOCF(roll_Acc, Gyro_y , &roll_FOC_Last);
pitch_FOC = FOCF(pitch_Acc , Gyro_x , &pitch_FOC_Last);

ka是加速度权重占比  

dt的取值为滤波器采样时间（https://www.cnblogs.com/LJWJL/p/7858262.html）

测试结果

 差异不大

更改权重占比 ：当处于静止状态 减少为0.1，此时pitch角经过一阶滤波后的数值偏小，但经过短暂的累积 会与未经过滤波的数值越来越接近 

二阶互补滤波

float SecondOrder(float newAngle, float newRate, float Dtime)
{
    
    x1=(newAngle-SecondOrder_angle)*(1-K2)*(1-K2);
    y1=y1+x1*Dtime;
    x2=y1+2*(1-K2)*(newAngle-SecondOrder_angle)+newRate;
    SecondOrder_angle=SecondOrder_angle+ x2*Dtime;
    
    return SecondOrder_angle;

}

求出的角度相差甚远

还有很多问待解决