UWB定位，不論是TWR 還是TDOA，實質(zhì)都是基于電磁信號空中飛行時間(TOF),然后換算成距離，通過軟件算法獲取被定位坐標。

電磁信號，收到干擾和遮擋等會出現(xiàn)定位誤差再說難免。 

而結(jié)合運動傳感器進行數(shù)據(jù)融合則是對UWB定位誤差的一個動態(tài)補償。 目前運動傳感器封裝小，易于集成，已經(jīng)很多實際項目采用UWB+運動傳感器方式進行定位。

但是這些資料很少，往往只能見于某篇論文或者某個商品的介紹。

這里，我們做拋磚引玉，做一些基礎(chǔ)功能，供各位開發(fā)者參考。

我們這里使用的是MPU9250 9軸傳感器。傳感器介紹各位可以參考官方介紹:

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

MPU9250 支持I2C 和 SPI接口，我們板子使用的I2C接口，我們這里使用STM32F4 軟件方式I2C 驅(qū)動，并獲取9軸數(shù)據(jù)。

UWB數(shù)據(jù)融合，我們打算一共做分三部分

1 MPU9250 驅(qū)動，主要是使用STM32 成功讀取MPU9250 內(nèi)部數(shù)據(jù)， 數(shù)據(jù)是融合的前提。

2 UWB固件，將MPU9250 集成到 UWB 程序內(nèi)，我們打算把這部分功能集成到之前開源的多標簽多基站固件，做到徹底開源。

3 上位機解析，修改我們之前開源上位機，上位機可以解析獲取MPU9250 原始數(shù)據(jù)。

剩下的，上位機中融合自行實現(xiàn)，大家可以自行對某一次定位更新，自行決定是使用UWB數(shù)據(jù)還是傳感器數(shù)據(jù)。

例如，MPU9250 數(shù)據(jù)顯示當前各個加速度 角速度和上次比沒有太大變化，我們可以推理模塊可能靜止，但是UWB 這次算出模塊位置和上次差異大于1米。

這個時候更偏向使用MPU9250，而UWB可能出現(xiàn)誤差。忽略本次UWB位置更新，保存使用上次的位置坐標。 

類似如上，完成融合算法或者更為復(fù)雜的算法。

測試硬件:

內(nèi)部板子情況:UWB+PA功放，MPU9250, 600ma內(nèi)置鋰電池，振動小電機。 后期補一張內(nèi)部靚照。

板子MPU9250 部分原理圖：

MPU9250 I2C 固件說明

i2c_sw.c  配置軟件I2C

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

代碼配置I2C 對應(yīng)接口

#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);

	}
}

測試結(jié)果：

驅(qū)動代碼下載：

完整代碼請到藍點論壇下載