平衡车学习记录（4）

2301_80201384

168人浏览 · 2026-08-05 22:04:32

2301_80201384 · 2026-08-05 22:04:32 发布

这一章我将学习使用MPU6050模块，结算欧拉角等。

第一部分 MPU6050简介

如图，最左边就是单片机的MPU6050芯片

传感器主要由加速度计和陀螺仪组成，以上是MPU6050可以测量的参数。

MU6050通过I2C接口和单片机通信。

接受6个数据，通过DMP处理获得四元数。四元数等效于欧拉角。但本项目里，我们手动计算欧拉角。虽然DMP结算效果更好，但是我这里先跟着教程手动解算，了解欧拉角解算原理，后续再尝试DMP解算。用DMP需要移植一个库，为了教学就先采用手动解算的的方式。

对于手动解算，采用互补滤波的方式，对于卡尔曼滤波，更复杂也需要更多数学知识，所以后续再进行相关研究。

第一部分传感器原理

如图是传感器的工作原理。只是模拟一下原理，假设物体向右加速运动，小球根据惯性就会向左压缩弹簧，就可以间接求出ay。ax，az同理。

把MPU6050安装在小车里，规定的x,y,z方向可以看芯片左上角的小圆点，此时小车向y方向加速运动，中间的这根弹簧，y+方向被拉长，y-方向被压缩。可以根据ay=a求出加速度。在x方向没有运动的加速度，所以加速度是0。

如图，提出一个问题：小车此时静止且保持直立，是否能得出三个方向的加速度为0？

由于存在重力，z轴方向有一个重力加速度。

如果是倾斜的情况，受力分析略，计算的俯仰角如图。

如图是测量角速度的原理，如果外面的箱体旋转起来，小球也会跟着偏移。

第二部分寄存器读写

先把任务分成如图的几个部分

首先，先看一下通信的硬件图

如图，找到I2C通信最重要的四根线，VCC，GND，SCL，SDA，接着找出后两根线连接单片机的引脚。

分别是PB8和PB9。

打开数据手册，找到两个引脚的功能，默认是TIM4的通道3和通道4，使用重映射功能才可以复用成SCL和SDA。

先进行重映射，再配置PB8和PB9为输出开漏模式。接着设置I2C的初始化参数。

对于通信速率，打开MPU6050数据手册，找到支持的快速模式400Khz。

完整初始化代码如下：

void App_MPU6050_Init(void)
{
	//#1.初始化I2C总线 PB8 PB9 - I2C1
	//将I2C的引脚重映射到PB8和PB9
	RCC_APB2PeriphClockCmd (RCC_APB2Periph_AFIO,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_I2C1,ENABLE);
	
	//#2.初始化PB8和PB9 - AF_OD
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStruct={0};
	
	GPIO_InitStruct.GPIO_Pin=GPIO_Pin_8|GPIO_Pin_9;
	GPIO_InitStruct.GPIO_Mode=GPIO_Mode_AF_OD;
	GPIO_InitStruct.GPIO_Speed=GPIO_Speed_2MHz;
	
	GPIO_Init(GPIOB,&GPIO_InitStruct);
	
	//#2.初始化I2C1
	//开启I2C1的时钟
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1,ENABLE);
	
	//设置I2C的参数
	I2C_InitTypeDef I2C_InitStruct={0};
	
	I2C_InitStruct.I2C_ClockSpeed=400000;//400khz，I2C通信速度，数据手册查找得知支持快速模式
	I2C_InitStruct.I2C_DutyCycle=I2C_DutyCycle_2;//占空比没有特殊要求默认2比1
	I2C_InitStruct.I2C_Mode=I2C_Mode_I2C;//选用标准I2C接口模式
	
	I2C_Init(I2C1,&I2C_InitStruct);

初始化完成后，接着对寄存器进行读写操作，设置量程还有读取寄存器的值。

如图，是写寄存器和读寄存器的示意图。

为了使用方便，封装两个函数，用于读写寄存器

根据以上思路，编写函数，其中I2C.h是模板工程里的封装好的驱动文件，暂时不研究。

#include"i2c.h"

//
//@简介：向寄存器写值
//@参数 reg - 要写入寄存器的地址 value - 要写入的值
//
static void reg_Write(uint8_t reg, uint8_t value)
{
	uint8_t bytesToSend[]={reg,value};
	
	My_I2C_SendBytes(I2C1,0xd0,bytesToSend,2);
}
//
//@简介：向寄存器读值
//@参数 reg - 要读取寄存器的地址 返回值：表示读取到的值
//
static uint8_t reg_Read(uint8_t reg)
{
	uint8_t regValue;
	
	My_I2C_SendBytes(I2C1,0xd0,&reg,1);//发送寄存器的地址
	
	My_I2C_ReceiveBytes(I2C1,0xd0,&regValue,1);//读取一个字节
	
	return regValue;
}

其中，关于为什么发送0xd0的地址，需要到MPU6050规格书里查找

从机地址如上，最后以为X受硬件引脚影响，

因为是接地，取0.

具体计算结果如上。

编写好读写函数后，设置参数部分的代码，up主提前给出了，用到刚才的读写函数

为了搞明白具体含义，打开寄存器手册，查找相关内容。

上表是电源管理相关的寄存器，地址是0x6B，和刚才代码对应

对于Bit7的解释是，设置成1的时候，设备所有的寄存器都会恢复到默认值。

所以把Bit7设置成1，其他都是0，那么寄存器的具体内容就是0x80。

修改陀螺仪量程的寄存器如下

Bit3和Bit4用于量程的选择，一共有4档量程可以选择。

对于平衡车来说，一秒钟旋转360°每秒已经很大了，取两倍720°，所以我们选择1000或者2000比较合适。这里选择2000；所以4,3位填11，代表二进制的3。其他位不动，0001 1000，所以填18。

修改加速度计量程的寄存器如下

设置的方式类似陀螺仪寄存器

具体分析一下，加速度的g分解到每个方向上的分量不会超过g，取2g作为上限，所以填0。

那么这个寄存器的值全是0，填00。

具体代码同上面的代码截图，这里就不写。

初始化完成后，接着就可以读取寄存器的值了。

以ax为例，先读取高八位数据，再读取第八位数据，最后合并成带符号的16位数据。

以下是读取到的所有数据的代码，包括三个方向的加速度还有角速度

	int16_t ax_raw=(int16_t)((reg_Read(0x3b)<<8)+reg_Read(0x3c));//ax的原始数据
	int16_t ay_raw=(int16_t)((reg_Read(0x3d)<<8)+reg_Read(0x3e));//ay的原始数据
	int16_t az_raw=(int16_t)((reg_Read(0x3f)<<8)+reg_Read(0x40));//az的原始数据
	
	int16_t temperature_raw=(int16_t)((reg_Read(0x41)<<8)+reg_Read(0x42));//温度计的原始数据
	
	int16_t gx_raw=(int16_t)((reg_Read(0x43)<<8)+reg_Read(0x44));//gx的原始数据
	int16_t gy_raw=(int16_t)((reg_Read(0x45)<<8)+reg_Read(0x46));//gy的原始数据
	int16_t gz_raw=(int16_t)((reg_Read(0x47)<<8)+reg_Read(0x48));//gz的原始数据

得到原始数据后，因为是带符号的16位整数，范围从-32768——+32768，要进行单位转换。比如对加速度进行转换，实际量程是+-2g，那么计算出的转换因子就是2/32768=6.1035e-5，需要把原始数据都乘上这个数。角速度同理，转换因子是2000/32768=6.1035e-2。

温度值的转换，用到数据手册的公式

up主提到了实际上使用的芯片型号并不是6050，而是6500，其他地方区别不大，但是在温度计计算上有区别。

	ax=ax_raw*6.1035e-5f;
	ay=ay_raw*6.1035e-5f;
	az=az_raw*6.1035e-5f;

    temperature=temperature_raw/333.87f+21.0f;//MPU6500的计算公式，区别于MPU6050

	gx=gx_raw*6.1035e-2f;
	gy=gy_raw*6.1035e-2f;
	gz=gz_raw*6.1035e-2f;

上面的代码就是换算公式，写在App_MPU6050_Update下。

再编写一些外部获取数据的接口

//
//@简介：获取x轴向加速度,单位g
//
float App_MPU6050_GetAx(void)
{
	return ax;
}
//
//@简介：获取y轴向加速度,单位g
//
float App_MPU6050_GetAy(void)
{
	return ay;
}
//
//@简介：获取z轴向加速度,单位g
//
float App_MPU6050_GetAz(void)
{
	return az;
}
//
//@简介：获取温度计的值，单位摄氏度
//
float App_MPU6050_Temperature(void)
{
	return temperature;
}

//
//@简介：获取绕x轴的角速度,单位°/s
//
float App_MPU6050_GetGx(void)
{
	return gx;
}
//
//@简介：获取绕y轴的角速度,单位°/s
//
float App_MPU6050_GetGy(void)
{
	return gy;
}
//
//@简介：获取绕z轴的角速度,单位°/s
//
float App_MPU6050_GetGz(void)
{
	return gz;
}

接下去编写测试代码，获取数据。

#include"MPU6050_test.h"
#include "app_usart2.h"
#include "app_mpu6050.h"
#include "delay.h"

void MPU6050_Test(void)
{
	 App_USART2_Init();
	 App_MPU6050_Init();
	
	while(1)
	{
		App_MPU6050_Update();
		float ax=App_MPU6050_GetAx();
		float ay=App_MPU6050_GetAy();
		float az=App_MPU6050_GetAz();
		
		float temperature=App_MPU6050_GetTemperature();
		
		float gx=App_MPU6050_GetGx();
		float gy=App_MPU6050_GetGy();
		float gz=App_MPU6050_GetGz();
		
		My_USART_Printf(USART2,"%f,%f,%f,%f,%f,%f,%f\n",ax,ay,az,temperature,gx,gy,gz);
		
		Delay(10);
	}
}

调用相关函数，获取数据后，用串口打印出数据。

数据经过验证，实验成功。az约等于1，室内温度也差不多。

第三部分解算欧拉角（互补滤波）陀螺仪部分

欧拉角由三个角组成：偏航角，俯仰角，翻滚角。如图所示

陀螺仪会受漂移影响，加速度计会受噪声影响，将它们通过互补滤波融合的结果更加准确。

举例，如下计算偏航角的方式

核心思想就是利用积分，求角速度关于时间的积分

同理，俯仰角和翻滚角也可以用类似的递推公式表示

其中翻滚角，因为芯片摆放的缘故，变化的方向和翻滚角规定的方向刚好相反，所以是减号。

Δt取最大频率的5ms。

对于裸机多任务模型，我们需要创建多个进程函数，上述的计算过程也需要写成进程函数的形式，通过对系统时间的比较来达到延时的目的，而不是使用阻塞型的delay函数。

创建一个陀螺仪解算欧拉角的进程函数，根据上面的公式写代码

void App_MPU6050_Proc(void)
{
	//通过陀螺仪的测量结果计算欧拉角
	
	App_MPU6050_Update();//更新传感器的值
	
	PERIODIC(5)//进程函数每5s执行一次
	
	
//	static uint32_t nxt=0;//下次程序运行的时间
//	
//	if(GetTick()<nxt)return;
	
	yaw=yaw+gz*0.005;
	pitch=pitch+gx*0.005;
	roll=roll-gy*0.005;
	
}

其中原本的延时代码被注释掉了，为了方便，up提供了一个通用的宏，只要填写延迟的时间，单位ms，就可以正常使用非阻塞性延迟。

编写测试代码检验实验是否正确，按照初始化和while(1)循环里，写进程函数的形式。

static void Usart2_Proc(void)
{
	PERIODIC(10)
	
	float yaw=App_MPU6050_Getyaw();
	float pitch=App_MPU6050_Getpitch();
	float roll=App_MPU6050_Getroll();
		
	My_USART_Printf(USART2,"%f,%f,%f\n",yaw,pitch,roll);
}

//
//@简介：测试欧拉角，用Vofa软件展示数据
//
void MPU6050_EularAngleTest(void)
{
	App_MPU6050_Init();
	App_USART2_Init();
	
	while(1)
	{
		App_MPU6050_Proc();
		Usart2_Proc();
	}
}

打开vofa观察实验结果，导入平衡车模型，绑定三个欧拉角和坐标轴，发现结果基本上符合实际小车运动趋势，但是因为存在误差，三个数据都在不断自增，这是后面需要解决的问题。

第四部分加速度计部分

对重力进行分解就可以求出分力，进而求出加速度。注意，这里一般不计算偏航角，因为重力始终垂直于偏航角的平面，无法分解。

首先计算俯仰角，原理图如下

其中，z轴上的重力加速度，我们会认为是负方向理应是-g，但是实际情况是，MPU6050测出来的是+g，因为弹簧被拉伸的方向才是MPU6050加速度计的加速度方向，用vofa观察结果如下

z方向的g确实是+g左右。

首先计算俯仰角，示意图如下

但是这里的arctan涉及到了取值范围的问题，arctanx这个函数在c语言里只能表示-90°-90°的角度，如果是-135°，tan（-135°）的值也是1，但是在函数里只能表示成45°，所以需要用atan2这个函数，能把范围扩大到-180°-180°。

虽然我认为这个小车项目里，俯仰角的角度就是-90°-90°，因为小车不可能倒过来，但是用于飞行器之类的项目时，是可能出现超过这个范围的情况的，所以现在学习这个知识点仍然是有必要的。

两个函数具体的区别，在于atanx会丢失边长的符号信息，还是以-135°举例，对边和邻边都是负二分之根号二，两者相除就变成1，可能会错误识别成45°。atan2需要输入原始两个边长，就可以准确判断象限了。

另外还需要考虑到单位换算问题，弧度和角度的互换，这个知识点是高中数学的内容，就不赘述了。

接着算翻滚角，示意图如下。另外，正常的平地行驶也是用不到这个角的，也是斜坡或者无人机会用上。

	//通过加速度计解算欧拉角
	
	yaw=0;//强制赋值0，解算不出
	pitch=atan2(ay,az)/3.1415927f*180;
	roll=atan2(ax,az)/3.1415927f*180;

编写代码，非常简单，替换掉上次写的陀螺仪代码进行测试。

结果如下，实际发现模型会产生抖动，因为受到外界噪声的影响，比如一些外部的抖动，因为不是理想环境。

第五部分互补滤波

基于上述两种计算模式的优缺点，将两者的结果进行互补滤波，效果图如图。

	//通过陀螺仪解算欧拉角
	float yaw_g=yaw+gz*0.005;
	float pitch_g=pitch+gx*0.005;
	float roll_g=roll-gy*0.005;
	
	//通过加速度计解算欧拉角
	
	float pitch_a=atan2(ay,az)/3.1415927f*180;
	float roll_a=atan2(ax,az)/3.1415927f*180;
	
	//使用互补滤波器对陀螺仪和加速度计的计算结果进行融合
	yaw=yaw_g;
	pitch=pitch_g*0.95238+(1-0.95238)*pitch_a;
	roll=roll_g*0.95238+(1-0.95238)*roll_a;

代码如上。下载到单片机看结果，现在的小车姿态非常平稳了，和实际运动姿态基本一致。