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\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);
}
}
測試結果：
驅動代碼下載：
完整代碼請到藍點論壇下載