?  l6 Y6 I0 D& ^0 M
背景
        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
. i& j9 f& S: G& w# S9 m2 _1 Z
) r# N) r7 F$ [+ TMPU-6050简介
    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。
4 Q* X% N" [+ A" `) t% Z
    MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。



' r( O5 O' D# [) |
MPU-6050姿态获取与处理（惭愧，我也一知半解）
    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
& L$ g! }1 B. B* |( R; L: A
7 O$ S- d# t9 w7 n( G, ^' C; c' v
互补滤波、卡尔曼滤波计算位姿示例
7 j( a) ]0 s0 q2 g" h( g3 O$ y
/ n$ l8 _, m, n9 j9 Y: @" ^
4 h' X. i+ N- o6 F4 p& [
4 O6 |, T: Z) m; Q0 w* |' J互补滤波、卡尔曼滤波计算位姿示例
2 F% v, O) f, P- T) T. W
. v" X0 a9 w) C9 [# G, R8 Y    除了使用加速度计计算的噪声较大，卡尔曼滤波，一阶互补滤波，二阶互补滤波看着差不多，O(∩_∩)O哈哈~（四元数计算的示例就不演示了，有兴趣的可以自己研究一下。）
6 V7 i& s7 h5 F/ t/ H9 z! o    1）一阶互补滤波：因为加速度计有高频噪声，陀螺仪有低频噪声，需要互补滤波融合得到较可靠的角度值。
# S, M/ t9 q$ L3 x% S    2）卡尔曼滤波：利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程（来源于百度词条，我也看不懂这说的啥）。
    3）硬件DMP解算四元数：DMP将原始数据直接转换成四元数输出，运用欧拉角转换算法，从而得到Yaw、Roll和Pitch（使用四元数计算位姿，好像会将初始化的位姿设定为零位）。
* S- }# A9 B7 Y8 w4 c3 i4 ?2 z

7 ^+ ?1 a. i: b1 P: V
1 {" U9 q8 h& v- u, D
* _7 q& l' O; |" _% |一、一阶互补滤波算法
    MPU-6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，通过简单的计算即可得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角，需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正，通过加速度和角速度就可以计算 Pitch 和 Roll 角（单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用）。

. O6 K; R6 u  \. u3 \
一阶互补算法如下：
' q3 _. Y& G# z: O

1. //一阶互补滤波
   : \7 e! u9 [% `+ y% w
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   & x% }6 ?# u9 d& N
4. 
   8 y+ r1 s6 D2 }- [; W3 x: D
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 G9 P7 i) w& J二阶互补算法如下：
* A1 ]& ~, V. G0 K. }8 c- b

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   7 x8 o9 u* j7 ^: Q# {  Q$ k
4. float SecondOrder_Pitch;
   2 A8 e& ^3 S: d/ C* l
5. 
   * D! @! J; l, S0 u5 G3 F3 Q
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   ' j+ W/ E1 j2 o/ X  f
7. {
   ) A6 ~7 a# X4 {3 b+ j) G
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   + Q, H: N" w. E* y7 U8 y0 v
9. y1=y1+x1*dt;
   
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    6 ^9 Y' a" ?) m6 z+ P  k; u9 \5 E) N* x
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    
12. return SecondOrder_Pitch；
    ) ^0 ?5 U6 q4 a' U
13. }

复制代码

二、卡尔曼滤波
/ J, V9 w; v# t7 m# a) g

1. /* Kalman filter variables */
   " k4 M0 Q2 h. ~7 U4 @1 k; T8 q
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   ) ?# Q! u4 K5 U& C
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
   4 a+ G+ _% U! I. R1 N3 ^
5. 
   3 ?! c' d. v1 F8 o5 a. c
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   / F/ R, i, j/ s) z
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. 
   , C3 T' }: X& L$ A0 d# Y
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    . h  W8 ]8 A9 J7 I, r- k3 ?1 M
11. 
    3 k, f' R3 `3 N' Q; B
12. 
    $ X5 L& C8 g8 Q- S# q) q
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) {
    
15.     // KasBot V2  -  Kalman filter module - http://www.x-firm.com/?page_id=145
    ' l  [& B: p2 x, s* C" F- ^. {2 W
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
    ' v  \( z9 l0 s0 Q3 c
18. 
    
19.     // Discrete Kalman filter time update equations - Time Update ("Predict")
    ; I- G  W0 }$ y% j1 \
20.     // Update xhat - Project the state ahead
    1 J7 |/ a  J) v5 M9 P* r
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    3 m$ @, N% }  \( |, t; M! t
23.     angle += dt * rate;
    ( z3 H" o$ X6 p8 u! V
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];
    ) ^$ W6 c1 \& K$ X% U
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")
    
33.     // Calculate Kalman gain - Compute the Kalman gain
    
34.     /* Step 4 */
    1 z! y( ~% \, H! Q* x9 i# C$ X( z+ H
35.     float S = P[0][0] + R_measure; // Estimate error
    
36.     /* Step 5 */
    # o0 d) k. a2 b8 _- G
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;
    
40. 
    & j! N0 _" R' h: X; J0 B+ B' P
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    
42.     /* Step 3 */
    
43.     float y = newAngle - angle; // Angle difference
    4 V4 [3 Y  L! ^, T
44.     /* Step 6 */
    
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    : w9 k* K1 C( {: }5 x7 c
47. 
    
48.     // Calculate estimation error covariance - Update the error covariance
    * U2 k; [. }6 D3 B' V. O  u2 j0 f! M: g
49.     /* Step 7 */
    : j6 x7 f$ o- T6 K6 Z
50.     float P00_temp = P[0][0];
    ; d: T' C7 d1 v; X
51.     float P01_temp = P[0][1];
    8 T" m! Q2 \2 ?) ^" M
52. 
    9 _! m) y+ w& ]
53.     P[0][0] -= K[0] * P00_temp;
    4 |7 u0 N5 I$ E
54.     P[0][1] -= K[0] * P01_temp;
    
55.     P[1][0] -= K[1] * P00_temp;
    & n5 H6 u. M( {( H
56.     P[1][1] -= K[1] * P01_temp;
    ! L9 J% h1 S- U* ]) b; H7 A
57. 
    / o* P) G/ I# `) }3 ^
58.     return angle;
    ; L# W5 [3 Q2 c& t
59. };

复制代码

3 N8 S! g) _% S. R4 S+ Q) K, E7 }7 U三、四元数法

     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
1 s: v, ^/ C! X- v; W
  \8 I, f2 J& N7 o       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
6 [! }0 [# N& ]' a0 H

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
     j( G4 t! @8 @" E$ L
2. {
   0 k$ c  F3 h3 S  \1 A
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   7 B7 V, ^+ \7 d0 _6 |
5.         long quat[4];
   8 f( _1 u7 B0 \& I- V
6. 
   
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   
8.         if (sensors & INV_WXYZ_QUAT )
   
9.         {
   2 o  D) v; e4 t
10.                 q0=quat[0] / q30;
    $ V2 M+ l9 z  ~" U4 q- t
11.                 q1=quat[1] / q30;
    * S8 C- F8 b  V, U  t
12.                 q2=quat[2] / q30;
    % U( a8 b/ Q/ F
13.                 q3=quat[3] / q30;
    
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    ; T* i( L) L9 T4 p+ B
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.         }
    0 Z, M/ a/ R+ x; G/ F: J
18. 
    
19. }

复制代码

7 ]  C0 L2 p0 ]5 ?7 J    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。
/ j* Q' V8 f* K& Y
    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。
8 W5 h0 Y3 u( [# Q
& T& v' U7 C  ]* F    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
4 u/ I4 b5 T% J

: H' ]$ g3 H) e: [8 m- [3 ?
输入工程名字，点击Next

: l0 [4 ?/ c4 Q  |2 f1 [: ?
6 N% M& M! @2 o+ q" |7 p  ~; `
设置完成后，点击Finish

& R- `4 ]$ Q4 ?' s2 g: A
2 F# S8 T- u5 u: A' M
0 }& z: r7 ^) i8 c: ?# R进入MCU配置界面
& j2 E2 e, i1 i$ M' }2 x; J& K


, w$ H2 K9 s( V: E, A$ n设置I2C与MPU-6050通讯。

$ u" e3 m: z. Y& ?, R% o

设置串口与上位机通讯，实时显示MPU-6050位姿。
' N, R# J9 ]8 ]/ t+ L

9 ~3 x: J( r8 G& c1 u7 p2 b
9 u' F4 w% V( C8 K) |设置系统时钟及调试模式。


/ I/ R- A; v8 p3 ^
7 U& ^* Q. |# j: |; K# f8 Z设置时钟源。

" f' C" z+ O) a, h( T. d
& ~7 n' J. |+ ^. l; B3 V
: n; Q5 @7 c4 n/ H( `; n: W全部设置好后，芯片引脚的分配情况。
5 [0 y) q7 e4 ^1 l  R+ |


- A& v( _4 b: y) _4 N) p在时钟配置里设置时钟最大频率。
4 h' Z$ ~, d% M
. w. d/ c0 e2 _) Z8 }* x. t

4 {9 N' ]% `7 m% l7 \5 ]在工程管理——>代码生成器中进行设置
/ }( x( B: T! s; _/ }$ ^; i; w$ H
$ r' m) p: a! P  Q4 e3 R
8 r5 \3 j# R5 R. G
点击此处，生成代码
% y% s3 M" |0 P6 e* Z
/ X% [* N: Q( O7 Y5 U; M

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



将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。
. i/ o3 q( P: l" W- }; B
* r0 z9 y. }1 H" N6 W3 |7 C

& s0 h8 ~2 H6 B) c8 ]) i点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。



1 k* p( t0 N' i! }  B按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。
9 G* Z  o# h: w" G9 x" T
: L3 X' L; f  J& G% p0 u

* b! c" T0 ?3 `, Q. ]* s% k5 J8 c
. z# ^+ T% N( B
: p. C9 W7 l1 u/ \3 s! J然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
: |7 e. h" K+ P

1. /* USER CODE END Header */
   , a; {+ j, a9 |/ D/ H& y' r
2. /* Includes ------------------------------------------------------------------*/
   
3. #include "main.h"
   ; B! ^3 x- V1 R. z- p. S2 l7 h
4. #include "i2c.h"
   - ?1 T) R1 f9 K3 n6 N" m6 m
5. #include "usart.h"
   
6. #include "gpio.h"
   
7. 
   % R* G* ?' ]; b' a' u* S2 b
8. /* Private includes ----------------------------------------------------------*/
   " n# ]8 R, x1 P& e2 Z$ ?7 M1 a# `0 v2 @
9. /* USER CODE BEGIN Includes */
   # G2 ?* e! V' ?
10. #include "mpu6050.h"
    . D5 I! l/ Y3 o  ~: _3 X
11. #include <stdio.h>
    . V# i+ C/ Q. Q4 C8 W% c# ~( z
12. /* USER CODE END Includes */
    
13. 
    $ Q7 Q- `1 ?9 C3 B
14. /* Private typedef -----------------------------------------------------------*/
    * j9 r, H/ }9 J$ U! l5 ~
15. /* USER CODE BEGIN PTD */
    
16. 
      F* q- q9 [; A, ?' P
17. /* USER CODE END PTD */
    
18. 
    0 [2 V0 D# J$ r9 w0 @
19. /* Private define ------------------------------------------------------------*/
    5 d" `7 x# I! ?$ W5 _
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    # D; t" b) p& ]! I7 I9 Z! @
22. 
    
23. /* Private macro -------------------------------------------------------------*/
    
24. /* USER CODE BEGIN PM */
    
25. 
    
26. /* USER CODE END PM */
    " z4 R; O: Q8 y( v
27. 
    & `" }  n" i6 _, n: p; T
28. /* Private variables ---------------------------------------------------------*/
    
29. 
    1 ^4 G. Q7 ~; V8 ?6 O
30. /* USER CODE BEGIN PV */
    
31. 
    
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)
    # y* K) Q; A4 g9 B
37. #endif
    
38. 
    
39. PUTCHAR_PROTOTYPE
    
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    - n6 w4 q; ~8 C  H
42.     return ch;
    
43. }
    
44. /* ---------------------------------------------------------------------------*/
    + i6 y& ^8 H8 T& @8 u7 W9 s% n% ]
45. 
    
46. //获取系统时间，用于卡尔曼滤波
    
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    ; K9 T% k8 F! ^3 b% O+ q8 d
48. {
    # h- F7 Z1 s+ [5 U' ~! A
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    
50. }
    ) y) v# ^9 m8 }
51. static uint32_t getCurrentMicros(void)
    
52. {
    " ?' Y( M; F, ?9 F* p
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    
54.   GXT_SYSTICK_IsActiveCounterFlag();
    
55.   uint32_t m = HAL_GetTick();
    $ Q& V- _$ U/ [& g; E
56.   const uint32_t tms = SysTick->LOAD + 1;
    $ I3 h$ G# [( D( D) n
57.   __IO uint32_t u = tms - SysTick->VAL;
    + f9 M; E' s: [' ~" B9 d/ d
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    
59.     m = HAL_GetTick();
    
60.     u = tms - SysTick->VAL;
    
61.   }
    9 ~$ J+ L/ ?. b
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    & j1 M2 K2 M/ C  Q4 Z- Q" Y5 i5 ]
64. //获取系统时间，单位us
    ( |9 |$ x9 r! j$ r
65. uint32_t micros(void)
    
66. {
    
67.   return getCurrentMicros();
    
68. }
    , i/ h) u, H6 e; A" [
69. 
    
70. 
    
71. /* USER CODE END PV */

复制代码

& s# G4 T* t: E8 e& }使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面
" ~" ]7 m+ L# H+ R+ c* q3 S# @
1 g* k6 s* _6 W# Z: V
- ^% `6 w% C8 y! v& A; i+ v
; V0 E% R* u# |2 n! e5 H5 R: @在main（）函数中添加MPU6050初始化代码和发送位姿数据代码
$ Q0 f  S7 t% [

1. int main(void)
   ! Q& r9 p7 n( ?' Z1 B$ a' H
2. {
   ' O0 C# h7 {2 \1 r# n' g
3.   /* USER CODE BEGIN 1 */
   + n( R" `2 M* a0 D0 M
4. 
   
5.   /* USER CODE END 1 */
   
6. 
   3 J  K+ a# R: d" N* r
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
   7 g. J: Y0 j" q$ d) R2 u1 e
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   + B$ H5 C: L+ n, A
10.   HAL_Init();
    
11. 
    
12.   /* USER CODE BEGIN Init */
    : E) q1 H$ B: _8 L2 v3 E5 L
13. 
    : E. z* W1 `+ [9 p, X4 F7 q
14.   /* USER CODE END Init */
    
15. 
    4 ^  K- w4 E& O2 O2 ~4 q
16.   /* Configure the system clock */
    
17.   SystemClock_Config();
    
18. 
    
19.   /* USER CODE BEGIN SysInit */
    
20. 
    
21.   /* USER CODE END SysInit */
    4 v7 j4 _1 P# T4 l. v( P4 I* D8 H
22. 
    
23.   /* Initialize all configured peripherals */
    
24.   MX_GPIO_Init();
    
25.   MX_I2C1_Init();
    & ?9 l  u) v) ]7 N! v/ D- s
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    
28. 
      E9 m) n$ u6 `( d; E6 B4 X
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    2 S% b; i# T  N7 v6 W2 J* p0 [
30.           printf("MPU_Init_Fail...");
    ' _- x9 Z. S7 y( m
31.   }
    
32. 
    
33.   /* USER CODE END 2 */
    $ a6 p  |5 h( R
34. 
    
35.   /* Infinite loop */
    % @* C, n$ @2 F# E3 x/ T% ?
36.   /* USER CODE BEGIN WHILE */
    8 G" @, Z- t  u
37.   while (1)
    
38.   {
    % Z/ a; s$ v1 k2 [
39.           GetAngle();
    2 q# [  Z& e4 w, l% J8 o
40.           GetPitch();
    
41.           HAL_Delay(10);
    
42.     /* USER CODE END WHILE */
    
43. 
    9 r: X4 K$ D0 ^( n/ m9 p' e# T2 C2 Y
44.     /* USER CODE BEGIN 3 */
    
45.   }
    ' R4 X! @  ?5 e7 D
46.   /* USER CODE END 3 */
    
47. }

复制代码

点击，下载程序



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

/ ^! @* @  [0 q7 R
6 u8 ?+ m% i# y
作者：DIY攻城狮

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