基于STM32CubeIDE+MPU6050做的动量轮平衡自行车（一）

[复制链接]

STMCU小助手发布时间：2023-2-9 17:00

文章
文章封面:
文章简介:	基于STM32CubeIDE+MPU6050做的动量轮平衡自行车（一）

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

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。

Q(D3Y}_OBQFJ0FA501Q1PWI.png

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

3M4ZAPN6Y61O]`Q(`F`%P%T.png

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

LTC%Z6L{1J6%~U}3VE@]A$P.png

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

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

~3FY21~}NEG2BK(M@DI]NBR.png

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

一阶互补算法如下：

//一阶互补滤波
float K1 =0.1; // 对加速度计取值的权重
float FirstOrder_Pitch;
float FirstOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
{
FirstOrder_Pitch = K1 * newAngle + (1-K1) * (angle + newRate * dt);
return FirstOrder_Pitch;
}

复制代码

二阶互补算法如下：

//二阶互补滤波
float K2 =0.2; // 对加速度计取值的权重
float x1,x2,y1;
float SecondOrder_Pitch;
float SecondOrder(float newAngle, float newRate, float dt)//采集后计算的角度和角加速度
{
x1=(newAngle-SecondOrder_Pitch)*(1-K2)*(1-K2);
y1=y1+x1*dt;
x2=y1+2*(1-K2)*(newAngle-SecondOrder_Pitch)+newRate;
SecondOrder_Pitch=SecondOrder_Pitch+ x2*dt;
return SecondOrder_Pitch；
}

复制代码

二、卡尔曼滤波

/* Kalman filter variables */
float Q_angle = 0.001; // Process noise variance for the accelerometer
float Q_bias = 0.003; // Process noise variance for the gyro bias
float R_measure = 0.03; // Measurement noise variance - this is actually the variance of the measurement noise
float angle = 0.0; // The angle calculated by the Kalman filter - part of the 2x1 state vector
float bias = 0.0; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
float P[2][2]={0.0,0.0,0.0,0.0}; // Error covariance matrix - This is a 2x2 matrix
// The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
float Kalman_filter(float newAngle, float newRate, float dt) {
// KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
// Modified by Kristian Lauszus
// See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
// Discrete Kalman filter time update equations - Time Update ("Predict")
// Update xhat - Project the state ahead
/* Step 1 */
rate = newRate - bias;
angle += dt * rate;
// Update estimation error covariance - Project the error covariance ahead
/* Step 2 */
P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
P[0][1] -= dt * P[1][1];
P[1][0] -= dt * P[1][1];
P[1][1] += Q_bias * dt;
// Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
// Calculate Kalman gain - Compute the Kalman gain
/* Step 4 */
float S = P[0][0] + R_measure; // Estimate error
/* Step 5 */
float K[2]; // Kalman gain - This is a 2x1 vector
K[0] = P[0][0] / S;
K[1] = P[1][0] / S;
// Calculate angle and bias - Update estimate with measurement zk (newAngle)
/* Step 3 */
float y = newAngle - angle; // Angle difference
/* Step 6 */
angle += K[0] * y;
bias += K[1] * y;
// Calculate estimation error covariance - Update the error covariance
/* Step 7 */
float P00_temp = P[0][0];
float P01_temp = P[0][1];
P[0][0] -= K[0] * P00_temp;
P[0][1] -= K[0] * P01_temp;
P[1][0] -= K[1] * P00_temp;
P[1][1] -= K[1] * P01_temp;
return angle;
};

复制代码

三、四元数法

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

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

void Read_DMP(float *Pitch,float *Roll,float *Yaw)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
*Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
*Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
*Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw
}
}

复制代码

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

固件开发采用STM32CubeIDE开发，对于我这种菜鸡来说还是很友好的。

我使用的是STM32CubeIDE1.9.0，打开软件后，依次点击 File-->New-->Stm32 Project，弹出如下界面，输入MCU型号-->选择封装形式-->Next

6Z1)SWD0)NGR@]O%J3~_A`G.png

输入工程名字，点击Next

KU9167BQ{6[RN@@Z{Q}@K7B.png

设置完成后，点击Finish

)W%A{MBY6WZ9DC`3B@F1KM8.png

进入MCU配置界面

设置I2C与MPU-6050通讯。

CO(V62(7YX0CZ4}@DZ)$Q.png

设置串口与上位机通讯，实时显示MPU-6050位姿。

$ZJQU$P3UR1R4)729_G][%{S.png$

设置系统时钟及调试模式。

(~J`34%%TXA1IXKWT(T@WN7.png

设置时钟源。

$~VCI0F0AO}W_{IQM08)~J44.png$

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

$SL6GUUTZ2Q)$R5{_){T9V@T.png$

在时钟配置里设置时钟最大频率。

3_0F%FN3W27QQvC$O5S$I.png

在工程管理——>代码生成器中进行设置

2ET9LUB057L`M4NNOJ@@HRK.png

点击此处，生成代码

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

~3XUPXUKJ3X311X9L%MRRSX.png

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

XFBF%G7M)$$FIFDRGQDM6W1.png

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

Y%XW13SG7SJ%JBPZA1JIEAH.png

按照下列方式设置MPU-6050、卡尔曼滤波驱动文件的编译路径。

DJZL)$_$FQL20QWJM(8)A.png

然后再main.c文件中添加如下代码，用于重定义printf函数，用于串口输出；获取系统时间，用于滤波。

/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "mpu6050.h"
#include <stdio.h>
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
//重定义printf函数，用于串口输出
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE
{
HAL_UART_Transmit(&huart1, (uint8_t*)&ch,1,HAL_MAX_DELAY);
return ch;
}
/* ---------------------------------------------------------------------------*/
//获取系统时间，用于卡尔曼滤波
__STATIC_INLINE uint32_t GXT_SYSTICK_IsActiveCounterFlag(void)
{
return ((SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk) == (SysTick_CTRL_COUNTFLAG_Msk));
}
static uint32_t getCurrentMicros(void)
{
/* Ensure COUNTFLAG is reset by reading SysTick control and status register */
GXT_SYSTICK_IsActiveCounterFlag();
uint32_t m = HAL_GetTick();
const uint32_t tms = SysTick->LOAD + 1;
__IO uint32_t u = tms - SysTick->VAL;
if (GXT_SYSTICK_IsActiveCounterFlag()) {
m = HAL_GetTick();
u = tms - SysTick->VAL;
}
return (m * 1000 + (u * 1000) / tms);
}
//获取系统时间，单位us
uint32_t micros(void)
{
return getCurrentMicros();
}
/* USER CODE END PV */

复制代码

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

[I](O`R0}YFTDA{2SX~2%)J.png

在main（）函数中添加MPU6050初始化代码和发送位姿数据代码

int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
/* USER CODE BEGIN 2 */
while (MPU_Init()){ //如果初始化失败，继续初始化
printf("MPU_Init_Fail...");
}
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
GetAngle();
GetPitch();
HAL_Delay(10);
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}

复制代码

点击，下载程序

${90_LT8S2ACLQ4(5~ZX943B.png$

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

H5PKUP6Y02TULI6(N)Y_NZ5.png