初级会员
- 积分
- 81
- 金钱
- 81
- 注册时间
- 2016-5-19
- 在线时间
- 14 小时
|
发表于 2016-8-15 18:40:51
|
显示全部楼层
我在做平衡小车，在卡尔曼滤波计算出倾角之后我再进行PID调节的时候，因为在PID调节函数里面加上了if(Angle>-3.0&&Angle<3.0) {Angle=0.0;}，然后发现倾角在我缓慢变化至3度后，刚过3度时倾角会突然猛的增加，然后减小最后稳定在正常值上，如果我不加这句话，倾角变化一直正常，百思不得其解，希望有大神能帮帮我。。。。
另外，卡尔曼滤波和速度PID控制我都是写在中断里的，周期5ms，PID参数我暂时只加了一个P调节，其他均为零。
//******卡尔曼参数************
const float Q_angle=0.001;
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.07; //dt为kalman滤波器采样时间;或0.005,0.07
const char C_0 = 1;
float Q_bias=0.0, Angle_err=0.0;
float PCt_0=0, PCt_1=0, E=0;
float K_0=0, K_1=0, t_0=0, t_1=0;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
float position;
static const float Kp = 40.0; //PID参数 Angle
static const float Kd = 0.0; //PID参数 Gyro_y
static const float Ksp = 0.0; //PID参数 motor_speed
static const float Ksi = 0.0; //PID参数 position
/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)
{
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; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度
}
void Angle_Calculate(void)
{
/****************************加速度****************************************/
Accel_x = MPU6050_Real_Data.Accel_X; //读取X轴加速度
Angle_ax = Accel_x*1.2*180/3.14; //弧度转换为度
/****************************角速度****************************************/
Gyro_y = MPU6050_Real_Data.Gyro_Y;
Angle_gy += (Gyro_y)*0.003;
/***************************卡尔曼融合*************************************/
Kalman_Filter(Angle_ax,Angle_gy); //卡尔曼滤波计算倾角
}
void Speed_Pid_Calculate(void)
{
if(Angle>-3.0&&Angle<3.0) {Angle=0.0;}
Speed_control = (Kp*Angle + Kd*Gyro_y); //PID：角速度和角度调节
Speed_control += (Ksp*motor_speed + Ksi*position);
//PID：车速度调节
speed_control_l = (int)Speed_control ; //左轮速度
speed_control_r = (int)- Speed_control; //右轮速度
if(speed_control_l > MAX_SPEED) speed_control_l = MAX_SPEED;
if(speed_control_l < -MAX_SPEED) speed_control_l = -MAX_SPEED;
if(speed_control_r > MAX_SPEED) speed_control_r = MAX_SPEED;
if(speed_control_r < -MAX_SPEED) speed_control_r = -MAX_SPEED;
}
|
|
[复制链接]
/2 
|手机版|OpenEdv-开源电子网
( 粤ICP备12000418号-1 )
楼主
。还有什么新的屌炸天的算法吗？终于等到大神回复我这条“坐等大神”的评论了。
