|
在此贴上以前做过的平衡小车资料,一来供众网友学习交流,二来申请stm32f429探索套件。。。: w, R/ u& O3 J9 T7 Q 先上图% l9 P( f8 H8 T* g
原材料清单:( U% v! S3 R1 o ——————————————————————————————————————————# z& J$ n E' G- w+ T$ j/ | 主控:stm32f103rct6 传感器:mpu6050(带陀螺仪和加速度)/ D ~5 b6 I: t6 v; \% r 电机驱动芯片:l298n0 O( G3 `0 A1 K5 l 电机:冯哈伯2224& y: @9 f' A; i/ v+ A 编码器:电机自带16线 电池:两节186503 ]& c$ B. N+ ]1 K% p 轮子架子:淘宝店购买,减速比1:5 ——————————————————————————————————————————# B2 B6 J/ T* x* V' B* G7 K 处理过程:3 b3 x! f d. Q2 v/ b8 C/ A —————————————————————————————————————————— 其实程序过程很简单,先是采集传感器MPU6050的数据,并进行加工,获得当前小车倾斜角,然后根据此角度做PID调节,得到小车两个电机的PWM脉宽,调整轮子速度,使之回到倾角为0的状态,即保持平衡。然后就不停地重复采集->处理->调节->处理这一过程。 在此基础上,附加两电机的PWM值,即可实现前进,倒退,左转,右转动作 ——————————————————————————————————————————: i- A* j) Q5 n; o 难点分析:' O6 V# p( c7 V, V5 F! @! f9 J9 k+ R —————————————————————————————————————————— 有两个难点. C }/ t" h4 i# N: y+ } 1.传感器的数据处理是小车的最大难点。一是传感器受震动影响很大,很容易超量程;二是传感器数据漂移比较厉害。而关键的小车倾角就来源于此传感器数据。为此,参考网上数据滤波处理方式,采用卡尔曼滤波算法,融合陀螺仪和加速度传感器数据得出小车倾角。 2.PID,比例系数的调节不用多讲,需要耐心的测试和调节。我要说的是,单纯靠倾角做PID不能使小车平衡,因为当小车倾角接近0度时,改变的PWM值无法使小车越过平衡点,造成小车朝一边加速前进,所以要将速度也添加至PID调节中,做积分 ——————————————————————————————————————————$ J: e% b* q$ ?/ {! p 制作心得:2 z! x& A2 N7 F$ o" {4 I: J, M ——————————————————————————————————————————1 s( j$ O* f! x$ ?9 H; T 关于平衡小车,大多数网友应该都曾做过或者想过要做。于是百度搜资料,其中最多的应该是飞思卡尔直立车的资料,当初我也看下下,平衡车的原理都懂了,但那时还没搞过PID调节和数据滤波,所以具体怎么写程序并不知道。至于平衡车的程序资料也非常少。后来在网上参考相关数据滤波,PID调节后,才渐渐掌握。在此我也就贴上我的程序代码,供大家学习交流,少走弯路。有什么问题,欢迎回帖。当然也希望大家多支持此贴,祝我申请F429套件成功!哈哈 3 S. U8 u6 U! U——————————————————————————————————————————
å¡å°æ¼æ»¤æ³¢.rar
(2.76 MB, 下载次数: 12100)
|
微信公众号
手机版
回复:【MCU实战经验】+基于stm32两轮平衡车制作
回复:【MCU实战经验】+基于stm32两轮平衡车制作
问题:1,我快速改变板子倾角时Angle的变化正常吗?
2,Angle正常变化的时候是应该与Angle_ax的值相近吗?, e% ~+ b# p4 R1 A& C; M
现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大/ f9 z* w; e0 x q
*************读取数据********************
//定义MPU6050内部地址
#define SMPLRT_DIV 0x19 //陀螺仪采样率 典型值 0X07 125Hz9 B: `3 ]# J8 l' U7 z6 A
#define CONFIG 0x1A //低通滤波频率 典型值 0x00
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围 典型值 0x18 不自检 2000deg/s5 q$ }- I4 r8 w2 s( J
#define ACCEL_CONFIG 0x1C //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define INT_STATUS 0x3A //只读0 ^$ m; e6 F6 g% d1 x
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D( q4 g' Z/ u$ [( g4 y9 @1 ~. A
#define ACCEL_YOUT_L 0x3E& m6 T+ l0 n' W
#define ACCEL_ZOUT_H 0x3F* s8 T4 h7 g9 R7 E4 i8 Z! H
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42: J6 A. J, e% ^" T- z
#define GYRO_XOUT_H 0x43+ ~1 V+ ?* S0 C( K& u* {) u2 ~$ T
#define GYRO_XOUT_L 0x44 8 v6 g# f5 o3 N* c$ Y2 @/ h
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47/ W. K* y$ d9 V1 C" i
#define GYRO_ZOUT_L 0x481 P: h2 U" d" K! o
//读取寄存器原生数据" P& v" d2 B z0 W) \* N% `1 t+ u
/ d+ s: e% E, E& n) T
MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);% o/ V% G2 u5 b0 {$ K+ i; S
MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);; O* [7 L: h: K
MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]); 9 K1 [8 m& K9 M4 f/ G! `$ L
MPU6050_Raw_Data.Temp = (buf[6]<<8 | buf[7]);
MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);/ H8 f; j2 l* y6 q4 Y& c! B4 O
MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);% \# a7 U5 s- n: L0 u
MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);& d9 Y" t* I# [* \7 T! h
//将原生数据转换为实际值,计算公式跟寄存器的配置有关
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; # J0 \- E, Q3 u0 B& j2 y; ?
MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5; 1 t& e7 I1 H. t. z8 I! U
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;
}
% E* M; Z- w& e1 _2 _1 k
//******卡尔曼参数************" n/ [$ y% d! f3 e( [. T- J }
const float Q_angle=0.001;
const float Q_gyro=0.003;3 w: J6 T0 u* m+ W' ~
const float R_angle=0.5;
const float dt=0.01; //dt为kalman滤波器采样时间;: v% l: |5 l, h/ \' B' O" u
const char C_0 = 1;4 L$ e, n J s! Y0 H' g) z) `
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;* ?1 c7 T p7 |# r3 e+ e/ S$ l
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };
/*****************卡尔曼滤波**************************************************/6 J y; x& U9 m8 [0 q q4 F
void Kalman_Filter(float Accel,float Gyro) : D g+ o) q5 |9 `: Q+ v* V
{
Angle+=(Gyro - Q_bias) * dt; //先验估计$ J# S$ b ^6 t& c% P
( |4 x9 y+ A, D( t) c& ^
Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
Pdot[1]= -PP[1][1];
Pdot[2]= -PP[1][1];& p- T) M: u8 g, |, ]
Pdot[3]=Q_gyro;! ^# h6 J# S4 D2 U9 P C
PP[0][0] += Pdot[0] * dt; // Pk-先验估计误差协方差微分的积分
PP[0][1] += Pdot[1] * dt; // =先验估计误差协方差
PP[1][0] += Pdot[2] * dt;% v) T( l: @+ m- v# q
PP[1][1] += Pdot[3] * dt;3 Y/ H$ q* u V* O/ W
Angle_err = Accel - Angle; //zk-先验估计3 a2 b/ I6 E( l$ A7 ~. T+ D8 Y) ]
: _' S; t. _6 n) R
PCt_0 = C_0 * PP[0][0];: M0 _$ ~+ [0 ^5 X1 E
PCt_1 = C_0 * PP[1][0];2 L+ O; I$ z/ V" }* K C! u
- N- h. Y1 K- K, P
E = R_angle + C_0 * PCt_0;3 U5 B0 y! U4 ~! `" z/ t
K_0 = PCt_0 / E;" l% S9 s7 R7 i% J3 a$ u! G
K_1 = PCt_1 / E;
# N# b/ u# \: R' V. p& Z! b
t_0 = PCt_0;) [+ U' w' y2 t8 D m7 c: }. U' v
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; //后验估计% k8 o: f0 d( }# N% T3 b z
Q_bias += K_1 * Angle_err; //后验估计
Gyro_y = Gyro - Q_bias; //输出值(后验估计)的微分=角速度1 \4 q0 e y* o5 O) B W
! d0 _; L0 E, U; l9 W$ C" i5 g
}
% X4 C, e; u6 i
******************倾角计算*****************
void Angle_Calculate(void)
{
" n) W; O/ P# X& V4 W
/****************************加速度****************************************/
Accel_x = MPU6050_Real_Data.Accel_X; //读取X轴加速度5 ^2 Q: F# M4 {5 s$ F- H
Angle_ax = Accel_x*1.2*180/3.14; //弧度转换为度' r7 v |5 M2 N3 V$ `7 K6 o; k( r6 \
/****************************角速度****************************************/
* N+ J5 h3 R9 t3 o
Gyro_y = MPU6050_Real_Data.Gyro_Y;
时间dt,所以此处不用积分( U% H2 ?" Y* Z- n
/***************************卡尔曼融合*************************************/& b8 z" V \5 M: ?7 y1 d# j5 H
Kalman_Filter(Angle_ax,Gyro_y); //卡尔曼滤波计算倾角
+ K0 {$ W9 a/ q8 d
' y: L v; u5 J6 w
回复:【MCU实战经验】+基于stm32两轮平衡车制作
RE:【MCU实战经验】+基于stm32两轮平衡车制作
回复:【MCU实战经验】+基于stm32两轮平衡车制作
收了 感谢楼主
" Z, I* l. u. q
客气。。。
RE:【MCU实战经验】+基于stm32两轮平衡车制作
RE:【MCU实战经验】+基于stm32两轮平衡车制作
回复:【MCU实战经验】+基于stm32两轮平衡车制作
回复:【MCU实战经验】+基于stm32两轮平衡车制作
回复:【MCU实战经验】+基于stm32两轮平衡车制作
RE:【MCU实战经验】+基于stm32两轮平衡车制作
RE:【MCU实战经验】+基于stm32两轮平衡车制作
RE:【MCU实战经验】+基于stm32两轮平衡车制作
RE:【MCU实战经验】+基于stm32两轮平衡车制作