PID控制卡爾曼濾波設計實現STM32單片機自平衡小車設計源碼

ID:365793 · 發表于 2018-7-5 18:30

基于單片機平衡車設置
PID控制卡爾曼濾波設計實現自平衡小車設計

單片機源程序如下:

#include "sys.h"
#include "usart.h"
//#include "delay.h"
// 編碼器：100線；電機減速比：150
#include "ofme_led.h"
#include "ofme_filter.h"
#include "ofme_iic.h"
#include "ofme_iic_dev.h"
#include "ofme_pwm.h"
#include "ofme_pid.h"
#include "ofme_encoder.h"
#include "ofme_io.h"
#include "ofme_time.h"
#include "ofme_ir_nec.h"
#define PI (3.14159265)
// 度數表示的角速度*1000
#define MDPS (70)
// 弧度表示的角速度
#define RADPS ((float)MDPS*PI/180000)
// 每個查詢周期改變的角度
#define RADPT (RADPS/(-100))
// 平衡的角度范圍；+-60度(由于角度計算采用一階展開，實際值約為46度)
#define ANGLE_RANGE_MAX (60*PI/180)
#define ANGLE_RANGE_MIN (-60*PI/180)
// 全局變量
pid_s sPID; // PID控制參數結構體
float radian_filted=0; // 濾波后的弧度
accelerometer_s acc; // 加速度結構體，包含3維變量
gyroscope_s gyr; // 角速度結構體，包含3維變量
int speed=0, distance=0; // 小車移動的速度，距離
int tick_flag = 0; // 定時中斷標志
int pwm_speed = 0; // 電機pwm控制的偏置值，兩個電機的大小、正負相同，使小車以一定的速度前進
int pwm_turn = 0; // 電機pwm控制的差異值，兩個電機的大小相同，正負相反，使小車左、右轉向
float angle_balance = 0; // 小車的平衡角度。由于小車重心的偏移，小車的平衡角度不一定是radian_filted為零的時候
// 定時器周期中斷，10ms
void sys_tick_proc(void)
{
static unsigned int i = 0;
tick_flag++;
i++;
if(i>=100) i=0;
if(i==0) // 綠燈的閃爍周期為1秒
{
LED1_OFF();
}
else if(i==50)
{
LED1_ON();
}
}
void control_proc(void)
{
int i = ir_key_proc(); // 將紅外接收到的按鍵值，轉換為小車控制的相應按鍵值。
switch(i)
{
case KEY_TURN_LEFT:
if(pwm_turn<500) pwm_turn += 50;
break;
case KEY_TURN_RIGHT:
if(pwm_turn>-500) pwm_turn -= 50;
break;
case KEY_TURN_STOP:
pwm_turn = 0;
distance = 0;
pwm_speed = 0;
break;
case KEY_SPEED_UP:
if(pwm_speed<500) pwm_speed+=100;
break;
case KEY_SPEED_DOWN:
if(pwm_speed>-500) pwm_speed-=100;
break;
case KEY_SPEED_0:
pwm_speed = 0;
break;
case KEY_SPEED_F1:
pwm_speed = 150;
break;
case KEY_SPEED_F2:
pwm_speed = 300;
break;
case KEY_SPEED_F3:
pwm_speed = 450;
break;
case KEY_SPEED_F4:
pwm_speed = 600;
break;
case KEY_SPEED_F5:
pwm_speed = 750;
break;
case KEY_SPEED_F6:
pwm_speed = 900;
break;
case KEY_SPEED_B1:
pwm_speed = -150;
case KEY_SPEED_B2:
pwm_speed = -300;
case KEY_SPEED_B3:
pwm_speed = -450;
break;
default:
break;
}
pwm_turn *= 0.9; // pwm_turn的值以0.9的比例衰減，使小車在接收到一個轉向信號后只轉動一定的時間后停止轉動。
speed = speed*0.7 +0.3*(encoder_read()); // 定周期（10ms）讀取編碼器數值得到實時速度，再對速度進行平滑濾波
if(speed!=0)
{
printf("speed: %d, dst: %d, pwm: %d \r\n", speed,distance,(int)(speed*6+distance*0.1));
}
encoder_write(0); // 編碼器值重新設為0
distance += speed; // 對速度進行積分，得到移動距離
if(distance>6000) distance = 6000; // 減少小車懸空、空轉對控制的影響
else if(distance<-6000) distance = -6000;
}
void balance_proc(void)
{
static unsigned int err_cnt=0;
// float tFloat;
int pwm_balance;
// static float angle;
// float angle_t;
float radian, radian_pt; // 當前弧度及弧度的微分(角速度，角度值用弧度表示）
adxl345_read(&acc); // 讀取當前加速度。由于傳感器按照的位置原因，傳感器的值在函數內部經過處理，變為小車的虛擬坐標系。
l3g4200d_read(&gyr); // 讀取當前角速度。同樣經過坐標系變換。
// 此段程序用于傳感器出錯時停止小車
err_cnt = err_cnt*115>>7; // err_cnt以0.9的比例系數衰減(115>>7的值約為0.9，避免浮點數，提高速度)
if(acc.flag != 0x0F || gyr.flag != 0x0F) // 讀取的角度、角速度值有誤。可能是電磁干擾、iic線太長等導致出錯。
{
LED0_ON(); // 亮紅燈
err_cnt +=100; // 等比數列，比例系數0.9(115>>7)，常數項100；根據公式，連續10項的和約為657
if(err_cnt>657) goto err; // 當連續發生約10次（約0.1秒）錯誤則超過657而溢出。
}
// 此段程序用于倒立或失重時停止小車
if(acc.z<=0)
{
goto err;
}
// 小車的虛擬x軸方向為小車前進方向，虛擬y軸為小車左邊，虛擬z軸為小車上升方向。
// 前傾角度為負，后傾角度為正。
// 通過計算加速度分量，得到小車傾斜弧度（未濾波）
radian = (float)(acc.x)/acc.z; // 一階展開：Q =f(x)=x-x^3/3+x^5/5+...+(-1)^k*x^(2k+1)/(2k+1)+...
// 通過角速度傳感器，得到小車的角速度（單位為弧度/秒）
radian_pt = gyr.y*RADPT;
radian_filted = ofme_filter(radian_filted, radian, radian_pt); // 互補濾波得到小車的傾斜角度
// 此段程序用于小車傾斜角度過大時，停止小車
if(radian_filted> ANGLE_RANGE_MAX || radian_filted<ANGLE_RANGE_MIN)
{
goto err;
}
// 通過PID計算，得到保持小車角度為零所需要的電機pwm輸出
pwm_balance = pid_proc(&sPID, radian_filted, radian_pt);
// printf("%d\r\n",speed);
// 通過小車移動速度與移動距離，調整小車平衡所需的pwm輸出
pwm_balance += (speed*6+distance*0.1);
// 在pwm_balance的基礎上，加上速度分量與轉動分量，調整小車兩個電機的轉速。
pwm_control(pwm_balance+pwm_speed+pwm_turn, pwm_balance+pwm_speed-pwm_turn);
// 如果pwm超出有效值，紅燈亮。用于調試，了解系統狀態。
if(pwm_balance>=1000||pwm_balance<=-1000) LED1_ON();
LED0_OFF();
return;
err:
puts("balance error.\r\n");
pwm_control(0, 0); // 關閉電機
return;
}
int main(void)
{
// int i=0, t;
// int pwm;
// float radian, radian_pt;
Stm32_Clock_Init(9);//系統時鐘設置
uart_init(72,57600); //串口1初始化
hw_tick_start(); // 定時器周期性中斷，用于提供系統脈搏
////////////////////////////////////////////////////////////////////////////////
led_init();
pwm_init();
iic_init();
adxl345_init(&acc);
l3g4200d_init(&gyr);
hw_ir_init();
encoder_init();
while(0)
{
if(tick_flag>100)
{
tick_flag = 0;
printf("count: %d\r\n",encoder_read());
}
}
////////////////////////////////////////////////////////////////////////////////
// pid_init(&sPID, 4500,0,-300);6000--350 ;8000--350;11000--350;
// pid_init(&sPID, 6000,0,-35000);
pid_init(&sPID, 7500,0,-35000); // 調節PID參數，后3個形參分別為：比例系數P，積分系數I，微分系數D
sPID.target = -3.5*PI/180;
radian_filted = 0;
adxl345_init(&acc);
l3g4200d_init(&gyr);
while(1)
{
if(tick_flag)
{
tick_flag = 0;
balance_proc(); // 調節小車，保持平衡
control_proc(); // 根據遙控接收到的數據，調整電機輸出參數
}
}
}