3 a  K$ g9 O4 B
# j. D7 q5 {3 N: V

! p5 X0 v- b' i* c
背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。

MPU-6050简介
    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。
  ?# T1 `$ p- ^/ i* [0 i) Z
4 b5 `; }( X- G9 t, v6 y$ S3 @    MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。
+ J3 R; O- T: m+ c1 y6 X
6 |. a2 B0 U' d) E3 W& b0 ?' f


MPU-6050姿态获取与处理（惭愧，我也一知半解）
1 q+ I1 A- i: W    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
* R- t; j% d) f; p- C9 N& t' M5 h
. Y( {+ B8 U6 i0 E/ @
! X& M/ f& _( a* T互补滤波、卡尔曼滤波计算位姿示例

' g  t. e+ {* m9 K8 _% F+ M. Z+ v

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

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

  W- U0 U) |! T& [% }
8 Z4 n( l$ ?( F
* o- k  o$ c+ {5 K# U8 @7 k; S
一、一阶互补滤波算法
    MPU-6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，通过简单的计算即可得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角，需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正，通过加速度和角速度就可以计算 Pitch 和 Roll 角（单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用）。
. B5 s; G& {3 F! ]

7 q4 \8 @3 @- I: W7 C一阶互补算法如下：

1. //一阶互补滤波
   1 J6 Q$ N: R1 ^* x6 k' g
2. float K1 =0.1;         // 对加速度计取值的权重
   2 n6 X4 z) @& e7 J
3. float FirstOrder_Pitch;
   % {# M8 c" N  R. h6 X+ t; O# R& `
4. 
   : k- M, r# L: `* h
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   6 g0 r# T/ C9 [0 z
6. {
   * ?. N: p# b; [  h- a# ?
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   
8.   return FirstOrder_Pitch;
   ( d' j1 ~" Q2 k; P
9. }

复制代码

3 K7 z; n9 m: p6 X二阶互补算法如下：

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   
4. float SecondOrder_Pitch;
   8 a8 `4 X% N4 w0 U$ b3 S0 p
5. 
   
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   ( k2 ^. Z- E; e! M& D# v0 p3 F: x1 s
7. {
   + `4 ]+ P1 n9 s" ?; j9 K6 c# @8 V
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   0 {& ?5 M) R# y0 F8 V. d  h; I
9. y1=y1+x1*dt;
   ! W) a+ p  s: Q% K6 \0 c
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    , D, S! E) w# y6 `' g1 z/ I
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    ) x, X% G6 G- g3 C5 ?: `
12. return SecondOrder_Pitch；
    
13. }

复制代码

二、卡尔曼滤波

1. /* Kalman filter variables */
   
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
   
4.         float R_measure = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise
   % J, }* `  p" M, F6 l* b
5. 
   
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   3 p$ v. q5 Z: {$ T0 n% n
7.         float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
   8 q2 ?5 N1 X& G
8.         float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
   
9. 
   , I' v6 G4 Z5 ^4 v' h2 a
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    
11. 
    $ c* h  ]) o8 x1 H  k
12. 
    ! X9 X% G, t; \' q0 R8 S/ e
13. // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    * k5 E+ ?6 O- Z8 e. V4 W
14. float Kalman_filter(float newAngle, float newRate, float dt) {
    
15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    
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
    % ^9 `# O  `+ y
18. 
    3 {& x/ |+ L, v9 R2 ^7 P$ {# ?
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    % k8 R% y# o( S5 X1 T7 N' T3 N2 F' F
20.     // Update xhat - Project the state ahead
    
21.     /* Step 1 */
    & B: t8 j, A1 C# n( u' Y
22.     rate = newRate - bias;
    " c1 @, W7 I7 t! F
23.     angle += dt * rate;
    - L+ ~0 M0 f! R
24. 
    * S; c# _, q9 R) k" k8 x
25.     // Update estimation error covariance - Project the error covariance ahead
    8 X( a  r% n' ?
26.     /* Step 2 */
    0 t2 U# N1 ]+ Q2 E
27.     P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    # d) Y+ E# a+ ~0 `+ t
28.     P[0][1] -= dt * P[1][1];
    
29.     P[1][0] -= dt * P[1][1];
    . L* F" o. k) n9 Q' p: n% @, y
30.     P[1][1] += Q_bias * dt;
      S  M  ~' B$ D+ e4 S
31. 
    
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    
33.     // Calculate Kalman gain - Compute the Kalman gain
    
34.     /* Step 4 */
    
35.     float S = P[0][0] + R_measure; // Estimate error
    
36.     /* Step 5 */
    
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;
    : G6 f, ?# s6 H; c( |, x, _8 q# P
40. 
    
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    5 i( Y# w. w4 f) ?
42.     /* Step 3 */
    & d3 {) B' ~1 K0 |, b& g
43.     float y = newAngle - angle; // Angle difference
    8 e5 `/ `$ w# g! o% C& @% I
44.     /* Step 6 */
      D  Q% q. D3 E. W0 z+ x2 }
45.     angle += K[0] * y;
    ( w+ d: T) z- p9 S- c
46.     bias += K[1] * y;
    6 I  ]/ p2 F  _/ b
47. 
    
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];
    
52. 
    
53.     P[0][0] -= K[0] * P00_temp;
    1 n2 z. ]  V$ P0 z
54.     P[0][1] -= K[0] * P01_temp;
    8 Q- @' e0 [# }# n0 Z: T0 d2 J
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    
57. 
    + Y% |) d! ^' @5 S$ @
58.     return angle;
    6 A" J( ], R6 T2 e
59. };

复制代码

三、四元数法
8 h! q+ F( }2 U9 w  R
     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。

       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
/ q* |3 D# M6 v# T

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   
2. {
   3 C  d0 l2 a" ~/ t
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   3 A# R) ^# u. Q+ I+ l/ g
5.         long quat[4];
   
6. 
   
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   3 z3 Y: y4 i6 I
8.         if (sensors & INV_WXYZ_QUAT )
   / e2 W; x' s# C. {1 ^' w
9.         {
   
10.                 q0=quat[0] / q30;
    % o, @/ G4 E0 a5 y) 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
    
16.                  *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    
17.         }
    
18. 
    
19. }

复制代码

, H: W6 q! U2 P    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。

    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。

    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next

  a7 S. J2 k& @. Z

( K# H, J6 z) `) i+ `3 j输入工程名字，点击Next
6 {  w2 b' m, z* A" N' _
4 J9 Z; ^- s, i1 Z( F* _

+ Z( w" d4 L& L设置完成后，点击Finish


) n8 L: H/ c0 C5 h: M- n* K* |
进入MCU配置界面

  z$ |" K  k; m7 k  [

5 h1 }) Z, @" L3 y设置I2C与MPU-6050通讯。
) |9 B( _6 f0 p. i6 ^* |( d& Y


设置串口与上位机通讯，实时显示MPU-6050位姿。


- {  X. N( Z4 q8 S7 o  h
设置系统时钟及调试模式。



" G% p+ a  ]2 r- u5 p* L设置时钟源。



0 E# I, H0 J7 |$ _: w" C全部设置好后，芯片引脚的分配情况。

7 I5 O  S* l6 t4 z7 G

. a! T, Y' D" E6 R% H- w+ a& u' y在时钟配置里设置时钟最大频率。



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


: g3 R+ j% w. V6 \9 z% E1 s
点击此处，生成代码



3 z$ K* Q# k# W" J' Q( P' r9 N在工程文件夹中创建iCode文件。


6 A; e' Y3 G0 }, S6 P- Y3 u2 ?$ J
将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。
, E  E# o, v( t$ j% C- \% C3 u
9 Y: `- X1 U' M/ T0 x2 J9 u- d
; o. a% Q; N( H0 U& B  {
6 S( \' D4 k2 I% P. t% B/ W点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。
3 M- M8 @0 s0 i# X: X

$ ?4 z2 }( u% |8 j5 Z/ K  c
2 w/ ^3 V/ `0 U, A% j+ H. q" E按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

6 \# S/ ?# J; @+ j" j& D
) p% g; C3 k  Q+ P1 O0 s$ e


8 ?, _! d* K7 B# O( }然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
4 L8 m4 f. d7 h

1. /* USER CODE END Header */
   
2. /* Includes ------------------------------------------------------------------*/
   # N+ E8 A; y2 y( E
3. #include "main.h"
   
4. #include "i2c.h"
   
5. #include "usart.h"
   
6. #include "gpio.h"
   8 e; N) `1 ^: E4 k/ R( ]; t8 K% V2 l
7. 
   
8. /* Private includes ----------------------------------------------------------*/
   6 f4 b+ R2 M# ~$ L0 b
9. /* USER CODE BEGIN Includes */
   3 P* ^" a9 S0 p+ L$ G
10. #include "mpu6050.h"
    2 M8 t. z, X# T# O, ?% ]$ i, Y' Z
11. #include <stdio.h>
    : Z8 M" x* Y$ Q; W! A" D4 r) c
12. /* USER CODE END Includes */
    
13. 
    
14. /* Private typedef -----------------------------------------------------------*/
    0 g. e. D/ ?  }  Y% x
15. /* USER CODE BEGIN PTD */
    * C7 c1 @0 D: p0 _1 m& z
16. 
    
17. /* USER CODE END PTD */
    
18. 
    * @& I$ i7 p5 l* z' v" J
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    # k; R$ G' n9 D2 P, u
22. 
    + Y+ d/ Y) i. h1 S$ f2 e* Q
23. /* Private macro -------------------------------------------------------------*/
    & X" s7 K3 y7 W8 s, m' e3 M
24. /* USER CODE BEGIN PM */
    1 j  K: I5 |0 Z* E, _
25. 
    
26. /* USER CODE END PM */
    " G( W2 i, i1 P' `
27. 
    
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    ! \& l! c8 W, I9 F
30. /* USER CODE BEGIN PV */
    
31. 
    " ^" `* a2 T3 f# S
32. //重定义printf函数，用于串口输出
    
33. #ifdef __GNUC__
    
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    
35. #else
    
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    
37. #endif
    2 ~- w4 V6 H& ^8 V# O  L, B2 O, e  A
38. 
    : N5 R2 r: d, z7 P4 s
39. PUTCHAR_PROTOTYPE
    ) t% {& ~$ a3 Z% {' C1 v
40. {
    & B  c; C" ^4 J
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    , |- h0 l$ A+ ^# N/ {
42.     return ch;
    ' L5 h: v. i1 q: a% ]
43. }
    
44. /* ---------------------------------------------------------------------------*/
    ' c' _3 V' D( J3 r
45. 
    5 s' A' i2 b6 X
46. //获取系统时间，用于卡尔曼滤波
    
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    
48. {
    ; d) d- z# T5 A2 h7 y6 V
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    2 c& c* `0 d& d
50. }
    
51. static uint32_t getCurrentMicros(void)
    ! x7 _0 l# i0 K4 T2 p  Z( K
52. {
    * A; S, J: V- Z9 i3 W2 g
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    
54.   GXT_SYSTICK_IsActiveCounterFlag();
    
55.   uint32_t m = HAL_GetTick();
    ! A! E) D3 N+ b6 r
56.   const uint32_t tms = SysTick->LOAD + 1;
    
57.   __IO uint32_t u = tms - SysTick->VAL;
    
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    + O6 Q& ~$ K7 F3 {3 f) Q+ C$ _
59.     m = HAL_GetTick();
    8 f+ |; C( w& S6 `3 W0 b
60.     u = tms - SysTick->VAL;
    
61.   }
    
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    
64. //获取系统时间，单位us
    
65. uint32_t micros(void)
    
66. {
    
67.   return getCurrentMicros();
    
68. }
    ' H6 J" [+ l" b0 n
69. 
    + ~# ]: u6 _- U* d
70. 
    
71. /* USER CODE END PV */

复制代码

使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
& S* g1 `% l/ ]/ b. k


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

1. int main(void)
   
2. {
   
3.   /* USER CODE BEGIN 1 */
   
4. 
   
5.   /* USER CODE END 1 */
   . j$ a' q; x% Z# U- G
6. 
   ; N# B$ b7 s/ j+ k" F! X
7.   /* MCU Configuration--------------------------------------------------------*/
   . N* H, ^: S, T: e
8. 
   : u! T- K8 U" x# t
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   2 {) Z% `+ m. F2 M# F/ Z
10.   HAL_Init();
    ! v2 d0 g+ o) c
11. 
    
12.   /* USER CODE BEGIN Init */
    
13. 
    ; T. j0 f, P4 v
14.   /* USER CODE END Init */
    8 z: ^$ Y2 Y+ v! |% l8 A% H: X
15. 
    
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    # X( S) S9 ~9 F- f' @4 ?0 w) q$ M
18. 
    
19.   /* USER CODE BEGIN SysInit */
    8 q, o. E) ~" i+ P) H
20. 
    - _* S% z" t3 j2 v$ b8 j
21.   /* USER CODE END SysInit */
    - S6 Y$ o9 m/ b, [6 q4 Z
22. 
    ; o+ T1 }" p; _$ G0 R
23.   /* Initialize all configured peripherals */
    0 |8 c! W; p; }' M/ A
24.   MX_GPIO_Init();
    
25.   MX_I2C1_Init();
    % H5 i" j0 ?4 }# h& c
26.   MX_USART1_UART_Init();
    8 O5 @$ D, R1 v" g* `4 E
27.   /* USER CODE BEGIN 2 */
    6 i$ b& n# I7 w8 H- b; X/ |% w6 V
28. 
    " l$ k- n3 ~' M, Z
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    
30.           printf("MPU_Init_Fail...");
    
31.   }
    
32. 
    ) t! j6 t+ v( ~! f$ ^$ ]0 a
33.   /* USER CODE END 2 */
    
34. 
    & p0 ]. n7 J( h8 ^
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    8 g" i; N! ]- Q, C2 d
38.   {
    
39.           GetAngle();
    
40.           GetPitch();
    
41.           HAL_Delay(10);
    $ I& I& j2 ~: ]5 j2 T: T4 }- P. w
42.     /* USER CODE END WHILE */
    
43. 
    
44.     /* USER CODE BEGIN 3 */
    2 ?/ X# o6 K  E+ V! |# d7 \( t
45.   }
    
46.   /* USER CODE END 3 */
    
47. }

复制代码

( ^' s% f( m1 ~# C) J' t& a& w点击，下载程序

/ S: l% {% ?4 P( S0 v2 H  P- ~
. o. B# O3 Q, D
程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。

7 c5 b4 V, c' Z/ _2 B. m

作者：DIY攻城狮

! o" O3 c/ g( N( O* r" g

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