亚博智能论坛  
  
查看: 573|回复: 2

MPU6050 姿态融合

  [复制链接]

该用户从未签到

5

主题

10

帖子

29

积分

新手上路

Rank: 1

积分
29
发表于 2017-8-12 17:53:16 | 显示全部楼层 |阅读模式

首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

本帖主要介绍三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波算法。

) P- y- n/ x8 r* q+ Z) Z6 L' u
1 C1 p* s' p  R! B# {
2 Y/ t- A, @2 j" N- u. R$ P2 J- V' n- n

一、四元数法

关于四元数的一些概念和计算就不写上来了,我也不懂。我能告诉你的是:通过下面的算法,可以把六个数据转化成四元数(q0、q1、q2、q3),然后四元数转化成欧拉角(P、R、Y 角)。


/ q* {0 t" F7 ~. m$ A
8 ~7 ?. ]8 i5 F1 P) x4 F" ^

虽然 MPU6050 自带的 DMP库可以直接输出四元数,减轻 STM32 的运算负担, 这里在此没有使用,因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的(http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735),DMP库需要对 I2C 函数进行修改,如 DMP 库中的 I2C 写:i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]));有4个输入变量,而 STM32 硬件 I2C 的 I2C 写为:void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr),只有 3 个输入量(这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的),所以必须进行修改,但是改固件库是一件很痛苦的事,你们应该都懂。当然,如果你用模拟 I2C 的话,是容易实现的,网上的 DMP 移植几乎都是基于模拟 I2C 的。

  1. #include<math.h>* }% o: M5 k/ V! x) h
  2. #include "stm32f10x.h"9 l6 b  E% L5 N- v
  3. //---------------------------------------------------------------------------------------------------; b9 [8 I1 h: C/ t" i0 q
  4. // 变量定义. V2 k5 I7 p9 j

  5. 1 A5 |- J& ]/ i. \; A
  6. #define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计
    % h9 G9 p7 _& ?/ [7 `* z! J
  7. #define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接
    5 K9 J& L, t7 m% {6 D$ E
  8. #define halfT 0.001f                // 采样周期的一半  f& F3 s0 _; g( T

  9. : C( l2 Z% ]2 u6 I: D8 F1 B
  10. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素,代表估计方向, e7 G) [, O8 d- z# n3 n0 r: m9 L
  11. float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差" x9 P: G; D8 [! k- U5 u* X, z
  12. % ^1 ~/ u' D5 K" M
  13. float Yaw,Pitch,Roll;  //偏航角,俯仰角,翻滚角% @" o" S0 J" y

  14. 0 X$ W/ t& q: s% o+ }

  15. 5 S, N7 X- I. `2 U1 m# b
  16. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)% S3 D. o, T: p1 ?8 F
  17. {+ J. O  E* K6 C$ z$ w
  18.         float norm;
    % M: `) J% S$ N! t/ ^
  19.         float vx, vy, vz;
    $ F" m, D( t7 b% _% t! ~
  20.         float ex, ey, ez;  
    1 ^5 {( |, q' P8 }
  21. 6 s+ W! P& @3 J% U$ S" a
  22.         // 测量正常化
    4 g7 |5 W, }- ?; |& g0 ^! B
  23.         norm = sqrt(ax*ax + ay*ay + az*az);      
    / `# L9 r6 W( w! a1 Z0 y* A" f
  24.         ax = ax / norm;                   //单位化
    # D& G( `& K" G, v
  25.         ay = ay / norm;) H  O9 B# P2 _
  26.         az = az / norm;      0 ?: W; n/ }) [
  27. , a; m! ?$ N. W& J/ K( [
  28.         // 估计方向的重力9 E* I/ w- z' p: Y* I
  29.         vx = 2*(q1*q3 - q0*q2);
    . @0 m4 G* o1 t! n/ ^5 _0 U
  30.         vy = 2*(q0*q1 + q2*q3);
    : A4 c) b% a- J( ?0 M+ Q
  31.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;' X# @. D, g6 ]$ n

  32. ) F, x7 D! R/ @5 `
  33.         // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
    - _- a( T2 Z& t3 B4 U" O" L
  34.         ex = (ay*vz - az*vy);+ F) y: l* S- @
  35.         ey = (az*vx - ax*vz);
    8 \' I3 `7 D1 \! A' q, H/ K
  36.         ez = (ax*vy - ay*vx);& ~+ g7 z- G, Y) E- b; c/ l8 t

  37. 4 L. }. O4 F9 O, S
  38.         // 积分误差比例积分增益
    5 D* k5 T+ y6 s8 r5 B0 r) b& e. I
  39.         exInt = exInt + ex*Ki;" @% Z' J8 I  m, F! V
  40.         eyInt = eyInt + ey*Ki;
    8 O0 H' p( u9 S# r/ X
  41.         ezInt = ezInt + ez*Ki;
    9 M  o5 @1 g0 X  g+ L( x

  42. % z/ y! i- I! W* y, M/ h/ w
  43.         // 调整后的陀螺仪测量3 _, S5 ]# L  j4 [$ c/ j4 B) S
  44.         gx = gx + Kp*ex + exInt;
    * a  ~! s; X, g
  45.         gy = gy + Kp*ey + eyInt;8 W5 r! `$ [7 b2 `! N
  46.         gz = gz + Kp*ez + ezInt;
    ' I0 h% h* u+ Y! a, y. P3 _

  47. 1 w7 }$ k" k& m# F. v5 y: g* U- p
  48.         // 整合四元数率和正常化
    ! F& k: a& v) |' P9 h, H6 ~; y5 J3 E
  49.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    & L( h) s- }9 W* P, W9 [: _
  50.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    8 F2 T) J: B* N& [8 X
  51.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;7 n# L& o1 C: r& }0 h
  52.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  & H9 P- \5 m" q; Y

  53. , r* K. S: I2 r
  54.         // 正常化四元
    / I9 u( B7 i1 a
  55.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    ) r3 K# f! @/ D  B3 h) u/ m
  56.         q0 = q0 / norm;
    : f6 @: P6 W, F
  57.         q1 = q1 / norm;8 p8 c/ C+ Y7 B; T* |
  58.         q2 = q2 / norm;% `6 m# B5 g) F5 t
  59.         q3 = q3 / norm;6 Z8 ]: K; m  E9 w/ |

  60. ' T1 A, @3 @# C* q7 H& }0 C6 @, u
  61.         Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
    % z4 W3 W2 M: c$ N4 N
  62.         Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv2 }8 Y- f6 ?  o0 A
  63.         //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值,注掉
    # [7 y" E$ _$ ?, M8 k% D1 @
  64. }, |% o, B7 b! L; R
复制代码

要注意的的是,四元数算法输出的是三个量 Pitch、Roll 和 Yaw,运算量很大。而像平衡小车这样的例子只需要一个角(Pitch 或 Roll )就可以满足工作要求,个人觉得做平衡小车最好不用四元数法。


+ [$ {  A) Y1 b  E0 [6 x

二、一阶互补算法

MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角(加速度不能得到 Yaw 角),就是说有两组 Pitch、Roll 角,到底应该选哪组呢?别急,先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,且通过算 tan()  可以得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以,不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角,需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值,把它们结合到一起,进行修正。得到 Pitch 角的程序如下:

  1. //一阶互补滤波
    4 g7 H6 j& b. L
  2. float K1 =0.1; // 对加速度计取值的权重
    + A5 q8 F/ X1 e7 h, b- Q" f
  3. float dt=0.001;//注意:dt的取值为滤波器采样时间
    # q5 a; ~$ e7 |  G, D9 h
  4. float angle;
    ; c# u6 B( \( @' \# z

  5. " y  E! L+ c9 v1 K/ l
  6. angle_ax=atan(ax/az)*57.3;     //加速度得到的角度
    6 x3 t/ B) @* J1 ]: H- X5 h
  7. gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度4 H8 T+ L, H9 e2 N" F
  8. Pitch = yijiehubu(angle_ax,gy);
    . z' i2 z" T& C1 `7 j
  9. 0 t. D- `# ]6 _2 U6 L; K2 d
  10. float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度
    " W* `2 m$ a  B0 k9 f$ o/ O
  11. {
    ; K8 a$ p2 ?/ v: r  a8 S: O" ^
  12.      angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
    . q0 _, f# T4 {" `
  13.      return angle;6 @' d. ]8 a- i, ~' e% ?
  14. }
    # J6 }  t! s& |- V0 G
复制代码

互补算法只能得到一个倾角,这在平衡车项目中够用了,而在四轴飞行器设计中还需要 Roll 和 Yaw,就需要两个 互补算法,我是这样写的,注意变量不要搞混:

  1. //一阶互补滤波, [2 D4 |) j! |
  2. float K1 =0.1; // 对加速度计取值的权重; }# M2 e  Q3 B! a$ w+ }
  3. float dt=0.001;//注意:dt的取值为滤波器采样时间
    / y) N- a, w4 D& L" S
  4. float angle_P,angle_R;
    & z: V0 k% V+ ~9 @* Q/ V

  5. * V; r) {: D, `) E6 ~2 e
  6. ! Y% y6 X" X5 _5 O( A0 V0 x; x
  7. float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度$ u- t% {- G& `. U
  8. {
    & q3 o. m( a8 A5 o0 u5 P
  9.      angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
    ; n' ~5 d4 U* o, v% X  R% B7 o
  10.          return angle_P;7 M5 E6 ^% T4 R; a6 e1 x
  11. }& a! @4 G' w8 L. d* T5 c
  12. 9 _: g" W: C6 i4 _
  13. float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度
    - V, ~7 d( M9 W- H
  14. {
    4 z! v! e5 D3 P8 S, v
  15.      angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);
    2 O( y3 L( c8 ?4 i6 ?" l: ^
  16.          return angle_R;
    / b4 @3 K6 x3 O# W7 L
  17. }
复制代码

单靠 MPU6050 无法准确得到 Yaw 角,需要和地磁传感器结合使用。


2 i9 ^% O4 l( R( N) `6 u7 @4 b8 h
回复

使用道具 举报

该用户从未签到

5

主题

10

帖子

29

积分

新手上路

Rank: 1

积分
29
 楼主| 发表于 2017-8-12 17:53:37 | 显示全部楼层
本帖最后由 haihuawu 于 2017-8-12 17:55 编辑
4 Y4 t1 {7 s% g4 M. V' ^; b$ U
. m6 [5 v4 N) X2 g2 p4 C三、卡尔曼滤波2 R6 Z& O3 \, C; f+ F
其实卡尔曼滤波和一阶互补有些相似,输入也是一样的。卡尔曼原理以及什么5个公式等等的,我也不太懂,就不写了,感兴趣的话可以上网查。在此给出具体程序,和一阶互补算法一样,每次卡尔曼滤波只能得到一个方向的角度。
  1. #include<math.h>$ I' d! m9 \, n; h& h
  2. #include "stm32f10x.h"
    8 b1 [* Q. D9 v
  3. #include "Kalman_Filter.h"
    , r. ]" q. |# h; g! ]; o! c/ _; F
  4. 3 M3 L* k; D0 i$ T9 x, W$ M

  5. ' V1 H3 B; L7 b

  6. + t  t7 x& H: ]/ U. t8 ~( E% I
  7. //卡尔曼滤波参数与函数. e$ Z  I/ H0 ~: Z5 T
  8. float dt=0.001;//注意:dt的取值为kalman滤波器采样时间
    4 m! @) {9 _+ R  S
  9. float angle, angle_dot;//角度和角速度# Z! e2 g6 g: V; D+ c9 j1 ]7 ]
  10. float P[2][2] = {{ 1, 0 },6 ^& y& Z2 F# u% P  F+ z2 q
  11.                  { 0, 1 }};
    . O+ @6 a5 V" Y- y. }* x- D
  12. float Pdot[4] ={ 0,0,0,0};7 E5 t. i; N6 a' D. _, m2 h
  13. float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度4 A% d- ^0 Z) F
  14. float R_angle=0.5 ,C_0 = 1;
    + }' s; Q4 J7 o# R/ t: t. g2 s7 E
  15. float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
      K2 v3 P2 o* w; X) u: }  j

  16. + X$ l, t5 A; S
  17. //卡尔曼滤波$ L, W8 v! ]) @' o0 j* ]8 z2 @% J
  18. float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy7 ~- g# M* U4 m, h, r$ n
  19. {8 x! t/ s6 f$ j9 L1 U
  20.         angle+=(gyro_m-q_bias) * dt;
    , |' N, Q: x/ z8 e. s$ y
  21.         angle_err = angle_m - angle;
    5 T$ e: f0 b& D6 v) A
  22.         Pdot[0]=Q_angle - P[0][1] - P[1][0];6 Q$ a0 E- M" ~/ P
  23.         Pdot[1]=- P[1][1];
    ( o: H: b$ U$ f2 E& K
  24.         Pdot[2]=- P[1][1];
    $ F7 v9 I7 n% n& y( D" g8 _0 p
  25.         Pdot[3]=Q_gyro;
    7 E% T8 Y4 H- {( f: L% @
  26.         P[0][0] += Pdot[0] * dt;$ p" }* N. d) l
  27.         P[0][1] += Pdot[1] * dt;2 V6 X) P* W, K* y4 t, Q( g' i) K
  28.         P[1][0] += Pdot[2] * dt;7 a( c  O  w* k( B# z2 N, s
  29.         P[1][1] += Pdot[3] * dt;' d/ v" g: y& O( {8 y/ y$ p
  30.         PCt_0 = C_0 * P[0][0];6 q/ n" M9 |9 L2 V1 ~2 P
  31.         PCt_1 = C_0 * P[1][0];
    2 t1 A# Y' a# d  W$ }
  32.         E = R_angle + C_0 * PCt_0;6 O* X4 t# m$ U& J" ^' z$ }5 [0 y7 n
  33.         K_0 = PCt_0 / E;6 t. b$ t9 G- b5 j1 @
  34.         K_1 = PCt_1 / E;
    / \" m3 b% c) Y( J
  35.         t_0 = PCt_0;5 b; n$ z- i( S  R  K4 H  c0 _. f: r
  36.         t_1 = C_0 * P[0][1];4 {6 I& ~7 H4 j0 ?
  37.         P[0][0] -= K_0 * t_0;
    6 X& S; |8 @+ H  U- u  O- C& a- b
  38.         P[0][1] -= K_0 * t_1;( j$ e7 B) T! o; x, ?# y
  39.         P[1][0] -= K_1 * t_0;6 }( G3 B, K) D5 J% h
  40.         P[1][1] -= K_1 * t_1;7 l6 ]6 `$ k' }$ Z6 \7 N% p4 n
  41.         angle += K_0 * angle_err; //最优角度+ a9 v) w; I, |4 u
  42.         q_bias += K_1 * angle_err;
    . C& v: Q% }' I. M
  43.         angle_dot = gyro_m-q_bias;//最优角速度
    , d; `. O4 |. t5 ?) L5 P
  44. * I9 S& d3 u4 Q$ i5 A2 U) h
  45.         return angle;# s  ~: A0 k$ I- o* u* v
  46. }! c  B/ m+ ?& ~: \0 p
复制代码

作个总结:三种融合算法都能够输出姿态角(Pitch 和 Roll ),一次四元数法可以输出 P、R、Y 三个倾角,计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。

6 w, [! ?+ D& ~6 A3 F
, ~" h8 m0 C& a

注:使用“匿名地面站”上位机软件,给调试过程带来了很大便捷。软件可在附件中下载。

通过上位机可以看到,姿态融合成功!

* ]# f+ n! j4 @4 O

220439w884hoaa7ommaosa.jpg

, j. [# f& P- E' c
$ C$ a2 Y7 G# d* ]& p
回复

使用道具 举报

该用户从未签到

5

主题

10

帖子

29

积分

新手上路

Rank: 1

积分
29
 楼主| 发表于 2017-8-12 17:56:17 | 显示全部楼层
附件:上位机软件(无需安装,使用简单方便) 匿名地面站.zip (4.76 MB, 下载次数: 2)
回复

使用道具 举报

*滑动验证:
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

快速回复 返回顶部 返回列表