基于STM32F103的二輪平衡車（6軸上位機(jī) 源代碼卡爾曼濾波資料）心得分享

ID:127084 · 發(fā)表于 2016-6-17 14:44

       前段時(shí)間搞了個(gè)平衡車，涉及stm32F3  步進(jìn)電機(jī)驅(qū)動(dòng) 陀螺儀mpu3050 加速度計(jì)adxl345（也可以用6軸mpu6050）  無線NRF24L01
      當(dāng)初最大問題是卡爾曼濾波（進(jìn)行陀螺儀與加速度計(jì)的數(shù)據(jù)融合）和pid調(diào)節(jié)
      對(duì)于卡爾曼濾波，經(jīng)過自己不斷深究，其實(shí)也不是很復(fù)雜，核心是五大公式，涉及矩陣運(yùn)算，思想是預(yù)測(cè)值最優(yōu)估計(jì)值噪聲  協(xié)方差的概念，難點(diǎn)：一些參數(shù)選擇

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

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

/***********************************************
標(biāo)題: 24L01.c
日期: 2013/12/27
版本：v1.0
功能: 初始化以spi及數(shù)據(jù)讀寫
說明：24l01的初始化及spi2的調(diào)用
注意：在24l01.h中的管腳配置
*************************************************/
#include "stm32f10x.h"
#include "24l01.h"
#include "spi.h"
#include
const u8 TX_ADDRESS[TX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //發(fā)送地址
const u8 RX_ADDRESS[RX_ADR_WIDTH]={0x34,0x43,0x10,0x10,0x01}; //發(fā)送地址
//初始化24L01的IO口
void NRF24L01_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOD, ENABLE );
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;//CE PB12
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ; //推挽輸出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_SetBits(GPIOB,GPIO_Pin_12);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_2|GPIO_Pin_3;//LSN
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP ; //推挽輸出
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOD, &GPIO_InitStructure);
GPIO_SetBits(GPIOD,GPIO_Pin_8);
GPIO_SetBits(GPIOD,GPIO_Pin_2);//LED1
GPIO_SetBits(GPIOD,GPIO_Pin_3);//LED2
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU ; //上拉輸入
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOD, &GPIO_InitStructure);
SPI2_Init(); //初始化SPI2
Clr_NRF24L01_CE; //使能24L01 NRF24L01_CE
Set_NRF24L01_CSN; //SPI片選取消 NRF24L01_CSN
}
//檢測(cè)24L01是否存在
//返回值:0，成功;1，失敗
u8 NRF24L01_Check(void)
{
u8 buf[5]={0XA5,0XA5,0XA5,0XA5,0XA5};
u8 i;
NRF24L01_Write_Buf(NRF24L01_WRITE_REG+TX_ADDR,buf,5);//寫入5個(gè)字節(jié)的地址.
NRF24L01_Read_Buf(TX_ADDR,buf,5); //讀出寫入的地址
for(i=0;i<5;i++)if(buf[i]!=0XA5)break;
if(i!=5)return 1;//檢測(cè)24L01錯(cuò)誤
return 0; //檢測(cè)到24L01
}
//SPI寫寄存器
//reg:指定寄存器地址
//value:寫入的值
u8 NRF24L01_Write_Reg(u8 reg,u8 value)
{
u8 status;
Clr_NRF24L01_CSN; //使能SPI傳輸
status =SPIx_ReadWriteByte(reg);//發(fā)送寄存器號(hào)
SPIx_ReadWriteByte(value); //寫入寄存器的值
Set_NRF24L01_CSN; //禁止SPI傳輸
return(status); //返回狀態(tài)值
}
//讀取SPI寄存器值
//reg:要讀的寄存器
u8 NRF24L01_Read_Reg(u8 reg)
{
u8 reg_val;
Clr_NRF24L01_CSN; //使能SPI傳輸
SPIx_ReadWriteByte(reg); //發(fā)送寄存器號(hào)
reg_val=SPIx_ReadWriteByte(0XFF);//讀取寄存器內(nèi)容
Set_NRF24L01_CSN; //禁止SPI傳輸
return(reg_val); //返回狀態(tài)值
}
//在指定位置讀出指定長(zhǎng)度的數(shù)據(jù)
//reg:寄存器(位置)
//*pBuf:數(shù)據(jù)指針
//len:數(shù)據(jù)長(zhǎng)度
//返回值,此次讀到的狀態(tài)寄存器值
u8 NRF24L01_Read_Buf(u8 reg,u8 *pBuf,u8 len)
{
u8 status,u8_ctr;
Clr_NRF24L01_CSN; //使能SPI傳輸
status=SPIx_ReadWriteByte(reg);//發(fā)送寄存器值(位置),并讀取狀態(tài)值
for(u8_ctr=0;u8_ctr<len;u8_ctr++)pbuf[u8_ctr]=spix_readwritebyte(0xff); 讀出數(shù)據(jù)
Set_NRF24L01_CSN; //關(guān)閉SPI傳輸
return status; //返回讀到的狀態(tài)值
}
//在指定位置寫指定長(zhǎng)度的數(shù)據(jù)
//reg:寄存器(位置)
//*pBuf:數(shù)據(jù)指針
//len:數(shù)據(jù)長(zhǎng)度
//返回值,此次讀到的狀態(tài)寄存器值
u8 NRF24L01_Write_Buf(u8 reg, u8 *pBuf, u8 len)
{
u8 status,u8_ctr;
Clr_NRF24L01_CSN; //使能SPI傳輸
status = SPIx_ReadWriteByte(reg);//發(fā)送寄存器值(位置),并讀取狀態(tài)值
for(u8_ctr=0; u8_ctr<len; u8_ctr++)spix_readwritebyte(*pbuf++);="" 寫入數(shù)據(jù)=""
Set_NRF24L01_CSN; //關(guān)閉SPI傳輸
return status; //返回讀到的狀態(tài)值
}
//啟動(dòng)NRF24L01發(fā)送一次數(shù)據(jù)
//txbuf:待發(fā)送數(shù)據(jù)首地址
//返回值:發(fā)送完成狀況
u8 NRF24L01_TxPacket(u8 *txbuf)
{
u8 sta;
Clr_NRF24L01_CE;
NRF24L01_Write_Buf(NRF24L01_WR_TX_PLOAD,txbuf,TX_PLOAD_WIDTH);//寫數(shù)據(jù)到TX BUF 32個(gè)字節(jié)
Set_NRF24L01_CE;//啟動(dòng)發(fā)送
while(NRF24L01_IRQ!=0);//等待發(fā)送完成
sta=NRF24L01_Read_Reg(STATUS); //讀取狀態(tài)寄存器的值
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中斷標(biāo)志
if(sta&MAX_TX)//達(dá)到最大重發(fā)次數(shù)
{
NRF24L01_Write_Reg(NRF24L01_FLUSH_TX,0xff);//清除TX FIFO寄存器
return MAX_TX;
}
if(sta&TX_OK)//發(fā)送完成
{
return TX_OK;
}
return 0xff;//其他原因發(fā)送失敗
}
//啟動(dòng)NRF24L01發(fā)送一次數(shù)據(jù)
//txbuf:待發(fā)送數(shù)據(jù)首地址
//返回值:0，接收完成；其他，錯(cuò)誤代碼
u8 NRF24L01_RxPacket(u8 *rxbuf)
{
u8 sta;
sta=NRF24L01_Read_Reg(STATUS); //讀取狀態(tài)寄存器的值
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+STATUS,sta); //清除TX_DS或MAX_RT中斷標(biāo)志
if(sta&RX_OK)//接收到數(shù)據(jù)
{
NRF24L01_Read_Buf(NRF24L01_RD_RX_PLOAD,rxbuf,RX_PLOAD_WIDTH);//讀取數(shù)據(jù)
NRF24L01_Write_Reg(NRF24L01_FLUSH_RX,0xff);//清除RX FIFO寄存器
return 0;
}
return 1;//沒收到任何數(shù)據(jù)
}
//該函數(shù)初始化NRF24L01到RX模式
//設(shè)置RX地址,寫RX數(shù)據(jù)寬度,選擇RF頻道,波特率和LNA HCURR
//當(dāng)CE變高后,即進(jìn)入RX模式,并可以接收數(shù)據(jù)了
void RX_Mode(void)
{
Clr_NRF24L01_CE;
NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH);//寫RX節(jié)點(diǎn)地址
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01); //使能通道0的自動(dòng)應(yīng)答
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01);//使能通道0的接收地址
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40); //設(shè)置RF通信頻率
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RX_PW_P0,RX_PLOAD_WIDTH);//選擇通道0的有效數(shù)據(jù)寬度
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f);//設(shè)置TX發(fā)射參數(shù),0db增益,2Mbps,低噪聲增益開啟
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG, 0x0f);//配置基本工作模式的參數(shù);PWR_UP,EN_CRC,16BIT_CRC,接收模式
Set_NRF24L01_CE; //CE為高,進(jìn)入接收模式
}
//該函數(shù)初始化NRF24L01到TX模式
//設(shè)置TX地址,寫TX數(shù)據(jù)寬度,設(shè)置RX自動(dòng)應(yīng)答的地址,填充TX發(fā)送數(shù)據(jù),選擇RF頻道,波特率和LNA HCURR
//PWR_UP,CRC使能
//當(dāng)CE變高后,即進(jìn)入RX模式,并可以接收數(shù)據(jù)了
//CE為高大于10us,則啟動(dòng)發(fā)送.
void TX_Mode(void)
{
Clr_NRF24L01_CE;
NRF24L01_Write_Buf(NRF24L01_WRITE_REG+TX_ADDR,(u8*)TX_ADDRESS,TX_ADR_WIDTH);//寫TX節(jié)點(diǎn)地址
NRF24L01_Write_Buf(NRF24L01_WRITE_REG+RX_ADDR_P0,(u8*)RX_ADDRESS,RX_ADR_WIDTH); //設(shè)置TX節(jié)點(diǎn)地址,主要為了使能ACK
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_AA,0x01); //使能通道0的自動(dòng)應(yīng)答
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+EN_RXADDR,0x01); //使能通道0的接收地址
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+SETUP_RETR,0x1a);//設(shè)置自動(dòng)重發(fā)間隔時(shí)間:500us + 86us;最大自動(dòng)重發(fā)次數(shù):10次
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_CH,40); //設(shè)置RF通道為40
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+RF_SETUP,0x0f); //設(shè)置TX發(fā)射參數(shù),0db增益,2Mbps,低噪聲增益開啟
NRF24L01_Write_Reg(NRF24L01_WRITE_REG+CONFIG,0x0e); //配置基本工作模式的參數(shù);PWR_UP,EN_CRC,16BIT_CRC,接收模式,開啟所有中斷
Set_NRF24L01_CE;//CE為高,10us后啟動(dòng)發(fā)送
}

復(fù)制代碼

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

2016-6-17 14:43 上傳
點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5

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

2016-6-17 14:43 上傳
點(diǎn)擊文件名下載附件
下載積分: 黑幣 -5

ID:106341 · 發(fā)表于 2016-6-20 08:47

好玩的東西都要MARK一下

ID:127462 · 發(fā)表于 2016-6-20 19:55

學(xué)習(xí)，學(xué)習(xí)，謝謝

ID:136370 · 發(fā)表于 2016-8-9 18:34

我用printf("%0.2f %0.2f %0.2f\r\n",Angle,Angle_ax,Gyro_y);函數(shù)分別讀取的加速度，角速度和傾角，我發(fā)現(xiàn)當(dāng)我快速的改變板子的傾角的時(shí)候，比如快速變化10度，Angle（卡爾曼濾波后的傾角）瞬時(shí)變化非�？欤赡軙�(huì)瞬間變化幾十度然后回到正常角度，而當(dāng)我緩慢變化10度的時(shí)候，Angle變化是正常線性變化到10度，在這兩種變化中，Angle_ax（從MPU6050讀取的值經(jīng)過處理后的陀螺儀的Y軸數(shù)據(jù)）的變化一直都是線性正常的，并且Angle的值特別接近Angle_ax的值問題：1，我快速改變板子傾角時(shí)Angle的變化正常嗎？ 2，Angle正常變化的時(shí)候是應(yīng)該與Angle_ax的值相近嗎？現(xiàn)在情況就是，就算我是在減小傾角，只要我快速地改變，它顯示的傾角都會(huì)先增大再減小，而如果我慢速改變的話，傾角就會(huì)緩慢減小而不會(huì)出現(xiàn)中間的角度增大 *************讀取數(shù)據(jù)******************** //定義MPU6050內(nèi)部地址 #define SMPLRT_DIV 0x19 //陀螺儀采樣率典型值 0X07 125Hz #define CONFIG 0x1A //低通濾波頻率典型值 0x00 #define GYRO_CONFIG 0x1B //陀螺儀自檢及測(cè)量范圍典型值 0x18 不自檢 2000deg/s #define ACCEL_CONFIG 0x1C //加速度計(jì)自檢及測(cè)量范圍及高通濾波頻率典型值 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)換為實(shí)際值，計(jì)算公式跟寄存器的配置有關(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濾波器采樣時(shí)間; 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; //先驗(yàn)估計(jì) Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分 Pdot[1]= -PP[1][1]; Pdot[2]= -PP[1][1]; Pdot[3]=Q_gyro; PP[0][0] += Pdot[0] * dt; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分 PP[0][1] += Pdot[1] * dt; // =先驗(yàn)估計(jì)誤差協(xié)方差 PP[1][0] += Pdot[2] * dt; PP[1][1] += Pdot[3] * dt; Angle_err = Accel - Angle; //zk-先驗(yàn)估計(jì) 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; //后驗(yàn)估計(jì)誤差協(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; //后驗(yàn)估計(jì) Q_bias += K_1 * Angle_err; //后驗(yàn)估計(jì) Gyro_y = Gyro - Q_bias; //輸出值(后驗(yàn)估計(jì))的微分=角速度 } ******************傾角計(jì)算***************** 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; 時(shí)間dt，所以此處不用積分 /***************************卡爾曼融合*************************************/ Kalman_Filter(Angle_ax,Gyro_y); //卡爾曼濾波計(jì)算傾角