高级会员
- 积分
- 912
- 威望
- 453
- 贡献
- 287
- 兑换币
- 286
- 注册时间
- 2012-12-9
- 在线时间
- 86 小时
- 毕业学校
- XMUT
|
本帖最后由 哈达恋 于 2014-11-24 22:23 编辑
首先,先贴下我的姿态解算程序:
AHRSupdate四元数姿态解算:
复制代码 卡尔曼滤波:
- //============================================================================
- // 卡尔曼滤波参数
- 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功能,昨晚再次移植成功,四轴非常稳定飞行,达到了几乎快悬停的效果,尽管在狭窄的走廊也能操控。这时候我意识到了问题所在,是角度融合有问题!!
以下是我用以上四元数的函数解算出来的角度波形
如上波形,第一图左边的波形相对还是比较完美的,原地直立是没有问题的,如果使用在直立车上是可以妥妥直立的。但是四轴是在空中飞行的,只要运动,就很容易叠加上运动的加速度,这时候加速度计和陀螺仪融合出来的角度就出错了。看第一图的右边,这是我在角度维持不变,进行平移加速突然停止的动作而呈现的波形,很明显看到运动的加速度成分的存在。
我在做实验的时候虽然手拿会有波动,但不至于产生二三十度的波动,这能很明显的发现是由加速度产生的。那么问题就来了?!!!
同样的姿态解算,这个四元数和卡尔曼已经是网上被移植很多的程序,不过大家的四轴好像都飞行得很好,难道就没有出现遥控难度难的荡秋千现象么?
不知道在这论坛里是否有人尝试去解决这个问题或者发现这个问题的呢?你们是如何去解决的?
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看,没有帐号?注册
x
|