【蓝点开源】UWB 运动传感器融合定位
论坛开源项目，不作为淘宝订单资料，任何疑问不走淘宝旺旺交流。有问题直接发帖【蓝点开源】UWB 运动传感器融合定位项目，基于TWR 算法和MPU9250 运动传感器，实现简单数据融合。
目录： 引言 硬件 固件 固件-读取MPU9250 9轴数据
固件- wake-on-Motion模式
固件 - 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定位和加速度二者简单融合，更为复杂的功能算法大家可以自行研究。
硬件：
基站使用我们现有定位套件的BP30或者BP400，而标签需要使用一个集成运动传感器的板子，我们目前有一批商用模块。这部些模块有完整的外壳，内置600ma锂电池，PCB包括UWB+PA功放，MPU9250加速度传感器以及振动小电机等。标签外形：
MPU9250部分原理图：
固件：
固件部分主要通过STM32实现驱动MPU9250模块，硬件实现部分，我们分为了三部分，第一部分实现读取9轴数据，第二部分实现wake-up-motion中断，第三部分将MPU9250 全部代码集成到现有UWB固件中。
固件-读取MPU9250 9轴数据
i2c_sw.c配置软件I2Cmpu9250.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</font></b></font></font>测试接口代码：
#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,mpu9250.acc,mpu9250.acc);
MPU9250_ReadGyro(&mpu9250);
HAL_Delay(300);
printf("gyro.x = %0.2f gyro.y = %0.2f gyro.z= %0.2f\n",mpu9250.gyro,mpu9250.gyro,mpu9250.gyro);
MPU9250_ReadMag(&mpu9250);
printf("mag.x = %0.2f mag.y = %0.2f mag.z = %0.2f\n",mpu9250.mag,mpu9250.mag,mpu9250.mag);
HAL_Delay(300);
MPU9250_ReadTemperature(&mpu9250);
printf("Temp = %0.2f\n",mpu9250.temp);
HAL_Delay(300);
}
}测试结果：
固件- wake-on-Motion模式
使用MPU9250 通常会读取9轴数据，或者DMP 四元数据，而我们这里使用的是一个运动检测功能， Wake-on-Motion 模式。这个模式也是MPU9250 自带的一个功能。大概功能描述：设定一个加速度变化阈值，当MPU9250 检测到加速度超过这个阈值后，可以发送一个中断信息给主控。官方文档说明:
UWB定位，尤其给人定位，其实人的行走都不是一个匀速过程，存在微小的加速度，适当调整MPU9250 加速度阈值，即可实现人的运动检测功能。再结合UWB本身完成数据融合。测试代码实现：按照MPU9250 流程图，实现Wake-on-Motion ，并拉STM32中断，在中断里点亮LED。1. Wake-on-Motion 配置函数void Enable_MPU9250_MovetionDetection(MPU9250_t *MPU9250)
{
//0x6b, 0b00000001
writeByte(&hi2c1, MPU9250->I2C_Addr, PWR_MGMT_1, 0x01);
//0x6c, 0b00000111
writeByte(&hi2c1, MPU9250->I2C_Addr, PWR_MGMT_2, 0x07);
//0x1d, 0b00000101
writeByte(&hi2c1, MPU9250->I2C_Addr, ACCEL_CONFIG_2, 0x05);
//0x37, 0b0011 0000 -- 只有读取值才clear 中断
//writeByte(&hi2c1, MPU9250->I2C_Addr, INT_PIN_CFG, 0x00);
//0x69, 0b11000000
writeByte(&hi2c1, MPU9250->I2C_Addr, MOT_DETECT_CTRL, 0xC0);
//0x1f, 0x7f
writeByte(&hi2c1, MPU9250->I2C_Addr, WOM_THR, 0x01);
//0x38, 0x40
writeByte(&hi2c1, MPU9250->I2C_Addr, INT_ENABLE, 0x40);
//0x1e, 0b00000100
writeByte(&hi2c1, MPU9250->I2C_Addr, LP_ACCEL_ODR, 0x04);
//0x6b, 0b0010 0000
writeByte(&hi2c1, MPU9250->I2C_Addr, PWR_MGMT_1, 0x20);
// writeByte(&hi2c1, MPU9250->I2C_Addr, INT_STATUS, 0xFF);
}2. STM32 中断配置
我们提供了MPU9250 和 STM32 的链接图，MPU9250 输出中断链接到STM32 PB15，中断配置代码如下
GPIO_InitStruct.Pin = GPIO_PIN_15;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
HAL_NVIC_SetPriority(EXTI15_10_IRQn,
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);3 中断处理函数中断处理函数中，实习点亮LED
void EXTI15_10_IRQHandler(void)
{
/* USER CODE BEGIN EXTI15_10_IRQn 0 */
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, GPIO_PIN_SET); //PB7 = 1 LED_ON
/* USER CODE END EXTI15_10_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
/* USER CODE BEGIN EXTI15_10_IRQn 1 */
/* USER CODE END EXTI15_10_IRQn 1 */
}</font></b></font></font>4 顶层测试函数初始化MPU9250 后，配置Wake-on-Motion，在while循环中，关闭由中断点亮的LED，从而实现当运动时LED亮，1S内没有运动LED熄灭。int Mpu9250_Test(void)
{
Sw_I2C_Init();
MPU9250_Init(&mpu9250, MPU9250_Device_0, ACCEL_SCALE_16G, GYRO_SCALE_2000dps, MAG_SCALE_16bit);
Enable_MPU9250_MovetionDetection(&mpu9250);
HAL_Delay(1000);
while (1)
{
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
HAL_Delay(1000);
}
}5 关于阈值设定说明加速度阈值设定如下语句实现，这里设定值为1. 实际测试，当设定为1，人体微动可以检测。设置2-3，摆手可以检测，如果设置为F，用力甩臂可以检测。writeByte(&hi2c1, MPU9250->I2C_Addr, WOM_THR, 0x01);
固件 - UWB集成MPU9250
现固件代码，代码git 链接参见本文末尾。我们的固件实现基础是之前的《TWR算法-多基站多标签固件》， 再此基础上将PU9250 Wake-on-Motion 模式 MPU9250 相关内容进行整合。具体改动1. 将MPU9250 相关驱动和初始化移植2 定义全局变量isMpu9250_moved 用来保存是否在该定位周期内模块发生运动if(Count_Anthor() < 4)
{
gProcess_Dis = 0;
BPhero_TAG_Broadcast();
gSend_index = 0;
// HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_8);
isMpu9250_moved = 0;
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
}
else
{
if(gSend_index ==Count_Anthor())
{
gSend_index= 0;
Send_Dis_To_Anthor0();
isMpu9250_moved = 0;
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
} else
{
gProcess_Dis = 1;
BPhero_Distance_Measure_Specail_ANTHOR();// 从1 2 3 4发送
// HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);
}2 标签将isMpu9250_moved 打包并以广播的形式发送给各个模块static void Send_Dis_To_Anthor0(void)
{
static int framenum = 0 ;
char crc_byte = 0;
//only send this message to anthor0:short address equal 0x0001
msg_f_send.destAddr = 0xFF;
msg_f_send.destAddr = 0xFF;
msg_f_send.seqNum = distance_seqnum;
msg_f_send.messageData='M';
msg_f_send.messageData = 0;//数据包长度
uint8 *pAnthor_Str = &msg_f_send.messageData;
int str_len = 0x20;
sprintf(pAnthor_Str, "&&&:%02X$%04X:%d:%02X,str_len,SHORT_ADDR,isMpu9250_moved,msg_f_send.seqNum);//AA55 ANTHORID
pAnthor_Str = pAnthor_Str + 15+2;
isMpu9250_moved = 0;
for(uint8 index = 0 ; index < MAX_ANTHOR; index++)
{
if(anthor_info.alive == 1)
{
sprintf(pAnthor_Str, "%04X:%04X:%02X#",anthor_info.short_address,anthor_info.distance,anthor_info.rssi_info);
pAnthor_Str = pAnthor_Str + 13;
}
}
pAnthor_Str = pAnthor_Str - 1;
sprintf(pAnthor_Str, "$AA##\r\n");
while(msg_f_send.messageData != '\n')
{
crc_byte =crc_byte^msg_f_send.messageData;
str_len++;
}
str_len++;//字符串最后追加'\n'
printf(&msg_f_send.messageData);
msg_f_send.messageData = str_len - 2;//有用数据，其他模块需要传输到串口的数据
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
dwt_writetxdata(11 + str_len,(uint8 *)&msg_f_send, 0) ;// write the frame data
dwt_writetxfctrl(11 + str_len, 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
framenum++;
LCD_Display_Distance();
}3 基站收到‘M’信息，通过串口送出case 'M':
//将收到的距离信息通过串口发送给电脑上位机
USART_puts(&msg_f->messageData,msg_f->messageData);
break;标签发送‘M’信息，第一个字节是‘M’，第二个字节保存了数据长度，在处理‘M’信息，直接使用msg_f->messageData 定义送到串口的数据长度。上位机测试效果：
源码链接:
固件源码已经放到git上，V1.0 版本开发完成，请详细看下面的描述
https://tuzhuke@bitbucket.org/tuzhuke/bp30_multianthor.gitHash:3ec3d27cabc914365549fb0d907e034f3caa21ec
上位机：
这个部分实现上位机代码，上位机使用我们之前开源Python版本TWR上位机，代码可以在末尾论坛链接下载
直接上代码，代码主要是在《[开源项目] 蓝点无限 UWB Python版本上位机》基础上修改，这里列出代码更改部分。1 解析数据包中的运动变量'acc'，并存放到字典中result_dict = {'tag': 0x1005, 'acc':0, 'seq': 7, 'time': 1234, 'anthor_count': 4,'anthor': []}
# 数据包以&&& 开头
res = re.findall(r'&&&', string)
flag = 1
if len(res) > 0:
# step1 print message length,ex 76
temp_string = string.split(":")# &&&:80$
data_len = int(temp_string.split(":"), 16)
# tag info
temp_string = string.split()# 000A:20
tag_id = int(temp_string.split(":"), 16)# 000A
tag_acc = int(temp_string.split(":"), 16)
tag_seq = int(temp_string.split(":"), 16)# 20
# print("标签ID: %02XSeq: %X" % (tag_id, tag_seq))
result_dict['tag'] = tag_id
result_dict['acc'] = tag_acc
result_dict['seq'] = tag_seq2 在返回结果中，将运动信息一并返回给上层
def twr_main(input_string):
print(input_string)
error_flag, result_dic = Process_String_Before_Udp(input_string)
if error_flag == 0:
= Compute_Location(result_dic)
return location_result, location_seq, location_addr, location_x, location_y, result_dic['acc']
return 0, 0, 0, 0, 0, 03 顶层收到定位结果和运动信息，打印结果，并发送给处理函数 = twr_main(msg)
print(tag_acc)
if location_result == 1:
self.data_result.emit(
'%d %d %0.2f %0.2f %d' % (location_seq, location_addr, location_x, location_y, tag_acc))4 UWB和运动信息进行简单融合，当模块静止，不更新坐标信息def insert_result(self, input_str):
strlist = input_str.split(' ')
location_addr = int(strlist)
location_x = float(strlist)
location_y = float(strlist)
tag_acc = int(strlist)
print("acc = %d"%tag_acc)
print("insert result")
if tag_acc == 1:#只有模块移动的时候更新坐标
self.Insert_Tag_Result(location_addr,
{"x": location_x, "y": location_y, "z": 0, "qt": QGraphicsEllipseItem(-10, -10, 10, 10)})
其他代码，为了调试方便，在上位机增加了串口接收功能class ComThread(QtCore.QThread):
data_result = QtCore.pyqtSignal(object)
data_draf = QtCore.pyqtSignal(object)
def __init__(self):
super(ComThread, self).__init__()
self.l_serial = None
self.alive = False
self.waitEnd = None
self.ID = None
self.data = None
self.port = None
def set_port(self,port):
self.port = port
print(self.port)
def waiting(self):
if not self.waitEnd is None:
self.waitEnd.wait()
def SetStopEvent(self):
if not self.waitEnd is None:
self.waitEnd.set()
self.alive = False
self.stop()
def start(self):
self.l_serial = serial.Serial()
self.l_serial.port = self.port
self.l_serial.baudrate = 115200
self.l_serial.timeout = 2
self.l_serial.open()
if self.l_serial.isOpen():
self.waitEnd = threading.Event()
self.alive = True
self.thread_read = None
self.thread_read = threading.Thread(target=self.FirstReader)
self.thread_read.setDaemon(1)
self.thread_read.start()
return True
else:
return False
def SendDate(self, i_msg, send):
lmsg = ''
isOK = False
if isinstance(i_msg):
lmsg = i_msg.encode('gb18030')
else:
lmsg = i_msg
try:
# 发送数据到相应的处理组件
self.l_serial.write(send)
except Exception as ex:
pass;
return isOK
def FirstReader(self):
while self.alive:
data = ''
data = data.encode('utf-8')
n = self.l_serial.inWaiting()
if n:
data = self.l_serial.readline()
print(data)
msg =str(data, encoding="utf-8")
self.data_draf.emit(msg)# for debug only
= twr_main(msg)
print(tag_acc)
if location_result == 1:
self.data_result.emit(
'%d %d %0.2f %0.2f %d' % (location_seq, location_addr, location_x, location_y, tag_acc))
# # bphero_dispose(str(data))
self.waitEnd.set()
self.alive = False
def stop(self):
self.alive = False
self.thread_read.join()
if self.l_serial.isOpen():
self.l_serial.close()数据融合版本上位机源码下载：
UWB运动传感器融合相关视频：
视频1：UWB 和 运动传感器 数据融合定位 硬件实现示例
https://www.bilibili.com/video/BV1Eq4y1U7Vs/
视频2：UWB 和 运动传感器 数据融合定位软件实现以及室内测试
https://www.bilibili.com/video/BV1PQ4y1y7An/
感谢分享！！！！！ 赞一个！！！！ 感谢大佬分享 感谢楼主分享！ 带壳标签的内部安装图可以发一下吗？壳太小了有点塞不进去，而且上层壳有6个安装孔，下层只有两个 xiao郭 发表于 2021-11-8 16:28
带壳标签的内部安装图可以发一下吗？壳太小了有点塞不进去，而且上层壳有6个安装孔，下层只有两个
由于需要拆开锂电池发货，所以模块都需要拆开。同时我们帮你焊接了下载接口4根线。
如果测试没有问题，需要安装外壳，首先要把四根下载线焊接下去。然后按照如下三个步骤组装
Step1 连接电源线
Step2 将底板 和上盖通过螺丝固定（提前嵌入开关拨码）
Step3 将结合的上半部分，和底部通过两个螺丝连接
蓝点无限 发表于 2021-11-9 08:03
由于需要拆开锂电池发货，所以模块都需要拆开。同时我们帮你焊接了下载接口4根线。
如果测试没有问题 ...
嗯嗯，先下载代码测试，如果没问题，去掉四根焊接线，然后才能顺利组装。这样理解可以吗？ xiao郭 发表于 2021-11-9 08:56
嗯嗯，先下载代码测试，如果没问题，去掉四根焊接线，然后才能顺利组装。这样理解可以吗？
对的，模块它只有触点，目前发货的是给大家焊接好下载线的。
后期有配套转接板，有需要可以联系客服