# P) z1 T, g+ ?7 {
' A9 n7 @1 H3 Y6 b  D0 ~

/ Q+ m# D$ x8 C5 {# v; r. q5 k
背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
' U+ e6 }* n- N, R
6 [! |- G  L- s9 yMPU-6050简介
    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。
9 E) _6 l7 u* K# `( A1 ~
: K. c- e" H4 d7 s


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


1 [8 Y; Q4 i) ]* _7 o  o0 c3 [互补滤波、卡尔曼滤波计算位姿示例
; u/ {2 k2 A" V6 B4 G1 P. y0 _


: \" R& P" M; v' _6 s互补滤波、卡尔曼滤波计算位姿示例
7 n  F" T4 i! C1 V3 Y4 A) Y
4 ]1 R+ v* @2 m* K8 J- x) z, Y6 I, P    除了使用加速度计计算的噪声较大，卡尔曼滤波，一阶互补滤波，二阶互补滤波看着差不多，O(∩_∩)O哈哈~（四元数计算的示例就不演示了，有兴趣的可以自己研究一下。）
    1）一阶互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
    2）卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程（来源于百度词条，我也看不懂这说的啥）。
    3）硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到Yaw、Roll和Pitch（使用四元数计算位姿，好像会将初始化的位姿设定为零位）。
- |9 ^# N* S7 W' p  N( C



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

* A, e. u+ @! ]3 K
一阶互补算法如下：

1. //一阶互补滤波
   
2. float K1 =0.1;         // 对加速度计取值的权重
   * c8 v8 y- M8 Z2 ^# J+ T
3. float FirstOrder_Pitch;
   # y' E% b; D( `- a- z& u* \1 G
4. 
   
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   
8.   return FirstOrder_Pitch;
   
9. }

复制代码

二阶互补算法如下：

1. //二阶互补滤波
   0 B8 ^* d$ s( W% u
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   # s3 }4 T# U) E6 c& L; J
4. float SecondOrder_Pitch;
   ! W% W! W2 u( o7 c( j
5. 
   7 b9 J: [. R2 @* b+ b% l
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   
7. {
   + L" A4 ~; w2 M
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   6 g9 u5 x" R$ m  k
9. y1=y1+x1*dt;
   1 F  x- A4 A: c+ n& y
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    
12. return SecondOrder_Pitch；
    8 {+ g6 H$ ?5 e9 k6 [
13. }

复制代码

二、卡尔曼滤波
! `) o* A4 s5 \7 q- L6 F, n1 i

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
   ( ^1 u2 z  P2 G
5. 
   
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   1 c+ q+ v) O( S2 z3 ^  r, I
7.         float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
   8 [* @' J1 P4 N/ b; j# u% _
8.         float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
   
9. 
   $ Z! g. S' x2 r
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    
11. 
    ; c. R1 x, l6 Z; Z* x7 j
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) {
    , e* y/ O2 `: K) C; p' f! O
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
    
18. 
    
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    : C% @' d7 V" ^( o* d! ?
20.     // Update xhat - Project the state ahead
    & E/ v. }. [; {
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    4 F) F* e7 l' e5 C& J. H
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);
    
28.     P[0][1] -= dt * P[1][1];
    . `4 {; g" Y+ R$ V- U0 w5 m
29.     P[1][0] -= dt * P[1][1];
    
30.     P[1][1] += Q_bias * dt;
    
31. 
    
32.     // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
    " a8 a& x  B9 n; X* w
33.     // Calculate Kalman gain - Compute the Kalman gain
    
34.     /* Step 4 */
    
35.     float S = P[0][0] + R_measure; // Estimate error
    
36.     /* Step 5 */
    ! ^* Z7 ^+ o1 U% W, x! G8 h, j
37.     float K[2]; // Kalman gain - This is a 2x1 vector
    
38.     K[0] = P[0][0] / S;
    8 P. Y7 a' R7 m5 c6 u' Z1 P5 L
39.     K[1] = P[1][0] / S;
    
40. 
    9 C. N+ ?4 e! i8 S; h- B& r
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    
42.     /* Step 3 */
    
43.     float y = newAngle - angle; // Angle difference
    / H$ R' H( a3 z, X# v4 L
44.     /* Step 6 */
    5 s1 Q+ J& o7 Z4 Y% w* F
45.     angle += K[0] * y;
    # a  p$ h! ?3 X/ G7 D$ S" ^
46.     bias += K[1] * y;
    
47. 
    
48.     // Calculate estimation error covariance - Update the error covariance
    
49.     /* Step 7 */
    * r2 r( `$ l5 d2 u# h  f" f
50.     float P00_temp = P[0][0];
    5 o1 m3 D/ w* u$ B3 g  d
51.     float P01_temp = P[0][1];
    9 s, w) A% B$ W5 R) z2 N
52. 
    9 M% U1 w9 v" h$ f
53.     P[0][0] -= K[0] * P00_temp;
    $ J2 H* n- h' y2 Z- l' k
54.     P[0][1] -= K[0] * P01_temp;
    7 A- S7 z9 ]) Y7 ]- @
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    ' a) A2 i3 f0 t6 f5 d; o( l& L3 j
57. 
    
58.     return angle;
    - t5 f0 z, ?" f% d# C$ p5 x& R6 P
59. };

复制代码

$ t1 Z1 D3 x) U4 C, q- s! ]三、四元数法

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

; T: e6 ^1 @1 J# N+ O       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   
2. {
   8 L" f4 I) \2 q  C
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   
5.         long quat[4];
   
6. 
   ) z. y/ p; W) Z4 Q
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   " N5 \9 {7 A# T. `0 P
8.         if (sensors & INV_WXYZ_QUAT )
   - c7 T7 a6 i3 h0 F( T
9.         {
   
10.                 q0=quat[0] / q30;
    
11.                 q1=quat[1] / q30;
    6 x/ ?' {, z$ m5 P0 R
12.                 q2=quat[2] / q30;
    
13.                 q3=quat[3] / q30;
    . m8 Z8 t, h1 ~+ m$ |; Y2 b
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    , {( T3 ?5 E" b& g! r7 W  u
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.         }
    7 p7 m. U. u. k' s, \6 V% c
18. 
    
19. }

复制代码

    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。
2 p0 J2 u/ ^/ S
    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。

  G1 d& a' R2 n: z. v9 A    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
9 S" l" Y% j( B; T9 j$ W* ]: E


输入工程名字，点击Next
. X; s+ l1 {& M/ E! c0 [
, g1 E: r* [2 P& ?& A  N! \
9 ]8 w2 Z" g: P% f) I; W+ ]
+ _/ D$ s/ t; D5 u- [( k* R; h3 U设置完成后，点击Finish
. d% C4 C" U' g) S! j


进入MCU配置界面
0 \$ @" ^& `' D# a
8 p1 z$ [' o0 n9 Z$ Y+ X5 I$ s
7 C8 j3 L6 @  R4 A3 F
; `5 \0 Q' y* x2 z3 ]& ]设置I2C与MPU-6050通讯。


4 |. W7 O! p3 L4 S% `5 G
; z- N9 b5 c5 u" F6 p设置串口与上位机通讯，实时显示MPU-6050位姿。
+ a6 {. J7 J5 O: _- {4 S: ~3 b

: w. Q6 l1 V/ W( X6 F) O% N) W9 u
设置系统时钟及调试模式。


' c/ R5 t" C  u0 r1 l( ?
设置时钟源。
% k; P! f8 [; k- g8 ?

, s2 @# ?+ N. y2 \% i! o
全部设置好后，芯片引脚的分配情况。


) Z( S3 F3 J  q9 F0 e
5 |) {1 t8 P/ I( y0 p: \. z$ |9 z在时钟配置里设置时钟最大频率。

5 U1 v- p3 s3 b( s5 [7 R
5 t$ ^+ q4 ^0 o: O0 A
在工程管理——>代码生成器中进行设置



$ ]& t) b$ Q& t, R3 s点击此处，生成代码

2 _! r7 y! w, z

# E4 E, O. m; f$ \在工程文件夹中创建iCode文件。



+ h( b/ z" j+ x+ B$ s; C* \6 s( G将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。

) ^$ }; B) j( `, E

点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。

& v: ?2 }- M+ k( Q0 X7 a
7 W+ P* i- h, J+ {
按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

& z. d" u- t5 ^) c5 V) {

# V" l+ _! K3 U3 F) _0 U

9 n! g. N- O3 W. H; a然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
' r7 t$ q7 x$ @. F

1. /* USER CODE END Header */
   
2. /* Includes ------------------------------------------------------------------*/
   + S# X! G9 @7 d3 r
3. #include "main.h"
   * ~; g& z2 M' n7 D
4. #include "i2c.h"
   / ^8 C  u8 G( e5 n" O& P4 N
5. #include "usart.h"
   
6. #include "gpio.h"
   
7. 
   - B3 x- b% ^& U1 b+ X) q
8. /* Private includes ----------------------------------------------------------*/
   3 v8 U6 z( \- ~8 l* U$ |4 m+ d
9. /* USER CODE BEGIN Includes */
   
10. #include "mpu6050.h"
    : _! u' Z/ q  ]" Q' `$ o+ k- \7 e
11. #include <stdio.h>
    
12. /* USER CODE END Includes */
    , j- n+ j- N4 i% N
13. 
    ! W0 `: r$ W6 D( m/ E4 m9 \, q! L
14. /* Private typedef -----------------------------------------------------------*/
    - W9 E" ]! @( G( Z  y
15. /* USER CODE BEGIN PTD */
    
16. 
    
17. /* USER CODE END PTD */
    
18. 
    / R2 ~+ I3 b) C* U! R
19. /* Private define ------------------------------------------------------------*/
    : _* M2 N! B/ a
20. /* USER CODE BEGIN PD */
      Y6 C# I" F( e( O
21. /* USER CODE END PD */
    # E4 N9 w+ a1 e
22. 
    
23. /* Private macro -------------------------------------------------------------*/
    ! h, y( Z; E. b" C
24. /* USER CODE BEGIN PM */
    
25. 
    
26. /* USER CODE END PM */
    
27. 
    
28. /* Private variables ---------------------------------------------------------*/
    / S# C6 ?7 u3 ~( m1 z' g, \
29. 
    ; {8 J5 p3 L+ m" C) |( N) O
30. /* USER CODE BEGIN PV */
    
31. 
    
32. //重定义printf函数，用于串口输出
    
33. #ifdef __GNUC__
    $ }' h" I( x+ h# a3 L0 l; j
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    + m  w0 e1 ^3 `, g" b
35. #else
    
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    ' y  L4 j6 E1 d# V0 [. Q1 C' P* ~
37. #endif
    
38. 
    
39. PUTCHAR_PROTOTYPE
    3 H4 [. B3 M% Q. V# C
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    
42.     return ch;
    
43. }
    3 C, r+ I  A$ a! V
44. /* ---------------------------------------------------------------------------*/
    
45. 
    : t1 @9 L8 |) s2 l; {, X4 |
46. //获取系统时间，用于卡尔曼滤波
    ' n' R  O4 D& {1 F
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    
48. {
    
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    
50. }
    " m/ V* \/ K+ M  p" U
51. static uint32_t getCurrentMicros(void)
    
52. {
    
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    
54.   GXT_SYSTICK_IsActiveCounterFlag();
    
55.   uint32_t m = HAL_GetTick();
    # t3 x) z; [. d' l
56.   const uint32_t tms = SysTick->LOAD + 1;
    
57.   __IO uint32_t u = tms - SysTick->VAL;
    : ]6 t5 @' ?) [7 k/ q
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    # {6 y3 h. X* J: ~" j* f
59.     m = HAL_GetTick();
    ' Z: V) N- Y8 O9 k3 _# j2 T& R5 @
60.     u = tms - SysTick->VAL;
    , @9 S2 U( b( P, ]& l6 s9 @
61.   }
    7 L7 H/ r% ^3 Q6 H
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    6 q$ k7 @) }, V7 U7 ]; z
64. //获取系统时间，单位us
    ( n/ R# [3 k4 F% e7 |! |
65. uint32_t micros(void)
    
66. {
    - K: `- ^) o% l7 p; G9 L% w' j0 n: @# l
67.   return getCurrentMicros();
    
68. }
    
69. 
    
70. 
    
71. /* USER CODE END PV */

复制代码

. n& O) J6 d0 U& Y$ Q% M0 G使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
7 w& e/ R6 U/ Y! X7 ?" y: P8 Z

" D2 ~, q5 S0 J9 \0 ?! x
在main（）函数中添加MPU6050初始化代码和发送位姿数据代码
: }0 p. G5 l# \

1. int main(void)
   
2. {
   
3.   /* USER CODE BEGIN 1 */
   
4. 
   : L+ R, M1 b5 g/ ]
5.   /* USER CODE END 1 */
   & n# X8 _8 z' J0 a
6. 
     @, k+ g" }9 [5 v
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
   
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   ! t: M# `' U. m+ }! b6 I
10.   HAL_Init();
    
11. 
    
12.   /* USER CODE BEGIN Init */
    
13. 
    
14.   /* USER CODE END Init */
    1 z( M2 ?& P! O5 p) z' F5 e
15. 
    ! ]0 O0 Z8 K& ]) u5 J1 T
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
      s3 j1 D7 j" P* r0 v8 i$ Z0 y
18. 
    
19.   /* USER CODE BEGIN SysInit */
    
20. 
    + {0 }" b6 P& R6 e& w0 _
21.   /* USER CODE END SysInit */
    
22. 
    2 p& U3 ^" v* c6 }2 Y
23.   /* Initialize all configured peripherals */
    
24.   MX_GPIO_Init();
    - f- k% [7 c; K2 L% L% M4 C& c
25.   MX_I2C1_Init();
    & ?/ Q! ^" a* u" i. D7 k
26.   MX_USART1_UART_Init();
    ) r2 b5 L5 q) l7 |. r
27.   /* USER CODE BEGIN 2 */
    
28. 
    ; V- h( e1 M3 v% U  @; S, g" \
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    
30.           printf("MPU_Init_Fail...");
    7 S5 }+ ]& I& [( C8 ^1 }6 ~; n8 m  Y
31.   }
    
32. 
    % R% V2 E0 N  w+ i* d, D
33.   /* USER CODE END 2 */
    
34. 
    
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    7 @, }% l) {2 \$ ^
38.   {
    ' d/ G+ X( j( a
39.           GetAngle();
    2 |% _, w; I- R3 p) d$ x, R2 `
40.           GetPitch();
    ! z" C% x( N  C8 U
41.           HAL_Delay(10);
    5 L  k, w4 y* W% f" \) P0 j
42.     /* USER CODE END WHILE */
      P1 M8 I6 g' O7 v
43. 
    
44.     /* USER CODE BEGIN 3 */
    8 Q+ t6 M# |& D* m3 E
45.   }
    
46.   /* USER CODE END 3 */
    + v" W; [- Y; l
47. }

复制代码

+ |- A1 E9 G/ ~4 |, t2 L点击，下载程序

; y  ~) n) U& Y# v
0 F/ H/ ~3 {/ G, J
程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
! @1 D4 a$ G; P1 Q- M


作者：DIY攻城狮
' u& `( M6 S" V# ~! J! R
$ k' p7 `' Q5 T' |8 S. l  [4 m
* _( A4 d$ `# I2 g5 V8 {! a

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