亚博智能论坛  
  
查看: 459|回复: 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 角。

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

* y  z# |# N; g. S- s: |# p
2 `6 b7 d$ G' V" N0 l
2 |6 Y0 ]7 d1 k) B" `

一、四元数法

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


( u2 w* g# j- X, N' a3 X% M+ q, u  a3 y: }( W

虽然 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* o5 X) ~. A, Q* h. C1 K% X
  2. #include "stm32f10x.h"
    " F+ H, Z8 M/ z. c+ W- }  ?: y$ T
  3. //---------------------------------------------------------------------------------------------------$ p% U/ U: T; |- B' S; \
  4. // 变量定义
    7 N1 z' j. J$ N/ Y
  5. - t* j) r' u7 M0 D* h! V4 E
  6. #define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计) f$ r  g8 N! S, X; |/ B9 j
  7. #define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接
    * k6 N3 b1 K& B" h
  8. #define halfT 0.001f                // 采样周期的一半: f/ }  c; a) ~) J+ F4 F* d9 l
  9. . p1 k, y9 p# Z, h& D; J
  10. float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素,代表估计方向
    " I$ a( {/ I5 [2 f
  11. float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差
    4 K  f* D# t6 @1 m. k

  12. / d* A7 F/ s: l5 _  \( p# S; f* I
  13. float Yaw,Pitch,Roll;  //偏航角,俯仰角,翻滚角
    # x: \+ a  ]2 B* A3 v

  14. % d2 `6 i0 A* S7 l5 ^* X) l

  15. / \  f* i+ R  W9 Y3 C5 v2 G2 J! D
  16. void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)9 d" f. O# f; ~" e' C$ P4 L
  17. {& Q$ n! V/ F; M
  18.         float norm;( F% ?5 R# p; X
  19.         float vx, vy, vz;
    , D# c7 g6 O& j9 S
  20.         float ex, ey, ez;  
    ) @- z* ]3 P6 L0 T6 ]" q
  21. + \( M6 U( N- n, r: U( ~0 t
  22.         // 测量正常化
    ) C! b8 Z1 \4 I0 P
  23.         norm = sqrt(ax*ax + ay*ay + az*az);      
    7 z' p) k2 h; v0 u
  24.         ax = ax / norm;                   //单位化
    # n  A% x  I$ E; J! X' f. k
  25.         ay = ay / norm;9 R2 M! r2 g9 Y. c; q
  26.         az = az / norm;      
    0 ^) r6 T' K3 G

  27. $ G( l  n* y* |/ S. S0 C9 F
  28.         // 估计方向的重力* C2 M) O1 V0 v) b% w% i$ U
  29.         vx = 2*(q1*q3 - q0*q2);
    0 @- U" H* ^! x" \1 L6 e8 o
  30.         vy = 2*(q0*q1 + q2*q3);
    4 o, U! ^# z, M/ H/ F& f' d
  31.         vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
    ) y$ `5 o) p% E- Z3 A

  32. & S# E0 v' K% }" x0 l- i
  33.         // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
    2 J& j6 u- B; \! S
  34.         ex = (ay*vz - az*vy);9 D9 S: o6 j: r+ n! l! g
  35.         ey = (az*vx - ax*vz);; [/ D5 a& e( w7 [6 W5 i
  36.         ez = (ax*vy - ay*vx);
    + K( A" w$ S/ ?2 q

  37. 4 R: g9 _5 F$ U- J
  38.         // 积分误差比例积分增益
    8 \# ~9 J8 t9 t0 j: n8 {
  39.         exInt = exInt + ex*Ki;
    ! C* X8 M% \" U5 z! c
  40.         eyInt = eyInt + ey*Ki;) U  C2 l% D6 Z0 l) b
  41.         ezInt = ezInt + ez*Ki;
    2 K4 Z8 t# O5 I( \
  42. ' N# a9 N! j# D% \0 Q* l! q( g
  43.         // 调整后的陀螺仪测量: Y  N$ v$ h  \  c4 k6 `  b5 G
  44.         gx = gx + Kp*ex + exInt;
    $ ^! ?1 K4 K: p3 S3 J  n* g. q
  45.         gy = gy + Kp*ey + eyInt;* B8 r6 m& \& E0 Q! a! |' }( D
  46.         gz = gz + Kp*ez + ezInt;* \) A) A6 M+ {( q9 {& |* z
  47. ! z3 }5 |9 G6 H) r: \) B
  48.         // 整合四元数率和正常化
    : I5 [+ B7 d5 v, g# x9 y, X
  49.         q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
    4 Q  d; @( S; R8 |$ X
  50.         q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
    ! [9 z+ f" {  u9 V( _3 ?+ m2 }  a) x
  51.         q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;2 W% P' l, G. P
  52.         q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;  6 T2 D: L8 s# R

  53. 3 x5 O2 f3 O( X/ y. P
  54.         // 正常化四元
    6 l5 I& J+ F. O( z* I
  55.         norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    + S9 Y+ x6 z2 i: c6 t% `
  56.         q0 = q0 / norm;5 ]. z4 L; x+ l* i3 L, f* ~4 }* U
  57.         q1 = q1 / norm;
    ' U; _$ A1 y( ^. b6 \
  58.         q2 = q2 / norm;
    ( H! L% ^) R! f$ B* v
  59.         q3 = q3 / norm;
    5 e7 T4 Y, y( }/ B7 G; ?
  60. ( C2 `% L6 N& M( m+ b3 P4 ]2 s1 m
  61.         Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数
    # I3 T. L3 p  W+ F3 o, a
  62.         Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv" w: u5 o( ?. i. s
  63.         //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值,注掉
    : o2 O* b9 \" X
  64. }- U& F  ^$ J: l( q2 r
复制代码

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

+ F7 k# Y* u4 [4 w: G: D7 `$ l9 J' G

二、一阶互补算法

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

  1. //一阶互补滤波5 w6 m6 p; O7 r3 Y7 z0 z# a& j+ Q8 r& t
  2. float K1 =0.1; // 对加速度计取值的权重
    , ~; g+ T6 v9 T' H
  3. float dt=0.001;//注意:dt的取值为滤波器采样时间
    # K" v  g. e( _! C% R
  4. float angle;- y9 K( Y, @; v  v1 {' [
  5. : S, Z! o2 K9 j' c( O4 p  [% M8 b
  6. angle_ax=atan(ax/az)*57.3;     //加速度得到的角度9 p; R8 w: Z$ i" q! W# M
  7. gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度5 Z4 {, x* @  u/ S: M$ g' l9 u3 m0 j
  8. Pitch = yijiehubu(angle_ax,gy);
    - M# m2 b5 R; v) i8 G

  9. 6 Z% S% c8 Z  f5 V  a
  10. float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度" X, X5 {# L% G- U* Q) @
  11. {
    . v. R% p6 Q' N+ L: M% ^1 a
  12.      angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);
    ; t: T# b+ ]- v  I
  13.      return angle;
    6 Z/ F3 T! u' m5 m, s8 R/ o
  14. }' J3 f4 T6 d# s! ]
复制代码

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

  1. //一阶互补滤波8 `. x8 w( T0 `
  2. float K1 =0.1; // 对加速度计取值的权重
    2 |- T% H, ]1 z6 f" @) |
  3. float dt=0.001;//注意:dt的取值为滤波器采样时间. b* }# U- Z0 ?6 h  b8 S
  4. float angle_P,angle_R;: L- v/ j  i: E" |5 m* {/ X) p

  5. 1 |3 e5 y4 t) Y0 c0 k

  6. / j' L0 Q7 c& |  y5 E5 K, j
  7. float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度
    6 \: R0 b" d/ m2 K* Q
  8. {; J6 C/ x5 h/ E; I- \7 j' I6 o3 N9 A
  9.      angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);
    - a% C0 a+ ^. o- F( E
  10.          return angle_P;
    ( \4 n3 _  N' J2 K6 O, b5 {& u% `
  11. }
    ( _7 j' E% D2 U8 X* D+ V
  12. ' Q5 {$ B. i; Z
  13. float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度5 ]( a5 C' t* d5 W9 B
  14. {7 e& A9 N, N4 @' K
  15.      angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);- G! \4 s9 R+ r( p: I4 V
  16.          return angle_R;( B: ?; q! ~+ `1 J
  17. }
复制代码

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

7 B& [( I( P" o8 ~4 {$ v5 b  s. Y3 C
回复

使用道具 举报

该用户从未签到

5

主题

10

帖子

29

积分

新手上路

Rank: 1

积分
29
 楼主| 发表于 2017-8-12 17:53:37 | 显示全部楼层
本帖最后由 haihuawu 于 2017-8-12 17:55 编辑
+ {  u& M5 M9 W' {  o8 L  i0 F% U
三、卡尔曼滤波
  u% I2 l% f5 V* `) i4 f其实卡尔曼滤波和一阶互补有些相似,输入也是一样的。卡尔曼原理以及什么5个公式等等的,我也不太懂,就不写了,感兴趣的话可以上网查。在此给出具体程序,和一阶互补算法一样,每次卡尔曼滤波只能得到一个方向的角度。
  1. #include<math.h>/ X2 V% s8 ^8 Q4 |, {% t
  2. #include "stm32f10x.h"
    7 S  I6 A$ `9 c; V7 g% J  n
  3. #include "Kalman_Filter.h"1 A0 f( M+ q9 p' X+ b

  4. & f  U% R2 k$ ~! o4 E

  5. % d9 ~7 j& _6 v1 w' j2 s4 o6 k

  6. 4 E+ h1 Z. J* G" s* [
  7. //卡尔曼滤波参数与函数3 v; X7 \- @8 _9 a
  8. float dt=0.001;//注意:dt的取值为kalman滤波器采样时间2 K0 Z& ~& Y* G' F9 N( p* ~
  9. float angle, angle_dot;//角度和角速度
    3 a/ [5 X4 U( A
  10. float P[2][2] = {{ 1, 0 },; U% }1 X  [1 b8 Y+ F. {6 e) |& m
  11.                  { 0, 1 }};
      C' F7 @' z% L& F: i6 x8 @# Q
  12. float Pdot[4] ={ 0,0,0,0};
    ( c$ Y/ t6 a/ Y3 W  Y! @# L5 C, T! y
  13. float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度8 {6 {4 i6 O. \5 S( ~. \' g( P
  14. float R_angle=0.5 ,C_0 = 1;5 G1 l  {( @4 g8 T& F' ?
  15. float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
    ( l% v, M. ]+ T8 s  D
  16. & L: X* C' M. j: k& i1 V) A
  17. //卡尔曼滤波( Q' T8 [4 x; |5 ^  H
  18. float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy9 H7 R2 _& e8 [2 \9 o; y
  19. {
    ! m+ B0 o$ A& K# y& Y
  20.         angle+=(gyro_m-q_bias) * dt;% D9 ~, M6 d' ?# |: W- B8 E
  21.         angle_err = angle_m - angle;" N% M& t# w" U  W' g, T
  22.         Pdot[0]=Q_angle - P[0][1] - P[1][0];# t- O! q% L9 ]3 O% G
  23.         Pdot[1]=- P[1][1];# X) z, Y. H- Q4 R& Y5 r
  24.         Pdot[2]=- P[1][1];
    7 U! H# z* `' |6 e8 G% H: ~
  25.         Pdot[3]=Q_gyro;: B9 }+ @9 ^9 U) ?. j* p% O
  26.         P[0][0] += Pdot[0] * dt;
    - ^6 ~7 Z3 j5 X8 p7 L' n
  27.         P[0][1] += Pdot[1] * dt;' p1 O, T1 s. h# a/ ^/ k  L
  28.         P[1][0] += Pdot[2] * dt;, w: ^/ d  C9 _
  29.         P[1][1] += Pdot[3] * dt;
    * G1 ?: y/ S' J: ~2 \' \6 o/ ?
  30.         PCt_0 = C_0 * P[0][0];( @$ E  u. }' b
  31.         PCt_1 = C_0 * P[1][0];
    5 e5 b& A& _3 C0 k7 |
  32.         E = R_angle + C_0 * PCt_0;5 X/ F- C+ C9 a  T0 r9 G2 z
  33.         K_0 = PCt_0 / E;
    - s7 H# D+ O7 g* o9 C5 o; i+ F- R
  34.         K_1 = PCt_1 / E;2 o) h# P1 {: k% P
  35.         t_0 = PCt_0;8 E* L/ v8 F5 ~! p2 v( G5 r  O
  36.         t_1 = C_0 * P[0][1];! @0 u8 p( C% l) H( Z' G; f
  37.         P[0][0] -= K_0 * t_0;
    " |5 P; U3 g9 O/ B+ y. r8 K( C
  38.         P[0][1] -= K_0 * t_1;
    . O; M+ n0 a8 h/ _; ]- [
  39.         P[1][0] -= K_1 * t_0;
    # n7 S/ n( C& H) M& F9 c
  40.         P[1][1] -= K_1 * t_1;4 S8 C; R1 x. |0 _! P* o% S$ G
  41.         angle += K_0 * angle_err; //最优角度. Z+ U6 w2 O5 m3 C0 N" ~/ ^% [
  42.         q_bias += K_1 * angle_err;
    # {9 t! v! ?/ R9 t: m  F0 l
  43.         angle_dot = gyro_m-q_bias;//最优角速度
    & V" b" ~/ f: G, ]

  44. ! T. W5 A0 H+ W5 _2 T$ O- x
  45.         return angle;
    + S$ b1 Z. t5 |
  46. }/ t1 P5 N) C4 v  D; ?
复制代码

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


7 A- \, j% B; p7 ]1 A! P. y$ j
! x: V' E3 a* v+ h+ E8 t

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

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


6 u/ o8 x- H& I4 X! E: j3 k  b

220439w884hoaa7ommaosa.jpg


$ S2 h, Z2 ]! h* }  m
! A) D3 A5 R$ \* ]1 u7 D
回复 支持 反对

使用道具 举报

该用户从未签到

5

主题

10

帖子

29

积分

新手上路

Rank: 1

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

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

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