: |3 j0 }' _* G6 P, P0 Z9 h2 o


背景
* ]+ u1 E9 P! }& w3 |        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。

MPU-6050简介
    MPU-6050集成了 3 轴陀螺仪、3 轴加速度计及温度传感器，并且预留一个IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。
' `6 O; I1 s5 J. G* Y$ L: d, o
    MPU-6050 对陀螺仪和加速度计分别用了三个16 位的ADC（0~65535），将其测量的模拟量转化为可输出的数字量。且传感器的测量范围都是可以根据需求设定的，陀螺仪可测范围为±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为±2，±4，±8，±16g。
$ W  Q8 J# }5 L! ]6 s. ~; e: \( h
; [, k& x: Y" ?% u5 O
5 F, h, v. ]$ {

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


互补滤波、卡尔曼滤波计算位姿示例
  z- c9 L' X0 r+ C4 O
+ t8 K" V  p0 \" O+ B6 O! t+ v! d9 `' |

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




; U1 l) E0 f: t3 {: F6 m) [' x一、一阶互补滤波算法
    MPU-6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，通过简单的计算即可得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角，需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正，通过加速度和角速度就可以计算 Pitch 和 Roll 角（单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用）。
% w- t; B) B% @8 `  ]# C' L8 x
: ^3 ?1 J  ], h
* L# j7 z7 |& r9 Y: b0 x! j+ s一阶互补算法如下：
# M& o' d6 K5 u- h7 q9 s. E

1. //一阶互补滤波
   
2. float K1 =0.1;         // 对加速度计取值的权重
   # u% I3 d& \% U, ?" x# v
3. float FirstOrder_Pitch;
   
4. 
   & s, y3 O+ Y& _, ^% |
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   $ }; i( y/ q  z
6. {
   5 V4 f2 L( S$ q
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   . q, }2 U  H& Z/ A0 x. K4 _* z
8.   return FirstOrder_Pitch;
   & R& ]  z" {0 F" {9 w
9. }

复制代码

4 ^' C' l2 W/ z0 R% K二阶互补算法如下：

1. //二阶互补滤波
   " x' q' r' W* _9 w- `2 f$ t; }
2. float K2 =0.2; // 对加速度计取值的权重
   
3. float x1,x2,y1;
   
4. float SecondOrder_Pitch;
   8 A7 `' [; S8 K( {0 L7 B
5. 
   
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   
7. {
   
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   
9. y1=y1+x1*dt;
   5 b0 Q7 ?) c6 m7 j4 {
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    * d0 m( W+ ~& w
12. return SecondOrder_Pitch；
    
13. }

复制代码

) l3 A9 K" U) W* ]二、卡尔曼滤波
4 y+ g/ G$ c  S3 z' G% S

1. /* Kalman filter variables */
   
2.         float Q_angle = 0.001; // Process noise variance for the accelerometer
   + y+ f- E2 ?& ?& f( m
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. 
   
6.         float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
   . f( q6 ^4 g" l, C' w# v/ K
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. 
   
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
    / Q! \4 }/ m( s& B9 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
    % U) V: T6 w' j" T
16.     // Modified by Kristian Lauszus
    : }! @/ p- j& i
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")
    
20.     // Update xhat - Project the state ahead
    * \5 G. r) [5 n' X) H1 l0 r) u
21.     /* Step 1 */
    
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    ! z3 |6 k5 h6 X/ i. r% Y
24. 
    0 B4 l0 s8 ]* p: I5 s
25.     // Update estimation error covariance - Project the error covariance ahead
    
26.     /* Step 2 */
    * c8 l  r8 T8 D2 }
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 C) u; l; Z6 L- O) X- A0 \
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")
    2 l- P# \- i* P& e$ I, @0 q  L. z
33.     // Calculate Kalman gain - Compute the Kalman gain
    - X5 Y3 t5 {4 r0 L* }
34.     /* Step 4 */
    
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;
    : \0 [8 f* @+ i! e
40. 
    . @; ~% W7 s; P8 [+ V1 ?5 Y8 r
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    ! o1 v6 H& R, c8 W$ f" m% @  I
42.     /* Step 3 */
    0 r! ?  R6 M/ x: [
43.     float y = newAngle - angle; // Angle difference
    
44.     /* Step 6 */
    
45.     angle += K[0] * y;
    " S7 k$ Q) Y: \! C/ z. h
46.     bias += K[1] * y;
    
47. 
    ; Q9 |( f6 Y2 H
48.     // Calculate estimation error covariance - Update the error covariance
    ; r; ^: C+ Z- v0 n1 ~
49.     /* Step 7 */
    ) v4 o- r& Y) w' m2 N2 ^& x* R; [
50.     float P00_temp = P[0][0];
    1 u/ |% Q' j- q- u5 l: q
51.     float P01_temp = P[0][1];
    " @  s7 Y3 X" w) l, ?
52. 
    / I9 S2 I. Q1 U0 g& ~; c+ B8 A
53.     P[0][0] -= K[0] * P00_temp;
    
54.     P[0][1] -= K[0] * P01_temp;
    4 B( V. Q' k) f/ ?5 r  m2 m& m
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    # X8 ~# N8 ~3 B- ?- H' `
57. 
    0 |3 N9 E" f6 M4 a( p# E
58.     return angle;
    ; ~) r, v  ]9 k! z) G' T1 D
59. };

复制代码

三、四元数法
2 z6 R3 b  Q5 N$ A" j/ s! {* L5 \
     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
1 s( G4 I; b" B1 v8 J
       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
* G! l$ |( J+ U! K% T7 _# e0 w- `

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
     X0 e4 F* B6 i7 \/ _+ ~+ `
2. {
   6 C. {/ d7 s9 E2 c
3.         unsigned long sensor_timestamp;
   
4.         unsigned char more;
   
5.         long quat[4];
   
6. 
     E- T1 }! C9 A) b  d5 H0 \2 o" i2 l, Z
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   8 a: |' m' l+ B& Q
8.         if (sensors & INV_WXYZ_QUAT )
   
9.         {
   
10.                 q0=quat[0] / q30;
    
11.                 q1=quat[1] / q30;
    
12.                 q2=quat[2] / q30;
    0 n* k" g2 \" u3 v/ I  d
13.                 q3=quat[3] / q30;
    
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    # T7 L; \. G: j. {4 j4 @! A
15.                 *Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
    6 I" T( V! n) ~' G- Y2 Y5 F
16.                  *Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;        //yaw
    3 C* G1 d4 W; r2 X
17.         }
    
18. 
    
19. }

复制代码

6 Q) Z1 H% {9 G1 d+ A    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。

    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。
# h- G  }" g# ~8 J! I+ Y& g" m
    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next
7 W; U& u+ w2 n$ K4 I! X" ^


8 B0 e. O; }- r0 I& q输入工程名字，点击Next

, n3 l8 X# E" S- }, o
( P, ~& B0 }' u! Q5 e( f
设置完成后，点击Finish



+ N7 w3 }0 \9 b6 R9 x进入MCU配置界面
- E$ I+ l( m3 x# E
, ?9 O2 c/ I9 z9 K$ q

设置I2C与MPU-6050通讯。

7 v4 g- \- g( a) W6 ^; O. h
# E) t, ?0 H1 a9 u+ p* q
设置串口与上位机通讯，实时显示MPU-6050位姿。
5 y6 m% {" \. R: b" @
1 |: F$ R; [  s! E+ |

6 v& C9 j* I7 w4 n% \设置系统时钟及调试模式。



5 c6 _9 j. r2 C- T8 I设置时钟源。


$ s# ^0 j* ~- l7 b
/ Y7 e2 m) h# k% R8 I7 v; l( N  z全部设置好后，芯片引脚的分配情况。
$ G/ u4 j! K/ V9 d$ E/ K- G
* S/ T8 c( N" r3 q/ T
4 m. ?: b- x! S+ Q* B# o* _) a
在时钟配置里设置时钟最大频率。

6 W' d& E+ H6 q+ V

在工程管理——>代码生成器中进行设置
! H' R; }/ s9 d5 e! S
3 Y" Q0 G# b: a; j
( e& Z+ U3 l7 }: K5 M6 D+ s0 _
  }: p) i" X5 j5 {6 N点击此处，生成代码
. h' b3 r# m- z, w8 d


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

( _! C4 c; a( G3 W

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

/ O5 I8 {0 `3 J* d  r/ ^# b
6 Z' q' j* v% ~1 L  F: L
& V2 }! n6 L6 o点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。
! W1 O* X  h! I; b
, K" T/ d' K# J+ Y" \% B
! B! S# ^7 H/ P2 I2 c( u9 V9 W5 k
按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

* ^) ]1 h% W6 a" G, \* H- ^4 U
% W! Q0 c2 r6 q% U+ r! p; C3 b
8 X5 Y& J4 O1 c

然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
% h7 ]0 j0 b* J7 [" G6 r

1. /* USER CODE END Header */
   ) B+ G# I9 Y/ I- J( a# A
2. /* Includes ------------------------------------------------------------------*/
   - o( `5 _0 ^5 H! N! Q: Q
3. #include "main.h"
   
4. #include "i2c.h"
   3 H' W% V1 l# \* y" R
5. #include "usart.h"
   
6. #include "gpio.h"
   
7. 
   3 ]0 E; O3 m- K* Q5 h
8. /* Private includes ----------------------------------------------------------*/
   & r. ~6 W+ r  k, F  I$ e! u
9. /* USER CODE BEGIN Includes */
   ) C: X8 f2 n" ~4 Z$ s+ T- F3 @
10. #include "mpu6050.h"
    % V# T. [, K# E7 b$ D
11. #include <stdio.h>
    , d# }( h1 @0 y" y
12. /* USER CODE END Includes */
    % Z9 Z8 e: o( f) i* i2 Z7 ^
13. 
    9 \5 t1 V, L, Z+ a5 a7 A; a% `4 Z
14. /* Private typedef -----------------------------------------------------------*/
    
15. /* USER CODE BEGIN PTD */
    
16. 
    , w) `+ W* A. L
17. /* USER CODE END PTD */
    6 C* U+ Q% i1 l' V4 G, w
18. 
    
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    
22. 
    7 T0 Z  @! b6 o. D; l2 D
23. /* Private macro -------------------------------------------------------------*/
    
24. /* USER CODE BEGIN PM */
    
25. 
    7 B; [, A5 G, {
26. /* USER CODE END PM */
    
27. 
    % [  M1 A, ?: y2 D4 R
28. /* Private variables ---------------------------------------------------------*/
    / B8 }# \* t/ g! Y
29. 
    2 A* R& Z6 G3 p+ f- K! J
30. /* USER CODE BEGIN PV */
    
31. 
    & @0 T  z; Z& |5 ]+ u
32. //重定义printf函数，用于串口输出
    - n* N/ `! |$ \* b" p4 X
33. #ifdef __GNUC__
    
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    4 {- V1 ]# p/ V1 |
35. #else
    
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    
37. #endif
    
38. 
    - F3 _+ ^  w4 I4 H7 r# C
39. PUTCHAR_PROTOTYPE
    
40. {
    
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    
42.     return ch;
    
43. }
    # ]( e. U' T6 {  }8 q8 A" n
44. /* ---------------------------------------------------------------------------*/
    8 M# H9 x- q  b0 @  p0 I
45. 
    
46. //获取系统时间，用于卡尔曼滤波
    " N8 U6 }% N7 W& y% J/ T1 t
47. __STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
    
48. {
    
49.   return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
    
50. }
    , ^7 X( X5 m/ n& f+ V6 O1 I8 E
51. static uint32_t getCurrentMicros(void)
    9 f% v5 E5 R3 R( O
52. {
    9 X, e. P% p% q& C7 t; N
53.   /* Ensure COUNTFLAG is reset by reading SysTick control and status register */
    " k: a8 y- t9 {0 r  i
54.   GXT_SYSTICK_IsActiveCounterFlag();
    
55.   uint32_t m = HAL_GetTick();
    
56.   const uint32_t tms = SysTick->LOAD + 1;
    
57.   __IO uint32_t u = tms - SysTick->VAL;
    
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    
59.     m = HAL_GetTick();
    
60.     u = tms - SysTick->VAL;
    - p1 D1 y# {" r  F7 s# L
61.   }
    % _6 X( G8 z4 e: s+ d( H
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    
64. //获取系统时间，单位us
    
65. uint32_t micros(void)
    " o% t7 @7 S  p/ s2 j$ Q
66. {
    
67.   return getCurrentMicros();
    
68. }
    # A7 @$ N8 U8 g6 [: U2 i5 X- K
69. 
    
70. 
    1 X$ v# @* S/ l$ D
71. /* USER CODE END PV */

复制代码

使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面

, ~) }) `+ m% y, q6 [( V
( |; ]/ P" F7 f' k' _  R
在main（）函数中添加MPU6050初始化代码和发送位姿数据代码

1. int main(void)
   
2. {
   + e2 i) g8 {( h5 N  B8 I8 E. w1 B
3.   /* USER CODE BEGIN 1 */
   
4. 
   + C9 L2 ~" d* o) y$ L" i8 E. t
5.   /* USER CODE END 1 */
   , q$ a# |+ \6 [8 L7 f
6. 
   / u7 \* b+ K4 u$ z
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
   
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   2 V+ U: H3 C5 I8 ?# H1 K" h6 ]
10.   HAL_Init();
    1 S: S0 Y# z* ]6 ~; {
11. 
    $ b4 }9 O) u, t. T6 ^
12.   /* USER CODE BEGIN Init */
    " y+ {' a5 `. S, Z0 P/ U+ Z! J9 g5 @
13. 
    
14.   /* USER CODE END Init */
    2 f6 F8 d8 s! p& D( o2 f# V% p! T
15. 
    
16.   /* Configure the system clock */
    4 h) k  F& a' ^  b4 \& m
17.   SystemClock_Config();
    
18. 
    
19.   /* USER CODE BEGIN SysInit */
    9 N& F  {5 n4 _+ B2 j
20. 
    
21.   /* USER CODE END SysInit */
    9 k! V) H) t+ v7 f. P
22. 
    
23.   /* Initialize all configured peripherals */
    + B8 a' K, e; e- m% \& u: p
24.   MX_GPIO_Init();
    / t7 j8 ]0 D& \; P
25.   MX_I2C1_Init();
    
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    : W; i# I5 }- _1 \# t
28. 
    ( s# q/ p0 V6 N* C9 M9 g
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    : n7 }; ]) u. [4 |
30.           printf("MPU_Init_Fail...");
    
31.   }
    
32. 
    1 r( j) t3 Y7 r- I6 ]5 O& I
33.   /* USER CODE END 2 */
    
34. 
    
35.   /* Infinite loop */
    1 y$ p: k2 `: E5 Q4 J" V* n. T
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    
38.   {
    
39.           GetAngle();
    
40.           GetPitch();
    8 d# X+ I8 S* T, L
41.           HAL_Delay(10);
    8 w+ {( U; a( L# _# O
42.     /* USER CODE END WHILE */
    
43. 
    7 e% H9 L) g$ m/ h9 ^
44.     /* USER CODE BEGIN 3 */
    ; ~- o* ^* A. X" d
45.   }
    - ^( R, W* Y& p7 G+ g$ h/ s& e
46.   /* USER CODE END 3 */
    ! q8 Y6 B3 x! Q0 W% A: N
47. }

复制代码

点击，下载程序


% g! D; R- j# G6 E+ Z
) \  U9 g0 v# M/ e; t6 r  b程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
2 V1 {4 }% T; l% _
% o, |! ^2 O) N% o

5 L* M; w9 q2 C: R9 K( P 作者：DIY攻城狮

7 v( r! w- K7 ?; y
1 e6 A% ]9 k  ]# X

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