7 `( \3 F2 l3 D# q



4 E( b2 X% r- k# N4 A6 H背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
9 p# ~1 }2 r, I8 ^5 d
MPU-6050简介
: X' \0 w2 ]- v4 `( N2 S  r    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。

    MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。
  M4 a* }) A$ E, |& ]

: F0 ^, _6 h2 y; Z; r& V" |0 C# }
6 U* }/ r% q4 k. j8 w9 l3 C) ^. E
MPU-6050姿态获取与处理（惭愧，我也一知半解）
/ w; p2 L1 I+ O9 a4 ?0 T0 s    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。


互补滤波、卡尔曼滤波计算位姿示例

( j, H0 D" j7 V9 ?

互补滤波、卡尔曼滤波计算位姿示例

    除了使用加速度计计算的噪声较大，卡尔曼滤波，一阶互补滤波，二阶互补滤波看着差不多，O(∩_∩)O哈哈~（四元数计算的示例就不演示了，有兴趣的可以自己研究一下。）
    1）一阶互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
    2）卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程（来源于百度词条，我也看不懂这说的啥）。
    3）硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到Yaw、Roll和Pitch（使用四元数计算位姿，好像会将初始化的位姿设定为零位）。
" p5 a* K+ J7 ?+ t
2 M8 S# T0 f4 a' ~; a& @3 x- A7 O


# ~; E/ e8 ?% t一、一阶互补滤波算法
+ v) Z6 e' K- Y; n    MPU-6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，通过简单的计算即可得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角，需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正，通过加速度和角速度就可以计算 Pitch 和 Roll 角（单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用）。


一阶互补算法如下：

1. //一阶互补滤波
   * V/ q2 j, x$ P
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   
4. 
   
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   . X1 _+ H( G9 z; G
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   5 `+ F% O  z( w- h+ n
8.   return FirstOrder_Pitch;
   * k" X+ d' o9 e+ ?: s3 B
9. }

复制代码

) a: \* Q5 A1 ^) H" i2 b/ b二阶互补算法如下：
) j' V, g4 c) K, r& C

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   
4. float SecondOrder_Pitch;
   9 D. H! X# K" e! f3 V6 ?2 G( H1 a
5. 
   1 Z3 Q. L2 ~& {6 ^# k) A: Z
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   8 M2 y/ u9 f5 ]. g% U
7. {
   
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   
9. y1=y1+x1*dt;
   
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
      ]4 t  o, c$ p: u4 _2 u, _
12. return SecondOrder_Pitch；
    ' R  j5 y5 b# p
13. }

复制代码

二、卡尔曼滤波
- Q: F6 Y7 E! N/ g9 u- {4 l5 S

1. /* Kalman filter variables */
   " X" [! ?0 @( Y1 L
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   8 x( d9 Z: }& K6 B$ x
3.         float Q_bias = 0.003; // Process noise variance for the gyro bias
   
4.         float R_measure = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise
   ) D& W2 X$ G# k' N9 E) w
5. 
   ( u! u) H- S  m  q, W. n
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   9 b. e: ^) K0 ^7 M! P! t; G4 Y
7.         float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
   : B/ ?0 P# A5 S; `% j  ~+ K
8.         float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
   $ j1 O% f! ]  m0 W) z
9. 
   
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    
11. 
    
12. 
    + S5 v3 n9 n+ G, ~) @
13. // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    4 [2 f' U# m6 _) n- p5 n/ l
14. float Kalman_filter(float newAngle, float newRate, float dt) {
    % e) v* x4 B- ^" I: u0 W% d0 @
15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    
16.     // Modified by Kristian Lauszus
    ! n9 k8 y% F+ q3 T! N. }
17.     // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
    
18. 
    
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    * g6 l8 Y( t+ ?" a
20.     // Update xhat - Project the state ahead
    ' W# R. R* z3 f
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    2 x: S0 k9 x6 [, s0 L/ v5 o: H( m
24. 
    $ L1 e5 P* |, b* k+ _# I& V
25.     // Update estimation error covariance - Project the error covariance ahead
    
26.     /* Step 2 */
    
27.     P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    # g  R  |* [$ s. ?. G/ I# Y- d
28.     P[0][1] -= dt * P[1][1];
    
29.     P[1][0] -= dt * P[1][1];
    
30.     P[1][1] += Q_bias * dt;
    
31. 
    : q0 y8 n* a: h# M! g9 s% g
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    ! H1 M0 ]# l) K* t- p: d  S
33.     // Calculate Kalman gain - Compute the Kalman gain
    
34.     /* Step 4 */
    
35.     float S = P[0][0] + R_measure; // Estimate error
    6 l7 V9 d1 T+ ~
36.     /* Step 5 */
    
37.     float K[2]; // Kalman gain - This is a 2x1 vector
    0 h, Y( j% i+ A
38.     K[0] = P[0][0] / S;
    
39.     K[1] = P[1][0] / S;
    
40. 
    # V/ y2 [. {% C
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    
42.     /* Step 3 */
    
43.     float y = newAngle - angle; // Angle difference
    $ s$ V2 f- u* C+ M# }& C5 J
44.     /* Step 6 */
    
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    
47. 
    ( H+ k$ n& ^% N) w: l, O; Q/ f
48.     // Calculate estimation error covariance - Update the error covariance
    ( i$ T" F& ^7 u; |, V
49.     /* Step 7 */
    
50.     float P00_temp = P[0][0];
    * u/ x+ _9 r4 u" z2 F
51.     float P01_temp = P[0][1];
    2 Q0 N& m) Q' O5 q+ o
52. 
    % `; f) r+ _) a& ]
53.     P[0][0] -= K[0] * P00_temp;
    ( {+ l7 g, S& A) Z7 A
54.     P[0][1] -= K[0] * P01_temp;
    : Z' ~  c  J+ q, n7 A; Z: W
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    
57. 
    1 j& g% s: b# [( e
58.     return angle;
    , c$ Y. o$ Y! i& U2 n" H$ e2 n$ ]
59. };

复制代码

三、四元数法

0 V0 H% B! C, S, T     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
5 g4 v5 j$ I% }8 R0 j* H1 a. q( Y
/ |/ U' r, k2 _( s, e       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
* h3 {$ ]6 V6 U* X3 Y3 L7 [7 K

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   . x7 M) W& K4 t
2. {
   
3.         unsigned long sensor_timestamp;
   6 q: H( `$ Y1 V6 l
4.         unsigned char more;
   $ I- W2 O/ G+ t) |$ u
5.         long quat[4];
   8 t% d" t. I0 i* ?4 M) t3 c. s
6. 
   
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   + }0 a5 f' C3 ?9 o5 c: U0 A4 k- {0 l
8.         if (sensors & INV_WXYZ_QUAT )
   1 A6 X+ R( u6 p4 E/ q7 ^$ M/ j, z7 d% F
9.         {
   1 j: z8 b# O( }% h
10.                 q0=quat[0] / q30;
    
11.                 q1=quat[1] / q30;
      x2 S- j& m1 w" Q& V0 D( g/ F4 `
12.                 q2=quat[2] / q30;
    
13.                 q3=quat[3] / q30;
      R# p& B6 d7 |* d- ~: _
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    6 g' y: J8 R6 z8 J2 o
15.                 *Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    . `5 l: t! o& @2 s3 H4 a9 R
16.                  *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    7 f+ _( z6 g4 p( N
17.         }
    
18. 
    
19. }

复制代码

    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。
$ Z* s' u$ X2 s& p3 g! Z3 [
    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。

/ G, ^/ H9 l: R6 K9 M; }    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
* ]1 M9 a' V8 X9 S$ O

$ J1 c8 g' {' f( I8 _
" y$ O/ h; h( J6 g* w输入工程名字，点击Next



8 ^$ [/ \& T: x$ x! [% G! m设置完成后，点击Finish

+ I4 e. ~% A* i9 \: y

进入MCU配置界面



设置I2C与MPU-6050通讯。
3 t; T' x! |" e( j
  R2 D% K6 E8 c9 v) M6 k! g
) I+ l# S% X/ t5 h5 r
1 O! P! k7 O* N: g  S, `6 N: I0 N设置串口与上位机通讯，实时显示MPU-6050位姿。
- c& N" w" u' ^4 U6 J, I
( P4 N2 q3 h3 b7 w6 t  [, v. N
9 G) f( Q! U+ b. m
. ]% A$ O9 I* h: j+ h# B/ E5 U: M设置系统时钟及调试模式。

, v' k9 C6 s! n  o

设置时钟源。
$ i) w2 X0 \) Y
+ q' y. W! D& ?, [4 i
. e2 z1 G/ x% i1 D6 F, }. |
全部设置好后，芯片引脚的分配情况。



* m4 ~, ]; u( O在时钟配置里设置时钟最大频率。
3 G7 S" [3 h5 p
9 A5 W3 ^* q1 M+ \+ U' ?, [

# ^+ I3 L) {5 u在工程管理——>代码生成器中进行设置
9 ]7 N% Q) P% |4 \4 m9 K. a  o% x


& x9 c5 _/ G/ }& g点击此处，生成代码
6 i* ~. L. z& }% u# v$ x  p
+ \2 g7 J" ]! g$ P

: q3 `" V0 _) r" F8 R# G* O. E在工程文件夹中创建iCode文件。


9 z9 A' A1 P; _
1 O$ ?3 S0 W' g将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。



点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。


1 y+ F+ K8 d5 y& f; O& _
" ]8 x: G: e8 |% r4 y" u8 @按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

+ ^2 J; s4 X! y6 i6 C% `
+ A; b/ ~/ |; M3 ]* [# M( M# }9 o

" w# r5 D& K3 {& B
然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
! q0 l+ x# P& W$ D( q/ l

1. /* USER CODE END Header */
   ; }4 }7 ?. n& c1 G
2. /* Includes ------------------------------------------------------------------*/
   & Z$ X  F( n2 Z1 E2 A& V
3. #include "main.h"
   - H) S* Z/ C+ i0 x
4. #include "i2c.h"
   
5. #include "usart.h"
   
6. #include "gpio.h"
   0 _7 T4 s$ ]2 ~7 S/ K
7. 
   
8. /* Private includes ----------------------------------------------------------*/
   
9. /* USER CODE BEGIN Includes */
   
10. #include "mpu6050.h"
    
11. #include <stdio.h>
    
12. /* USER CODE END Includes */
    - [1 [0 V5 o, D! b1 T/ u! l! U/ q
13. 
    
14. /* Private typedef -----------------------------------------------------------*/
    
15. /* USER CODE BEGIN PTD */
    
16. 
    
17. /* USER CODE END PTD */
    4 X; C: _1 Z+ i! k4 z2 P0 o
18. 
    
19. /* Private define ------------------------------------------------------------*/
    & r3 G3 y4 O* W( e' K4 M9 D! `
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    1 s- c8 U8 Q% V# b7 ], N) H5 e2 G
22. 
    " r1 s, o: d- J( Z
23. /* Private macro -------------------------------------------------------------*/
    
24. /* USER CODE BEGIN PM */
    
25. 
    ! ]) m4 ]/ P$ L' P
26. /* USER CODE END PM */
    ( \8 @- p6 y! l+ ^0 ?
27. 
    8 g  V8 o( x6 V7 {: C  t7 e
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    
30. /* USER CODE BEGIN PV */
    
31. 
    
32. //重定义printf函数，用于串口输出
    3 S2 k8 N9 }- b  A( x' J. H5 v
33. #ifdef __GNUC__
    ( y* `( e* _% E( D0 j( i9 V
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    # h+ \& O3 y9 U: ]+ U
35. #else
    , P. }& |! S+ h% }0 Q$ v
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    
37. #endif
    * O6 h4 h& e" B
38. 
    
39. PUTCHAR_PROTOTYPE
    ! E' y5 C. X) g# t/ @8 S
40. {
    - a% T; A) v9 X% V1 p; P  [/ _
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    0 I0 l/ i; u1 l/ Q& p. Y# i2 I/ K- v" _
42.     return ch;
    
43. }
    
44. /* ---------------------------------------------------------------------------*/
    5 ]- q' x' ?0 ?4 @
45. 
    $ O. E2 @2 u" Z. \
46. //获取系统时间，用于卡尔曼滤波
    
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    
48. {
    ; z0 M. ~$ L9 J: c3 k0 C
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
      }6 M2 d+ G* T; y4 Y: k
50. }
    
51. static uint32_t getCurrentMicros(void)
    / S) s$ D" d  r: W5 L  U: b
52. {
    2 l" `& M# y& f; D" p
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    
54.   GXT_SYSTICK_IsActiveCounterFlag();
    
55.   uint32_t m = HAL_GetTick();
    
56.   const uint32_t tms = SysTick->LOAD + 1;
    1 i3 Y0 R3 T4 Y+ V& C$ I: o
57.   __IO uint32_t u = tms - SysTick->VAL;
    ; _7 {% S- \: P* G8 |2 h* c
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    8 o) J4 {/ T/ s- A# p
59.     m = HAL_GetTick();
    " y9 H) v# l5 R: B
60.     u = tms - SysTick->VAL;
    ! `9 `6 N! B0 B. _% _% J! x
61.   }
    / j! L: {. D# ^1 f. Z
62.   return (m * 1000 + (u * 1000) / tms);
    ' ^/ P( F* R% F$ I: O6 w
63. }
    
64. //获取系统时间，单位us
    
65. uint32_t micros(void)
    
66. {
    
67.   return getCurrentMicros();
    
68. }
    
69. 
    
70. 
    
71. /* USER CODE END PV */

复制代码

0 |2 X6 v+ O2 z  t# ~8 Z( T使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面



在main（）函数中添加MPU6050初始化代码和发送位姿数据代码

1. int main(void)
   7 v1 ?& q6 c9 p$ s. w
2. {
   4 H& C6 L" J3 A
3.   /* USER CODE BEGIN 1 */
   
4. 
   
5.   /* USER CODE END 1 */
   * S( O  X6 i9 ~" ^; s
6. 
   
7.   /* MCU Configuration--------------------------------------------------------*/
   / \1 Z! ?) E9 h$ u4 K. g" S
8. 
   - @/ ?" Y5 c" z
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   
10.   HAL_Init();
    5 B. [$ w- a# U; a8 w1 ^. U
11. 
    
12.   /* USER CODE BEGIN Init */
    
13. 
    , p) U5 M9 U! \0 J
14.   /* USER CODE END Init */
    6 l; y$ ~% E8 Z) ]
15. 
    
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    
18. 
    , a# W4 G. U7 R- u% V( h
19.   /* USER CODE BEGIN SysInit */
    
20. 
    5 [/ v, d2 p% q3 v- l2 y
21.   /* USER CODE END SysInit */
    % B4 L4 U% Y$ z! J
22. 
    
23.   /* Initialize all configured peripherals */
    9 v$ g( |/ k- {
24.   MX_GPIO_Init();
    
25.   MX_I2C1_Init();
    ( x; @- {" ~) d5 n' S
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    ' m+ W  ?8 z8 @2 {) v
28. 
    3 ~2 V7 N; u4 F  c
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    
30.           printf("MPU_Init_Fail...");
    7 c0 r/ ]; s0 m* @
31.   }
    . m- \% S. \9 n: }9 f4 e, d
32. 
    
33.   /* USER CODE END 2 */
    
34. 
    
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    
38.   {
    
39.           GetAngle();
    
40.           GetPitch();
    
41.           HAL_Delay(10);
    
42.     /* USER CODE END WHILE */
    
43. 
    
44.     /* USER CODE BEGIN 3 */
    
45.   }
    
46.   /* USER CODE END 3 */
    $ p% H9 v/ j% y$ L6 a) J
47. }

复制代码

点击，下载程序


9 s+ i$ i" g  p9 ~
程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
$ F1 W3 A1 S+ Q0 k# p3 D! s# o

; O' A7 [' }% K7 \: h; r  P
0 w- y8 c1 ]! O% P* t4 j 作者：DIY攻城狮
# A& K/ z3 S, w2 J  t/ I

4 \& W7 ^: |" d( [3 K) p

MPU-6050、卡尔曼滤波库在哪里呀