你的浏览器版本过低,可能导致网站不能正常访问!
为了你能正常使用网站功能,请使用这些浏览器。

【MCU实战经验】+基于stm32两轮平衡车制作  

[复制链接]
思考的大兵 发布时间:2014-4-12 20:54
在此贴上以前做过的平衡小车资料,一来供众网友学习交流,二来申请stm32f429探索套件。。。: w, R/ u& O3 J9 T7 Q
先上图% l9 P( f8 H8 T* g
wifi0s0-806875369IMG_20140412_203839.jpg
9 q2 M) i% _/ ~; q+ c6 `. _原材料清单:( U% v! S3 R1 o
——————————————————————————————————————————# z& J$ n  E' G- w+ T$ j/ |
主控:stm32f103rct6
" b4 v7 L' t' ~$ Q传感器:mpu6050(带陀螺仪和加速度)/ D  ~5 b6 I: t6 v; \% r
电机驱动芯片:l298n0 O( G3 `0 A1 K5 l
电机:冯哈伯2224& y: @9 f' A; i/ v+ A
编码器:电机自带16线
5 _+ z% q: Z# O5 a电池:两节186503 ]& c$ B. N+ ]1 K% p
轮子架子:淘宝店购买,减速比1:5
# [, p/ a# m+ n" j& w; _  [——————————————————————————————————————————# B2 B6 J/ T* x* V' B* G7 K
处理过程:3 b3 x! f  d. Q2 v/ b8 C/ A
——————————————————————————————————————————
; A9 m3 ^3 t; q5 q+ _其实程序过程很简单,先是采集传感器MPU6050的数据,并进行加工,获得当前小车倾斜角,然后根据此角度做PID调节,得到小车两个电机的PWM脉宽,调整轮子速度,使之回到倾角为0的状态,即保持平衡。然后就不停地重复采集->处理->调节->处理这一过程。
$ L* ]' {, B3 F在此基础上,附加两电机的PWM值,即可实现前进,倒退,左转,右转动作
, @/ E# f# X( @7 M5 t4 }! `2 u% J——————————————————————————————————————————: i- A* j) Q5 n; o
难点分析:' O6 V# p( c7 V, V5 F! @! f9 J9 k+ R
——————————————————————————————————————————
* F5 M' z" ^2 E. e4 i6 @有两个难点. C  }/ t" h4 i# N: y+ }
1.传感器的数据处理是小车的最大难点。一是传感器受震动影响很大,很容易超量程;二是传感器数据漂移比较厉害。而关键的小车倾角就来源于此传感器数据。为此,参考网上数据滤波处理方式,采用卡尔曼滤波算法,融合陀螺仪和加速度传感器数据得出小车倾角。
/ {  g: Y* j0 k0 m$ F% r: ^& T: a2.PID,比例系数的调节不用多讲,需要耐心的测试和调节。我要说的是,单纯靠倾角做PID不能使小车平衡,因为当小车倾角接近0度时,改变的PWM值无法使小车越过平衡点,造成小车朝一边加速前进,所以要将速度也添加至PID调节中,做积分
+ Z9 d! K' s0 R) Z3 z——————————————————————————————————————————$ 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
——————————————————————————————————————————
) N3 M) H# G; @" v+ f 卡尔曼滤波.rar (2.76 MB, 下载次数: 12100)
wifi0s0-270124067IMG_20140412_203825.jpg
wifi0s0-1287192638IMG_20140412_203903.jpg
wifi0s0-1379330287IMG_20140412_204252.jpg
wifi0s0690935772IMG_20140412_204349.jpg

平衡小车.rar

下载

3.02 MB, 下载次数: 54095

评分

参与人数 1 ST金币 +5 收起 理由
努力的人 + 5 感谢分享

查看全部评分

赞 3 收藏 24 评论180 发布时间:2014-4-12 20:54

举报

180个回答
青春不再荡漾 回答时间:2014-9-16 11:32:17

回复:【MCU实战经验】+基于stm32两轮平衡车制作

楼主有几个问题请教下,您用的编码器是16线的,是不是接线就三根呢,一根电源,一根接地,还有一根信号线,可是我从你的程序中看不出来信号线是接几号引脚啊?其二,楼主的PID看上去没有用增量式或者位置式的,PWM是直接根据KP*角度+KD*速度计算出来的,这样的话这个PWM是不是不准,不知楼主最后的效果咋样?求回复哈
heheqian 回答时间:2014-8-26 14:07:09

回复:【MCU实战经验】+基于stm32两轮平衡车制作

  又强又好,顶!
孤独的文艺青年 回答时间:2016-8-9 18:34:51
我用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的值& R& [" U" p- i( F( ?
问题:1,我快速改变板子倾角时Angle的变化正常吗?
  {/ Z/ s5 X8 n" T! E" d9 P# e0 G      2,Angle正常变化的时候是应该与Angle_ax的值相近吗?, e% ~+ b# p4 R1 A& C; M

* Y% d- a) X8 d  E现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大/ f9 z* w; e0 x  q

  y0 u, ?- V# D4 B! ]7 d' q8 F
9 |5 P. r. u  V6 ^) [3 ]2 Y0 ^*************读取数据********************
0 t2 v" P, z1 i: Q/ S  ?1 b& L//定义MPU6050内部地址
5 e4 P1 X! v6 @1 U# J: Y6 X0 `7 E+ f# o- G#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz9 B: `3 ]# J8 l' U7 z6 A
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
1 E& K: h  `' M; d#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s5 q$ }- I4 r8 w2 s( J
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
# @+ y. p8 K3 Y#define INT_PIN_CFG     0x37
1 m; z4 \" |# ~# [; z0 q#define INT_ENABLE      0x38
' Z7 H  r+ R: D' T5 r, D#define INT_STATUS      0x3A    //只读0 ^$ m; e6 F6 g% d1 x
#define        ACCEL_XOUT_H        0x3B
" `0 ]1 c- h$ Z. |#define        ACCEL_XOUT_L        0x3C
7 w( J1 c6 {2 d, s8 g( M- R2 Z+ J  s& z#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
5 r+ O' o& p" i6 U0 _  q#define        TEMP_OUT_H                0x41
& Q! H) h& t$ I/ x( `#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
% x6 S) s0 C3 k& [' `#define        GYRO_YOUT_L                0x46
) e: O2 i2 C3 d% x#define        GYRO_ZOUT_H        0x47/ W. K* y$ d9 V1 C" i
#define        GYRO_ZOUT_L                0x481 P: h2 U" d" K! o

$ z# Q5 `) q- w) i//读取寄存器原生数据" 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]);  
0 `) b  F; R) H! M* q  A9 s        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
      
# F9 W( X( ?3 q      
# I) f* N' I4 B* F7 k1 A1 l) a1 w        //将原生数据转换为实际值,计算公式跟寄存器的配置有关
! M( \! u* V: l: X3 @/ a9 H        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
! g0 {7 A3 f! q        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
! o4 F2 W! D2 k1 \        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;   
, K: y- j- T: @! ]# {: ?$ ~        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
3 C/ C  l: T( V. ^9 g! t5 L5 ]) N$ h    }
1 d7 `" m" p2 E5 B+ y9 ^/ k8 B   
$ a- e/ C, `1 d
* M  x# u5 I' `# W% E* M; Z- w& e1 _2 _1 k
//******卡尔曼参数************" n/ [$ y% d! f3 e( [. T- J  }
               
) T5 l, u$ l3 K! rconst float Q_angle=0.001;  
0 {6 ?/ q/ q; y8 r* K1 Z$ qconst float Q_gyro=0.003;3 w: J6 T0 u* m+ W' ~
const float R_angle=0.5;
4 E7 }0 W- e8 L; Xconst 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;
8 R3 R9 `, C8 [4 B# J3 k) Zfloat PCt_0, PCt_1, E;
: G7 A) N! ]3 v9 u( }1 O2 k5 m. x% Zfloat K_0, K_1, t_0, t_1;* ?1 c7 T  p7 |# r3 e+ e/ S$ l
float Pdot[4] ={0,0,0,0};
' t2 e8 D8 ~5 C. a7 w# C' o- T" c0 _9 Lfloat PP[2][2] = { { 1, 0 },{ 0, 1 } };
, p% Y# \# _1 k. D. \6 C
$ l" _& k+ a9 z4 j/*****************卡尔曼滤波**************************************************/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
{
3 I$ O( [" H& f: X% u/ d/ T        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-先验估计误差协方差的微分
& X( z9 Z$ q( A& l8 ]8 b( ^
% e  e& c5 O. A        Pdot[1]= -PP[1][1];
6 X; o, s& |& S; o) W1 Q. ~        Pdot[2]= -PP[1][1];& p- T) M: u8 g, |, ]
        Pdot[3]=Q_gyro;! ^# h6 J# S4 D2 U9 P  C
       
1 c. l3 J) x, K9 q! I) ]        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
- [* |9 Z( W+ q        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
, Y: [$ v/ d) P  L2 i        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
               
8 Z# W0 O8 Q5 s" y" V2 h        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
       
2 U8 l  L+ P) _! S: o) U        K_0 = PCt_0 / E;" l% S9 s7 R7 i% J3 a$ u! G
        K_1 = PCt_1 / E;
5 a% v$ @& O; w        # 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];
8 i# c, g1 ^6 O
  W4 S' x& f. A        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
1 Q% d6 K2 f0 H6 H' ^* S        PP[0][1] -= K_0 * t_1;
( F9 x7 H& ~# n        PP[1][0] -= K_1 * t_0;
* q3 m3 f$ y4 l' n        PP[1][1] -= K_1 * t_1;
' w, j/ _- X9 s! [0 |' I3 B6 b               
9 u' g% f- Q7 J- r- Q6 N        Angle        += K_0 * Angle_err;         //后验估计% k8 o: f0 d( }# N% T3 b  z
        Q_bias        += K_1 * Angle_err;         //后验估计
7 [" s2 P( E9 I6 @3 F1 t        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度1 \4 q0 e  y* o5 O) B  W
! d0 _; L0 E, U; l9 W$ C" i5 g
}
" {2 L( ?) ]! k) r+ q& i1 c# _% X4 C, e; u6 i
******************倾角计算*****************
7 k: z$ u" o2 P! U( T6 rvoid Angle_Calculate(void)
4 c$ Y1 Z3 Y+ e: r3 I{
& E: l) d" |7 z4 k6 |+ y& ]" n) W; O/ P# X& V4 W
/****************************加速度****************************************/
# [& q2 d: u- ?( O       
$ C3 ^6 q: {8 m' t) h* _# ^        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 \

& _' o# O2 P; n/****************************角速度****************************************/
- S" u3 i) J- G9 H5 e2 l        * N+ J5 h3 R9 t3 o
         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
/ o5 j3 G0 X( o  k) K时间dt,所以此处不用积分( U% H2 ?" Y* Z- n
/***************************卡尔曼融合*************************************/& b8 z" V  \5 M: ?7 y1 d# j5 H
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
& [) k5 R9 J9 ~& Z        + K0 {$ W9 a/ q8 d
' y: L  v; u5 J6 w
大米周822 回答时间:2014-4-12 21:39:32

回复:【MCU实战经验】+基于stm32两轮平衡车制作

顶下,楼主好人。。。
疯子咬手指 回答时间:2014-4-12 22:19:27

RE:【MCU实战经验】+基于stm32两轮平衡车制作

收了  感谢楼主
思考的大兵 回答时间:2014-4-13 08:48:38

回复:【MCU实战经验】+基于stm32两轮平衡车制作

回复第 3 楼 于2014-04-12 22:19:27发表:
& I& x4 B) v, R+ \8 U6 y+ e收了  感谢楼主 
& R0 A# `: m7 G0 S" Z, I* l. u. q
客气。。。
疯子咬手指 回答时间:2014-4-21 19:00:34

RE:【MCU实战经验】+基于stm32两轮平衡车制作

阔出去了  定起
robot0612 回答时间:2014-5-6 21:19:47

RE:【MCU实战经验】+基于stm32两轮平衡车制作

还不错
123de7 回答时间:2014-7-1 21:02:47

回复:【MCU实战经验】+基于stm32两轮平衡车制作

正需要呢,顶一个!!!
刘源爱电子 回答时间:2014-7-17 17:52:20

回复:【MCU实战经验】+基于stm32两轮平衡车制作

感谢
zhangkongziwo 回答时间:2014-8-3 16:39:17

回复:【MCU实战经验】+基于stm32两轮平衡车制作

 
chunyangjs 回答时间:2014-8-8 12:15:39

RE:【MCU实战经验】+基于stm32两轮平衡车制作

顶下,楼主好人。。。
熊二在深圳 回答时间:2014-8-8 14:53:43

RE:【MCU实战经验】+基于stm32两轮平衡车制作

这个是高大上的项目。赞一个
hengchu 回答时间:2014-8-10 01:15:37

RE:【MCU实战经验】+基于stm32两轮平衡车制作

做了好久,就是滤波不懂,楼主好人,愿好人一生平安
wqlovt 回答时间:2014-9-16 13:24:48

RE:【MCU实战经验】+基于stm32两轮平衡车制作

感谢分享!!!!!!!!!!!!!
关于
我们是谁
投资者关系
意法半导体可持续发展举措
创新与技术
意法半导体官网
联系我们
联系ST分支机构
寻找销售人员和分销渠道
社区
媒体中心
活动与培训
隐私策略
隐私策略
Cookies管理
行使您的权利
官方最新发布
人形机器人运动控制、感知与智能配电
半导体创新技术与应用方向
EE架构与软件定义汽车
12V/48V 汽车智能配电(SPD)
区域控制单元(ZCU)与分区架构
关注我们
st-img 微信公众号
st-img 手机版