智能车制作
标题: 自平衡车,直立控制,倾角控制车速 [打印本页]
作者: 疯狂土豆儿 时间: 2016-8-15 18:25
标题: 自平衡车,直立控制,倾角控制车速
我在做平衡小车,在卡尔曼滤波计算出倾角之后我再进行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;
}
[attach]87183[/attach][attach]87184[/attach][attach]87185[/attach][attach]87186[/attach]
作者: _益菌 时间: 2016-8-15 21:07
Angle是要用在卡尔曼滤波作为反馈的,在-3 ----3 这个范围里面反馈的值不对,最终融合出的角度值怎么会对呢
作者: _益菌 时间: 2016-8-15 21:09
要是真的想拿angle做文章,你可以从新定义一个变量angle1,让angle和angle1相等,然后卡尔曼用angle,速度调节用angle1
作者: 361369499 时间: 2016-8-15 21:27
楼上说得对,angle你在别的地方用到,这里不应该对它直接赋值。
最好用另外一个变量将其值保存一下。
作者: 疯狂土豆儿 时间: 2016-8-16 14:05
真理,谢谢啦
作者: 疯狂土豆儿 时间: 2016-8-16 14:07
现在我又遇到了另外一个问题,就是我平衡车烧完程序通电了,然后我手拿着它改变倾角它能跑,然后一放在地上,动一下就死了,你知道这是什么原因吗?
作者: 疯狂土豆儿 时间: 2016-8-16 14:07
现在我又遇到了另外一个问题,就是我平衡车烧完程序通电了,然后我手拿着它改变倾角它能跑,然后一放在地上,动一下就死了,你知道这是什么原因吗?
作者: _益菌 时间: 2016-8-16 15:09
死是什么意思?是单片机复位吗?复位的话看看是不是电池电压太低,或者压降太大
作者: 疯狂土豆儿 时间: 2016-8-16 17:18
是我程序写了一个保护,让它在倾角超过正负40度时判定为车已倒,停止转动,然后我觉得可能把车轮放在地上的时候有抖动,明明是0度,却瞬间卡尔曼滤波后的倾角超过了40度(不知道为什么),所以车就不动了。
作者: _益菌 时间: 2016-8-16 18:23
发上位机看看波形看看跟随是否正常,不正常的话一切都是空谈
欢迎光临 智能车制作 (http://111.231.132.190/) |
Powered by Discuz! X3.2 |