智能车制作

标题: 自平衡车，直立控制，倾角控制车速 [打印本页]

作者: 疯狂土豆儿 时间: 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-15 21:09
要是真的想拿angle做文章，你可以从新定义一个变量angle1，让angle和angle1相等，然后卡尔曼用angle，速度 ...

真理，谢谢啦

作者: 疯狂土豆儿 时间: 2016-8-16 14:07

_益菌发表于 2016-8-15 21:09
要是真的想拿angle做文章，你可以从新定义一个变量angle1，让angle和angle1相等，然后卡尔曼用angle，速度 ...

现在我又遇到了另外一个问题，就是我平衡车烧完程序通电了，然后我手拿着它改变倾角它能跑，然后一放在地上，动一下就死了，你知道这是什么原因吗？

作者: 疯狂土豆儿 时间: 2016-8-16 14:07

361369499 发表于 2016-8-15 21:27
楼上说得对，angle你在别的地方用到，这里不应该对它直接赋值。
最好用另外一个变量将其值保存一下。

作者: _益菌 时间: 2016-8-16 15:09
死是什么意思？是单片机复位吗？复位的话看看是不是电池电压太低，或者压降太大

作者: 疯狂土豆儿 时间: 2016-8-16 17:18

_益菌发表于 2016-8-16 15:09
死是什么意思？是单片机复位吗？复位的话看看是不是电池电压太低，或者压降太大

是我程序写了一个保护，让它在倾角超过正负40度时判定为车已倒，停止转动，然后我觉得可能把车轮放在地上的时候有抖动，明明是0度，却瞬间卡尔曼滤波后的倾角超过了40度（不知道为什么），所以车就不动了。

作者: _益菌 时间: 2016-8-16 18:23
发上位机看看波形看看跟随是否正常，不正常的话一切都是空谈

欢迎光临智能车制作 (http://dns.znczz.com/)