在此贴上以前做过的平衡小车资料，一来供众网友学习交流，二来申请stm32f429探索套件。。。
先上图

原材料清单：
——————————————————————————————————————————
主控：stm32f103rct6
传感器：mpu6050（带陀螺仪和加速度）
电机驱动芯片：l298n
电机：冯哈伯2224
编码器：电机自带16线
电池：两节18650
轮子架子：淘宝店购买，减速比1：5
——————————————————————————————————————————
处理过程：
——————————————————————————————————————————
其实程序过程很简单，先是采集传感器MPU6050的数据，并进行加工，获得当前小车倾斜角，然后根据此角度做PID调节，得到小车两个电机的PWM脉宽，调整轮子速度，使之回到倾角为0的状态，即保持平衡。然后就不停地重复采集->处理->调节->处理这一过程。
在此基础上，附加两电机的PWM值，即可实现前进，倒退，左转，右转动作
——————————————————————————————————————————
难点分析：
——————————————————————————————————————————
有两个难点
1.传感器的数据处理是小车的最大难点。一是传感器受震动影响很大，很容易超量程；二是传感器数据漂移比较厉害。而关键的小车倾角就来源于此传感器数据。为此，参考网上数据滤波处理方式，采用卡尔曼滤波算法，融合陀螺仪和加速度传感器数据得出小车倾角。
2.PID，比例系数的调节不用多讲，需要耐心的测试和调节。我要说的是，单纯靠倾角做PID不能使小车平衡，因为当小车倾角接近0度时，改变的PWM值无法使小车越过平衡点，造成小车朝一边加速前进，所以要将速度也添加至PID调节中，做积分
——————————————————————————————————————————
制作心得：
——————————————————————————————————————————
关于平衡小车，大多数网友应该都曾做过或者想过要做。于是百度搜资料，其中最多的应该是飞思卡尔直立车的资料，当初我也看下下，平衡车的原理都懂了，但那时还没搞过PID调节和数据滤波，所以具体怎么写程序并不知道。至于平衡车的程序资料也非常少。后来在网上参考相关数据滤波，PID调节后，才渐渐掌握。在此我也就贴上我的程序代码，供大家学习交流，少走弯路。有什么问题，欢迎回帖。当然也希望大家多支持此贴，祝我申请F429套件成功！哈哈
——————————————————————————————————————————
卡尔曼滤波.rar (2.76 MB, 下载次数: 12100)

2014-4-12 20:52 上传

点击文件名下载附件

楼主有几个问题请教下，您用的编码器是16线的，是不是接线就三根呢，一根电源，一根接地，还有一根信号线，可是我从你的程序中看不出来信号线是接几号引脚啊？其二，楼主的PID看上去没有用增量式或者位置式的，PWM是直接根据KP*角度+KD*速度计算出来的，这样的话这个PWM是不是不准，不知楼主最后的效果咋样？求回复哈

  又强又好，顶！

我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度，角速度和倾角，我发现当我快速的改变板子的倾角的时候，比如快速变化10度，Angle（卡尔曼滤波后的倾角）瞬时变化非常快，可能会瞬间变化几十度然后回到正常角度，而当我缓慢变化10度的时候，Angle变化是正常线性变化到10度，在这两种变化中，Angle_ax（从MPU6050读取的值经过处理后的陀螺仪的Y轴数据）的变化一直都是线性正常的，并且Angle的值特别接近Angle_ax的值
问题：1，我快速改变板子倾角时Angle的变化正常吗？
      2，Angle正常变化的时候是应该与Angle_ax的值相近吗？

现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现中间的角度增大


*************读取数据********************
//定义MPU6050内部地址
#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
#define INT_STATUS      0x3A    //只读
#define        ACCEL_XOUT_H        0x3B
#define        ACCEL_XOUT_L        0x3C
#define        ACCEL_YOUT_H        0x3D
#define        ACCEL_YOUT_L        0x3E
#define        ACCEL_ZOUT_H        0x3F
#define        ACCEL_ZOUT_L        0x40
#define        TEMP_OUT_H                0x41
#define        TEMP_OUT_L                0x42
#define        GYRO_XOUT_H    0x43
#define        GYRO_XOUT_L                0x44       
#define        GYRO_YOUT_H        0x45
#define        GYRO_YOUT_L                0x46
#define        GYRO_ZOUT_H        0x47
#define        GYRO_ZOUT_L                0x48

//读取寄存器原生数据
           
        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
        //将原生数据转换为实际值，计算公式跟寄存器的配置有关
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
    }
   


//******卡尔曼参数************
               
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01;                          //dt为kalman滤波器采样时间;
const char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/*****************卡尔曼滤波**************************************************/
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;             
时间dt，所以此处不用积分
/***************************卡尔曼融合*************************************/
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
       

顶下，楼主好人。。。

收了  感谢楼主

回复第 3 楼 于2014-04-12 22:19:27发表:
收了  感谢楼主 

客气。。。

阔出去了  定起

还不错

正需要呢，顶一个！！！

感谢

 

顶下，楼主好人。。。

这个是高大上的项目。赞一个

做了好久，就是滤波不懂，楼主好人，愿好人一生平安

感谢分享！！！！！！！！！！！！！