[蓝点无限] UWB 定位数据融合 之 MPU9250 驱动

UWB定位，不论是TWR 还是TDOA，实质都是基于电磁信号空中飞行时间(TOF),然后换算成距离，通过软件算法获取被定位坐标。

电磁信号，收到干扰和遮挡等会出现定位误差再说难免。 

而结合运动传感器进行数据融合则是对UWB定位误差的一个动态补偿。 目前运动传感器封装小，易于集成，已经很多实际项目采用UWB+运动传感器方式进行定位。

但是这些资料很少，往往只能见于某篇论文或者某个商品的介绍。

这里，我们做抛砖引玉，做一些基础功能，供各位开发者参考。

我们这里使用的是MPU9250 9轴传感器。传感器介绍各位可以参考官方介绍:

https://invensense.tdk.com/products/motion-tracking/9-axis/mpu-9250/

MPU9250 支持I2C 和 SPI接口，我们板子使用的I2C接口，我们这里使用STM32F4 软件方式I2C 驱动，并获取9轴数据。

 

UWB数据融合，我们打算一共做分三部分

1 MPU9250 驱动，主要是使用STM32 成功读取MPU9250 内部数据， 数据是融合的前提。

2 UWB固件，将MPU9250 集成到 UWB 程序内，我们打算把这部分功能集成到之前开源的多标签多基站固件，做到彻底开源。

3 上位机解析，修改我们之前开源上位机，上位机可以解析获取MPU9250 原始数据。

 

剩下的，上位机中融合自行实现，大家可以自行对某一次定位更新，自行决定是使用UWB数据还是传感器数据。

例如，MPU9250 数据显示当前各个加速度 角速度和上次比没有太大变化，我们可以推理模块可能静止，但是UWB 这次算出模块位置和上次差异大于1米。

这个时候更偏向使用MPU9250，而UWB可能出现误差。忽略本次UWB位置更新，保存使用上次的位置坐标。 

类似如上，完成融合算法或者更为复杂的算法。

测试硬件:

内部板子情况:UWB+PA功放，MPU9250, 600ma内置锂电池，振动小电机。 后期补一张内部靓照。

板子MPU9250 部分原理图：

MPU9250 I2C 固件说明

i2c_sw.c  配置软件I2C

mpu9250.c MPU9250初始化以及寄存器操作

代码配置I2C 对应接口

#define   GPIO_SW_I2C1_SCL			GPIOA
#define   GPIO_SW_I2C1_SDA			GPIOB
#define   GPIO_SW_I2C1_SCL_PIN		GPIO_PIN_8
#define   GPIO_SW_I2C1_SDA_PIN		GPIO_PIN_4

测试接口代码：

#include "i2c_sw.h"

void Sw_I2C_Init()	
{
	  SW_I2C_initial();
    i2c_port_initial(SW_I2C1);
}

#include "mpu9250.h"

/* Sensor Handler */
MPU9250_t mpu9250;
HAL_StatusTypeDef whoAmI_Check(MPU9250_t *mpu9250);

int Mpu9250_Test(void)
{
	Sw_I2C_Init();
	MPU9250_Init(&mpu9250, MPU9250_Device_0, ACCEL_SCALE_16G, GYRO_SCALE_2000dps, MAG_SCALE_16bit);
	while (1)
	{
		MPU9250_ReadAcc(&mpu9250);
		printf("acc.x = %0.2f acc.y = %0.2f acc.z= %0.2f
",mpu9250.acc[0],mpu9250.acc[1],mpu9250.acc[2]);
		MPU9250_ReadGyro(&mpu9250);
		HAL_Delay(300);

		printf("gyro.x = %0.2f gyro.y = %0.2f gyro.z= %0.2f
",mpu9250.gyro[0],mpu9250.gyro[1],mpu9250.gyro[2]);
		
		MPU9250_ReadMag(&mpu9250);
		printf("mag.x = %0.2f mag.y = %0.2f mag.z = %0.2f
",mpu9250.mag[0],mpu9250.mag[1],mpu9250.mag[2]);
		
		HAL_Delay(300);
		MPU9250_ReadTemperature(&mpu9250);
		printf("Temp = %0.2f
",mpu9250.temp);
		HAL_Delay(300);

	}
}

测试结果：

驱动代码下载：

完整代码请到蓝点论坛下载