久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 3783|回復: 1
打印 上一主題 下一主題
收起左側

STM32運用之自平衡小車的卡爾曼算法封裝

[復制鏈接]
跳轉到指定樓層
樓主
ID:113207 發表于 2016-4-10 00:45 | 只看該作者 回帖獎勵 |倒序瀏覽 |閱讀模式
最近研究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進行封裝~~~~~~~~
為平衡小車做準備

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

使用道具 舉報

沙發
ID:113207 發表于 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就死機，后來換光耦隔離才解決！


回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

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

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 日韩一区二区在线视频 | 国产一区二区三区久久久久久久久 | 日本人和亚洲人zjzjhd | 成人欧美一区二区三区在线播放 | 999久久久精品 | 国产高清性xxxxxxxx | 国产成人高清成人av片在线看 | 日本黄色不卡视频 | 欧美一区二区三区四区五区无卡码 | 一区二区国产在线 | 久久99网 | 国产人免费人成免费视频 | 中文字幕精品视频 | 亚洲夜夜爽| 91香蕉嫩草 | 亚洲在线一区二区 | 精品美女 | 亚洲综合国产 | 91精品国产综合久久久久蜜臀 | 在线成人 | 天堂一区二区三区 | 天天爱爱网 | 精品久久久久香蕉网 | 欧美精品在线播放 | 日韩高清中文字幕 | 亚洲第一成年免费网站 | 美女福利网站 | 一区二区三区在线 | 欧 | 欧美 中文字幕 | 免费看的黄网站 | 国产高清一区二区 | 91欧美| 国产精品久久久久久婷婷天堂 | 国产9 9在线 | 中文 | 伊人免费观看视频 | 91精品国产自产在线老师啪 | 久久精品视频在线免费观看 | 午夜在线免费观看 | 亚洲国产成人精品女人久久久 | 国产高清免费 | 日本在线免费看最新的电影 |