四轴飞行器&角度融合波形

哈达恋 · 发表于 2014-11-24 22:22:15

本帖最后由哈达恋于 2014-11-24 22:23 编辑

首先，先贴下我的姿态解算程序：
AHRSupdate四元数姿态解算：

// Definitions
#define Kp 6.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.03f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.0014 // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
//====================================================================================================
// Function
//====================================================================================================
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax/ norm;
ay = ay/ norm;
az = az/ norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx/ norm;
my = my/ norm;
mz = mz/ norm;
// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}

复制代码

卡尔曼滤波：

//============================================================================
// 卡尔曼滤波参数
float Q_angle=0.001,
Q_gyro=0.005,
R_angle=0.5,
dt=0.0055,//注意：dt的取值为kalman滤波器采样时间;
angle,
angle_dot;
float P[2][2] = { { 1, 0 },
{ 0, 1 }
}; //alpha_k
float Pdot[4] ={0,0,0,0}; //Pdot = (u_k-bias)
const char C_0 = 1; //H = [1 0]矩阵，用于提取
float q_bias , //陀螺仪常值偏差bias
angle_err, //观测值减去估计值
PCt_0, PCt_1, //计算K的中间步骤，分子 PCT = PH_T
E, //计算K的中间步骤，分母 E =(HPH_T + R)
K_0, K_1, //K为卡尔曼增益,K = P(K),矩阵
t_0, t_1; //更新alpha的中间步骤
//============================================================================
//函数名称：Kalman_Filter
//功能概要：卡尔曼滤波
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
// 卡尔曼滤波开始
angle+=(gyro_m-q_bias) * dt;//先验估计
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro; //Pdot = (u_k-bias)
P[0][0] += Pdot[0] * dt; // Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt; //alpha_k = alpha_k-1 + [(u_k-bias)*dt]
angle_err = angle_m - angle; //zk-先验估计观测值减去估计值
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0]; //计算K的中间步骤，分子 PCT = PH^T
E = R_angle + C_0 * PCt_0; //计算K的中间步骤，分母 E =(HPH^T + R)
K_0 = PCt_0 / E; //K = PH^T / (HPH^T + R)
K_1 = PCt_1 / E;
t_0 = PCt_0; //更新alpha的中间步骤
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0; //后验估计误差协方差
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0; //alpha_k+1 = alpha_k - KH*alpha_k
P[1][1] -= K_1 * t_1;
angle+= K_0 * angle_err;//后验估计更新估计值作为最终结果
q_bias+= K_1 * angle_err;//后验估计更新bias的值
angle_dot=gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度
// 卡尔曼滤波结束
}

复制代码

如上，我使用了以上的两个函数进行实验进行姿态解算，卡尔曼滤波成功使用，已经在去年的华南赛区直立摄像头组跑完全程，直立上是妥妥的。由于快毕业了，比赛结束后没有休息，继续做下了四轴，两个月后也成功飞行~
  可是飞行的时候发现遥控起来特别吃力，很容易在空中荡秋千，超难维持稳定，一开始以为是PID调试问题，后面发现不是。这几天使用了MP6050的DMP功能，昨晚再次移植成功，四轴非常稳定飞行，达到了几乎快悬停的效果，尽管在狭窄的走廊也能操控。这时候我意识到了问题所在，是角度融合有问题！！
  以下是我用以上四元数的函数解算出来的角度波形

如上波形，第一图左边的波形相对还是比较完美的，原地直立是没有问题的，如果使用在直立车上是可以妥妥直立的。但是四轴是在空中飞行的，只要运动，就很容易叠加上运动的加速度，这时候加速度计和陀螺仪融合出来的角度就出错了。看第一图的右边，这是我在角度维持不变，进行平移加速突然停止的动作而呈现的波形，很明显看到运动的加速度成分的存在。
  我在做实验的时候虽然手拿会有波动，但不至于产生二三十度的波动，这能很明显的发现是由加速度产生的。那么问题就来了？！！！
  同样的姿态解算，这个四元数和卡尔曼已经是网上被移植很多的程序，不过大家的四轴好像都飞行得很好，难道就没有出现遥控难度难的荡秋千现象么？
  不知道在这论坛里是否有人尝试去解决这个问题或者发现这个问题的呢？你们是如何去解决的？