2 z4 k' K  G5 p! F( M
) y7 w; d5 x6 H3 s% |8 x8 k
7 }) C0 H0 o3 r- Q( _
背景
1 A. N* x, e6 p6 @        本文用于记录平衡自行车的制作过程，及制作中遇到的问题；总体方案如下：采用STM32F103C8T6作为主控单元、MPU6050作为位姿采集单元、无刷电机带动动量轮调节小车平衡、1S锂电池配合5V和12V升压模块作为电源、蓝牙模块用于和微信小程序进行无线遥控及PID调试、舵机用于控制行驶方向和支撑小车站立。
* T3 K$ ]. x  S( x/ k
MPU-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。



) o9 L. v1 ~+ j+ f
+ r, Z" O5 C) }1 x4 MMPU-6050姿态获取与处理（惭愧，我也一知半解）
    理论上只需要对3轴陀螺仪的角度进行积分，就可以得到MPU-6050的姿态数据。实际上由于陀螺仪受噪声的影响，只对陀螺仪积分并不能得到完全准确的姿态，所以需要用加速度计进行辅助矫正，常用的方法有三种：互补滤波、卡尔曼滤波、硬件DMP解算四元数。
, M1 G- O$ M+ b: Q, Q2 ~$ Q

互补滤波、卡尔曼滤波计算位姿示例
& Y' u0 O+ a  ?  R


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

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

6 x3 y7 L4 o- |( C% V! J
: @1 k' {% X) u$ ]* V7 D! |一、一阶互补滤波算法
    MPU-6050 的加速度计和陀螺仪各有优缺点，三轴的加速度值没有累积误差，通过简单的计算即可得到倾角，但是它包含的噪声太多（因为待测物运动时会产生加速度，电机运行时振动会产生加速度等），不能直接使用；陀螺仪对外界振动影响小，精度高，通过对角速度积分可以得到倾角，但是会产生累积误差。所以不能单独使用MPU-6050的加速度计或陀螺仪来得到倾角，需要二者进行互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值，把它们结合到一起进行修正，通过加速度和角速度就可以计算 Pitch 和 Roll 角（单靠 MPU6050 无法准确得到 Yaw 角，需要和地磁传感器结合使用）。
3 L* N0 e& ^% ]1 B6 [- q
$ R3 n) G; c4 }# ]3 }! F3 J  ]$ Q& n
! j: s* i4 x& A7 i一阶互补算法如下：

1. //一阶互补滤波
   " ?9 L) z9 I* A; ]8 h2 T
2. float K1 =0.1;         // 对加速度计取值的权重
   
3. float FirstOrder_Pitch;
   
4. 
   
5. float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   # i: E6 [  K  o2 H& i0 V
6. {
   
7.   FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
   
8.   return FirstOrder_Pitch;
   
9. }

复制代码

3 z/ T3 o2 `% u2 B二阶互补算法如下：
$ }9 {$ ]/ m; w* ]

1. //二阶互补滤波
   
2. float K2 =0.2; // 对加速度计取值的权重
   0 h6 D* C9 l1 b  ]* [
3. float x1,x2,y1;
   . I, p+ V2 _' i0 `6 d
4. float SecondOrder_Pitch;
   
5. 
   
6. float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
   2 i4 d0 }2 X# D" F( l# T9 A- l
7. {
   
8. x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
   
9. y1=y1+x1*dt;
   
10. x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
    * ?! I8 d8 R. `5 G# |) q" ?
11. SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
    
12. return SecondOrder_Pitch；
    ) \# y2 U% y* E. G! `
13. }

复制代码

二、卡尔曼滤波

1. /* Kalman filter variables */
   & n/ h2 A% S' U' z3 j2 U' Y+ i
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
   & _6 N2 |8 _) Z+ \# [" L" W
5. 
   ! c. T4 P) R8 b0 U" D
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
   : w; R0 S( c+ ]$ D1 L6 \
8.         float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
   
9. 
   1 `0 d: |1 a% m9 L
10.         float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
    : r0 y# c) e0 f' C4 `! y
11. 
    
12. 
    ( {; X8 c# L3 S/ t
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) {
    2 C) B  z- O# c2 Z
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")
    ' a7 ]& `& X8 `5 j  `9 o$ \
20.     // Update xhat - Project the state ahead
    & }3 Y# J7 {$ U* m1 o
21.     /* Step 1 */
    6 L( C5 @% y1 L. Q. Y0 Q, U
22.     rate = newRate - bias;
    
23.     angle += dt * rate;
    
24. 
    5 b* s4 C. Q9 f5 k
25.     // Update estimation error covariance - Project the error covariance ahead
    
26.     /* Step 2 */
    ! _/ O! i! f! e: q: G2 N
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];
    
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")
    . M4 f6 D: ~; c2 {3 m& f; Z$ B; L
33.     // Calculate Kalman gain - Compute the Kalman gain
    $ h- Q! ~2 X4 Z0 q
34.     /* Step 4 */
    
35.     float S = P[0][0] + R_measure; // Estimate error
    
36.     /* Step 5 */
    $ U1 R0 ?8 D- g6 ]5 o
37.     float K[2]; // Kalman gain - This is a 2x1 vector
    
38.     K[0] = P[0][0] / S;
    , v8 A! S5 z+ E. Z
39.     K[1] = P[1][0] / S;
    
40. 
    . n5 z$ Z" L; U+ J6 ^
41.     // Calculate angle and bias - Update estimate with measurement zk (newAngle)
    
42.     /* Step 3 */
    
43.     float y = newAngle - angle; // Angle difference
    0 U! |" V$ h2 f6 e2 R/ w
44.     /* Step 6 */
    
45.     angle += K[0] * y;
    
46.     bias += K[1] * y;
    * h1 o8 b+ K5 a9 s4 I) n( e% p5 P
47. 
    - e8 e" s5 o0 B  R" k# |) S
48.     // Calculate estimation error covariance - Update the error covariance
    3 t+ N7 k1 }! i: v' M: c8 M' u$ ^
49.     /* Step 7 */
    
50.     float P00_temp = P[0][0];
    ( d( `# B3 `6 y. R
51.     float P01_temp = P[0][1];
    ! F9 B8 p9 k: E- o- f
52. 
    3 X2 `* G/ o2 j
53.     P[0][0] -= K[0] * P00_temp;
    : j" H* H& b6 i) a$ Z* `
54.     P[0][1] -= K[0] * P01_temp;
    
55.     P[1][0] -= K[1] * P00_temp;
    
56.     P[1][1] -= K[1] * P01_temp;
    ' l7 x) n: z+ V0 R1 f9 p7 @  A3 R
57. 
    : i- r* n1 j) I
58.     return angle;
    0 r/ Q& R& i% y+ V
59. };

复制代码

三、四元数法

     MPU-6050 自带了数字运动处理器，即 DMP，并且InvenSense公司 提供了一个 MPU-6050 的嵌入式运动驱动库，结合 MPU-6050 的 DMP，可以将我们的原始数据直接转换成四元数输出，而得到四元数之后，可以很方便的计算出欧拉角，从而得到 Yaw、Roll 和Pitch。
: H. t2 T7 i% H. R% G& P( Z
       使用内置的 DMP，大大简化了代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。
* n/ h. R0 x/ K: }: z

1. void Read_DMP(float *Pitch,float *Roll,float *Yaw)
   
2. {
   ; N8 p8 \5 u! e& `+ d+ F! t
3.         unsigned long sensor_timestamp;
   : u! }- p& j- ^, u3 S
4.         unsigned char more;
   
5.         long quat[4];
   7 D2 w  G, \4 ]7 j/ O# x+ [* |
6. 
   & p( O' A+ G1 `2 M
7.         dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
   9 h3 B4 P% w6 `  s5 O. v
8.         if (sensors & INV_WXYZ_QUAT )
   
9.         {
   
10.                 q0=quat[0] / q30;
    5 z8 n* J2 o2 `" y" p: T6 y0 C
11.                 q1=quat[1] / q30;
    
12.                 q2=quat[2] / q30;
    ( w8 Y: ~; G( `4 O) e
13.                 q3=quat[3] / q30;
    & o# q3 A( |5 b: o+ R3 c: `
14.                 *Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
    - j/ _5 [* u" s) a
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.         }
    
18. 
    ( F0 m! f7 C( ~0 v/ }* R
19. }

复制代码

    以下是我测试一阶互补滤波和卡尔曼滤波项目的搭建过程，老鸟请忽略。
* |5 b6 o8 _& V
    固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。
5 f; r% r9 g8 [: n5 H
    我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next

) ~4 v( [8 A- e; a

输入工程名字，点击Next
% y% l' d' @# C, {. h5 m

( X4 H& i$ e) E  f
5 ^0 O% N+ c- z6 z/ ^- _设置完成后，点击Finish


' d& Y% O3 l4 y" [" U( H+ L& K$ D
进入MCU配置界面
0 \( ^* ]' g# V

# M1 y1 e! F) q! t6 L
9 v8 L$ T* R- @- n) D, Q7 F( ]设置I2C与MPU-6050通讯。
1 n+ e4 u2 Z( r3 ]/ t. s/ g' k$ R4 h9 M
1 H; j# K# G% d
" T3 w# m9 F- a# x+ D
3 R3 X9 x8 U& v5 f设置串口与上位机通讯，实时显示MPU-6050位姿。


& X6 J) k2 J0 i% H
设置系统时钟及调试模式。
. j7 \& D0 e, v! T& z
+ y5 f- c/ Z/ R  _# {

设置时钟源。
' t% r# ?7 o3 s$ t. A
0 q- A) O4 y: S- A9 a
7 i  _0 W8 A3 e  }' ~! h4 B6 Z
全部设置好后，芯片引脚的分配情况。
' T3 Q& y* G# [
6 o9 p+ k/ E7 ^1 h# K3 r
+ A1 C2 L- K3 P3 N2 t
在时钟配置里设置时钟最大频率。
! G' J' P! C# f+ h
6 S$ H/ O6 v- `: m# z

' D  `' F" P+ j) o" e/ R在工程管理——>代码生成器中进行设置
% v9 D2 S: u' @) D$ |
! x7 a3 o" p' F3 r# u! ]

0 k8 Q' {% y2 n. F, C点击此处，生成代码


+ ~' R9 P0 H8 z" U+ A& D& t/ L
2 y, P; ?2 v6 j1 p在工程文件夹中创建iCode文件。



  g- ~5 N( {, b8 E9 D9 h将MPU-6050、卡尔曼滤波库文件复制到iCode文件中。
. O5 V: @2 R8 `- p6 ^1 @9 Q

+ M% }% A' K7 S1 k) G: @
点击Debug，刚刚添加的驱动文件全部在工程文件中显示出来。
4 I4 L0 m% A  E$ [) m/ v, G8 T! S
1 k7 [! k) ^8 |+ h$ X

按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。
1 e/ [1 [, }" t$ g+ G




4 T9 F0 }, B+ K& G4 E然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。
; t4 U& p. r! d7 P5 L4 d

1. /* USER CODE END Header */
   
2. /* Includes ------------------------------------------------------------------*/
   
3. #include "main.h"
     }7 E) S6 u. H) Q
4. #include "i2c.h"
   , y% |/ X( M& W# L
5. #include "usart.h"
   
6. #include "gpio.h"
   
7. 
   9 w1 E; h; |; c( \
8. /* Private includes ----------------------------------------------------------*/
   $ w+ `# W5 I, v* b# z
9. /* USER CODE BEGIN Includes */
   ( t, F$ o, U# U8 R$ E
10. #include "mpu6050.h"
    - K, _8 T; \5 O! `% A( W
11. #include <stdio.h>
    ; y$ o, l  Z; f$ m+ |1 g2 R
12. /* USER CODE END Includes */
    - o4 m/ G2 y/ E0 u9 C
13. 
    
14. /* Private typedef -----------------------------------------------------------*/
    & T# D- j0 q; @9 b! Y1 R9 }
15. /* USER CODE BEGIN PTD */
    
16. 
    
17. /* USER CODE END PTD */
    3 ]+ m0 U* D* g/ h# G
18. 
    , V! w7 B" k# s# P2 b3 P  \
19. /* Private define ------------------------------------------------------------*/
    
20. /* USER CODE BEGIN PD */
    
21. /* USER CODE END PD */
    
22. 
    * Q4 L9 b6 A5 W
23. /* Private macro -------------------------------------------------------------*/
    $ Q& v( _3 ~1 Q6 o' {1 K
24. /* USER CODE BEGIN PM */
    8 Y( X/ X: k/ k) w) _
25. 
    & H: u0 Q3 e4 i' F& H7 s8 n2 ]2 ]
26. /* USER CODE END PM */
    : t* I/ @) _* Y# i
27. 
    
28. /* Private variables ---------------------------------------------------------*/
    & _  z3 e! k, T( ?% X3 l+ `
29. 
    ( G, ?  w  U/ ]. ~* }8 k8 O7 r
30. /* USER CODE BEGIN PV */
    2 U1 K1 j8 |* F3 i
31. 
    
32. //重定义printf函数，用于串口输出
    
33. #ifdef __GNUC__
    
34. #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    : k' H1 G$ _( F; W4 J7 h
35. #else
    ! a5 Z. K9 G/ y& B/ K
36. #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    - D: B) V" J7 ~* J
37. #endif
    
38. 
    : Z' E9 D- ?' Z3 }/ {* z
39. PUTCHAR_PROTOTYPE
    
40. {
    * u' I. a5 i4 \
41.         HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
    8 Z# k+ ?9 d% e( J
42.     return ch;
    
43. }
    0 S# x& s" v# C$ N: ]! g& L, v
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));
    ! }5 P- V4 ]0 t7 \7 ]' W
50. }
    
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();
    % W2 x; s; d) N( r+ M+ e
56.   const uint32_t tms = SysTick->LOAD + 1;
    # T* W+ |5 _- m9 X$ a
57.   __IO uint32_t u = tms - SysTick->VAL;
    
58.   if (GXT_SYSTICK_IsActiveCounterFlag()) {
    
59.     m = HAL_GetTick();
    4 a( L: V, t, a
60.     u = tms - SysTick->VAL;
    
61.   }
    * m7 B4 {( _  i' i+ d
62.   return (m * 1000 + (u * 1000) / tms);
    
63. }
    # M' t" L6 B# ?: @
64. //获取系统时间，单位us
    
65. uint32_t micros(void)
    4 y# U% l+ B8 j% n3 W; `
66. {
    
67.   return getCurrentMicros();
    ' z: K% {/ c! b) z- T1 g( o) d
68. }
    ! k8 D2 F* d! u" @" V7 M( u
69. 
    ! \; e2 ?, \" M4 K
70. 
    7 g3 i$ q6 p- U, j
71. /* USER CODE END PV */

复制代码

6 _! Z7 C! m( f/ E. X) y- A8 m使用printf()函数，还需进行如下设置：点击菜单栏 Project-->properties，弹出如下界面

* A. v/ N: @2 l# n8 ]1 H
( N2 T. l% S& P0 |6 q
在main（）函数中添加MPU6050初始化代码和发送位姿数据代码

1. int main(void)
   0 \! [  P6 f4 I
2. {
   
3.   /* USER CODE BEGIN 1 */
   + k; v$ y" U! S3 C
4. 
   
5.   /* USER CODE END 1 */
   
6. 
   
7.   /* MCU Configuration--------------------------------------------------------*/
   
8. 
   # \4 P: |4 b/ O$ @5 E
9.   /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
   
10.   HAL_Init();
    3 e, _$ m8 x/ P0 B8 b+ }0 _
11. 
    ; O7 j& m* h9 [
12.   /* USER CODE BEGIN Init */
    ) ~8 D+ r8 |' {. b
13. 
    4 U0 h+ w( G  p5 A
14.   /* USER CODE END Init */
    7 H- X) V- z9 k+ [
15. 
    9 Y8 m+ D) ~1 ~: B# x
16.   /* Configure the system clock */
    7 e# i. h- [* Z4 I+ b) \) [: d
17.   SystemClock_Config();
    
18. 
    
19.   /* USER CODE BEGIN SysInit */
    
20. 
    ) U; j+ o  ?) g/ r. X# I
21.   /* USER CODE END SysInit */
    ( w" M" C) O6 V( \, K! O
22. 
    
23.   /* Initialize all configured peripherals */
    7 X) ^7 `" b; Q. P
24.   MX_GPIO_Init();
    7 v3 }' m" ?" H! Q, [" l& z
25.   MX_I2C1_Init();
    
26.   MX_USART1_UART_Init();
    
27.   /* USER CODE BEGIN 2 */
    " q/ a+ \7 {3 ~9 T
28. 
    8 S+ A. M( M) o' E" n
29.   while (MPU_Init()){                //如果初始化失败，继续初始化
    
30.           printf("MPU_Init_Fail...");
    0 L  D2 e7 ^# j1 Z' X" a
31.   }
    , ]& N1 z4 G+ I0 [/ z3 L+ c
32. 
    
33.   /* USER CODE END 2 */
    
34. 
    4 _7 v6 j  W# P' a3 D; u, U! d2 Z
35.   /* Infinite loop */
    
36.   /* USER CODE BEGIN WHILE */
    
37.   while (1)
    
38.   {
    * H% @* o4 `5 S1 i% @. M
39.           GetAngle();
    6 B; G* L  G* ?- Q
40.           GetPitch();
    
41.           HAL_Delay(10);
    
42.     /* USER CODE END WHILE */
    
43. 
    8 Y) u6 T9 \- }% U, g
44.     /* USER CODE BEGIN 3 */
    6 X& Z( ]8 o# O  r2 q3 j+ [; N
45.   }
    
46.   /* USER CODE END 3 */
    2 ]/ ]' h8 Y" ^8 z
47. }

复制代码

6 |  m! X: l7 b# v/ n点击，下载程序

& e- ]" d, r3 ?6 ]6 P0 [% ^

  v% M6 u# S$ \程序烧录好后就会通过串口通讯向上位机发送位姿数据（加速度计、卡尔曼、一阶二阶互补滤波计算的位姿），上位机我用的是Arduino编译器来接受并显示数据。设置好波特率后即可显示数据。
9 U4 P0 J, `, M  X

! Y- C  [6 P- i5 {$ r+ ?2 y2 Q
6 O5 L: C6 j7 y0 [  s 作者：DIY攻城狮


0 L/ A& g2 A3 N

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