找回密碼
 立即注冊(cè)

QQ登錄

只需一步，快速開始

搜索
查看: 3799|回復(fù): 1
打印 上一主題 下一主題
收起左側(cè)

STM32運(yùn)用之自平衡小車的卡爾曼算法封裝

[復(fù)制鏈接]
跳轉(zhuǎn)到指定樓層
樓主
ID:113207 發(fā)表于 2016-4-10 00:45 | 只看該作者 回帖獎(jiǎng)勵(lì) |倒序?yàn)g覽 |閱讀模式
最近研究STM32的自平衡小車，發(fā)現(xiàn)有兩座必過的大山，一為卡爾曼濾波，二為PID算法。
網(wǎng)上看了很多關(guān)于卡爾曼濾波的代碼，感覺寫得真不咋地。一怒之下，自己重寫，不廢話，貼代碼
  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.       *本人對(duì)卡爾曼的粗略理解：以本次測(cè)量角速度（陀螺儀測(cè)量值）的積分得出的角度值
  13.       * 與上次最優(yōu)角度值的方差產(chǎn)生一個(gè)權(quán)重來衡量本次測(cè)量角度（加速度測(cè)量值）
  14.       * 與上次最優(yōu)角度值，從而產(chǎn)生新的最優(yōu)角度值。好吧，比較拗口，有誤處忘指正。
  15.       *
  16.       ******************************************************************************
  17.       */
  18.      
  19.     #ifndef __KALMAN_H__
  20.     #define __KALMAN_H__
  21.      
  22.      
  23.     #define Q_angle                        0.001        ////角度過程噪聲的協(xié)方差
  24.     #define Q_gyro                        0.003        ////角速度過程噪聲的協(xié)方差
  25.     #define R_angle                        0.5                ////測(cè)量噪聲的協(xié)方差（即是測(cè)量偏差）
  26.     #define dt                                0.01                        ////卡爾曼濾波采樣頻率
  27.     #define C_0                                1
  28.      
  29.     /**************卡爾曼運(yùn)算變量定義**********************
  30.     *
  31.     ***由于卡爾曼為遞推運(yùn)算，結(jié)構(gòu)體需定義為全局變量
  32.     ***在實(shí)際運(yùn)用中只需定義一個(gè)KalmanCountData類型的變量即可
  33.     ***無需用戶定義多個(gè)中間變量，簡化函數(shù)的使用
  34.     */
  35.     typedef struct
  36.     {
  37.             float                                Q_bias;                ////最優(yōu)估計(jì)值的偏差，即估計(jì)出來的陀螺儀的漂移量
  38.             float                                Angle_err;                ////實(shí)測(cè)角度與陀螺儀積分角度的差值
  39.             float                                PCt_0;                                
  40.             float                                PCt_1;
  41.             float                                E;                        ////計(jì)算的過程量
  42.             float                                K_0;                        ////含有卡爾曼增益的另外一個(gè)函數(shù)，用于計(jì)算最優(yōu)估計(jì)值
  43.             float                                K_1;                        ////含有卡爾曼增益的函數(shù)，用于計(jì)算最優(yōu)估計(jì)值的偏差
  44.             float                                t_0;                                
  45.             float                                t_1;
  46.             float                                Pdot[4];                ////Pdot[4] = {0,0,0,0};過程協(xié)方差矩陣的微分矩陣
  47.             float                                PP[2][2];                //// PP[2][2] = { { 1, 0 },{ 0, 1 } };協(xié)方差(covariance)
  48.             float                                Angle_Final;        ////后驗(yàn)估計(jì)最優(yōu)角度值（即系統(tǒng)處理最終值）
  49.             float                                Gyro_Final;        ////后驗(yàn)估計(jì)最優(yōu)角速度值
  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
復(fù)制代碼



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   卡爾曼濾波計(jì)算中間量初始化
  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   卡爾曼濾波計(jì)算
  54.       *        
  55.       *
  56.       ******************************************************************************
  57.       * @attention
  58.       *                Accel:加速度計(jì)數(shù)據(jù)處理后進(jìn)來的角度值
  59.       *                Gyro :陀螺儀數(shù)據(jù)處理后進(jìn)來的角速度值
  60.       *                Kalman_Struct:遞推運(yùn)算所需要的中間變量，由用戶定義為全局結(jié)構(gòu)體變量
  61.       *                Kalman_Struct -> Angle_Final  為濾波后角度最優(yōu)值
  62.       *                Kalman_Struct -> Gyro_Final   為后驗(yàn)角度值
  63.       ******************************************************************************
  64.       */
  65.      
  66.     void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
  67.     {
  68.                     //陀螺儀積分角度（先驗(yàn)估計(jì)）
  69.                     Kalman_Struct -> Angle_Final += (Gyro - Kalman_Struct -> Q_bias) * dt;
  70.      
  71.                     //先驗(yàn)估計(jì)誤差協(xié)方差的微分
  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.                     //先驗(yàn)估計(jì)誤差協(xié)方差的積分
  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.                     //計(jì)算角度偏差
  84.                     Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;        
  85.      
  86.                     //卡爾曼增益計(jì)算
  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.                     //后驗(yàn)估計(jì)誤差協(xié)方差計(jì)算
  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;         //后驗(yàn)估計(jì)最優(yōu)角度值
  105.                     Kalman_Struct -> Q_bias        += Kalman_Struct -> K_1 * Kalman_Struct -> Angle_err;                 //更新最優(yōu)估計(jì)值的偏差
  106.                     Kalman_Struct -> Gyro_Final   = Gyro - Kalman_Struct -> Q_bias;                                                 //更新最優(yōu)角速度值
  107.      
  108.     }
復(fù)制代碼



代碼可以放在實(shí)際工程中使用，也可以用VS等C編譯工具進(jìn)行實(shí)驗(yàn)學(xué)習(xí)。在VS中的main()實(shí)例使用如下
  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.             //定義一個(gè)卡爾曼運(yùn)算結(jié)構(gòu)體
  13.             Kalman_Filter_Init(&k);
  14.             //講運(yùn)算變量初始化
  15.             int m,n;        
  16.      
  17.                for(int a = 0;a<80;a++)
  18.             //測(cè)試80次
  19.             {
  20.      
  21.                     //m,n為1到100的隨機(jī)數(shù)
  22.                     m = 1+ rand() %100;
  23.      
  24.                     n = 1+ rand() %100;
  25.      
  26.                     //卡爾曼濾波，傳遞2個(gè)測(cè)量值以及運(yùn)算結(jié)構(gòu)體
  27.      
  28.             Kalman_Filter((float)m,(float)n,&k);
  29.      
  30.                     //打印結(jié)果
  31.                     printf("%d and %d is %f - %f",m,n,k.Angle_Final,k.K_0);
  32.      
  33.             }
  34.      
  35.      
  36.      
  37.      
  38.     }
復(fù)制代碼



繼卡爾曼之后，繼續(xù)對(duì)PID進(jìn)行封裝~~~~~~~~
為平衡小車做準(zhǔn)備

分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏 分享淘帖 頂頂 踩踩
回復(fù)

使用道具 舉報(bào)

沙發(fā)
ID:113207 發(fā)表于 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.       *                        定義結(jié)構(gòu)體類型以及聲明函數(shù)
  9.       *                        #define IF_THE_INTEGRAL_SEPARATION  0/1  為積分分離標(biāo)志
  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; // 設(shè)定目標(biāo) Desired Value   
  22.             double Proportion; // 比例常數(shù) Proportional Const
  23.             double Integral; // 積分常數(shù) Integral Const
  24.             double Derivative; // 微分常數(shù) 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            //是否積分分離預(yù)編譯開始
  31.      
  32.     double PIDCalc(double NextPoint ,double SepLimit, PID *pp);   //帶積分分離的PID運(yùn)算
  33.      
  34.     #else
  35.      
  36.     double PIDCalc( double NextPoint, PID *pp);     //不帶積分分離的PID運(yùn)算
  37.      
  38.     #endif        //是否積分分離預(yù)編譯結(jié)束
  39.      
  40.     void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp);
  41.      
  42.     #endif
復(fù)制代碼

  1.     /**
  2.       ******************************************************************************
  3.       * @file    PID_Control.c
  4.       * @author  willieon
  5.       * @version V0.1
  6.       * @date    January-2015
  7.       * @brief   PID控制算法函數(shù)代碼
  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控制運(yùn)算
  19.     *        入口參數(shù)： PID *pp  - 定義的運(yùn)算所需變量的結(jié)構(gòu)體
  20.     *                           NextPoint - 負(fù)反饋輸入值
  21.     *                           SepLimit  - 積分分離上限
  22.     *        出口參數(shù)： 返回PID控制量
  23.     *        說    明： 默認(rèn)不進(jìn)行積分分離，如果用戶需要使用積分分離，需在PID_Control.h中
  24.     *                                將 #define IF_THE_INTEGRAL_SEPARATION  0  改為
  25.     *                            #define IF_THE_INTEGRAL_SEPARATION  1
  26.     *        調(diào)用方法： 進(jìn)行積分分離時(shí)入口參數(shù)為3個(gè)，具體方法如下：
  27.     *                                PID PIDControlStruct ;   //定義PID運(yùn)算結(jié)構(gòu)體
  28.     *                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//結(jié)構(gòu)體初始化，注意&符號(hào)不能省
  29.     *                                ControlData = PIDCalc(ReadData, 200, &PIDControlStruct);   //控制量 = PIDCalc(反饋值,積分分離上限,PID運(yùn)算結(jié)構(gòu)體)
  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)        //當(dāng)偏差大于分離上限積分分離
  41.             {
  42.                     Flag = 0;
  43.             }
  44.             else       //當(dāng)偏差小于分離上限，積分項(xiàng)不分離
  45.             {
  46.                     Flag = 1;
  47.                     pp->SumError += Error;         // 積分  
  48.             }
  49.             dError = pp->LastError - pp->PrevError;         // 當(dāng)前微分
  50.             pp->PrevError = pp->LastError;
  51.             pp->LastError = Error;  
  52.             return (
  53.                     pp->Proportion                *                Error                 // 比例項(xiàng)
  54.                     + Flag * pp->Integral        *                pp->SumError         // 積分項(xiàng)
  55.                     + pp->Derivative                *                dError                 // 微分項(xiàng)
  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;                // 當(dāng)前微分
  67.             pp->PrevError = pp->LastError;
  68.             pp->LastError = Error;  
  69.             return (pp->Proportion        *        Error                // 比例項(xiàng)
  70.                     + pp->Integral                *        pp->SumError         // 積分項(xiàng)
  71.                     + pp->Derivative        *        dError         // 微分項(xiàng)
  72.                     );
  73.     }
  74.      
  75.     #endif
  76.      
  77.      
  78.     /*************************************************************************************
  79.     *        名    稱： double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
  80.     *        功    能： PID初始化設(shè)定
  81.     *        入口參數(shù)： PID *pp  - 定義的運(yùn)算所需變量的結(jié)構(gòu)體
  82.     *                           SetPoint - 設(shè)定的目標(biāo)值
  83.     *                           Proportion，Integral ，Derivative - P,I,D系數(shù)
  84.     *        出口參數(shù)： 無
  85.     *        說    明：        
  86.     *        調(diào)用方法：  PID PIDControlStruct ;   //定義PID運(yùn)算結(jié)構(gòu)體
  87.     *                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//結(jié)構(gòu)體初始化，注意&符號(hào)不能省
  88.     *                                因?yàn)楹瘮?shù)需要傳入一個(gè)指針，需要對(duì)結(jié)構(gòu)體取首地址傳給指針
  89.     *
  90.     ***************************************************************************************
  91.     */
  92.      
  93.      
  94.     void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp)
  95.     {  
  96.             pp -> SetPoint = SetPoint; // 設(shè)定目標(biāo) Desired Value   
  97.             pp -> Proportion = Proportion; // 比例常數(shù) Proportional Const
  98.             pp -> Integral = Integral; // 積分常數(shù) Integral Const
  99.             pp -> Derivative = Derivative; // 微分常數(shù) 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.     }
復(fù)制代碼



好了，現(xiàn)在把卡爾曼濾波和PID算法聯(lián)合起來，在VS平臺(tái)中運(yùn)行實(shí)驗(yàn)

  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.     }
復(fù)制代碼



寫到此處，我覺得，自平衡小車的兩座大山應(yīng)該對(duì)讀者來說不是問題了，只要會(huì)調(diào)用函數(shù)，會(huì)做參數(shù)整定，就完全可以解決數(shù)據(jù)采集和數(shù)據(jù)融合以及控制了。

好吧！我承認(rèn)我的代碼是仿照 STM32的庫的風(fēng)格寫的，真心覺得這種代碼風(fēng)格很NB。學(xué)習(xí)ing!

代碼本人測(cè)試過，如果有不足之處望不吝賜教！請(qǐng)直接跟帖討論

我最近在做的平衡車，硬件清單：
香蕉電機(jī)+輪子   6.4元*2
STM32F103C8Tb 最小系統(tǒng)板   31.5元*1
newwayL298N   光耦隔離電機(jī)驅(qū)動(dòng)板   30元 *1
MPU6050姿態(tài)傳感器     9.3元*1
LM2596S DC-DC 降壓電源模塊  3.2元*1
藍(lán)牙4.0 BLE從模塊串口通信+直驅(qū)模式 CC2540 CC2541 RF-BM-S02   23元*1
光電測(cè)速傳感器模塊   4.8元*2


小車底盤用自己的小雕刻機(jī)雕的亞克力板，感覺可以用薄的鋁板做中間層，這樣底層放光電測(cè)速和姿態(tài)傳感器+薄鋁底板+電動(dòng)機(jī)驅(qū)動(dòng)板+鋁底板+STM32主控，可以屏蔽一部分干擾，在inventor里面建模畫好圖！

之前沒有用帶光耦隔離的驅(qū)動(dòng)，速度一快STM32就死機(jī)，后來換光耦隔離才解決！


回復(fù)

使用道具 舉報(bào)

本版積分規(guī)則

手機(jī)版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術(shù)交流QQ群281945664

Powered by 單片機(jī)教程網(wǎng)

快速回復(fù) 返回頂部 返回列表