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

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

[复制链接]
思考的大兵 发布时间:2014-4-12 20:54
赞 3 收藏 24 评论180 发布时间:2014-4-12 20:54
180个回答
hxl520521 回答时间:2016-8-6 05:52:43
一下载谢谢了
hxl520521 回答时间:2016-8-6 05:54:15
以下载谢谢了
hxl520521 回答时间:2016-8-7 08:28:48
好厉害,有图纸吗
jj_feng 回答时间:2016-8-9 15:16:58
厉害,谢谢分享
孤独的文艺青年 回答时间: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的值
- q8 a# J, l9 w) o& ]1 ^问题:1,我快速改变板子倾角时Angle的变化正常吗?
: J- `7 @  J( z/ J5 |6 ?- n      2,Angle正常变化的时候是应该与Angle_ax的值相近吗?% U( f; o* A! N) Z

: [* S, s, U" G6 H现在情况就是,就算我是在减小倾角,只要我快速地改变,它显示的倾角都会先增大再减小,而如果我慢速改变的话,倾角就会缓慢减小而不会出现中间的角度增大2 E! U. Y( p, X! [; X. G$ Q2 b

2 O5 l' M3 @8 b4 v4 X, r! _! \+ e% Z5 j% e) E( n# C2 N
*************读取数据********************+ }7 Z1 D+ s, E, @" z) n; k: O
//定义MPU6050内部地址5 \! a# N2 T7 |5 G6 U
#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz7 a- |- a" A% U* `' i. i# i
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
; j$ @+ Q. m7 O* e& M#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s" |$ t1 z% o* u, W0 H
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
( [! \9 h- \9 k  p9 v. ^; q) p#define INT_PIN_CFG     0x37( i& [. `5 Z+ l$ s
#define INT_ENABLE      0x38; |) ~# Q) H" _$ N9 K1 Q( R# b
#define INT_STATUS      0x3A    //只读# i6 P  ?2 o6 G3 e1 r
#define        ACCEL_XOUT_H        0x3B
" V0 N2 T' [) W/ w#define        ACCEL_XOUT_L        0x3C0 i* u1 u+ A" Z. X; E4 a0 i
#define        ACCEL_YOUT_H        0x3D$ d$ ~6 [3 I- N' ], c  G4 i4 [) f
#define        ACCEL_YOUT_L        0x3E  c- z7 \5 O. S  c0 _5 ?. |7 R- h( R, D
#define        ACCEL_ZOUT_H        0x3F  R" ^/ ^: P  h2 K4 q
#define        ACCEL_ZOUT_L        0x406 B9 J/ ~& L. R( \6 \9 @( E
#define        TEMP_OUT_H                0x412 y" ?5 Q0 _! ^% T7 o2 j9 V
#define        TEMP_OUT_L                0x42
/ ^0 g/ F) g) r( s#define        GYRO_XOUT_H    0x43
4 g5 o* |* H: y& ^! D7 w- d#define        GYRO_XOUT_L                0x44       
- A! U3 T7 p! G3 R#define        GYRO_YOUT_H        0x455 m& v4 D; T" K1 r; r# Y( [
#define        GYRO_YOUT_L                0x46+ F$ p; ?# C/ e0 u) j( v5 Z
#define        GYRO_ZOUT_H        0x47: a: [* a& P& j: |( t  E% ^# Z
#define        GYRO_ZOUT_L                0x48
! _: G7 ]: C% y7 Q5 h4 @8 Q! q' s! c4 N! [
//读取寄存器原生数据
8 m- D9 k6 e1 C8 ?, |2 y9 o9 F           
( v7 |! F5 ^. W        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);- n4 t1 }" }1 P. z7 Q6 }, B
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);. ]# a% I# Q$ I! B" a4 Y2 o
        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
: _( H7 k( N4 O        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  8 b$ I+ J" u' {
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
. E6 @1 ~+ H. O8 G) m        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
2 b; f. Y$ r9 G3 V" T        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
4 M2 B' a: |0 y) K       & d' m! L5 U9 j9 I% @; n$ ?  r' ]: x
       ! l4 K6 k/ r- z  Z* c1 n
        //将原生数据转换为实际值,计算公式跟寄存器的配置有关
1 l! \' \7 _* E( y        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0; " O  ?4 Z; w9 a, K! f: R
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0; - G6 f! b- K- A8 S
        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
6 s. r9 b- @# j( G, V              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;    2 r1 A, q7 A( M4 ]. {, Y6 Q/ w* ^! }
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
' t6 {% m% T, Y8 b* s" J        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   % h' q7 |3 s% g/ T
    }
' B9 v6 d0 L) D   
+ D2 u6 u  V) c# g4 D! k- x  U; y

8 U1 Y% w. G7 O& T: B//******卡尔曼参数************
; ^* s6 j+ ~2 n4 G* ^               
4 G9 p2 i/ h- v* C2 ^, K4 G) ?const float Q_angle=0.001;  
" u) @9 A" z: P! ?, Y8 h# S! Uconst float Q_gyro=0.003;
- J9 G2 o7 G( H# v+ @const float R_angle=0.5;% R$ i) B* @4 }
const float dt=0.01;                          //dt为kalman滤波器采样时间;
8 x: K( ^8 v+ m6 n1 tconst char  C_0 = 1;! k+ E' W" X, m) N' T( n& N1 a
float Q_bias, Angle_err;, y% }" }& n% }# L1 h% T  g2 q
float PCt_0, PCt_1, E;
- m6 g9 c+ f! M: X! n" mfloat K_0, K_1, t_0, t_1;1 R7 {. [  ~8 s3 g
float Pdot[4] ={0,0,0,0};  x9 G: t" B+ a- b+ Y# n
float PP[2][2] = { { 1, 0 },{ 0, 1 } };/ \6 N1 v9 y. F

( B7 D# [5 g& C' F/*****************卡尔曼滤波**************************************************/
/ }6 {% v2 v- E9 s4 q3 `void Kalman_Filter(float Accel,float Gyro)               
2 I  x( [! K" I3 R* u. t{& h- g& F# b0 R+ t9 N2 ]! O
        Angle+=(Gyro - Q_bias) * dt; //先验估计( w) a# {$ j9 f; m
       
% V$ d  C3 |) p- v  s        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分' [9 s9 @9 }, F7 V! v6 R
# r9 i" |( |+ i7 l$ a  K
        Pdot[1]= -PP[1][1];
, N$ U$ n& r+ A/ `        Pdot[2]= -PP[1][1];
. v+ a7 S3 d% g/ l, S2 `        Pdot[3]=Q_gyro;, E4 f5 |5 w8 R
        ; P" d/ s0 i- u6 v, j
        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
- ?5 s# ?6 s8 z$ N& p        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
2 N! o5 Y, ?7 U) v6 ]        PP[1][0] += Pdot[2] * dt;
) `3 P% h4 O# W2 M  z' B( t" i        PP[1][1] += Pdot[3] * dt;' X0 W: ^* e( D' a3 f
                5 I9 |3 t# r$ F4 C6 E6 J9 X/ U
        Angle_err = Accel - Angle;        //zk-先验估计. a: R2 a; p% G+ z% Q( f
        8 C6 |( n: X4 @2 U7 U5 }  h
        PCt_0 = C_0 * PP[0][0];- @9 z& `+ ?9 i7 ^
        PCt_1 = C_0 * PP[1][0];
0 }  b- |2 y* y. s          B# D0 ?' z8 g  [0 z2 s1 t0 d2 @+ ]
        E = R_angle + C_0 * PCt_0;
$ N% n+ a) a# u! Q' c6 w        7 v1 b5 U5 n0 l  ]+ @9 \8 p
        K_0 = PCt_0 / E;
# A& Z8 P$ h6 j        K_1 = PCt_1 / E;7 o. a9 @+ N1 g7 W$ G8 D2 N& D( ^
        0 W4 ^( ?* L2 f! W
        t_0 = PCt_0;& ?" ^# G, H3 U3 `. }6 J( S
        t_1 = C_0 * PP[0][1];
. V' e! c1 n$ v% @0 K: X, X: F6 i( l+ g" v# H) z% E4 f
        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
( a# v# \  c+ y* X3 G& C        PP[0][1] -= K_0 * t_1;' a: O2 p# o; X
        PP[1][0] -= K_1 * t_0;
; D& [( C/ W; b7 c6 E        PP[1][1] -= K_1 * t_1;
3 r. }: U# s% d' x2 ]                7 K  E' T% e' O, L
        Angle        += K_0 * Angle_err;         //后验估计8 y7 `- r. }2 ]$ q: E$ O4 _
        Q_bias        += K_1 * Angle_err;         //后验估计* v% R" t. R6 {# ~
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度, E0 e- ]. N$ K. t+ x8 P
0 h3 _/ c4 \3 X. L
}
+ i" {9 Q' J8 N! s, C. ~; K& v5 D0 O: |' D$ ^- X
******************倾角计算****************** g% c  S0 U0 T- q: O$ ]9 b7 L, d
void Angle_Calculate(void)
# o5 R9 h8 l. H- w/ Z3 V% _{
3 Z; e0 h% w9 G" ^$ Q# `! p  d% X/ R( o9 A
/****************************加速度****************************************/
( [2 L, ?) i$ X: d& g- f        8 T' I0 A. ?7 g; v1 ?3 b
        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度
) j$ F  f4 O4 ~9 l, Q        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度
) E, R. {' }& x" r/ X2 L/ J0 e+ g- q2 B& C- |0 T# N5 `- l$ _
/****************************角速度****************************************/
4 t  j; ]& [$ ]: `$ x       
5 d9 Q9 R6 s0 R9 |         Gyro_y = MPU6050_Real_Data.Gyro_Y;             / e( l' Q' v' q6 R
时间dt,所以此处不用积分+ d$ e6 S8 s' @  H: u5 m
/***************************卡尔曼融合*************************************/
  {6 }5 }( d7 m1 f/ ]) e        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角) N2 ?8 d. F1 w$ f. e' Q# }5 b& _
        8 I5 j2 ?# _3 z
0 a& V. M# D$ j% r
jj_feng 回答时间:2016-8-10 14:14:07
谢谢分享
+ J. u! B8 U; y1 Y; i) q7 a
大哥的诱惑 回答时间:2016-8-10 14:43:19
谢谢您的分享,不错的帖子哦!
nashchen17 回答时间:2016-8-11 12:58:14
太感謝了,好一篇分享文章
zbmxu 回答时间:2016-8-11 16:11:33
滤波学习下,谢谢
kernel_sa 回答时间:2016-9-20 01:03:14

/ x+ L) @+ \. C. n: c楼主这么强,学习一下
airuoshahen 回答时间:2016-10-28 11:48:26
好佩服楼主,厉害了我的哥!
悬崖上的废墟 回答时间:2016-11-20 11:45:58
谢谢楼主分享
andypanfan 回答时间:2016-12-9 08:53:35
正需要呢,顶一个!!!
5265325 回答时间:2016-12-9 10:49:51
maiunjour 回答时间:2016-12-9 17:12:03
特来学习~!
关于
我们是谁
投资者关系
意法半导体可持续发展举措
创新与技术
意法半导体官网
联系我们
联系ST分支机构
寻找销售人员和分销渠道
社区
媒体中心
活动与培训
隐私策略
隐私策略
Cookies管理
行使您的权利
官方最新发布
人形机器人运动控制、感知与智能配电
半导体创新技术与应用方向
EE架构与软件定义汽车
12V/48V 汽车智能配电(SPD)
区域控制单元(ZCU)与分区架构
关注我们
st-img 微信公众号
st-img 手机版