标题: STM32运用之自平衡小车的卡尔曼算法封装 [打印本页]
作者: 51hei学习    时间: 2016-4-10 00:45
标题: STM32运用之自平衡小车的卡尔曼算法封装
最近研究STM32的自平衡小车，发现有两座必过的大山，一为卡尔曼滤波，二为PID算法。
网上看了很多关于卡尔曼滤波的代码，感觉写得真不咋地。一怒之下，自己重写，不废话，贴代码
  1.     /**
  2.       ******************************************************************************
  3.       * @file    kalman.h
  4.       * @author  willieon
  5.       * @version V0.1
  6.       * @date    January-2015
  7.       * @brief   卡尔曼滤波算法
  8.       *        
  9.       *
  10.       ******************************************************************************
  11.       * @attention
  12.       *本人对卡尔曼的粗略理解：以本次测量角速度（陀螺仪测量值）的积分得出的角度值
  13.       * 与上次最优角度值的方差产生一个权重来衡量本次测量角度（加速度测量值）
  14.       * 与上次最优角度值，从而产生新的最优角度值。好吧，比较拗口，有误处忘指正。
  15.       *
  16.       ******************************************************************************
  17.       */
  18.      
  19.     #ifndef __KALMAN_H__
  20.     #define __KALMAN_H__
  21.      
  22.      
  23.     #define Q_angle                        0.001        ////角度过程噪声的协方差
  24.     #define Q_gyro                        0.003        ////角速度过程噪声的协方差
  25.     #define R_angle                        0.5                ////测量噪声的协方差（即是测量偏差）
  26.     #define dt                                0.01                        ////卡尔曼滤波采样频率
  27.     #define C_0                                1
  28.      
  29.     /**************卡尔曼运算变量定义**********************
  30.     *
  31.     ***由于卡尔曼为递推运算，结构体需定义为全局变量
  32.     ***在实际运用中只需定义一个KalmanCountData类型的变量即可
  33.     ***无需用户定义多个中间变量，简化函数的使用
  34.     */
  35.     typedef struct
  36.     {
  37.             float                                Q_bias;                ////最优估计值的偏差，即估计出来的陀螺仪的漂移量
  38.             float                                Angle_err;                ////实测角度与陀螺仪积分角度的差值
  39.             float                                PCt_0;                                
  40.             float                                PCt_1;
  41.             float                                E;                        ////计算的过程量
  42.             float                                K_0;                        ////含有卡尔曼增益的另外一个函数，用于计算最优估计值
  43.             float                                K_1;                        ////含有卡尔曼增益的函数，用于计算最优估计值的偏差
  44.             float                                t_0;                                
  45.             float                                t_1;
  46.             float                                Pdot[4];                ////Pdot[4] = {0,0,0,0};过程协方差矩阵的微分矩阵
  47.             float                                PP[2][2];                //// PP[2][2] = { { 1, 0 },{ 0, 1 } };协方差(covariance)
  48.             float                                Angle_Final;        ////后验估计最优角度值（即系统处理最终值）
  49.             float                                Gyro_Final;        ////后验估计最优角速度值
  50.      
  51.     }KalmanCountData;
  52.      
  53.     void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct);
  54.     void Kalman_Filter_Init(KalmanCountData * Kalman_Struct);
  55.      
  56.      
  57.      
  58.     #endif
复制代码



kalman.c
  1.     #include "kalman.h"
  2.      
  3.      
  4.     /**
  5.       ******************************************************************************
  6.       * @file    void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)
  7.       * @author  willieon
  8.       * @version V0.1
  9.       * @date    January-2015
  10.       * @brief   卡尔曼滤波计算中间量初始化
  11.       *        
  12.       *
  13.       ******************************************************************************
  14.       * @attention
  15.       *
  16.       *
  17.       *
  18.       *
  19.       ******************************************************************************
  20.       */
  21.      
  22.     void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)
  23.     {
  24.             Kalman_Struct -> Angle_err                 = 0;
  25.             Kalman_Struct -> Q_bias                         = 0;
  26.             Kalman_Struct -> PCt_0                         = 0;
  27.             Kalman_Struct -> PCt_1                         = 0;
  28.             Kalman_Struct -> E                                 = 0;
  29.             Kalman_Struct -> K_0                         = 0;
  30.             Kalman_Struct -> K_1                         = 0;
  31.             Kalman_Struct -> t_0                         = 0;
  32.             Kalman_Struct -> t_1                         = 0;
  33.             Kalman_Struct -> Pdot[0]                 = 0;
  34.             Kalman_Struct -> Pdot[1]                 = 0;
  35.             Kalman_Struct -> Pdot[2]                 = 0;
  36.             Kalman_Struct -> Pdot[3]                 = 0;        
  37.             Kalman_Struct -> PP[0][0]                 = 1;
  38.             Kalman_Struct -> PP[0][1]                 = 0;
  39.             Kalman_Struct -> PP[1][0]                 = 0;
  40.             Kalman_Struct -> PP[1][1]                 = 1;        
  41.             Kalman_Struct -> Angle_Final         = 0;
  42.             Kalman_Struct -> Gyro_Final                 = 0;
  43.      
  44.     }
  45.      
  46.      
  47.     /**
  48.       ******************************************************************************
  49.       * @file    void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
  50.       * @author  willieon
  51.       * @version V0.1
  52.       * @date    January-2015
  53.       * @brief   卡尔曼滤波计算
  54.       *        
  55.       *
  56.       ******************************************************************************
  57.       * @attention
  58.       *                Accel:加速度计数据处理后进来的角度值
  59.       *                Gyro :陀螺仪数据处理后进来的角速度值
  60.       *                Kalman_Struct:递推运算所需要的中间变量，由用户定义为全局结构体变量
  61.       *                Kalman_Struct -> Angle_Final  为滤波后角度最优值
  62.       *                Kalman_Struct -> Gyro_Final   为后验角度值
  63.       ******************************************************************************
  64.       */
  65.      
  66.     void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
  67.     {
  68.                     //陀螺仪积分角度（先验估计）
  69.                     Kalman_Struct -> Angle_Final += (Gyro - Kalman_Struct -> Q_bias) * dt;
  70.      
  71.                     //先验估计误差协方差的微分
  72.                     Kalman_Struct -> Pdot[0] = Q_angle - Kalman_Struct -> PP[0][1] - Kalman_Struct -> PP[1][0];
  73.                     Kalman_Struct -> Pdot[1] = - Kalman_Struct -> PP[1][1];
  74.                     Kalman_Struct -> Pdot[2] = - Kalman_Struct -> PP[1][1];
  75.                     Kalman_Struct -> Pdot[3] = Q_gyro;
  76.      
  77.                     //先验估计误差协方差的积分
  78.                     Kalman_Struct -> PP[0][0] += Kalman_Struct -> Pdot[0] * dt;   
  79.                     Kalman_Struct -> PP[0][1] += Kalman_Struct -> Pdot[1] * dt;   
  80.                     Kalman_Struct -> PP[1][0] += Kalman_Struct -> Pdot[2] * dt;
  81.                     Kalman_Struct -> PP[1][1] += Kalman_Struct -> Pdot[3] * dt;
  82.      
  83.                     //计算角度偏差
  84.                     Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;        
  85.      
  86.                     //卡尔曼增益计算
  87.                     Kalman_Struct -> PCt_0 = C_0 * Kalman_Struct -> PP[0][0];
  88.                     Kalman_Struct -> PCt_1 = C_0 * Kalman_Struct -> PP[1][0];
  89.      
  90.                     Kalman_Struct -> E = R_angle + C_0 * Kalman_Struct -> PCt_0;
  91.      
  92.                     Kalman_Struct -> K_0 = Kalman_Struct -> PCt_0 / Kalman_Struct -> E;
  93.                     Kalman_Struct -> K_1 = Kalman_Struct -> PCt_1 / Kalman_Struct -> E;
  94.      
  95.                     //后验估计误差协方差计算
  96.                     Kalman_Struct -> t_0 = Kalman_Struct -> PCt_0;
  97.                     Kalman_Struct -> t_1 = C_0 * Kalman_Struct -> PP[0][1];
  98.      
  99.                     Kalman_Struct -> PP[0][0] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_0;                 
  100.                     Kalman_Struct -> PP[0][1] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_1;
  101.                     Kalman_Struct -> PP[1][0] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_0;
  102.                     Kalman_Struct -> PP[1][1] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_1;
  103.      
  104.                     Kalman_Struct -> Angle_Final += Kalman_Struct -> K_0 * Kalman_Struct -> Angle_err;         //后验估计最优角度值
  105.                     Kalman_Struct -> Q_bias        += Kalman_Struct -> K_1 * Kalman_Struct -> Angle_err;                 //更新最优估计值的偏差
  106.                     Kalman_Struct -> Gyro_Final   = Gyro - Kalman_Struct -> Q_bias;                                                 //更新最优角速度值
  107.      
  108.     }
复制代码



代码可以放在实际工程中使用，也可以用VS等C编译工具进行实验学习。在VS中的main()实例使用如下
  1.     #include "kalman.h"
  2.      
  3.     #include "stdio.h"
  4.      
  5.     #include "stdlib.h"
  6.      
  7.     void main(void)
  8.     {
  9.      
  10.      
  11.             KalmanCountData k;
  12.             //定义一个卡尔曼运算结构体
  13.             Kalman_Filter_Init(&k);
  14.             //讲运算变量初始化
  15.             int m,n;        
  16.      
  17.                for(int a = 0;a<80;a++)
  18.             //测试80次
  19.             {
  20.      
  21.                     //m,n为1到100的随机数
  22.                     m = 1+ rand() %100;
  23.      
  24.                     n = 1+ rand() %100;
  25.      
  26.                     //卡尔曼滤波，传递2个测量值以及运算结构体
  27.      
  28.             Kalman_Filter((float)m,(float)n,&k);
  29.      
  30.                     //打印结果
  31.                     printf("%d and %d is %f - %f",m,n,k.Angle_Final,k.K_0);
  32.      
  33.             }
  34.      
  35.      
  36.      
  37.      
  38.     }
复制代码



继卡尔曼之后，继续对PID进行封装~~~~~~~~
为平衡小车做准备

作者: 51hei学习    时间: 2016-4-10 00:46
  1.     /**
  2.       ******************************************************************************
  3.       * @file    PID_Control.h
  4.       * @author  willieon
  5.       * @version V0.1
  6.       * @date    January-2015
  7.       * @brief   PID控制算法头文件
  8.       *                        定义结构体类型以及声明函数
  9.       *                        #define IF_THE_INTEGRAL_SEPARATION  0/1  为积分分离标志
  10.       ******************************************************************************
  11.       **/
  12.      
  13.     #ifndef __PID_CONTROL_H__
  14.     #define __PID_CONTROL_H__
  15.      
  16.     #define IF_THE_INTEGRAL_SEPARATION  0   
  17.     //#define IF_THE_INTEGRAL_SEPARATION  1   //是否积分分离  0-不分离，1 -分离
  18.      
  19.     typedef struct
  20.     {
  21.             double SetPoint; // 设定目标 Desired Value   
  22.             double Proportion; // 比例常数 Proportional Const
  23.             double Integral; // 积分常数 Integral Const
  24.             double Derivative; // 微分常数 Derivative Const   
  25.             double LastError; // Error[-1]
  26.             double PrevError; // Error[-2]
  27.             double SumError; // Sums of Errors  
  28.     }PID;
  29.      
  30.     #if IF_THE_INTEGRAL_SEPARATION            //是否积分分离预编译开始
  31.      
  32.     double PIDCalc(double NextPoint ,double SepLimit, PID *pp);   //带积分分离的PID运算
  33.      
  34.     #else
  35.      
  36.     double PIDCalc( double NextPoint, PID *pp);     //不带积分分离的PID运算
  37.      
  38.     #endif        //是否积分分离预编译结束
  39.      
  40.     void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp);
  41.      
  42.     #endif
复制代码

  1.     /**
  2.       ******************************************************************************
  3.       * @file    PID_Control.c
  4.       * @author  willieon
  5.       * @version V0.1
  6.       * @date    January-2015
  7.       * @brief   PID控制算法函数代码
  8.       *        
  9.       *
  10.       ******************************************************************************
  11.       **/
  12.      
  13.     #include "PID_Control.h"
  14.     #include "math.h"
  15.      
  16.     /*************************************************************************************
  17.     *        名    称： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
  18.     *        功    能： PID控制运算
  19.     *        入口参数： PID *pp  - 定义的运算所需变量的结构体
  20.     *                           NextPoint - 负反馈输入值
  21.     *                           SepLimit  - 积分分离上限
  22.     *        出口参数： 返回PID控制量
  23.     *        说    明： 默认不进行积分分离，如果用户需要使用积分分离，需在PID_Control.h中
  24.     *                                将 #define IF_THE_INTEGRAL_SEPARATION  0  改为
  25.     *                            #define IF_THE_INTEGRAL_SEPARATION  1
  26.     *        调用方法： 进行积分分离时入口参数为3个，具体方法如下：
  27.     *                                PID PIDControlStruct ;   //定义PID运算结构体
  28.     *                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化，注意&符号不能省
  29.     *                                ControlData = PIDCalc(ReadData, 200, &PIDControlStruct);   //控制量 = PIDCalc(反馈值,积分分离上限,PID运算结构体)
  30.     *
  31.     ***************************************************************************************
  32.     */
  33.      
  34.     #if IF_THE_INTEGRAL_SEPARATION
  35.      
  36.     double PIDCalc(double NextPoint ,double SepLimit, PID *pp)
  37.     {
  38.             double dError, Error,Flag;   
  39.             Error = pp->SetPoint - NextPoint;         // 偏差
  40.             if(abs(Error) > SepLimit)        //当偏差大于分离上限积分分离
  41.             {
  42.                     Flag = 0;
  43.             }
  44.             else       //当偏差小于分离上限，积分项不分离
  45.             {
  46.                     Flag = 1;
  47.                     pp->SumError += Error;         // 积分  
  48.             }
  49.             dError = pp->LastError - pp->PrevError;         // 当前微分
  50.             pp->PrevError = pp->LastError;
  51.             pp->LastError = Error;  
  52.             return (
  53.                     pp->Proportion                *                Error                 // 比例项
  54.                     + Flag * pp->Integral        *                pp->SumError         // 积分项
  55.                     + pp->Derivative                *                dError                 // 微分项
  56.                     );
  57.     }
  58.      
  59.     #else
  60.      
  61.     double PIDCalc( double NextPoint, PID *pp)
  62.     {  
  63.             double dError, Error;   
  64.             Error = pp->SetPoint - NextPoint;                         // 偏差
  65.             pp->SumError += Error;                                        // 积分  
  66.             dError = pp->LastError - pp->PrevError;                // 当前微分
  67.             pp->PrevError = pp->LastError;
  68.             pp->LastError = Error;  
  69.             return (pp->Proportion        *        Error                // 比例项
  70.                     + pp->Integral                *        pp->SumError         // 积分项
  71.                     + pp->Derivative        *        dError         // 微分项
  72.                     );
  73.     }
  74.      
  75.     #endif
  76.      
  77.      
  78.     /*************************************************************************************
  79.     *        名    称： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
  80.     *        功    能： PID初始化设定
  81.     *        入口参数： PID *pp  - 定义的运算所需变量的结构体
  82.     *                           SetPoint - 设定的目标值
  83.     *                           Proportion，Integral ，Derivative - P,I,D系数
  84.     *        出口参数： 无
  85.     *        说    明：        
  86.     *        调用方法：  PID PIDControlStruct ;   //定义PID运算结构体
  87.     *                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化，注意&符号不能省
  88.     *                                因为函数需要传入一个指针，需要对结构体取首地址传给指针
  89.     *
  90.     ***************************************************************************************
  91.     */
  92.      
  93.      
  94.     void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp)
  95.     {  
  96.             pp -> SetPoint = SetPoint; // 设定目标 Desired Value   
  97.             pp -> Proportion = Proportion; // 比例常数 Proportional Const
  98.             pp -> Integral = Integral; // 积分常数 Integral Const
  99.             pp -> Derivative = Derivative; // 微分常数 Derivative Const   
  100.             pp -> LastError = 0; // Error[-1]
  101.             pp -> PrevError = 0; // Error[-2]
  102.             pp -> SumError = 0; // Sums of Errors
  103.      
  104.             //memset ( pp,0,sizeof(struct PID));   //need include "string.h"
  105.     }
复制代码



好了，现在把卡尔曼滤波和PID算法联合起来，在VS平台中运行实验

  1.     #include "kalman.h"
  2.      
  3.     #include "stdio.h"
  4.     #include "stdlib.h"
  5.     #include "PID_Control.h"
  6.      
  7.     void main(void)
  8.      
  9.     {
  10.             KalmanCountData k;
  11.             PID PIDControlStruct;
  12.             Kalman_Filter_Init(&k);
  13.             PIDInit(50, 1, 0.04, 0.2, &PIDControlStruct);
  14.             int m,n;
  15.      
  16.             double out;
  17.      
  18.             for(int a = 0;a<80;a++)
  19.             {
  20.                     m = 1+ rand() %100;
  21.                     n = 1+ rand() %100;
  22.                     Kalman_Filter((float)m,(float)n,&k);
  23.                     out = PIDCalc(k.Angle_Final, &PIDControlStruct);
  24.                     printf("%3d and %3d is %6f -pid- %6f\r\n",m,n,k.Angle_Final,out);
  25.      
  26.             }
  27.     }
复制代码



写到此处，我觉得，自平衡小车的两座大山应该对读者来说不是问题了，只要会调用函数，会做参数整定，就完全可以解决数据采集和数据融合以及控制了。

好吧！我承认我的代码是仿照 STM32的库的风格写的，真心觉得这种代码风格很NB。学习ing!

代码本人测试过，如果有不足之处望不吝赐教！请直接跟帖讨论

我最近在做的平衡车，硬件清单：
香蕉电机+轮子   6.4元*2
STM32F103C8Tb 最小系统板   31.5元*1
newwayL298N   光耦隔离电机驱动板   30元 *1
MPU6050姿态传感器     9.3元*1
LM2596S DC-DC 降压电源模块  3.2元*1
蓝牙4.0 BLE从模块串口通信+直驱模式 CC2540 CC2541 RF-BM-S02   23元*1
光电测速传感器模块   4.8元*2


小车底盘用自己的小雕刻机雕的亚克力板，感觉可以用薄的铝板做中间层，这样底层放光电测速和姿态传感器+薄铝底板+电动机驱动板+铝底板+STM32主控，可以屏蔽一部分干扰，在inventor里面建模画好图！

之前没有用带光耦隔离的驱动，速度一快STM32就死机，后来换光耦隔离才解决！






欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1