极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 77233|回复: 43

STM32运用之自平衡小车的卡尔曼算法封装-申请加精

[复制链接]
发表于 2015-1-9 15:17:24 | 显示全部楼层 |阅读模式
本帖最后由 D调的华丽 于 2015-1-10 12:57 编辑

最近研究STM32的自平衡小车,发现有两座必过的大山,一为卡尔曼滤波,二为PID算法。
网上看了很多关于卡尔曼滤波的代码,感觉写得真不咋地。一怒之下,自己重写,不废话,贴代码



[pre lang="C" line="1" file="kalman.h"]/**
  ******************************************************************************
  * @file    kalman.h
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波算法
  *       
  *
  ******************************************************************************
  * @attention
  *本人对卡尔曼的粗略理解:以本次测量角速度(陀螺仪测量值)的积分得出的角度值
  * 与上次最优角度值的方差产生一个权重来衡量本次测量角度(加速度测量值)
  * 与上次最优角度值,从而产生新的最优角度值。好吧,比较拗口,有误处忘指正。
  *
  ******************************************************************************
  */

#ifndef __KALMAN_H__
#define __KALMAN_H__


#define Q_angle                        0.001        ////角度过程噪声的协方差
#define Q_gyro                        0.003        ////角速度过程噪声的协方差
#define R_angle                        0.5                ////测量噪声的协方差(即是测量偏差)
#define dt                                0.01                        ////卡尔曼滤波采样频率
#define C_0                                1

/**************卡尔曼运算变量定义**********************
*
***由于卡尔曼为递推运算,结构体需定义为全局变量
***在实际运用中只需定义一个KalmanCountData类型的变量即可
***无需用户定义多个中间变量,简化函数的使用
*/
typedef struct
{
        float                                Q_bias;                ////最优估计值的偏差,即估计出来的陀螺仪的漂移量
        float                                Angle_err;                ////实测角度与陀螺仪积分角度的差值
        float                                PCt_0;                               
        float                                PCt_1;
        float                                E;                        ////计算的过程量
        float                                K_0;                        ////含有卡尔曼增益的另外一个函数,用于计算最优估计值
        float                                K_1;                        ////含有卡尔曼增益的函数,用于计算最优估计值的偏差
        float                                t_0;                               
        float                                t_1;
        float                                Pdot[4];                ////Pdot[4] = {0,0,0,0};过程协方差矩阵的微分矩阵
        float                                PP[2][2];                //// PP[2][2] = { { 1, 0 },{ 0, 1 } };协方差(covariance)
        float                                Angle_Final;        ////后验估计最优角度值(即系统处理最终值)
        float                                Gyro_Final;        ////后验估计最优角速度值

}KalmanCountData;

void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct);
void Kalman_Filter_Init(KalmanCountData * Kalman_Struct);



#endif

[/code]

kalman.c

[pre lang="C" line="1"  file="kalman.c"]
#include "kalman.h"


/**
  ******************************************************************************
  * @file    void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波计算中间量初始化
  *       
  *
  ******************************************************************************
  * @attention
  *
  *
  *
  *
  ******************************************************************************
  */

void Kalman_Filter_Init(KalmanCountData * Kalman_Struct)
{
        Kalman_Struct -> Angle_err                 = 0;
        Kalman_Struct -> Q_bias                         = 0;
        Kalman_Struct -> PCt_0                         = 0;
        Kalman_Struct -> PCt_1                         = 0;
        Kalman_Struct -> E                                 = 0;
        Kalman_Struct -> K_0                         = 0;
        Kalman_Struct -> K_1                         = 0;
        Kalman_Struct -> t_0                         = 0;
        Kalman_Struct -> t_1                         = 0;
        Kalman_Struct -> Pdot[0]                 = 0;
        Kalman_Struct -> Pdot[1]                 = 0;
        Kalman_Struct -> Pdot[2]                 = 0;
        Kalman_Struct -> Pdot[3]                 = 0;       
        Kalman_Struct -> PP[0][0]                 = 1;
        Kalman_Struct -> PP[0][1]                 = 0;
        Kalman_Struct -> PP[1][0]                 = 0;
        Kalman_Struct -> PP[1][1]                 = 1;       
        Kalman_Struct -> Angle_Final         = 0;
        Kalman_Struct -> Gyro_Final                 = 0;

}


/**
  ******************************************************************************
  * @file    void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   卡尔曼滤波计算
  *       
  *
  ******************************************************************************
  * @attention
  *                Accel:加速度计数据处理后进来的角度值
  *                Gyro :陀螺仪数据处理后进来的角速度值
  *                Kalman_Struct:递推运算所需要的中间变量,由用户定义为全局结构体变量
  *                Kalman_Struct -> Angle_Final  为滤波后角度最优值
  *                Kalman_Struct -> Gyro_Final   为后验角度值
  ******************************************************************************
  */

void Kalman_Filter(float Accel,        float Gyro ,KalmanCountData * Kalman_Struct)
{
                //陀螺仪积分角度(先验估计)
                Kalman_Struct -> Angle_Final += (Gyro - Kalman_Struct -> Q_bias) * dt;
               
                //先验估计误差协方差的微分
                Kalman_Struct -> Pdot[0] = Q_angle - Kalman_Struct -> PP[0][1] - Kalman_Struct -> PP[1][0];
                Kalman_Struct -> Pdot[1] = - Kalman_Struct -> PP[1][1];
                Kalman_Struct -> Pdot[2] = - Kalman_Struct -> PP[1][1];
                Kalman_Struct -> Pdot[3] = Q_gyro;
               
                //先验估计误差协方差的积分
                Kalman_Struct -> PP[0][0] += Kalman_Struct -> Pdot[0] * dt;   
                Kalman_Struct -> PP[0][1] += Kalman_Struct -> Pdot[1] * dt;   
                Kalman_Struct -> PP[1][0] += Kalman_Struct -> Pdot[2] * dt;
                Kalman_Struct -> PP[1][1] += Kalman_Struct -> Pdot[3] * dt;
               
                //计算角度偏差
                Kalman_Struct -> Angle_err = Accel - Kalman_Struct -> Angle_Final;       
               
                //卡尔曼增益计算
                Kalman_Struct -> PCt_0 = C_0 * Kalman_Struct -> PP[0][0];
                Kalman_Struct -> PCt_1 = C_0 * Kalman_Struct -> PP[1][0];
               
                Kalman_Struct -> E = R_angle + C_0 * Kalman_Struct -> PCt_0;
               
                Kalman_Struct -> K_0 = Kalman_Struct -> PCt_0 / Kalman_Struct -> E;
                Kalman_Struct -> K_1 = Kalman_Struct -> PCt_1 / Kalman_Struct -> E;
               
                //后验估计误差协方差计算
                Kalman_Struct -> t_0 = Kalman_Struct -> PCt_0;
                Kalman_Struct -> t_1 = C_0 * Kalman_Struct -> PP[0][1];

                Kalman_Struct -> PP[0][0] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_0;                 
                Kalman_Struct -> PP[0][1] -= Kalman_Struct -> K_0 * Kalman_Struct -> t_1;
                Kalman_Struct -> PP[1][0] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_0;
                Kalman_Struct -> PP[1][1] -= Kalman_Struct -> K_1 * Kalman_Struct -> t_1;

                Kalman_Struct -> Angle_Final += Kalman_Struct -> K_0 * Kalman_Struct -> Angle_err;         //后验估计最优角度值
                Kalman_Struct -> Q_bias        += Kalman_Struct -> K_1 * Kalman_Struct -> Angle_err;                 //更新最优估计值的偏差
                Kalman_Struct -> Gyro_Final   = Gyro - Kalman_Struct -> Q_bias;                                                 //更新最优角速度值

}

[/code]

代码可以放在实际工程中使用,也可以用VS等C编译工具进行实验学习。在VS中的main()实例使用如下

[pre lang="C" line="1" file="main.c"]
#include "kalman.h"

#include "stdio.h"

#include "stdlib.h"

void main(void)
{


        KalmanCountData k;
        //定义一个卡尔曼运算结构体
        Kalman_Filter_Init(&k);
        //讲运算变量初始化
        int m,n;        

           for(int a = 0;a<80;a++)
        //测试80次
        {

                //m,n为1到100的随机数
                m = 1+ rand() %100;

                n = 1+ rand() %100;

                //卡尔曼滤波,传递2个测量值以及运算结构体
       
        Kalman_Filter((float)m,(float)n,&k);

                //打印结果
                printf("%d and %d is %f - %f\r\n",m,n,k.Angle_Final,k.K_0);
       
        }




}
[/code]
回复

使用道具 举报

发表于 2015-4-9 16:55:50 | 显示全部楼层
楼主,麻烦你看一下我的滤波效果,看看还需不需要调整参数
开电机,角度滤波效果

未开电机,陀螺仪滤波效果


本帖子中包含更多资源

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

x
回复 支持 1 反对 0

使用道具 举报

 楼主| 发表于 2015-1-10 12:40:20 | 显示全部楼层
继卡尔曼之后,继续对PID进行封装~~~~~~~~
为平衡小车做准备
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 12:41:15 | 显示全部楼层
本帖最后由 D调的华丽 于 2015-1-10 12:51 编辑

上代码。

[pre lang="C" line="1" file="PID_Control.h"]/**
  ******************************************************************************
  * @file    PID_Control.h
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   PID控制算法头文件
  *                        定义结构体类型以及声明函数
  *                        #define IF_THE_INTEGRAL_SEPARATION  0/1  为积分分离标志
  ******************************************************************************
  **/

#ifndef __PID_CONTROL_H__
#define __PID_CONTROL_H__

#define IF_THE_INTEGRAL_SEPARATION  0   
//#define IF_THE_INTEGRAL_SEPARATION  1   //是否积分分离  0-不分离,1 -分离

typedef struct
{
        double SetPoint; // 设定目标 Desired Value   
        double Proportion; // 比例常数 Proportional Const
        double Integral; // 积分常数 Integral Const
        double Derivative; // 微分常数 Derivative Const   
        double LastError; // Error[-1]
        double PrevError; // Error[-2]
        double SumError; // Sums of Errors  
}PID;

#if IF_THE_INTEGRAL_SEPARATION            //是否积分分离预编译开始

double PIDCalc(double NextPoint ,double SepLimit, PID *pp);   //带积分分离的PID运算

#else

double PIDCalc( double NextPoint, PID *pp);     //不带积分分离的PID运算

#endif        //是否积分分离预编译结束

void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp);

#endif[/code]
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 12:41:59 | 显示全部楼层
本帖最后由 D调的华丽 于 2015-1-10 12:52 编辑

[pre lang="C" line="1" file="PID_Control.c"]/**
  ******************************************************************************
  * @file    PID_Control.c
  * @author  willieon
  * @version V0.1
  * @date    January-2015
  * @brief   PID控制算法函数代码
  *       
  *
  ******************************************************************************
  **/

#include "PID_Control.h"
#include "math.h"

/*************************************************************************************
*        名    称: double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
*        功    能: PID控制运算
*        入口参数: PID *pp  - 定义的运算所需变量的结构体
*                           NextPoint - 负反馈输入值
*                           SepLimit  - 积分分离上限
*        出口参数: 返回PID控制量
*        说    明: 默认不进行积分分离,如果用户需要使用积分分离,需在PID_Control.h中
*                                将 #define IF_THE_INTEGRAL_SEPARATION  0  改为
*                            #define IF_THE_INTEGRAL_SEPARATION  1
*        调用方法: 进行积分分离时入口参数为3个,具体方法如下:
*                                PID PIDControlStruct ;   //定义PID运算结构体
*                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化,注意&符号不能省
*                                ControlData = PIDCalc(ReadData, 200, &PIDControlStruct);   //控制量 = PIDCalc(反馈值,积分分离上限,PID运算结构体)
*
***************************************************************************************
*/

#if IF_THE_INTEGRAL_SEPARATION

double PIDCalc(double NextPoint ,double SepLimit, PID *pp)
{
        double dError, Error,Flag;   
        Error = pp->SetPoint - NextPoint;         // 偏差
        if(abs(Error) > SepLimit)        //当偏差大于分离上限积分分离
        {
                Flag = 0;
        }
        else       //当偏差小于分离上限,积分项不分离
        {
                Flag = 1;
                pp->SumError += Error;         // 积分  
        }
        dError = pp->LastError - pp->PrevError;         // 当前微分
        pp->PrevError = pp->LastError;
        pp->LastError = Error;  
        return (
                pp->Proportion                *                Error                 // 比例项
                + Flag * pp->Integral        *                pp->SumError         // 积分项
                + pp->Derivative                *                dError                 // 微分项
                );
}

#else

double PIDCalc( double NextPoint, PID *pp)
{  
        double dError, Error;   
        Error = pp->SetPoint - NextPoint;                         // 偏差
        pp->SumError += Error;                                        // 积分  
        dError = pp->LastError - pp->PrevError;                // 当前微分
        pp->PrevError = pp->LastError;
        pp->LastError = Error;  
        return (pp->Proportion        *        Error                // 比例项
                + pp->Integral                *        pp->SumError         // 积分项
                + pp->Derivative        *        dError         // 微分项
                );
}

#endif


/*************************************************************************************
*        名    称: double PIDCalc( PID *pp, double NextPoint ,double SepLimit)
*        功    能: PID初始化设定
*        入口参数: PID *pp  - 定义的运算所需变量的结构体
*                           SetPoint - 设定的目标值
*                           Proportion,Integral ,Derivative - P,I,D系数
*        出口参数: 无
*        说    明:       
*        调用方法:  PID PIDControlStruct ;   //定义PID运算结构体
*                                PIDInit(50, 0.24, 0.04, 0.2, &PIDControlStruct);//结构体初始化,注意&符号不能省
*                                因为函数需要传入一个指针,需要对结构体取首地址传给指针
*
***************************************************************************************
*/


void PIDInit (double SetPoint, double Proportion, double Integral, double Derivative, PID *pp)
{  
        pp -> SetPoint = SetPoint; // 设定目标 Desired Value   
        pp -> Proportion = Proportion; // 比例常数 Proportional Const
        pp -> Integral = Integral; // 积分常数 Integral Const
        pp -> Derivative = Derivative; // 微分常数 Derivative Const   
        pp -> LastError = 0; // Error[-1]
        pp -> PrevError = 0; // Error[-2]
        pp -> SumError = 0; // Sums of Errors

        //memset ( pp,0,sizeof(struct PID));   //need include "string.h"
}[/code]
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 12:42:55 | 显示全部楼层
好了,现在把卡尔曼滤波和PID算法联合起来,在VS平台中运行实验
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 12:43:23 | 显示全部楼层
本帖最后由 D调的华丽 于 2015-1-10 12:59 编辑

[pre lang="C" line="1" file="main.c"]#include "kalman.h"

#include "stdio.h"
#include "stdlib.h"
#include "PID_Control.h"

void main(void)

{
        KalmanCountData k;
        PID PIDControlStruct;
        Kalman_Filter_Init(&k);
        PIDInit(50, 1, 0.04, 0.2, &PIDControlStruct);
        int m,n;

        double out;

        for(int a = 0;a<80;a++)
        {
                m = 1+ rand() %100;
                n = 1+ rand() %100;
                Kalman_Filter((float)m,(float)n,&k);
                out = PIDCalc(k.Angle_Final, &PIDControlStruct);
                printf("%3d and %3d is %6f -pid- %6f\r\n",m,n,k.Angle_Final,out);

        }
}

[/code]
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 12:45:47 | 显示全部楼层
本帖最后由 D调的华丽 于 2015-1-10 13:22 编辑

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

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

代码本人测试过,如果有不足之处望不吝赐教!请直接跟帖讨论
回复 支持 反对

使用道具 举报

发表于 2015-1-10 13:34:14 来自手机 | 显示全部楼层
大神,求点化,超喜欢双轮车,求加好友
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 13:35:15 | 显示全部楼层
我不是大神,呵呵,也在学习阶段!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-10 13:43:13 | 显示全部楼层
本帖最后由 D调的华丽 于 2015-1-10 13:47 编辑

我最近在做的平衡车,硬件清单:
香蕉电机+轮子   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就死机,后来换光耦隔离才解决!
回复 支持 反对

使用道具 举报

发表于 2015-1-12 21:24:08 | 显示全部楼层
谢谢分享学习一下
回复 支持 反对

使用道具 举报

发表于 2015-1-18 23:18:33 | 显示全部楼层
这个卡尔曼滤波后,角度的跟随性响应是不是太慢了;基本上需要快1秒才能跟随到目标值
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-1-19 11:18:09 | 显示全部楼层
275891381 发表于 2015-1-18 23:18
这个卡尔曼滤波后,角度的跟随性响应是不是太慢了;基本上需要快1秒才能跟随到目标值

这个速度是看你自己的采样速度。卡尔曼滤波正常情况下只需要三次比较好的测量就能跟随到目标值了
回复 支持 反对

使用道具 举报

发表于 2015-1-20 16:56:58 | 显示全部楼层
very good, nice job !!!
回复 支持 反对

使用道具 举报

 楼主| 发表于 2015-2-1 13:07:43 | 显示全部楼层

没人关注了
回复 支持 反对

使用道具 举报

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

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-19 11:52 , Processed in 0.049284 second(s), 25 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

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