背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
/ }# y- V" @6 W  g
2 b' p9 O" b5 p: d7 eMPU-6050简介
/ G  S8 H7 s- e2 _: q. a6 y5 e    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。

% i; Y) R' H# U6 Z0 E4 l0 U! s& z0 Z    MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。



; J: F9 M& X# s! Q' I& L( E
MPU-6050姿态获取与处理（惭愧，我也一知半解）
    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
8 h/ y! [2 X9 C
' g2 W5 J+ J; q6 T& e" ^0 c7 `
互补滤波、卡尔曼滤波计算位姿示例
( ?# Y1 x- s" y/ N


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

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

; n' ]- Y9 _$ W, G  y( r

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

7 l0 }( ?3 W4 ~5 U: a
一阶互补算法如下：

1. //一阶互补滤波
   
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   
4. 
   7 {1 c+ e1 ?0 W* ?% E
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   
8.   return FirstOrder_Pitch;
   * @" h- _- ^; C1 k0 k% Y1 ~
9. }

复制代码

9 w6 U" e8 }% I% c二阶互补算法如下：

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   " \3 v2 n' }, W. B1 _& |& n' y- `
4. float SecondOrder_Pitch;
   
5. 
   
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   2 J, B# J6 }  z6 {
7. {
   
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   & i6 f# {6 \- B: g4 r
9. y1=y1+x1*dt;
   * O3 U- }# Q) y9 s! t2 ?% G3 Q# g
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    
12. return SecondOrder_Pitch；
    
13. }

复制代码

二、卡尔曼滤波
: @% ~  E. o* ~

1. /* Kalman filter variables */
   
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   6 N6 T1 N& ^8 o" P
3.         float Q_bias = 0.003; // Process noise variance for the gyro bias
   " x3 J- j! e- g7 K
4.         float R_measure = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise
   ' L3 a' U+ \7 Y" U
5. 
   
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
   
8.         float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
   
9. 
   ) b" f( V3 g& E; t" L* Q) R
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    
11. 
    
12. 
    
13. // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    4 Y- Q+ H+ S2 z1 C
14. float Kalman_filter(float newAngle, float newRate, float dt) {
    
15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    7 T; N  T: x1 n3 N( C
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
    2 I) H: p' h, j* a& T/ z! m
18. 
    
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    * v, H* j- m- l" t$ Q: {$ W! q6 @6 ]
20.     // Update xhat - Project the state ahead
    
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    ( P  L& f) ~  v/ V) S
23.     angle += dt * rate;
    / v5 ^2 P4 }1 V" [( J
24. 
    
25.     // Update estimation error covariance - Project the error covariance ahead
    8 K7 S1 z, h, O7 I6 }
26.     /* Step 2 */
    
27.     P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    0 p6 l/ q1 F- B% G7 m( M
28.     P[0][1] -= dt * P[1][1];
    3 }6 S& D2 p7 q" ^& F
29.     P[1][0] -= dt * P[1][1];
    0 k# u% }5 q3 R/ f
30.     P[1][1] += Q_bias * dt;
    . ?3 \% x7 N0 o6 M% p5 x: x( j' q/ Z
31. 
    
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    8 m% o! v4 @! Q! T3 S7 u6 J
33.     // Calculate Kalman gain - Compute the Kalman gain
    
34.     /* Step 4 */
    - A8 }8 ]- ?5 X9 m# o4 {
35.     float S = P[0][0] + R_measure; // Estimate error
    6 _4 i- k  a2 A# e8 A! @: y2 W5 Q
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;
    9 I& i& p( f2 f. ^& ?" d2 J
40. 
    
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    
42.     /* Step 3 */
    - f  t: z$ n& q: Q
43.     float y = newAngle - angle; // Angle difference
    
44.     /* Step 6 */
    ; \* h, J' U8 e! |3 p: T* H$ i
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    
47. 
    
48.     // Calculate estimation error covariance - Update the error covariance
    
49.     /* Step 7 */
    
50.     float P00_temp = P[0][0];
    ; w+ C$ M; o2 @5 U" ?0 I
51.     float P01_temp = P[0][1];
    7 s9 _/ v' h+ y: @; f3 B+ I* o
52. 
    5 r2 ~4 u1 k5 A% i& X1 k# ]
53.     P[0][0] -= K[0] * P00_temp;
    
54.     P[0][1] -= K[0] * P01_temp;
    * |- s$ o* _' }6 \5 o, c% L
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    
57. 
    # o4 f5 ~) [3 q8 ]5 M
58.     return angle;
    
59. };

复制代码

三、四元数法

     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
) R* E- Q- L, ?- t2 G# j0 W9 F
       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
$ J* z+ P. D; D! \: o/ K/ K

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   , G8 B9 {5 U$ k/ ?7 u( S% ^
2. {
   
3.         unsigned long sensor_timestamp;
   1 I5 P) U6 U2 ]  D+ S' G1 _0 E* q
4.         unsigned char more;
   
5.         long quat[4];
   2 Q3 E# f( f. b2 @/ i) @
6. 
   ; O) x1 f, H0 ^4 |
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   
8.         if (sensors & INV_WXYZ_QUAT )
   : u" X! q4 [1 P. i
9.         {
   - @1 w8 Q( p. M/ D, c+ }
10.                 q0=quat[0] / q30;
    # p, T. i* }( t- n8 n
11.                 q1=quat[1] / q30;
    7 u" w! P' ^/ a! Z2 ~
12.                 q2=quat[2] / q30;
    
13.                 q3=quat[3] / q30;
    : n% Y# r' h9 \9 D' \
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
    9 k* R6 {7 I; c
16.                  *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    
17.         }
    . u3 y2 i& I( {  I" I
18. 
    $ |' m* T# P- f+ \# T- A" D! _
19. }

复制代码

    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。
7 j- d( V/ t* g$ T3 D' Q0 S* X
% {+ c$ y$ J/ c5 t- E1 [    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。

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



, z3 E6 ~. P8 F/ G1 {+ m) Y4 C# M3 o输入工程名字，点击Next

- ]6 Q7 H3 Y$ h2 [
+ ?" c+ W; _- z7 r8 U
5 c7 s) P" n% Z, T! O# J设置完成后，点击Finish


7 \" v# u5 V) W
6 H, ^  B. D' p! z  |. N进入MCU配置界面
" F" K: @/ h0 D! f# t7 v- T
, C) P6 G2 a" F

4 ~2 {: `/ w& \7 _- x' x设置I2C与MPU-6050通讯。

* c7 C/ Q8 m1 N( V  r  M

设置串口与上位机通讯，实时显示MPU-6050位姿。
+ \; }+ b1 J* w6 t
" N; G3 C$ o2 x0 J. o

5 w: z" ~( Z' W5 V! Z2 T5 A, j设置系统时钟及调试模式。


/ [+ y0 u& w6 F$ x
6 @, d) M) ~4 v: ^6 W设置时钟源。


" B. Y, R" l& M- M' h* N
# _+ e& }$ c1 ?" m8 P6 K. A$ ?3 ~全部设置好后，芯片引脚的分配情况。


+ j3 t0 y+ [. i7 R1 _
$ X' I& x6 z9 u1 P+ w9 s在时钟配置里设置时钟最大频率。
$ t* E3 C( G5 x! ]


在工程管理——>代码生成器中进行设置
$ T, v; @! x9 F$ m2 R* U


点击此处，生成代码
' j- V4 C9 U8 t) l. r0 L
2 x5 f2 j. q! _, H* ]* }

在工程文件夹中创建iCode文件。



0 f. o6 @! @/ p6 }将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。


) k9 N; `5 w/ B' Q' X
, _; y7 F2 E! `7 k! L点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。
' @% \! H( g7 r( U' K' \# e/ T4 [

+ W4 [0 B+ H) k1 \$ e
. \' k7 u4 [$ u按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

; {+ B' B& @2 d
( C/ \' J1 x, y
' s; J; Z2 m& P+ x% g& f% j

然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。

1. /* USER CODE END Header */
   ' G# u8 W- V4 V& O
2. /* Includes ------------------------------------------------------------------*/
   
3. #include "main.h"
   ; Q3 u: q/ l) \1 V4 J7 b) J0 b
4. #include "i2c.h"
   , n# ~) S, v) h, w& ]
5. #include "usart.h"
   8 @1 V6 a, `+ l. I% T1 _( e6 v
6. #include "gpio.h"
   
7. 
   
8. /* Private includes ----------------------------------------------------------*/
   % E. _" ^2 j# w+ g& w  Y6 H: }
9. /* USER CODE BEGIN Includes */
   5 Z4 X* G. n: e4 D/ C( S
10. #include "mpu6050.h"
    
11. #include <stdio.h>
    2 v, j" o# ~$ {
12. /* USER CODE END Includes */
    
13. 
    0 W# X' V( P  X( G5 \! D- z, b) D- b2 E
14. /* Private typedef -----------------------------------------------------------*/
    
15. /* USER CODE BEGIN PTD */
    
16. 
    
17. /* USER CODE END PTD */
    
18. 
    " B5 n0 n3 I. D8 E( L" O
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    
22. 
    
23. /* Private macro -------------------------------------------------------------*/
    ' H0 C+ ]: Z- t; X) G+ p$ L
24. /* USER CODE BEGIN PM */
    ; d6 L6 K0 d+ b$ {1 l7 J$ ]% X+ L
25. 
    " w2 V1 e6 w0 y2 ?# h
26. /* USER CODE END PM */
    
27. 
    $ n2 w' Q0 E: b! E0 ^
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    & ?' o( J: ~% U4 j
30. /* USER CODE BEGIN PV */
    ! {  _6 S" a' P; f2 \( t
31. 
    
32. //重定义printf函数，用于串口输出
    
33. #ifdef __GNUC__
    
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    6 \: ?* ?7 o8 t- W" s( _4 {9 J
35. #else
    
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    + _0 W) J0 ?& j2 j' V
37. #endif
    / @$ R9 |1 `& w4 x/ g- S5 h
38. 
    ; q& M3 m: _; W2 @1 j/ e" C; a+ c
39. PUTCHAR_PROTOTYPE
    
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    $ H9 k2 i. o- }0 Y
42.     return ch;
    ( Q, ]8 M) l: s+ x/ ?5 d" U
43. }
    , Q" d6 r' Q. B* C' |8 t
44. /* ---------------------------------------------------------------------------*/
    4 a* G$ }! L3 ^4 ?2 @. r9 Z
45. 
    
46. //获取系统时间，用于卡尔曼滤波
    # \2 l3 ~- L' d, }  T  E: Y. M
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    6 A- m8 h7 C+ g3 {. j: Y
48. {
    
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    
50. }
    % q1 t4 r2 z; m$ ]7 e
51. static uint32_t getCurrentMicros(void)
    " h& X0 `, b1 T% Z1 g
52. {
    
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;
    : [; T: m4 {4 p" G1 @# F' ~* O
57.   __IO uint32_t u = tms - SysTick->VAL;
    % k: `. T& S. \7 I: D% T
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    
59.     m = HAL_GetTick();
    
60.     u = tms - SysTick->VAL;
    . |; W+ B& K) K( P4 _: _% D( ]
61.   }
    ' V# i9 e( Z& i$ o4 j$ j9 \
62.   return (m * 1000 + (u * 1000) / tms);
    ) B. v% E4 q' L4 M7 a( D
63. }
    + y% ~1 W, V1 }! Y
64. //获取系统时间，单位us
    
65. uint32_t micros(void)
    ; F$ L6 U+ G# h. Y0 V. d
66. {
    4 ]8 `( V( m) o
67.   return getCurrentMicros();
    
68. }
    , A2 s" Q' X2 \2 J
69. 
    
70. 
    ! K" W. o6 a" M, y- E: d$ f% T
71. /* USER CODE END PV */

复制代码

5 G+ W8 k0 [/ J7 h- i# h* D使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
; ]% ?3 R" F. w

. O* }/ g5 d8 w* `% G% V
! V: ?4 S. q1 r1 j. ^$ {* d在main（）函数中添加MPU6050初始化代码和发送位姿数据代码
8 x4 s1 @: a) v. |! {3 P

1. int main(void)
   - Y  Z* o* T3 A5 ^) ?& K$ k4 ?; d) w
2. {
   ; }5 O. M4 w6 y2 U! M( \1 \* r
3.   /* USER CODE BEGIN 1 */
   
4. 
   2 |% @; m8 _& J% ^0 V8 I% K
5.   /* USER CODE END 1 */
   
6. 
   
7.   /* MCU Configuration--------------------------------------------------------*/
   ( Q. {& B- d1 @$ I
8. 
   6 @* J' `  c. S/ L* u
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   : u, u$ H! ^8 f3 o% L% Q
10.   HAL_Init();
    2 p' l+ e; O4 a, \0 R5 T; k# m
11. 
    
12.   /* USER CODE BEGIN Init */
    - r8 [" ?. k2 ?( Z% `
13. 
    0 ]* h) ]9 e9 }8 k- B+ G' B+ |
14.   /* USER CODE END Init */
    
15. 
    
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    
18. 
    
19.   /* USER CODE BEGIN SysInit */
    
20. 
    " B$ S1 B1 }& n
21.   /* USER CODE END SysInit */
    
22. 
    
23.   /* Initialize all configured peripherals */
    + \: l& w$ K' s. R
24.   MX_GPIO_Init();
    2 G( Z  R( p6 [' g& u9 U
25.   MX_I2C1_Init();
    
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    
28. 
    
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    : r8 m2 c  P7 s# r) V, P
30.           printf("MPU_Init_Fail...");
    & G; S, N4 E, m) l; ?4 Y9 o% G' T3 |9 [
31.   }
    1 e$ Q/ j$ Y( Q! I6 o# }+ d
32. 
    
33.   /* USER CODE END 2 */
    1 B+ g, h5 k: E9 j0 v  Q
34. 
    
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    6 t8 X% ^5 X( k4 n1 E# q/ T
37.   while (1)
    & R4 J3 X" O. @
38.   {
      u7 U) Z+ B: K9 F' H
39.           GetAngle();
    
40.           GetPitch();
    
41.           HAL_Delay(10);
    
42.     /* USER CODE END WHILE */
    $ N: G# y4 d+ k; t0 p: n! F$ C
43. 
    9 u; D# X& Y* o! h: b  l4 H; o! c
44.     /* USER CODE BEGIN 3 */
    8 O) m1 q& F/ Q9 X1 m
45.   }
    
46.   /* USER CODE END 3 */
    + m5 N5 V' w- k
47. }

复制代码

点击，下载程序
! _% j7 d! }7 {1 S1 ^
; w5 @$ C4 E9 ]% x# v1 I

: ^5 n( \7 C0 c2 K  m  x程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。

' C5 C4 t0 m9 Q9 T' j
; H! K* C# g" |4 T- {
作者：DIY攻城狮
. ]$ {( @* E- G( s! X

; O5 }* M, j/ _" ^5 G4 e' A

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