+ ]+ V( r' A' G% C# b! E

( f1 o0 t- G- ^! q) V

! [! f" j3 D2 g( ]3 F0 k背景
/ P$ P% H$ g, O        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
8 F$ o5 z# t, x+ b1 l" g
MPU-6050简介
    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。

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




MPU-6050姿态获取与处理（惭愧，我也一知半解）
    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
$ c3 r+ }  D. Q4 W6 z
9 d1 q: J8 p: ^7 |
互补滤波、卡尔曼滤波计算位姿示例
+ s% a9 `; H# L8 x9 J
( \) }  ?) D( L+ C
. H( [/ P2 V" C9 u" G9 v8 F0 }
5 ~7 k' M; U' p, o互补滤波、卡尔曼滤波计算位姿示例

9 _# x  X3 a! ?3 S7 L    除了使用加速度计计算的噪声较大，卡尔曼滤波，一阶互补滤波，二阶互补滤波看着差不多，O(∩_∩)O哈哈~（四元数计算的示例就不演示了，有兴趣的可以自己研究一下。）
7 u8 [0 n) s* O8 c! E1 P4 }% A2 D    1）一阶互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
    2）卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程（来源于百度词条，我也看不懂这说的啥）。
    3）硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到Yaw、Roll和Pitch（使用四元数计算位姿，好像会将初始化的位姿设定为零位）。
2 b+ h1 r$ C7 S9 q. S) Y) g! a

$ D8 c! U- s5 `+ Q% w, w

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

! t- k) b- l4 A/ c一阶互补算法如下：
0 t! L  L3 R+ a1 W) k: |

1. //一阶互补滤波
   " f# q! o! c) ^
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   ! Y0 h6 i* P( n0 @# @
4. 
   % m& }. c) n# j7 j8 }* a, w
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   " O* [% J5 p. O7 M( o! j5 c- W8 E, n
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   
8.   return FirstOrder_Pitch;
   ! d; M' v* J1 @7 j/ G" Y+ }
9. }

复制代码

' I. {' U# ~+ I  ~, [8 F0 Z( ]二阶互补算法如下：
5 h& B9 y' x# S3 q

1. //二阶互补滤波
   2 ~! Z, ]/ O6 y/ {2 ]
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   
4. float SecondOrder_Pitch;
   
5. 
   & j9 `7 D$ S6 M& H. ]
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   , R# e+ C* W' Y% W; c
7. {
   * r9 y+ B# N" E
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   
9. y1=y1+x1*dt;
   ( h4 W4 X3 |* e4 k: h. ~
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    
12. return SecondOrder_Pitch；
    - M! @5 ?3 q7 a" h: F2 m2 c
13. }

复制代码

二、卡尔曼滤波

1. /* Kalman filter variables */
   ( {+ d& ~* A  X  i  z; G
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   
3.         float Q_bias = 0.003; // Process noise variance for the gyro bias
   : ^. f' {; h3 d6 k0 ?/ ~
4.         float R_measure = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise
   / N5 [6 p1 ^6 b' @0 L% ?0 j
5. 
   
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   , [# v+ E; D' M/ B
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. 
   - H, B8 Z$ A! a' Z7 O, T
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    $ n& w1 r, Q9 c/ u3 u1 F
11. 
      ?2 n- m6 q, \% w, ~# n4 }
12. 
    
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
    * Z" t( @  A3 a: \
16.     // Modified by Kristian Lauszus
    0 W. ^3 l6 Q, x4 Y
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
    5 e3 P, i9 n) h8 a
18. 
    
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    " r$ e2 l1 g4 a8 s3 V
20.     // Update xhat - Project the state ahead
    ( H. f3 Z9 b4 V3 E
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    ; i0 E8 \  P1 z
24. 
      W3 }/ \7 ]: \! w/ g
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);
    
28.     P[0][1] -= dt * P[1][1];
    
29.     P[1][0] -= dt * P[1][1];
    
30.     P[1][1] += Q_bias * dt;
    ( W0 @# J; j6 O6 l
31. 
    
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    * L" q- L. i% k/ G
33.     // Calculate Kalman gain - Compute the Kalman gain
    ) S) m4 _# J3 E
34.     /* Step 4 */
    
35.     float S = P[0][0] + R_measure; // Estimate error
    $ K! B6 p; W3 t/ {: w
36.     /* Step 5 */
    " ?7 d, y( x, f# O1 H0 G
37.     float K[2]; // Kalman gain - This is a 2x1 vector
    $ O* u9 {3 u: K' n5 @
38.     K[0] = P[0][0] / S;
    2 t% K& B6 C! k  @! i
39.     K[1] = P[1][0] / S;
    
40. 
    ! N' n: w2 m, s" p5 i. n
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    7 y& F/ `* X) z/ b* V
42.     /* Step 3 */
    . i8 E" D) S  C2 E. g
43.     float y = newAngle - angle; // Angle difference
    
44.     /* Step 6 */
      n9 l6 h8 A7 D9 e' I
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    9 }0 l- P5 g' u% n. i
47. 
      P9 |0 S3 c# \( t  y
48.     // Calculate estimation error covariance - Update the error covariance
    
49.     /* Step 7 */
    
50.     float P00_temp = P[0][0];
    
51.     float P01_temp = P[0][1];
    " K: x$ E1 i4 U8 a8 \
52. 
    & @7 A1 _) y( I$ H9 [) y% L+ l$ L
53.     P[0][0] -= K[0] * P00_temp;
    
54.     P[0][1] -= K[0] * P01_temp;
    
55.     P[1][0] -= K[1] * P00_temp;
    % y; Y0 g& \: {. f
56.     P[1][1] -= K[1] * P01_temp;
      C' _8 n/ s# K% B1 A# R
57. 
    
58.     return angle;
    
59. };

复制代码

三、四元数法

     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
9 z: m: L7 T1 K8 _
       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
  |6 `7 k. m9 m1 x, u) X3 [

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   + {$ j# e4 ~& }3 i  \
2. {
   
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   
5.         long quat[4];
   : n; H9 n" H4 L8 B  o$ V
6. 
   
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   1 N* W6 |: ~% j* ]) l( e
8.         if (sensors & INV_WXYZ_QUAT )
   
9.         {
   : j1 Z# P5 [1 |0 V+ z
10.                 q0=quat[0] / q30;
    + X3 E& K7 c  ~' L) {* \
11.                 q1=quat[1] / q30;
    
12.                 q2=quat[2] / q30;
    
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
    " e" F+ S9 f: L& b1 R
16.                  *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    4 ~, \/ M+ }# W5 l4 A" N
17.         }
    
18. 
    
19. }

复制代码

+ g4 o! E/ r/ R$ h8 l( H9 }) o    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。
6 k" v5 A% Q$ @# E+ |3 b
    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。
4 G+ e5 s, A. ]* J. n2 O
    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
8 L: F3 n$ d& `3 D3 D1 H8 Q
, o% v( X9 l+ T/ }) P& t$ X

7 X- b/ [# S7 f: G输入工程名字，点击Next

% g7 L9 k- S& S# T3 _5 h/ }

1 @  b, [6 R& w4 M* |7 ~设置完成后，点击Finish



进入MCU配置界面


: O' ]& g" v* x& I' y
设置I2C与MPU-6050通讯。


4 |$ Q* I$ @( x- C' Y) m, Q
7 A4 a& ]. C- m& a设置串口与上位机通讯，实时显示MPU-6050位姿。

) v) u7 `: F1 l  Q0 W' @: H
; q# y: @& c6 I$ `! N% I5 _& B
设置系统时钟及调试模式。
) e+ E9 `) }% O  b6 b* v

/ ^; w3 \. q. s6 e
设置时钟源。


3 r: Y3 i2 c: U* w3 D$ i$ U
全部设置好后，芯片引脚的分配情况。
4 i; F9 Y; M- Y, s

9 b" j  f) U5 l/ r7 @. I  ~/ Q6 }: x
9 n# [  {8 v, l$ f$ S在时钟配置里设置时钟最大频率。

- F; T/ d4 g. Q, X/ I: ?4 m- R
4 y+ e4 d( Z, U  ^! k( o
在工程管理——>代码生成器中进行设置
4 ~( c0 E$ T: [! q6 Z/ Q
% H1 S, N6 e& U' i

) p$ ]: s) n0 T! D$ }2 Y- S( U3 Q点击此处，生成代码
! L5 `& a7 K$ \) J6 [* |& v# }' [
* d6 K6 z% S4 |* {2 O! P: z4 v

在工程文件夹中创建iCode文件。
0 c  D# [' q+ i9 }: @- c; ?2 u

3 Q/ ?, P" D0 i' C8 P' b6 N
& c+ L" m" v; Z9 u将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。
& J# X! j" p  |8 q. w/ y. C  ]" P

8 h' A$ p9 l2 g7 Q' S% S7 h
8 d2 h  K  I9 [# o- A* ^点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。
+ r; @( D# o' F2 g, e1 P; k: [


按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。



! _4 C+ t8 f; [: k: k4 o

然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
* ^1 Q# ]0 E8 T- [5 |

1. /* USER CODE END Header */
   ) K3 o* z2 ]% h- h6 E* B
2. /* Includes ------------------------------------------------------------------*/
   
3. #include "main.h"
   
4. #include "i2c.h"
   
5. #include "usart.h"
   * Q) ^. R4 Y+ _/ q- O3 T
6. #include "gpio.h"
   ; ^2 |+ s: a/ `; H# m% v8 r
7. 
   
8. /* Private includes ----------------------------------------------------------*/
   
9. /* USER CODE BEGIN Includes */
   8 N3 M! i( d0 r- T
10. #include "mpu6050.h"
    5 t" f" r7 S) \* x) B' _) i
11. #include <stdio.h>
    
12. /* USER CODE END Includes */
    
13. 
    0 V" m3 l! {7 Z/ T  x" ]& f  E
14. /* Private typedef -----------------------------------------------------------*/
    
15. /* USER CODE BEGIN PTD */
    " P- h7 s5 H4 y# ^4 g+ O
16. 
    2 b$ f# S3 m( D; l
17. /* USER CODE END PTD */
    4 f; I; r+ c* G
18. 
    
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    6 W* J' e0 H4 O+ ?3 C$ t1 P
22. 
      {3 Z* v, O: N) e3 L6 d$ [
23. /* Private macro -------------------------------------------------------------*/
      P' l; V4 V+ Y% X5 k: K
24. /* USER CODE BEGIN PM */
    - D' O+ T) B& j* ^
25. 
    
26. /* USER CODE END PM */
    
27. 
    
28. /* Private variables ---------------------------------------------------------*/
    . m$ |& o# D0 J2 t  r; \4 h
29. 
    
30. /* USER CODE BEGIN PV */
    
31. 
    
32. //重定义printf函数，用于串口输出
    ) T. I* F* V) \. [$ m) y& A
33. #ifdef __GNUC__
    
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    + n8 \) O) j. \' S2 A
35. #else
    
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    " o; }4 u: k: u* l; f  m
37. #endif
    
38. 
    
39. PUTCHAR_PROTOTYPE
    ! X9 Q' f* U- F# D5 `0 F
40. {
    7 I; V" R. f: [6 T3 b( t
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    ; ^: U: F. d* S! K: H* s3 b
42.     return ch;
    
43. }
    / S+ ^; C+ B, E& X0 E
44. /* ---------------------------------------------------------------------------*/
    
45. 
    : S9 O" J; t. M$ T& u
46. //获取系统时间，用于卡尔曼滤波
    # P" @" ^  s- q7 C$ k( y
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    ) E( e/ ]) i, s. v7 @2 [: |. X7 v7 B
48. {
    8 O% x! ^3 y9 d3 k/ ?
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    
50. }
    
51. static uint32_t getCurrentMicros(void)
    ; j; Y/ R$ q* f. u
52. {
    0 S- \1 |# t; o9 @
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    
54.   GXT_SYSTICK_IsActiveCounterFlag();
    ( ]+ Y2 V1 M/ H5 z& v+ c1 a9 D
55.   uint32_t m = HAL_GetTick();
    
56.   const uint32_t tms = SysTick->LOAD + 1;
    
57.   __IO uint32_t u = tms - SysTick->VAL;
    $ v7 M$ S( U4 |$ q* b1 o
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    
59.     m = HAL_GetTick();
    
60.     u = tms - SysTick->VAL;
    2 d' f) n( ^3 `% H3 {7 E7 j) h
61.   }
    # k/ m3 x* h+ b
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    
64. //获取系统时间，单位us
    ; U# C3 W# E5 e/ q  O+ c+ ~' e
65. uint32_t micros(void)
    / b, w' k0 `! e4 G0 n8 U+ i
66. {
    : V" b% o0 [* W; Q; Z1 e
67.   return getCurrentMicros();
    
68. }
    
69. 
    
70. 
    
71. /* USER CODE END PV */

复制代码

使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
6 o0 {% ^: ]9 q


在main（）函数中添加MPU6050初始化代码和发送位姿数据代码
( I* |/ P% W) |* Z+ ?

1. int main(void)
   
2. {
   
3.   /* USER CODE BEGIN 1 */
   
4. 
   
5.   /* USER CODE END 1 */
   : A+ L/ @8 S; u; a2 o' q3 f  i
6. 
   
7.   /* MCU Configuration--------------------------------------------------------*/
   : D$ n6 d" T/ n2 B1 r
8. 
   
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   % Z6 ~! t) N1 O- h# D" T
10.   HAL_Init();
    
11. 
    
12.   /* USER CODE BEGIN Init */
    6 L& `# V0 ]* V0 n/ `
13. 
    / r8 C- l3 x; O5 S
14.   /* USER CODE END Init */
    4 Z6 K2 g- Q9 R; K
15. 
    
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    # M1 A1 t" V1 x, S
18. 
    
19.   /* USER CODE BEGIN SysInit */
    
20. 
    ( X% O# ?# X) `! {
21.   /* USER CODE END SysInit */
    
22. 
    
23.   /* Initialize all configured peripherals */
    7 d! \& n7 S7 {9 p+ ?) v1 L/ ~* L5 R
24.   MX_GPIO_Init();
    8 b- _8 p$ \+ `/ a% D
25.   MX_I2C1_Init();
    
26.   MX_USART1_UART_Init();
    . m& E+ L/ F# O7 i* U
27.   /* USER CODE BEGIN 2 */
    
28. 
    
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    
30.           printf("MPU_Init_Fail...");
    
31.   }
    - C$ y$ P7 I+ B2 Q, c
32. 
    
33.   /* USER CODE END 2 */
    " w9 ~9 n: R! v4 F  a4 F
34. 
    
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    0 \+ r7 x7 T! ]" V
37.   while (1)
    4 w, Y0 i: I% f* p* w, o
38.   {
    
39.           GetAngle();
    
40.           GetPitch();
    / z+ |( P9 r9 ]3 b% v1 h4 ^  [" ~" @
41.           HAL_Delay(10);
    
42.     /* USER CODE END WHILE */
    - s8 U; }& x( d) x" a) B! `* F
43. 
    
44.     /* USER CODE BEGIN 3 */
    
45.   }
    
46.   /* USER CODE END 3 */
    ' M5 P0 f# b! ]8 x
47. }

复制代码

& I' L+ C- k3 |9 ?+ M1 k4 f点击，下载程序
9 N. C; y- y+ u- w
3 `- w0 j6 ]0 F$ C# t3 Z2 f' Y

9 g7 Q+ I* w# x程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
" l% W" }' _) L* f+ {5 z( c


# ^- d7 N( R  Q2 C+ y4 r 作者：DIY攻城狮
4 u% }# H7 I- {* }/ K

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