[蓝点无限] 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内置锂电池,振动小电机。 后期补一张内部靓照。

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

板子MPU9250 部分原理图:

[蓝点无限] UWB 定位数据融合 之 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\n",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\n",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\n",mpu9250.mag[0],mpu9250.mag[1],mpu9250.mag[2]);
		
		HAL_Delay(300);
		MPU9250_ReadTemperature(&mpu9250);
		printf("Temp = %0.2f\n",mpu9250.temp);
		HAL_Delay(300);

	}
}

 

测试结果:

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

驱动代码下载:

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

 

 
上一篇:Oracle复制表结构且不复制数据,进行列的移位(20210507)


下一篇:由两个重要极限推导常见等价无穷小以及常见导数公式