单片机教程网

电脑版
提示:原网页已由神马搜索转码, 内容由www.51hei.com提供.
查看:12676|回复:9

基于STM32F103的二轮平衡车（6轴上位机 源代码 卡尔曼滤波资料）心得分享

 [复制链接]
ID:127084发表于 2016-6-17 14:44|显示全部楼层
      前段时间搞了个平衡车，涉及stm32F3  步进电机驱动   陀螺仪mpu3050   加速度计adxl345（也可以用6轴mpu6050）  无线NRF24L01
       当初最大问题是卡尔曼滤波（进行陀螺仪与加速度计的数据融合）和pid调节
       对于卡尔曼滤波，经过自己不断深究，其实也不是很复杂，核心是五大公式，涉及矩阵运算，思想是预测值 最优估计值 噪声  协方差的概念，难点：一些参数选择
b6c31d01d41c9e1714958f9c56d01d8f.png
       说下用卡尔曼滤波的出发点，陀螺仪 加速度计都可以得到角度，而陀螺仪是先得到角速度再经积分才得到角度， 陀螺仪相比加速度计短时间内动态性能好，得到角度精准，但本身有小漂移，随着时间变长，不断积分，误差会越来越大，那就需要用加速度计进行校正


     对于pid算法，里面涉及二级pid，首先要明白小车速度跟给步进电机的频率是成正比的,就把频率等效为速度
     第一个pid，角度pid，通过测角度反馈给stm32f3产生频率(速度)来进行平衡调节(即调节角度)
     第二个pid，速度pid，由于角度调节产生了速度变化，而为了不改变设定的速度，需要进行速度调节，它的反馈来自不断角度pid的结果（由于速度跟频率成正比，不需要测速反馈）
     难点：pid整定参数
dced2f010c1ef04643993a5e26c68477.jpg

eed39bb1a85277ac150f8872704f0a55.jpg

a7b8f3f8fde79ff6a9df96b46007ce63.jpg


  1. /***********************************************
  2. 标题: 24L01.c
  3. 日期: 2013/12/27
  4. 版本：v1.0
  5. 功能: 初始化以spi及数据读写
  6. 说明：24l01的初始化及spi2的调用
  7. 注意：在24l01.h中的管脚配置

  9. *************************************************/
  10. #include "stm32f10x.h"
  11. #include "24l01.h"
  12. #include "spi.h"
  13. #include
  14.       
  15. const u8 TX_ADDRESS[TX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //发送地址
  16. const u8 RX_ADDRESS[RX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //发送地址
  17.                                         
  18. //初始化24L01的IO口
  19. void NRF24L01_Init(void)
  20.        {
  21.           
  22.        GPIO_InitTypeDef GPIO_InitStructure;
  23.        RCC_APB2PeriphClockCmd(       RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOD, ENABLE );      

  25.        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;//CE  PB12
  26.        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ;   //推挽输出
  27.        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  28.        GPIO_Init(GPIOB,& GPIO_InitStructure);
  29.        GPIO_SetBits(GPIOB,GPIO_Pin_12);

  31.        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_2|GPIO_Pin_3;//LSN
  32.        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ;   //推挽输出
  33.        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  34.        GPIO_Init(GPIOD,& GPIO_InitStructure);
  35.        GPIO_SetBits(GPIOD,GPIO_Pin_8);
  36.           
  37.            GPIO_SetBits(GPIOD,GPIO_Pin_2);//LED1
  38.            GPIO_SetBits(GPIOD,GPIO_Pin_3);//LED2

  40.        GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
  41.        GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU  ;   //上拉输入
  42.        GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
  43.        GPIO_Init(GPIOD,& GPIO_InitStructure);

  45.        SPI2_Init();   //初始化SPI2
  46.       
  47.        Clr_NRF24L01_CE;       //使能24L01  NRF24L01_CE
  48.        Set_NRF24L01_CSN;   //SPI片选取消 NRF24L01_CSN                  
  49.        }

  51. //检测24L01是否存在
  52. //返回值:0，成功;1，失败      
  53. u8 NRF24L01_Check(void)
  54.        {
  55.        u8 buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
  56.        u8 i;
  57.        NRF24L01_Write_Buf(NRF24L01_WRITE_REG+TX_ADDR,buf,5);//写入5个字节的地址.      
  58.        NRF24L01_Read_Buf(TX_ADDR,buf,5); //读出写入的地址  
  59.        for(i=0;i<5;i++)if(buf[i]!=0XA5)break;                                              
  60.        if(i!=5)return 1;//检测24L01错误      
  61.        return 0;             //检测到24L01
  62.        }      
  63.       
  64. //SPI写寄存器
  65. //reg:指定寄存器地址
  66. //value:写入的值
  67. u8 NRF24L01_Write_Reg(u8 reg,u8 value)
  68.        {
  69.        u8 status;      
  70.        Clr_NRF24L01_CSN;             //使能SPI传输
  71.        status =SPIx_ReadWriteByte(reg);//发送寄存器号
  72.        SPIx_ReadWriteByte(value);     //写入寄存器的值
  73.        Set_NRF24L01_CSN;             //禁止SPI传输        
  74.        return(status);                     //返回状态值
  75.        }

  77. //读取SPI寄存器值
  78. //reg:要读的寄存器
  79. u8 NRF24L01_Read_Reg(u8 reg)
  80.        {
  81.        u8 reg_val;        
  82.        Clr_NRF24L01_CSN;       //使能SPI传输          
  83.        SPIx_ReadWriteByte(reg);   //发送寄存器号
  84.        reg_val=SPIx_ReadWriteByte(0XFF);//读取寄存器内容
  85.        Set_NRF24L01_CSN;       //禁止SPI传输              
  86.        return(reg_val);         //返回状态值
  87.        }      

  89. //在指定位置读出指定长度的数据
  90. //reg:寄存器(位置)
  91. //*pBuf:数据指针
  92. //len:数据长度
  93. //返回值,此次读到的状态寄存器值
  94. u8 NRF24L01_Read_Buf(u8 reg,u8 *pBuf,u8 len)
  95.        {
  96.        u8 status,u8_ctr;          
  97.        Clr_NRF24L01_CSN;         //使能SPI传输
  98.        status=SPIx_ReadWriteByte(reg);//发送寄存器值(位置),并读取状态值          
  99.        for(u8_ctr=0;u8_ctr<len;u8_ctr++)pbuf[u8_ctr]=spix_readwritebyte(0xff); 读出数据
  100.        Set_NRF24L01_CSN;     //关闭SPI传输
  101.        return status;       //返回读到的状态值
  102.        }

  104. //在指定位置写指定长度的数据
  105. //reg:寄存器(位置)
  106. //*pBuf:数据指针
  107. //len:数据长度
  108. //返回值,此次读到的状态寄存器值
  109. u8 NRF24L01_Write_Buf(u8 reg, u8 *pBuf, u8 len)
  110.        {
  111.        u8 status,u8_ctr;        
  112.        Clr_NRF24L01_CSN;       //使能SPI传输
  113.        status = SPIx_ReadWriteByte(reg);//发送寄存器值(位置),并读取状态值
  114.        for(u8_ctr=0; u8_ctr<len; u8_ctr++)spix_readwritebyte(*pbuf++);="" 写入数据=""
  115.        Set_NRF24L01_CSN;     //关闭SPI传输
  116.        return status;       //返回读到的状态值
  117.        }
  118.                         
  119. //启动NRF24L01发送一次数据
  120. //txbuf:待发送数据首地址
  121. //返回值:发送完成状况
  122. u8 NRF24L01_TxPacket(u8 *txbuf)
  123.        {
  124.        u8 sta;
  125.        Clr_NRF24L01_CE;
  126.        NRF24L01_Write_Buf(NRF24L01_WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);//写数据到TX BUF  32个字节
  127.        Set_NRF24L01_CE;//启动发送        
  128.        while(NRF24L01_IRQ!=0);//等待发送完成
  129.        sta=NRF24L01_Read_Reg(STATUS);  //读取状态寄存器的值        
  130.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中断标志
  131.        if(sta&MAX_TX)//达到最大重发次数
  132.            {
  133.            NRF24L01_Write_Reg(NRF24L01_FLUSH_TX,0xff);//清除TX FIFO寄存器
  134.            return MAX_TX;
  135.            }
  136.        if(sta&TX_OK)//发送完成
  137.            {
  138.            return TX_OK;
  139.            }
  140.        return 0xff;//其他原因发送失败
  141.        }

  143. //启动NRF24L01发送一次数据
  144. //txbuf:待发送数据首地址
  145. //返回值:0，接收完成；其他，错误代码
  146. u8 NRF24L01_RxPacket(u8 *rxbuf)
  147.        {
  148.        u8 sta;                                                    
  149.        sta=NRF24L01_Read_Reg(STATUS);  //读取状态寄存器的值        
  150.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中断标志
  151.        if(sta&RX_OK)//接收到数据
  152.            {
  153.            NRF24L01_Read_Buf(NRF24L01_RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//读取数据
  154.            NRF24L01_Write_Reg(NRF24L01_FLUSH_RX,0xff);//清除RX FIFO寄存器
  155.            return 0;
  156.            }        
  157.        return 1;//没收到任何数据
  158.        }      
  159.                               
  160. //该函数初始化NRF24L01到RX模式
  161. //设置RX地址,写RX数据宽度,选择RF频道,波特率和LNA HCURR
  162. //当CE变高后,即进入RX模式,并可以接收数据了            
  163. void RX_Mode(void)
  164.        {
  165.        Clr_NRF24L01_CE;      
  166.        NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);//写RX节点地址
  167.       
  168.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01);   //使能通道0的自动应答  
  169.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01);//使能通道0的接收地址        
  170.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40);         //设置RF通信频率            
  171.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);//选择通道0的有效数据宽度        
  172.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f);//设置TX发射参数,0db增益,2Mbps,低噪声增益开启  
  173.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG, 0x0f);//配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,接收模式
  174.        Set_NRF24L01_CE; //CE为高,进入接收模式
  175.        }
  176.                                       
  177. //该函数初始化NRF24L01到TX模式
  178. //设置TX地址,写TX数据宽度,设置RX自动应答的地址,填充TX发送数据,选择RF频道,波特率和LNA HCURR
  179. //PWR_UP,CRC使能
  180. //当CE变高后,即进入RX模式,并可以接收数据了            
  181. //CE为高大于10us,则启动发送.      
  182. void TX_Mode(void)
  183.        {                                                                            
  184.        Clr_NRF24L01_CE;        
  185.        NRF24L01_Write_Buf(NRF24L01_WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);//写TX节点地址
  186.        NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); //设置TX节点地址,主要为了使能ACK      
  187.       
  188.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01);     //使能通道0的自动应答  
  189.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01); //使能通道0的接收地址  
  190.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+SETUP_RETR,0x1a);//设置自动重发间隔时间:500us + 86us;最大自动重发次数:10次
  191.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40);     //设置RF通道为40
  192.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f);  //设置TX发射参数,0db增益,2Mbps,低噪声增益开启  
  193.        NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG,0x0e);   //配置基本工作模式的参数;PWR_UP,EN_CRC,16BIT_CRC,接收模式,开启所有中断
  194.        Set_NRF24L01_CE;//CE为高,10us后启动发送
  195.        }            
复制代码


代码资料（完美）见下
balance car nrf24l01程序 完美.zip(6.43 MB, 下载次数: 242)

评分

黑币 +23
收起理由
+ 9
赞一个！
+ 5
共享资料的黑币奖励！
+ 9
很给力！

查看全部评分

ID:106341发表于 2016-6-20 08:47|显示全部楼层
好玩的东西都要MARK一下
ID:127462发表于 2016-6-20 19:55|显示全部楼层
学习，学习，谢谢
ID:136370发表于 2016-8-9 18:34|显示全部楼层
我用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);     //卡尔曼滤波计算倾角    
ID:116662发表于 2016-8-24 20:10|显示全部楼层
都是高手学习了，谢谢分享
ID:140906发表于 2017-1-6 18:00来自手机|显示全部楼层
学习学习
ID:575955发表于 2019-12-14 10:53|显示全部楼层
rout那一行：为什么不是pi（直立环）+PD(速度环)控制
ID:685093发表于 2020-1-12 00:32|显示全部楼层
学习，谢谢分享
ID:1095426发表于 2023-10-8 21:17|显示全部楼层
我看到第四横，#include，后面没有带头文件，能编译通过吗？？
ID:1115551发表于 2024-4-6 16:58|显示全部楼层
lcr39101 发表于 2016-6-20 19:55
学习，学习，谢谢

学习

手机版|小黑屋|51黑电子论坛|51黑电子论坛6群QQ管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网