/ T  u2 f! J) G7 ]. a; _
9 y* i' C- W- u

: V% W, a7 q; {
# z2 e2 d' c2 X背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。

$ \& U% ~, l8 m1 a: zMPU-6050简介
2 m' M* V3 Q- f( n1 I3 C5 G" C, N    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。

: K8 e0 q% ~9 J/ ?3 T
! d  i& d/ s+ |) f4 g* n

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


9 X9 a3 U8 E" X4 }' G互补滤波、卡尔曼滤波计算位姿示例


. l( Q, |+ t, P
+ t4 ~" ^' a2 P$ ^" w( z! d4 A互补滤波、卡尔曼滤波计算位姿示例
. b+ }& u$ y/ Z  m% z& t2 r) H
    除了使用加速度计计算的噪声较大，卡尔曼滤波，一阶互补滤波，二阶互补滤波看着差不多，O(∩_∩)O哈哈~（四元数计算的示例就不演示了，有兴趣的可以自己研究一下。）
    1）一阶互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
7 C2 e. l& l2 ^+ O- |0 A    2）卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程（来源于百度词条，我也看不懂这说的啥）。
    3）硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到Yaw、Roll和Pitch（使用四元数计算位姿，好像会将初始化的位姿设定为零位）。
0 H/ [. ]+ u. U& h) P2 `& L

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

" H: e  Q! B: M, S# _2 O& j3 J一阶互补算法如下：

1. //一阶互补滤波
   
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   
4. 
   6 z+ J: r* u6 W6 S8 g; ]
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   4 |- B1 C! n3 ~
6. {
   2 B  c2 p# c0 S
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   * r7 o0 q8 S; G( O% @
8.   return FirstOrder_Pitch;
   
9. }

复制代码

二阶互补算法如下：

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   : s8 K, _- Y6 q" W( |# J! ?
3. float x1,x2,y1;
   
4. float SecondOrder_Pitch;
   + Y! w0 R" m- @
5. 
   
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
     ?' g" F# `& d2 u+ Q7 V
7. {
   / H, Y/ y4 |) k: o. L0 k/ V
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   
9. y1=y1+x1*dt;
   / U) E% o+ U$ I0 L# x3 f3 w
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    ( |8 ^% n! L* N% j8 Y4 }9 W( V% O
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    * [% G  r, N; ?# M% k
12. return SecondOrder_Pitch；
    
13. }

复制代码

; ?0 k1 y% `0 z$ o# x; n  ~2 E二、卡尔曼滤波
9 N9 A0 o5 |4 l5 V) S+ y* u

1. /* Kalman filter variables */
   * ]/ _: `& S3 L; o$ L3 s8 K
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   9 Q& S6 K8 q6 ]4 Z( ]8 j! k
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. 
   % L# g7 q  ~9 l4 N, n  K
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
   ' ?  ^( _! o; q- \* D
9. 
   
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    
11. 
    
12. 
    ) d* v1 M$ i" N$ C: y$ m5 S
13. // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
    * Z/ ~3 Z+ U' z3 i- m* _; _- `5 T$ z7 I
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
    % l/ C+ q8 W3 k. L* g
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. 
    6 S. e' g$ a4 M& x8 t& q7 H
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    
20.     // Update xhat - Project the state ahead
    
21.     /* Step 1 */
    5 B* X& F1 u  y  l4 u$ n; d
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    1 P9 e% `# M: `7 D& _5 A
24. 
    3 a7 ~8 d6 m- p3 _3 v
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; z" T" X2 t0 N) X& @, F9 [
28.     P[0][1] -= dt * P[1][1];
    
29.     P[1][0] -= dt * P[1][1];
    
30.     P[1][1] += Q_bias * dt;
    
31. 
    2 m8 h8 V0 K8 w' H
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    3 W  l! p1 S7 g4 f
33.     // Calculate Kalman gain - Compute the Kalman gain
    
34.     /* Step 4 */
    9 @( D' G# T/ u, T
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;
    : z% ^5 C9 m: `; |8 {( k! f" ?7 H
40. 
    : ]7 ?- s/ F1 v
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    ' ?/ ^; l' D# X! G4 D- E
42.     /* Step 3 */
    ; A8 L2 |0 s: o6 ~/ y
43.     float y = newAngle - angle; // Angle difference
    
44.     /* Step 6 */
    " y# \4 z; l: _% L* A: ?
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    0 o- T' I; Y- c6 E/ M
47. 
    & k' Q# K' g6 M4 {
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];
    % h+ b9 K' ?; s# @0 W
52. 
    % q3 b, X6 m3 _
53.     P[0][0] -= K[0] * P00_temp;
    
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. 
    
58.     return angle;
    
59. };

复制代码

- @6 A+ P" ^$ Y5 U8 v7 @0 E三、四元数法

     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
+ p2 ?' H# }) g9 @" d% X" |) x5 R
6 W/ R4 }; j2 W7 f$ p       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   4 z9 B6 q; O7 f8 J
2. {
   " x5 \( m! P$ L
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   3 Y" d, R+ B! a+ l+ o
5.         long quat[4];
   ( r( M" H, w" W
6. 
   
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   : w/ u; T8 ^" b- s& k
8.         if (sensors & INV_WXYZ_QUAT )
   
9.         {
   
10.                 q0=quat[0] / q30;
    
11.                 q1=quat[1] / q30;
    , Z! n6 ]$ w; x5 `0 X
12.                 q2=quat[2] / q30;
    
13.                 q3=quat[3] / q30;
    # P% W: K9 X% y3 V6 w4 @0 W- U
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    4 Q3 F# p  a6 \: y" Z! W+ ^2 X' D
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
    $ S1 }' P6 M2 b9 u2 }; @9 J- p
17.         }
    
18. 
    ( t3 G9 {* |( J" \) z& K
19. }

复制代码

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

; X( @/ K  D5 Z! d7 X* L8 }- \    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。

7 H7 i$ t" Y' [4 q3 I7 x0 o    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
2 L. {  D; b& s2 [& F; W6 i# F
3 r- ?/ \5 S) p6 M! B+ M
: O* O8 m" O6 ?+ u+ r1 n3 y$ V# D
输入工程名字，点击Next
; A6 h$ Q6 u) x9 B* t& g, A1 i& z* e# K

" @: M6 G6 u0 u: b" i1 z. i
设置完成后，点击Finish
. I$ h  e. X" k1 N  K; E1 ~

6 J. |" o% B* ?0 v6 F4 c+ }/ i0 I/ l
进入MCU配置界面


' b3 M% G! g& s
设置I2C与MPU-6050通讯。
. ^# r0 [" p% [+ D6 F; N; k# O# Q
3 Q) j( k. _1 L7 S2 e7 G! s

设置串口与上位机通讯，实时显示MPU-6050位姿。
, `/ V8 I1 P5 R' h7 N
% W5 O, [2 _! r# L; d$ z+ ~
; A- ?& h- U4 D. ^$ e/ u  J1 ]; O
0 t6 s% T3 `2 z3 [4 L+ ?  Z设置系统时钟及调试模式。



  m4 }. b+ k* c4 C/ T) u: E, P设置时钟源。



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

" G9 O3 |" c2 `- z+ K

; |8 ^7 v% p8 s# {/ Y4 a+ g3 w在时钟配置里设置时钟最大频率。
+ _- u/ Z8 P/ N9 W6 k* w/ ~& V
( _; z' r! z, F( n+ p% n. c4 h
: Z; x  F, I& q' @$ p
" p+ \) W- l& ^" v2 w3 _在工程管理——>代码生成器中进行设置
8 \5 ?/ T' e2 d* \
# z$ e# N  r1 m2 Z- q6 `% U' j

( X$ N8 a) Y- q5 y! F. W点击此处，生成代码
( ?7 w% W# I9 v" |! U

( l4 C5 Q7 t7 n' `" w0 C$ Q
在工程文件夹中创建iCode文件。
- O' G7 ?6 _# F8 w2 l$ r% f. t
5 m' U) T# r' ^, C5 m4 _3 m1 ]

8 Q4 g# x3 v7 q) z9 M% G1 u- ]将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。
0 ^: C7 s: e- |* k; l

* k7 j' \/ X9 o- K" a# ~# A
/ M! j6 e  T. V) J" L点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。



: w; e2 f8 w4 m) E4 p( P按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。
3 `7 }( m8 X# x2 Z8 v7 H/ I" k


8 r+ a, Y# ?# p, M5 @3 U5 C
8 Y$ T0 E, j' p+ _
然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
( J% V; f4 `! Y" [, J' Y" e

1. /* USER CODE END Header */
   
2. /* Includes ------------------------------------------------------------------*/
   , k4 `. H: t2 A( y' E/ F( U& n
3. #include "main.h"
   - Z, x0 }: _- C/ Z7 `8 A
4. #include "i2c.h"
   
5. #include "usart.h"
   
6. #include "gpio.h"
   
7. 
     s" `% \6 O. t# B
8. /* Private includes ----------------------------------------------------------*/
   
9. /* USER CODE BEGIN Includes */
   
10. #include "mpu6050.h"
    
11. #include <stdio.h>
    8 s" j' T5 E9 d+ |; z
12. /* USER CODE END Includes */
    
13. 
    3 G( P1 T; w. D! V# F( J$ |
14. /* Private typedef -----------------------------------------------------------*/
    
15. /* USER CODE BEGIN PTD */
    : ?" ]5 Q; I7 E9 V
16. 
    ) N5 ?- F" K5 o3 T7 V2 _* R' T$ X5 X
17. /* USER CODE END PTD */
    
18. 
    0 k  w' T  k9 @8 x9 V
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    * S/ V% j. V# U, w6 T% p" \, q4 L" T
21. /* USER CODE END PD */
      j& N$ }2 Y3 h
22. 
    - J8 |; G) I, k" X- Q
23. /* Private macro -------------------------------------------------------------*/
    1 h4 s' Q$ R6 a. _
24. /* USER CODE BEGIN PM */
    
25. 
    1 }; F( X0 r9 F
26. /* USER CODE END PM */
    
27. 
    ' J  V5 @8 q% n0 P' K
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    & u0 I! g, ^9 ?! @3 `
30. /* USER CODE BEGIN PV */
    
31. 
    
32. //重定义printf函数，用于串口输出
    8 \" [. ~  q' w0 W3 `' \
33. #ifdef __GNUC__
    
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    
35. #else
    + S8 s& S+ \* v$ g+ V" W
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    $ ^, M. `+ i2 F7 S
37. #endif
    
38. 
    
39. PUTCHAR_PROTOTYPE
    
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    ) H. D5 V9 [; d; P2 J( K( |
42.     return ch;
      I- K! v; V" }; z- S; |
43. }
    
44. /* ---------------------------------------------------------------------------*/
    
45. 
    
46. //获取系统时间，用于卡尔曼滤波
    
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    
48. {
    
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    
50. }
    
51. static uint32_t getCurrentMicros(void)
    5 E. U& d1 D0 J: ^3 J
52. {
    
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    1 \3 n" ^6 o: B
54.   GXT_SYSTICK_IsActiveCounterFlag();
    ) l1 G6 E- t- Y# N/ z
55.   uint32_t m = HAL_GetTick();
    
56.   const uint32_t tms = SysTick->LOAD + 1;
    5 V6 w+ o  D: U. B3 `
57.   __IO uint32_t u = tms - SysTick->VAL;
    
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    2 n6 {% B6 R# k3 F
59.     m = HAL_GetTick();
    ! h. \% D# y% j4 G/ E# s
60.     u = tms - SysTick->VAL;
    ' p+ S6 e1 h1 X6 l; H. K+ a8 {
61.   }
    5 M; L5 }$ j3 z
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    
64. //获取系统时间，单位us
    
65. uint32_t micros(void)
    
66. {
    8 I0 c" j, m2 V3 E! f
67.   return getCurrentMicros();
    . e% o3 W6 z, ]+ ]
68. }
    9 `8 ^% Q) I3 o! B$ a, ^
69. 
    ) h# G- I( Z# ^+ H: _9 Z
70. 
    
71. /* USER CODE END PV */

复制代码

使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
, W6 G3 \4 ~- e# K% x8 D' }9 ?


在main（）函数中添加MPU6050初始化代码和发送位姿数据代码
: M4 T- q& v. P: Z

1. int main(void)
   7 b/ ^0 q, I9 m8 ]- s7 P1 b
2. {
   4 K& p1 I+ ]4 x  J6 u+ P7 T/ i
3.   /* USER CODE BEGIN 1 */
   & [6 Z1 z6 U7 ]% \6 u6 h
4. 
   / E8 u2 n$ o( O, U- F: z
5.   /* USER CODE END 1 */
   
6. 
   
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
   - \$ }, V0 j( I& S
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   " |8 U2 |) }' b5 _
10.   HAL_Init();
    * `. o. ~- H; S; a
11. 
    
12.   /* USER CODE BEGIN Init */
    
13. 
    " d3 y6 {& ?1 `1 C6 e
14.   /* USER CODE END Init */
    
15. 
    
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    
18. 
    : d, V6 ~$ q  V+ H. n7 O
19.   /* USER CODE BEGIN SysInit */
    3 I/ [8 N& M# X. F% V; c& s
20. 
      O: y1 A1 i4 i2 ~
21.   /* USER CODE END SysInit */
    & t8 `4 f$ o& E0 o4 m- W
22. 
    
23.   /* Initialize all configured peripherals */
    % O- y' p. b! E  C
24.   MX_GPIO_Init();
    
25.   MX_I2C1_Init();
    / g" V% w7 C- k. @( e! N
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    ) `$ d' M" M: N; C
28. 
    
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    
30.           printf("MPU_Init_Fail...");
    
31.   }
    
32. 
    
33.   /* USER CODE END 2 */
    ' Y- C6 b9 a7 O
34. 
    & |# \. q7 T) v3 t' i
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    $ ?, y% q% D. l* C
38.   {
    
39.           GetAngle();
    
40.           GetPitch();
    0 {7 S( U; h* Z' ^$ A. f* ?4 f, b% `
41.           HAL_Delay(10);
    , U% @$ M( Y7 w$ O: U& ]
42.     /* USER CODE END WHILE */
    
43. 
    
44.     /* USER CODE BEGIN 3 */
    
45.   }
    ! f4 S) t5 D" p* w6 m8 e% E- v
46.   /* USER CODE END 3 */
    
47. }

复制代码

. G1 {8 W8 l  Q点击，下载程序



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

( O( K  G& k  L, g

作者：DIY攻城狮


3 J; ?: \: Z( _( W

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