: n1 r2 x# A; C% n' L& B
4 \5 W5 u3 J# K+ m( J

: W2 f( C" j7 s% H背景
7 x6 a1 ~% l1 B0 v        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。

MPU-6050简介
    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。

; D, G. P0 K) C( h) Q1 B+ s2 [    MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。


  P+ e+ [6 n  x5 z: Q

MPU-6050姿态获取与处理（惭愧，我也一知半解）
    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。

0 [0 t  X0 o) N2 t! K0 J2 D
互补滤波、卡尔曼滤波计算位姿示例


! N% n: M# N# \3 g: @7 e
互补滤波、卡尔曼滤波计算位姿示例

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

1 j8 ?2 k8 q2 z) u. J8 Y) D2 s
* M. }  P$ ?; l2 u* p7 I! h

. l: S1 x% n, {9 K% @$ `& S一、一阶互补滤波算法
3 w" ]4 S) m: m% X  A$ ]- j    MPU-6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，通过简单的计算即可得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角，需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正，通过加速度和角速度就可以计算 Pitch 和 Roll 角（单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用）。
( [( o* N( X; M( ]
  v1 n3 U! B( _/ T
( J3 @1 ]4 p! C, d' d2 W. }8 T一阶互补算法如下：
, x- i& ?6 s1 m/ v0 J. j0 z& @$ `3 \

1. //一阶互补滤波
   
2. float K1 =0.1;         // 对加速度计取值的权重
   0 R# J# j8 Q0 `
3. float FirstOrder_Pitch;
   4 C/ r: h* N- J8 W
4. 
   
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   : B! J* W, ?8 T
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   / _; {; `) U" Q$ H
8.   return FirstOrder_Pitch;
   ) K+ D3 E& s+ p$ Y3 j4 L$ s
9. }

复制代码

- O  b7 Y9 a( c* F- Z- b1 H二阶互补算法如下：
9 F% ]( k, {( Q  |  C

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   
4. float SecondOrder_Pitch;
   ; n; s/ ]% K9 m& ?
5. 
   
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   
7. {
   
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   : C: ?* w! @: m) Y3 c  {
9. y1=y1+x1*dt;
   % W: h) z0 q( ?  L6 L0 r
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    6 E8 o7 C) [2 E# O; S. g
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    
12. return SecondOrder_Pitch；
    5 y' Q0 D+ B+ W4 t2 {! |7 l( k
13. }

复制代码

3 e" h  f& E& ^! |5 x+ P; W# @二、卡尔曼滤波
3 j3 F  K! y1 J$ R; A) |1 W0 m. L

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
   
5. 
   / j& h7 `3 R  _
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   
7.         float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
   ) {  F5 D. _9 i! C+ b! S
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
    
11. 
    # D- Z% Q8 Z8 c
12. 
    3 w( n! h! J* U/ ]) k
13. // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    ) g  A6 k" [9 B! A
14. float Kalman_filter(float newAngle, float newRate, float dt) {
    : K/ [, p5 W) y: G6 a
15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    * s% y# e) L% Z
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
    
18. 
    . O# ]4 q4 P2 t, ]' V
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;
    3 y# t% @) O6 }' \
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);
    4 s# u" b5 |: Y3 ?2 V
28.     P[0][1] -= dt * P[1][1];
    : Q& Q8 m+ c  c" E
29.     P[1][0] -= dt * P[1][1];
    
30.     P[1][1] += Q_bias * dt;
    / R  @; P) j# x- C5 V; s
31. 
    3 A1 j- a& K% a1 W. r
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    
33.     // Calculate Kalman gain - Compute the Kalman gain
    ' {6 H: @5 L- L3 ~
34.     /* Step 4 */
    - o, e: u, m( F$ M. D& S5 }
35.     float S = P[0][0] + R_measure; // Estimate error
    ( l' [* k: `* |6 H
36.     /* Step 5 */
    ; t& l  A$ {) p0 ~
37.     float K[2]; // Kalman gain - This is a 2x1 vector
    
38.     K[0] = P[0][0] / S;
    # Q* g; E+ y0 Y- }3 W& V4 L
39.     K[1] = P[1][0] / S;
    
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 */
    ( Z; Z9 B8 k8 G4 N3 C% A5 S- M
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    
47. 
    + Y7 W- e! B- B- \/ l
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;
    7 |" H4 i0 }( K) D$ P. g5 |
54.     P[0][1] -= K[0] * P01_temp;
    
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    
57. 
    % r$ {" J# C- I& O) i! o
58.     return angle;
    , f. v/ j  F1 K. j; a  Q
59. };

复制代码

/ p# H5 k9 o1 f6 M; }三、四元数法

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

       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
  b! F4 p6 k+ h" Z2 ?! Y4 P

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   
2. {
   
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   7 k) c! R8 D- k9 E& o2 f* S0 L
5.         long quat[4];
   
6. 
   ' x6 R$ j* `  ?
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   
8.         if (sensors & INV_WXYZ_QUAT )
   
9.         {
   1 v4 W3 h) u% Y  \& {! x5 F- I6 R, b
10.                 q0=quat[0] / q30;
    / Q5 q9 b$ K7 E! ]. A" b
11.                 q1=quat[1] / q30;
    6 N& x  ~8 b& l, H! ?$ R! I
12.                 q2=quat[2] / q30;
    0 k! o% T( n" \3 x* ?, ^0 U
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
    7 ]' g3 _2 u/ R! v7 E' x7 I
17.         }
    
18. 
    
19. }

复制代码

' A# }; }3 v! u/ h% m9 g    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。

    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。
, q( u, C/ L$ ^
# L9 y: A2 E: F/ m    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
; p- t5 L. x' p- J7 k7 o3 N  _
( Y, @, v' F, o( u

输入工程名字，点击Next


$ X# R# r+ a3 i% [* |- X- @
设置完成后，点击Finish
/ W4 w2 p' d% b

6 o; m' G" U4 u; V
: k' x! |# M* g. Q' Y7 z$ n. z进入MCU配置界面

# O2 o+ b$ ]7 o: P
1 V% F2 E2 F+ d( C
设置I2C与MPU-6050通讯。



设置串口与上位机通讯，实时显示MPU-6050位姿。
6 p, a$ o# f& X6 @  |


9 [% M% w) h" v" U设置系统时钟及调试模式。
" V, `7 u: R5 k  m/ z* v/ |


设置时钟源。



全部设置好后，芯片引脚的分配情况。
9 d7 ]7 Z/ d3 ~& N0 I6 E4 F
, ]+ C; J7 F# Z* m0 c6 r
+ D' t6 X% o5 r6 G4 Q2 S
  C- x. _6 @6 h# z在时钟配置里设置时钟最大频率。
0 b: g5 r1 H3 y5 p1 d# t# B. k
3 ]- [0 w/ V$ R6 {/ \' }- i

; x5 `' H+ h2 ~' r% }在工程管理——>代码生成器中进行设置

0 W4 j1 _$ h; c: ?4 g$ p

点击此处，生成代码


, Q4 Z7 K* Y5 R* x
* z! X; f$ V2 w& l. I/ U  i在工程文件夹中创建iCode文件。
8 O5 {# B' d/ W" O


将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。


: F5 ~( X' r3 w; W7 l$ A& O
1 d2 G$ z6 i( j$ |点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。

  E9 V9 h5 k1 F: Z6 M
0 Q) t" o2 [! N/ C* p; X, n$ H
按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

9 [$ _( F! ?' n1 q% b
# ]  a$ |# c* D

0 L4 M. E. h$ S( E/ P
0 ~. g) M8 i- U8 f然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。

1. /* USER CODE END Header */
   + i9 ]" W7 I) \( N' q1 r! ^6 v$ R
2. /* Includes ------------------------------------------------------------------*/
   " }$ c& P$ E; z! D
3. #include "main.h"
   
4. #include "i2c.h"
   
5. #include "usart.h"
   ! S- E4 U  o) F7 P8 e3 C; \
6. #include "gpio.h"
   
7. 
   ( ]6 V, v# \7 h1 ~  B
8. /* Private includes ----------------------------------------------------------*/
   9 A* E! L3 I: [
9. /* USER CODE BEGIN Includes */
   8 i' d1 u) I. f* L( X$ r# |1 |
10. #include "mpu6050.h"
    " ]" r4 A' S. U  {/ @
11. #include <stdio.h>
    1 z, L! k) v/ Y
12. /* USER CODE END Includes */
    * v) M9 F7 I2 y3 M9 K
13. 
    
14. /* Private typedef -----------------------------------------------------------*/
    & h: K7 b3 i8 P( `
15. /* USER CODE BEGIN PTD */
    
16. 
    9 O2 C% m) D- u/ A# c, T
17. /* USER CODE END PTD */
    2 t, z/ y- z6 J0 Z4 h1 t/ U9 n
18. 
    
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    3 E3 g8 i% P7 J0 @& F5 v+ z% I
21. /* USER CODE END PD */
    
22. 
    
23. /* Private macro -------------------------------------------------------------*/
    
24. /* USER CODE BEGIN PM */
    
25. 
    7 ]- b# y& @  H0 s7 Z
26. /* USER CODE END PM */
    1 H0 v$ H; U( W1 E6 U
27. 
    ' o  |9 s3 Y; `  {
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    ) r2 Y' h" U* s
30. /* USER CODE BEGIN PV */
    + a( B1 ?1 i0 a" V' c) x, R2 k
31. 
    
32. //重定义printf函数，用于串口输出
    . C9 p# E( G" t1 ?
33. #ifdef __GNUC__
    . P% r4 q# y, t7 ]
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    
35. #else
    , ?4 `' `' j# h6 H
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    # Q6 t- M4 u: @3 p
37. #endif
    
38. 
    
39. PUTCHAR_PROTOTYPE
    
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    
42.     return ch;
    
43. }
    7 q' N8 C+ J" [, `+ i
44. /* ---------------------------------------------------------------------------*/
    
45. 
    
46. //获取系统时间，用于卡尔曼滤波
    % D6 x" _  u1 ^6 N& [. o$ H
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    " i* _! y3 m& D- X
48. {
    0 k9 N- w: T# k, g- J; M' H
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    7 x( }3 a! a: \8 m2 d+ I6 j- `+ b% \
50. }
    " I) U; w. o9 _+ d' p$ z
51. static uint32_t getCurrentMicros(void)
    
52. {
    
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    ) j% I* e/ A) M9 z) g
54.   GXT_SYSTICK_IsActiveCounterFlag();
    
55.   uint32_t m = HAL_GetTick();
    , i  Y+ d0 Q" O7 E4 y5 j
56.   const uint32_t tms = SysTick->LOAD + 1;
    
57.   __IO uint32_t u = tms - SysTick->VAL;
    / t, v  h' L: ?2 e' j; ]& s& _
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    , |& h: A" X# j  a" e
59.     m = HAL_GetTick();
    
60.     u = tms - SysTick->VAL;
    3 f+ Y9 s6 `/ P0 q8 Q& t
61.   }
    % l) k' \  t& d& Q
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    * Q9 d/ w& g! i6 H7 D
64. //获取系统时间，单位us
    ' X& D' g9 e7 P
65. uint32_t micros(void)
    
66. {
    
67.   return getCurrentMicros();
    ! V( k1 T9 `9 F- u
68. }
    
69. 
    
70. 
    & N- f! A7 A/ b& n- }" V' S3 ?
71. /* USER CODE END PV */

复制代码

* z, a$ h" K3 y- ?使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面



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

1. int main(void)
   # o8 v7 S( M1 m3 _$ V9 K$ n- ~
2. {
   
3.   /* USER CODE BEGIN 1 */
   
4. 
   0 _, L0 N- ?+ f/ h, c8 N) I
5.   /* USER CODE END 1 */
   ( q/ m4 G' y: x! u4 q9 l
6. 
   3 Q( X: p4 w2 _
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
   
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   8 z4 W! v/ H9 c4 n, Q
10.   HAL_Init();
    
11. 
    
12.   /* USER CODE BEGIN Init */
    6 k7 V% `& W# z1 y" j
13. 
    3 x# F3 a" _% c8 [; j( e3 t; a
14.   /* USER CODE END Init */
    7 r- v7 F1 i+ D6 T! }5 U5 r1 I3 b8 a! e
15. 
    7 }; l4 t3 U) C6 x7 y" ~
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    
18. 
    ) w+ X7 h0 ^: M: v, |. t
19.   /* USER CODE BEGIN SysInit */
    ( K3 {( z1 d8 \
20. 
    9 m/ G  {$ K& W4 }$ |2 q. C: z
21.   /* USER CODE END SysInit */
    & {  B/ B6 ?7 E1 U0 T
22. 
    1 c2 \4 L- r4 ]( ?' s: R
23.   /* Initialize all configured peripherals */
      y4 g3 {0 e6 A% \8 y  T' I$ e
24.   MX_GPIO_Init();
    5 ^: N  g! |0 e
25.   MX_I2C1_Init();
    ! Q6 b5 {. R+ P) Y
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    
28. 
    / `+ c% t6 W8 |* F
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    5 O+ F1 A- [' ?, s8 [; u
30.           printf("MPU_Init_Fail...");
    
31.   }
    
32. 
    8 B# c# U4 V: G) i1 J% m0 a/ ?
33.   /* USER CODE END 2 */
    , i% p  Q9 |8 A) K5 a
34. 
    
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    
38.   {
    2 v8 [( W" R$ }, {- G
39.           GetAngle();
    : E. u. o' u; U; A
40.           GetPitch();
    - }9 {0 E; Z- G
41.           HAL_Delay(10);
    ' \5 l/ U7 Z2 N: j. M
42.     /* USER CODE END WHILE */
    
43. 
    1 c2 a1 ]( q; U/ K/ {% G
44.     /* USER CODE BEGIN 3 */
    ! y$ X: U# Q' A
45.   }
    
46.   /* USER CODE END 3 */
    " ]  a  |/ T! W0 k- M
47. }

复制代码

, W+ n4 v  z( F1 G9 Q' n( w点击，下载程序
- Y! b1 F) w' [* g1 m7 Z* K

, s/ j2 C$ k. y' r& A9 r) }; b
" a% c4 W! U4 J1 ]% o* U  i8 t程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。

3 y# a0 i( B5 d/ ?2 Y( ?
% |  Q; U$ ?- O) h5 I+ ]% l
) ?7 M+ ], j( M, `3 ~  y, { 作者：DIY攻城狮

  X' ^$ C* Z9 C8 ]3 u1 O

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