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

標題: 基于STM32F103的二輪平衡車（6軸上位機 源代碼 卡爾曼濾波資料）心得分享 [打印本頁]
作者: 51hei大小    時間: 2016-6-17 14:44
標題: 基于STM32F103的二輪平衡車（6軸上位機 源代碼 卡爾曼濾波資料）心得分享
         前段時間搞了個平衡車，涉及stm32F3  步進電機驅(qū)動   陀螺儀mpu3050   加速度計adxl345（也可以用6軸mpu6050）  無線NRF24L01
        當初最大問題是卡爾曼濾波（進行陀螺儀與加速度計的數(shù)據(jù)融合）和pid調(diào)節(jié)
        對于卡爾曼濾波，經(jīng)過自己不斷深究，其實也不是很復雜，核心是五大公式，涉及矩陣運算，思想是預測值 最優(yōu)估計值 噪聲  協(xié)方差的概念，難點：一些參數(shù)選擇

        說下用卡爾曼濾波的出發(fā)點，陀螺儀 加速度計都可以得到角度，而陀螺儀是先得到角速度再經(jīng)積分才得到角度， 陀螺儀相比加速度計短時間內(nèi)動態(tài)性能好，得到角度精準，但本身有小漂移，隨著時間變長，不斷積分，誤差會越來越大，那就需要用加速度計進行校正


       對于pid算法，里面涉及二級pid，首先要明白小車速度跟給步進電機的頻率是成正比的,就把頻率等效為速度
       第一個pid，角度pid，通過測角度反饋給stm32f3產(chǎn)生頻率(速度)來進行平衡調(diào)節(jié)(即調(diào)節(jié)角度)
       第二個pid，速度pid，由于角度調(diào)節(jié)產(chǎn)生了速度變化，而為了不改變設(shè)定的速度，需要進行速度調(diào)節(jié)，它的反饋來自不斷角度pid的結(jié)果（由于速度跟頻率成正比，不需要測速反饋）
       難點：pid整定參數(shù)




  1. /***********************************************
  2. 標題: 24L01.c
  3. 日期: 2013/12/27
  4. 版本：v1.0
  5. 功能: 初始化以spi及數(shù)據(jù)讀寫
  6. 說明：24l01的初始化及spi2的調(diào)用
  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}; //發(fā)送地址
  16. const u8 RX_ADDRESS[RX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //發(fā)送地址
  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個字節(jié)的地址.        
  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);//發(fā)送寄存器號
  72.         SPIx_ReadWriteByte(value);      //寫入寄存器的值
  73.         Set_NRF24L01_CSN;                 //禁止SPI傳輸           
  74.         return(status);                               //返回狀態(tài)值
  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);   //發(fā)送寄存器號
  84.         reg_val=SPIx_ReadWriteByte(0XFF);//讀取寄存器內(nèi)容
  85.         Set_NRF24L01_CSN;          //禁止SPI傳輸                    
  86.         return(reg_val);           //返回狀態(tài)值
  87.         }        

  89. //在指定位置讀出指定長度的數(shù)據(jù)
  90. //reg:寄存器(位置)
  91. //*pBuf:數(shù)據(jù)指針
  92. //len:數(shù)據(jù)長度
  93. //返回值,此次讀到的狀態(tài)寄存器值
  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);//發(fā)送寄存器值(位置),并讀取狀態(tài)值              
  99.         for(u8_ctr=0;u8_ctr<len;u8_ctr++)pbuf[u8_ctr]=spix_readwritebyte(0xff); 讀出數(shù)據(jù)
  100.         Set_NRF24L01_CSN;       //關(guān)閉SPI傳輸
  101.         return status;        //返回讀到的狀態(tài)值
  102.         }

  104. //在指定位置寫指定長度的數(shù)據(jù)
  105. //reg:寄存器(位置)
  106. //*pBuf:數(shù)據(jù)指針
  107. //len:數(shù)據(jù)長度
  108. //返回值,此次讀到的狀態(tài)寄存器值
  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);//發(fā)送寄存器值(位置),并讀取狀態(tài)值
  114.         for(u8_ctr=0; u8_ctr<len; u8_ctr++)spix_readwritebyte(*pbuf++);="" 寫入數(shù)據(jù)=""
  115.         Set_NRF24L01_CSN;       //關(guān)閉SPI傳輸
  116.         return status;          //返回讀到的狀態(tài)值
  117.         }
  118.                                    
  119. //啟動NRF24L01發(fā)送一次數(shù)據(jù)
  120. //txbuf:待發(fā)送數(shù)據(jù)首地址
  121. //返回值:發(fā)送完成狀況
  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);//寫數(shù)據(jù)到TX BUF  32個字節(jié)
  127.         Set_NRF24L01_CE;//啟動發(fā)送           
  128.         while(NRF24L01_IRQ!=0);//等待發(fā)送完成
  129.         sta=NRF24L01_Read_Reg(STATUS);  //讀取狀態(tài)寄存器的值           
  130.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中斷標志
  131.         if(sta&MAX_TX)//達到最大重發(fā)次數(shù)
  132.                 {
  133.                 NRF24L01_Write_Reg(NRF24L01_FLUSH_TX,0xff);//清除TX FIFO寄存器
  134.                 return MAX_TX;
  135.                 }
  136.         if(sta&TX_OK)//發(fā)送完成
  137.                 {
  138.                 return TX_OK;
  139.                 }
  140.         return 0xff;//其他原因發(fā)送失敗
  141.         }

  143. //啟動NRF24L01發(fā)送一次數(shù)據(jù)
  144. //txbuf:待發(fā)送數(shù)據(jù)首地址
  145. //返回值:0，接收完成；其他，錯誤代碼
  146. u8 NRF24L01_RxPacket(u8 *rxbuf)
  147.         {
  148.         u8 sta;                                                                              
  149.         sta=NRF24L01_Read_Reg(STATUS);  //讀取狀態(tài)寄存器的值            
  150.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中斷標志
  151.         if(sta&RX_OK)//接收到數(shù)據(jù)
  152.                 {
  153.                 NRF24L01_Read_Buf(NRF24L01_RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//讀取數(shù)據(jù)
  154.                 NRF24L01_Write_Reg(NRF24L01_FLUSH_RX,0xff);//清除RX FIFO寄存器
  155.                 return 0;
  156.                 }           
  157.         return 1;//沒收到任何數(shù)據(jù)
  158.         }        
  159.                                             
  160. //該函數(shù)初始化NRF24L01到RX模式
  161. //設(shè)置RX地址,寫RX數(shù)據(jù)寬度,選擇RF頻道,波特率和LNA HCURR
  162. //當CE變高后,即進入RX模式,并可以接收數(shù)據(jù)了                  
  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節(jié)點地址
  167.         
  168.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01);    //使能通道0的自動應(yīng)答   
  169.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01);//使能通道0的接收地址           
  170.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40);             //設(shè)置RF通信頻率                  
  171.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);//選擇通道0的有效數(shù)據(jù)寬度            
  172.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f);//設(shè)置TX發(fā)射參數(shù),0db增益,2Mbps,低噪聲增益開啟   
  173.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG, 0x0f);//配置基本工作模式的參數(shù);PWR_UP,EN_CRC,16BIT_CRC,接收模式
  174.         Set_NRF24L01_CE; //CE為高,進入接收模式
  175.         }
  176.                                                          
  177. //該函數(shù)初始化NRF24L01到TX模式
  178. //設(shè)置TX地址,寫TX數(shù)據(jù)寬度,設(shè)置RX自動應(yīng)答的地址,填充TX發(fā)送數(shù)據(jù),選擇RF頻道,波特率和LNA HCURR
  179. //PWR_UP,CRC使能
  180. //當CE變高后,即進入RX模式,并可以接收數(shù)據(jù)了                  
  181. //CE為高大于10us,則啟動發(fā)送.         
  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節(jié)點地址
  186.         NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); //設(shè)置TX節(jié)點地址,主要為了使能ACK         
  187.         
  188.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01);     //使能通道0的自動應(yīng)答   
  189.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01); //使能通道0的接收地址  
  190.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+SETUP_RETR,0x1a);//設(shè)置自動重發(fā)間隔時間:500us + 86us;最大自動重發(fā)次數(shù):10次
  191.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40);       //設(shè)置RF通道為40
  192.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f);  //設(shè)置TX發(fā)射參數(shù),0db增益,2Mbps,低噪聲增益開啟   
  193.         NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG,0x0e);    //配置基本工作模式的參數(shù);PWR_UP,EN_CRC,16BIT_CRC,接收模式,開啟所有中斷
  194.         Set_NRF24L01_CE;//CE為高,10us后啟動發(fā)送
  195.         }                  
復制代碼


代碼資料（完美）見下
balance car nrf24l01程序 完美.zip (6.43 MB, 下載次數(shù): 243)

平衡車卡爾曼濾波資料.zip (1.2 MB, 下載次數(shù): 213)


作者: kingbobo    時間: 2016-6-20 08:47
好玩的東西都要MARK一下
作者: lcr39101    時間: 2016-6-20 19:55
學習，學習，謝謝
作者: 瘋狂土豆兒    時間: 2016-8-9 18:34
我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函數(shù)分別讀取的加速度，角速度和傾角，我發(fā)現(xiàn)當我快速的改變板子的傾角的時候，比如快速變化10度，Angle（卡爾曼濾波后的傾角）瞬時變化非常快，可能會瞬間變化幾十度然后回到正常角度，而當我緩慢變化10度的時候，Angle變化是正常線性變化到10度，在這兩種變化中，Angle_ax（從MPU6050讀取的值經(jīng)過處理后的陀螺儀的Y軸數(shù)據(jù)）的變化一直都是線性正常的，并且Angle的值特別接近Angle_ax的值 問題：1，我快速改變板子傾角時Angle的變化正常嗎？       2，Angle正常變化的時候是應(yīng)該與Angle_ax的值相近嗎？  現(xiàn)在情況就是，就算我是在減小傾角，只要我快速地改變，它顯示的傾角都會先增大再減小，而如果我慢速改變的話，傾角就會緩慢減小而不會出現(xiàn)中間的角度增大   *************讀取數(shù)據(jù)******************** //定義MPU6050內(nèi)部地址 #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  //讀取寄存器原生數(shù)據(jù)                     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]);                         //將原生數(shù)據(jù)轉(zhuǎn)換為實際值，計算公式跟寄存器的配置有關(guān)         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;        }         //******卡爾曼參數(shù)************                  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-先驗估計誤差協(xié)方差的微分          Pdot[1]= -PP[1][1];         Pdot[2]= -PP[1][1];         Pdot[3]=Q_gyro;                  PP[0][0] += Pdot[0] * dt;   // Pk-先驗估計誤差協(xié)方差微分的積分         PP[0][1] += Pdot[1] * dt;   // =先驗估計誤差協(xié)方差         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;                 //后驗估計誤差協(xié)方差         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;     //弧度轉(zhuǎn)換為度  /****************************角速度****************************************/                   Gyro_y = MPU6050_Real_Data.Gyro_Y;              時間dt，所以此處不用積分 /***************************卡爾曼融合*************************************/         Kalman_Filter(Angle_ax,Gyro_y);       //卡爾曼濾波計算傾角        
作者: 雙贏電子    時間: 2016-8-24 20:10
都是高手學習了，謝謝分享
作者: WZUOZ    時間: 2017-1-6 18:00
學習學習
作者: 八寶粥    時間: 2019-12-14 10:53
rout那一行：為什么不是pi（直立環(huán)）+PD(速度環(huán))控制
作者: shower721    時間: 2020-1-12 00:32
學習，謝謝分享
作者: lkf_8888    時間: 2023-10-8 21:17
我看到第四橫，#include，后面沒有帶頭文件，能編譯通過嗎？？
作者: Loser'    時間: 2024-4-6 16:58
lcr39101 發(fā)表于 2016-6-20 19:55
學習，學習，謝謝

學習




歡迎光臨 (http://www.zg4o1577.cn/bbs/) Powered by Discuz! X3.1
主站蜘蛛池模板： 黄在线免费观看 | 日本啊v在线 | 狠狠干av | 久久亚洲国产精品日日av夜夜 | 日韩国产中文字幕 | 男女啪啪网址 | 伦理二区 | 精品日韩在线 | 精品福利在线 | 男插女下体视频 | 国产成人99久久亚洲综合精品 | 九色在线观看 | xxxxxx国产 | 7777精品伊人久久精品影视 | 狠狠色综合网站久久久久久久 | 日韩免费 | 国产一级片在线播放 | 午夜电影一区二区 | 在线视频成人 | 久久狠狠 | 久久视频免费观看 | 一区二区国产在线观看 | 国产精品久久久久久久久免费软件 | 97在线播放 | 碰碰视频 | 久久久成人免费一区二区 | 一区二区三区在线观看免费视频 | 91成人精品 | 99精品99 | 婷婷综合 | 精品视频一区二区 | 婷婷久| 国产精品久久久久久久 | 激情久久网 | 久久伊人精品 | 日本久久精品视频 | h在线观看 | 精品欧美乱码久久久久久1区2区 | 久久久久亚洲视频 | 一级黄色录像片子 | 亚洲精品丝袜日韩 |