- @$ u5 o( S& i6 x* U; e  O4 y
: d2 d1 b, [0 m0 F7 D& d1 h  k" |) v
2 M4 u* Y/ c" J1 H' c8 X
: ^! W! J8 v: |
; Z- y' Y  v9 i) m( r背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
& J& {6 T3 t& \3 r) A: C
MPU-6050简介
    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。
" z/ i# F7 W! ~6 `( I! _( I

, p- o/ n; L" ?1 u  T; l% u
: R+ h+ e, G9 z8 B8 c9 U
3 x, g: ]/ Z3 L0 A  [MPU-6050姿态获取与处理（惭愧，我也一知半解）
6 Z& P7 J2 S& t2 P* a1 E    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。

1 u7 }; G+ T8 ~1 M
' }% r2 f- r% p, A: z6 v  P互补滤波、卡尔曼滤波计算位姿示例

/ a/ |) z- x% J1 K1 G1 p6 s3 N

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


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


8 S5 R7 h2 ^8 r7 O$ n' M一阶互补算法如下：

1. //一阶互补滤波
   
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   
4. 
   & p. T0 ^/ C8 P4 K3 G8 ]
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   6 E/ o) f, N$ U+ k8 ^2 M! ~
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   , W3 B- ~' n" k, m+ u- f9 Z$ H  A+ t
8.   return FirstOrder_Pitch;
   
9. }

复制代码

* L$ U: W, `7 u+ e& ~/ [二阶互补算法如下：
' K9 I; q; F9 u" }; t2 m. w

1. //二阶互补滤波
   - l$ f1 T  H* R, H: v- B* j6 T
2. float K2 =0.2; // 对加速度计取值的权重
   % r! k+ Y; o. K1 J  U
3. float x1,x2,y1;
   + u+ Y9 `+ M* K: n6 @( S
4. float SecondOrder_Pitch;
   
5. 
   $ Z) _$ w0 b* N" o6 {1 Z
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   
7. {
   * {* l; ?! n. b
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   7 ?; [, {' e/ s, C
9. y1=y1+x1*dt;
   
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    8 i+ q$ |' N- u( Q. E0 G  W6 o2 y
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    6 K( @" z. r9 @: r; F- i( `# O6 X
12. return SecondOrder_Pitch；
    " J  R7 z& |0 C/ J! M
13. }

复制代码

5 I" I/ T3 t5 v0 b/ I! C" K二、卡尔曼滤波

1. /* Kalman filter variables */
   $ g% i3 T7 _& q# I. U
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   : M" _  N- e2 y! G! C
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
   - F! F! G- `  v1 z1 O- M- X9 G
5. 
   
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   5 q3 I" v+ d& A" F; I+ g
7.         float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
   
8.         float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
   
9. 
   
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    5 w6 O6 |0 T8 I, P: e1 l
11. 
    8 g0 X/ |; g+ a! F' w; `) w
12. 
    ) F) `5 N  L9 E" ~
13. // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    
14. float Kalman_filter(float newAngle, float newRate, float dt) {
    
15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    ; [2 W! f) ^; Y7 B8 I) B
16.     // Modified by Kristian Lauszus
    
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
    ! o! \0 b0 p, w: m8 E
18. 
    
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    
20.     // Update xhat - Project the state ahead
    
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    
24. 
    
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);
    . e- J) p, l$ Q1 l6 g% m7 Y4 k# p
28.     P[0][1] -= dt * P[1][1];
    , ^0 P" E+ a) p  {' b$ \
29.     P[1][0] -= dt * P[1][1];
    5 }' `3 c: T8 X- y6 ]
30.     P[1][1] += Q_bias * dt;
    
31. 
    1 y( o' z& |9 M/ g# ^6 e% `
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    
33.     // Calculate Kalman gain - Compute the Kalman gain
    # f  ?6 }$ Z7 {/ X/ |
34.     /* Step 4 */
    5 ]' w8 L- s/ a, L+ |) v
35.     float S = P[0][0] + R_measure; // Estimate error
    & B/ Q, f' D" M1 {  e
36.     /* Step 5 */
      `0 q1 Q0 e) T5 A5 }! y. h3 X' Z
37.     float K[2]; // Kalman gain - This is a 2x1 vector
    
38.     K[0] = P[0][0] / S;
    
39.     K[1] = P[1][0] / S;
    ' _* c0 l8 a3 |  c9 y$ Y
40. 
    
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    
42.     /* Step 3 */
    
43.     float y = newAngle - angle; // Angle difference
    
44.     /* Step 6 */
    
45.     angle += K[0] * y;
    ; {8 r7 S  h5 }% E: S, j+ D
46.     bias += K[1] * y;
    " O6 g+ B" w' B9 s6 v' @0 Z8 t; ]
47. 
    / v/ }: K3 }* [0 M. @: Q& ~
48.     // Calculate estimation error covariance - Update the error covariance
    ; r& s# s) E2 M4 r1 i9 G$ k
49.     /* Step 7 */
    
50.     float P00_temp = P[0][0];
    0 M$ k0 `8 j  G' X$ Y
51.     float P01_temp = P[0][1];
    
52. 
    
53.     P[0][0] -= K[0] * P00_temp;
      y" }- K3 y  ~) j; ?
54.     P[0][1] -= K[0] * P01_temp;
    
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    ; H6 z* j; [& J
57. 
    ( M. Y( m2 q: H# D" c' o
58.     return angle;
    4 H$ a* M& k7 Z8 o: h2 U7 F
59. };

复制代码

' D2 i, }- d+ f0 {三、四元数法

     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
7 ^2 O) l2 d4 t2 f* d+ Z7 R
       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
/ N# K+ i2 q" |# z

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   
2. {
   
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   
5.         long quat[4];
   6 f! ~! M9 R( X" p
6. 
   
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   
8.         if (sensors & INV_WXYZ_QUAT )
   ( ], J" K* z" J; c8 r# `# o0 {9 Z
9.         {
   . g+ E. y$ B" t$ _5 P4 S, Y# \( K! R
10.                 q0=quat[0] / q30;
    
11.                 q1=quat[1] / q30;
    " Y) Y- Z4 D/ F% H+ O, o3 N8 u
12.                 q2=quat[2] / q30;
    % J" L  d* v5 Y+ e4 r' x6 C, k
13.                 q3=quat[3] / q30;
    
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    
15.                 *Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    & M. ~4 X1 e; H1 V3 T
16.                  *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    1 n6 g+ j, S, K+ J
17.         }
    / q) v  [6 A- s. e2 c
18. 
    ! K5 r% C5 N- M
19. }

复制代码

    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。

$ Z2 T2 r9 G) I1 U( q    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。
$ ?5 x% T& B/ P( ^
( h9 \, X8 ?) w    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next

* u9 D- v. J/ o% |; a* H! z2 u
- `# I3 L  E: M/ T) y
输入工程名字，点击Next
. }, ?1 \3 [  A4 M! v" R. ]' F
6 S) }3 L+ V, D

设置完成后，点击Finish



8 I3 P1 C& Y# A: P# G: C  s! a# ?进入MCU配置界面

1 ^* b2 p3 w+ ~! G
0 J) u0 L7 e2 L* T# _! _
/ ]/ A. r9 x2 i) g2 E* {设置I2C与MPU-6050通讯。

, K; k+ q# I; o8 m3 M. U8 M
0 M6 j4 @. Y& c" h; L2 @% K' i
2 ^  R; I6 [4 e设置串口与上位机通讯，实时显示MPU-6050位姿。

5 z* Z( n% _+ z3 c
4 k  \+ w, m- t
设置系统时钟及调试模式。


/ z, |0 ~0 d# ^$ W& Z
/ }2 E' M# j- B1 u% A! m7 c& n设置时钟源。

+ L/ P) Q8 u9 g7 c& n. x

全部设置好后，芯片引脚的分配情况。

- P  B- ]7 q; v5 @/ X6 {
6 m* I' c4 B  c
在时钟配置里设置时钟最大频率。



在工程管理——>代码生成器中进行设置

8 p, v: T1 m" R, r2 _9 U7 E
3 F: q& y) a, j
: A" |2 {+ l6 _; z' t点击此处，生成代码

$ o2 n3 M9 |) `3 P" p2 P( F$ A- \

- g: J4 }5 P0 c, x: C% m8 R在工程文件夹中创建iCode文件。
( R, V/ i) o" a7 ]- H. z

) \6 |& v8 A. M6 Z$ Z# F. l% B
将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。


& u- V. h" m: d$ S% B
* i' G# \- ~% w* `6 l( c点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。
" @  |+ M/ j+ l9 a6 t0 g" u


, Q7 R! Y3 {* A( E按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。
7 K! s4 X6 f& z- W8 Q+ K  N
: U; E- k0 U& Z  }# p. Z


& Z0 O5 m  T( `8 \
; N( `  X6 v2 G5 M( J$ ~然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。

1. /* USER CODE END Header */
   & v; U, S; `- C; [  h  Q! Z5 Y
2. /* Includes ------------------------------------------------------------------*/
   
3. #include "main.h"
   / _+ x& W/ a% v9 ~; S5 K1 |5 |
4. #include "i2c.h"
   8 g" t( A  M1 @1 S( e! ?) n( z  ~
5. #include "usart.h"
   ) A& `0 z# _8 w9 X2 p$ |
6. #include "gpio.h"
   1 O- T' O2 Y6 m' m2 _
7. 
   4 T( O7 O, Q1 [0 R
8. /* Private includes ----------------------------------------------------------*/
   ( L* |7 M( ]( t- @& {; F4 z6 ?
9. /* USER CODE BEGIN Includes */
   * I2 g+ r: N. V
10. #include "mpu6050.h"
    8 s2 g* _6 Y0 r' s# x6 E3 T) n
11. #include <stdio.h>
    " d2 l' V5 p' d% @* q% @
12. /* USER CODE END Includes */
    9 ~& n0 U8 O. w4 w% Q/ F% @1 C
13. 
    
14. /* Private typedef -----------------------------------------------------------*/
    $ e' A7 q5 ?+ g& ~
15. /* USER CODE BEGIN PTD */
      w. H8 a( f# [* Q. a
16. 
    
17. /* USER CODE END PTD */
    
18. 
    
19. /* Private define ------------------------------------------------------------*/
    + K. y# C3 {/ j) G. d) _, z
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    ' S% r: q# W2 h  M* z! o) m
22. 
    
23. /* Private macro -------------------------------------------------------------*/
    7 Z" @- B' {4 i) z1 K
24. /* USER CODE BEGIN PM */
    
25. 
    
26. /* USER CODE END PM */
    3 O8 i0 U0 T% v* i5 h
27. 
    
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    
30. /* USER CODE BEGIN PV */
    
31. 
    1 L. f( v: _* `  i- y
32. //重定义printf函数，用于串口输出
    
33. #ifdef __GNUC__
    9 B/ V0 W' G9 x3 Q" J+ G
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    5 S) [7 U2 t0 x7 Q3 j9 b
35. #else
    " w- M7 M$ R2 m' ^& J7 z1 i
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    
37. #endif
    : ]+ i# s" R! B  }
38. 
    
39. PUTCHAR_PROTOTYPE
    7 g0 X4 q" I2 ~
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    , Y$ _! O) s0 }- n- c
42.     return ch;
    " ]: G+ Y  H5 n. N
43. }
    
44. /* ---------------------------------------------------------------------------*/
    
45. 
    
46. //获取系统时间，用于卡尔曼滤波
    ; g6 ]2 o0 x+ ^: A# h. j; u3 \
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    
48. {
    2 f6 w" w8 V/ V. y
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    3 i6 g' V' x+ j7 v
50. }
    
51. static uint32_t getCurrentMicros(void)
    
52. {
    % {* M. r3 b1 t. [
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    0 T* T4 P3 i+ Z- J( D- n  m9 e$ C& @
54.   GXT_SYSTICK_IsActiveCounterFlag();
    9 N: W3 U( A+ [5 H3 A- o
55.   uint32_t m = HAL_GetTick();
    
56.   const uint32_t tms = SysTick->LOAD + 1;
    
57.   __IO uint32_t u = tms - SysTick->VAL;
    6 P5 b7 j( a  u) F# @) Z  r
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    
59.     m = HAL_GetTick();
    & F9 U' a0 Q5 e& o/ @
60.     u = tms - SysTick->VAL;
    
61.   }
    $ y/ R2 g- ~& t# y- [9 p5 n
62.   return (m * 1000 + (u * 1000) / tms);
    # w4 i- R% T! M+ H7 O
63. }
    
64. //获取系统时间，单位us
    : A; T" z- u, [+ n, f; v+ Y
65. uint32_t micros(void)
    
66. {
    ' P  {: p+ A# f+ R# Z# E! Q' F
67.   return getCurrentMicros();
    
68. }
    
69. 
    ' \3 H& q7 `2 f! M
70. 
      d% R/ N$ r  A& z, x' t( R2 a9 P
71. /* USER CODE END PV */

复制代码

使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
! A1 o! @  N; c5 z
: J2 ~4 f/ M# z: r8 H- W4 i
4 a8 E' q5 M! i: P0 \
1 \- i3 m- o& D5 b4 |+ F6 J) C8 _8 h0 J在main（）函数中添加MPU6050初始化代码和发送位姿数据代码

1. int main(void)
   
2. {
   
3.   /* USER CODE BEGIN 1 */
   ' i# h8 q) G  q7 x: y4 N. C: h4 I
4. 
   
5.   /* USER CODE END 1 */
   
6. 
   ( j0 R1 s5 w2 z4 @5 J0 c% H
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
     Z6 D% w! R6 `# a
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   
10.   HAL_Init();
    
11. 
    0 w2 j9 ?  }$ Z* T2 y* U
12.   /* USER CODE BEGIN Init */
    
13. 
    
14.   /* USER CODE END Init */
    % {  h% L6 z3 r0 O/ K
15. 
    " ]5 b% F% m/ G
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    # W* m/ h8 w7 Z- W$ _. d5 @! E# _
18. 
    " {- |1 ?& V# I! I, `5 M1 A
19.   /* USER CODE BEGIN SysInit */
    3 O* z" K  O' u( i
20. 
    
21.   /* USER CODE END SysInit */
    
22. 
    
23.   /* Initialize all configured peripherals */
    
24.   MX_GPIO_Init();
    8 i& `: v: T0 u: X6 x, l
25.   MX_I2C1_Init();
    
26.   MX_USART1_UART_Init();
    / k6 `: `! }( W
27.   /* USER CODE BEGIN 2 */
    % m8 C" w1 U9 [, j4 Z9 Q3 u
28. 
    
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    - s7 m- C- \, @7 p) b
30.           printf("MPU_Init_Fail...");
    4 u) i# L( A' o: L
31.   }
    ! \/ E6 i8 K* p# R
32. 
    ) |' B& H6 v* y
33.   /* USER CODE END 2 */
    
34. 
      g' S' z- E0 J! }7 K
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    
38.   {
    & P% C7 N  j( p1 p6 i! d9 L; g
39.           GetAngle();
    ; q! Q2 f8 @8 g; U
40.           GetPitch();
    
41.           HAL_Delay(10);
    
42.     /* USER CODE END WHILE */
    # \2 N1 Z( I9 ?7 b: {7 {
43. 
    
44.     /* USER CODE BEGIN 3 */
    0 q- R! u  h: C
45.   }
    
46.   /* USER CODE END 3 */
    
47. }

复制代码

点击，下载程序


8 s, M- W- C1 q
; a* I+ |& m: K8 Y程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
* i) C5 {: h6 [, d. K' d# y" X9 _

! _# ]( @3 c; S" X" M  {# x" t# e3 y5 v
. }% X, A- O0 n5 C  { 作者：DIY攻城狮
, d/ r( n0 ?# C

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