智能车制作

 找回密码
 注册

扫一扫,访问微社区

查看: 12303|回复: 9

自平衡车,直立控制,倾角控制车速

[复制链接]

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
发表于 2016-8-15 18:25:39 | 显示全部楼层 |阅读模式
我在做平衡小车,在卡尔曼滤波计算出倾角之后我再进行PID调节的时候,因为在PID调节函数里面加上了if(Angle>-3.0&&Angle<3.0){Angle=0.0;}然后发现倾角在我缓慢变化至3度后,刚过3度时倾角会突然猛的增加,然后减小最后稳定在正常值上,如果我不加这句话,倾角变化一直正常,百思不得其解,希望有大神能帮帮我。。。。
另外,卡尔曼滤波和速度PID控制我都是写在中断里的,周期5msPID参数我暂时只加了一个P调节,其他均为零。
//******卡尔曼参数************
                  
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.07;                           //dtkalman滤波器采样时间;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;
}



本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

6

主题

92

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
2395
QQ
威望
1202
贡献
737
兑换币
749
注册时间
2014-8-28
在线时间
228 小时
毕业学校
日照实验高中
发表于 2016-8-15 21:09:22 | 显示全部楼层
要是真的想拿angle做文章,你可以从新定义一个变量angle1,让angle和angle1相等,然后卡尔曼用angle,速度调节用angle1
回复 支持 1 反对 0

使用道具 举报

6

主题

92

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
2395
QQ
威望
1202
贡献
737
兑换币
749
注册时间
2014-8-28
在线时间
228 小时
毕业学校
日照实验高中
发表于 2016-8-15 21:07:49 | 显示全部楼层
Angle是要用在卡尔曼滤波作为反馈的,在-3 ----3 这个范围里面反馈的值不对,最终融合出的角度值怎么会对呢
回复 支持 反对

使用道具 举报

15

主题

383

帖子

0

精华

杰出人士

三轮飙车狂魔

Rank: 12Rank: 12Rank: 12

积分
17913
威望
12299
贡献
3268
兑换币
2783
注册时间
2015-10-15
在线时间
1173 小时
毕业学校
北方邮政大学
发表于 2016-8-15 21:27:43 | 显示全部楼层
楼上说得对,angle你在别的地方用到,这里不应该对它直接赋值。
最好用另外一个变量将其值保存一下。
回复 支持 反对

使用道具 举报

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
 楼主| 发表于 2016-8-16 14:05:35 | 显示全部楼层
_益菌 发表于 2016-8-15 21:09
要是真的想拿angle做文章,你可以从新定义一个变量angle1,让angle和angle1相等,然后卡尔曼用angle,速度 ...

真理,谢谢啦

回复 支持 反对

使用道具 举报

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
 楼主| 发表于 2016-8-16 14:07:23 | 显示全部楼层
_益菌 发表于 2016-8-15 21:09
要是真的想拿angle做文章,你可以从新定义一个变量angle1,让angle和angle1相等,然后卡尔曼用angle,速度 ...

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

使用道具 举报

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
 楼主| 发表于 2016-8-16 14:07:42 | 显示全部楼层
361369499 发表于 2016-8-15 21:27
楼上说得对,angle你在别的地方用到,这里不应该对它直接赋值。
最好用另外一个变量将其值保存一下。

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

使用道具 举报

6

主题

92

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
2395
QQ
威望
1202
贡献
737
兑换币
749
注册时间
2014-8-28
在线时间
228 小时
毕业学校
日照实验高中
发表于 2016-8-16 15:09:27 | 显示全部楼层
死是什么意思?是单片机复位吗?复位的话看看是不是电池电压太低,或者压降太大
回复 支持 反对

使用道具 举报

2

主题

15

帖子

0

精华

注册会员

Rank: 2

积分
124
威望
72
贡献
34
兑换币
33
注册时间
2016-8-8
在线时间
9 小时
毕业学校
北方工业大学
 楼主| 发表于 2016-8-16 17:18:00 | 显示全部楼层
_益菌 发表于 2016-8-16 15:09
死是什么意思?是单片机复位吗?复位的话看看是不是电池电压太低,或者压降太大

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

使用道具 举报

6

主题

92

帖子

0

精华

金牌会员

Rank: 6Rank: 6

积分
2395
QQ
威望
1202
贡献
737
兑换币
749
注册时间
2014-8-28
在线时间
228 小时
毕业学校
日照实验高中
发表于 2016-8-16 18:23:56 | 显示全部楼层
发上位机看看波形看看跟随是否正常,不正常的话一切都是空谈
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则

关于我们|联系我们|小黑屋|智能车制作 ( 黑ICP备2022002344号

GMT+8, 2024-7-19 20:39 , Processed in 0.066329 second(s), 32 queries , Gzip On.

Powered by Discuz! X3.2

© 2001-2013 Comsenz Inc.

快速回复 返回顶部 返回列表