【蓝点开源】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  配置软件I2C

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

代码配置I2C 对应接口

1. #define   GPIO_SW_I2C1_SCL                        GPIOA
   
2. #define   GPIO_SW_I2C1_SDA                        GPIOB
   
3. #define   GPIO_SW_I2C1_SCL_PIN                GPIO_PIN_8
   
4. #define   GPIO_SW_I2C1_SDA_PIN                GPIO_PIN_4</font></b></font></font>

复制代码

测试接口代码：

1. #include "i2c_sw.h"
   
2. void Sw_I2C_Init()        
   
3. {
   
4.           SW_I2C_initial();
   
5.     i2c_port_initial(SW_I2C1);
   
6. }
   
7. 
   
8. #include "mpu9250.h"
   
9. 
   
10. /* Sensor Handler */
    
11. MPU9250_t mpu9250;
    
12. HAL_StatusTypeDef whoAmI_Check(MPU9250_t *mpu9250);
    
13. 
    
14. int Mpu9250_Test(void)
    
15. {
    
16.         Sw_I2C_Init();
    
17.         MPU9250_Init(&mpu9250, MPU9250_Device_0, ACCEL_SCALE_16G, GYRO_SCALE_2000dps, MAG_SCALE_16bit);
    
18.         while (1)
    
19.         {
    
20.                 MPU9250_ReadAcc(&mpu9250);
    
21.                 printf("acc.x = %0.2f acc.y = %0.2f acc.z= %0.2f\n",mpu9250.acc[0],mpu9250.acc[1],mpu9250.acc[2]);
    
22.                 MPU9250_ReadGyro(&mpu9250);
    
23.                 HAL_Delay(300);
    
24. 
    
25.                 printf("gyro.x = %0.2f gyro.y = %0.2f gyro.z= %0.2f\n",mpu9250.gyro[0],mpu9250.gyro[1],mpu9250.gyro[2]);
    
26.                
    
27.                 MPU9250_ReadMag(&mpu9250);
    
28.                 printf("mag.x = %0.2f mag.y = %0.2f mag.z = %0.2f\n",mpu9250.mag[0],mpu9250.mag[1],mpu9250.mag[2]);
    
29.                
    
30.                 HAL_Delay(300);
    
31.                 MPU9250_ReadTemperature(&mpu9250);
    
32.                 printf("Temp = %0.2f\n",mpu9250.temp);
    
33.                 HAL_Delay(300);
    
34. 
    
35.         }
    
36. }

复制代码

测试结果：


固件- wake-on-Motion模式

使用MPU9250 通常会读取9轴数据，或者DMP 四元数据，而我们这里使用的是一个运动检测功能， Wake-on-Motion 模式。

这个模式也是MPU9250 自带的一个功能。

大概功能描述：

设定一个加速度变化阈值，当MPU9250 检测到加速度超过这个阈值后，可以发送一个中断信息给主控。

官方文档说明:

UWB定位，尤其给人定位，其实人的行走都不是一个匀速过程，存在微小的加速度，适当调整MPU9250 加速度阈值，即可实现人的运动检测功能。再结合UWB本身完成数据融合。

测试代码实现：

按照MPU9250 流程图，实现Wake-on-Motion ，并拉STM32中断，在中断里点亮LED。

1. Wake-on-Motion 配置函数

1. void Enable_MPU9250_MovetionDetection(MPU9250_t *MPU9250)
   
2. {
   
3.         //0x6b, 0b00000001
   
4.         writeByte(&hi2c1, MPU9250->I2C_Addr, PWR_MGMT_1, 0x01);
   
5.         //0x6c, 0b00000111
   
6.         writeByte(&hi2c1, MPU9250->I2C_Addr, PWR_MGMT_2, 0x07);
   
7.         //0x1d, 0b00000101
   
8.         writeByte(&hi2c1, MPU9250->I2C_Addr, ACCEL_CONFIG_2, 0x05);
   
9.         
   
10.         //0x37, 0b0011 0000 -- 只有读取值才clear 中断
    
11.         //writeByte(&hi2c1, MPU9250->I2C_Addr, INT_PIN_CFG, 0x00);
    
12.                
    
13.         //0x69, 0b11000000
    
14.         writeByte(&hi2c1, MPU9250->I2C_Addr, MOT_DETECT_CTRL, 0xC0);
    
15.         //0x1f, 0x7f
    
16.         writeByte(&hi2c1, MPU9250->I2C_Addr, WOM_THR, 0x01);
    
17.           //0x38, 0x40
    
18.         writeByte(&hi2c1, MPU9250->I2C_Addr, INT_ENABLE, 0x40);
    
19.         //0x1e, 0b00000100
    
20.         writeByte(&hi2c1, MPU9250->I2C_Addr, LP_ACCEL_ODR, 0x04);
    
21.         //0x6b, 0b0010 0000
    
22.         writeByte(&hi2c1, MPU9250->I2C_Addr, PWR_MGMT_1, 0x20);
    
23. 
    
24. //        writeByte(&hi2c1, MPU9250->I2C_Addr, INT_STATUS, 0xFF);
    
25. }

复制代码

2. STM32 中断配置
我们提供了MPU9250 和 STM32 的链接图，MPU9250 输出中断链接到STM32 PB15，中断配置代码如下

1.   GPIO_InitStruct.Pin = GPIO_PIN_15;
   
2.   GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
   
3.   GPIO_InitStruct.Pull = GPIO_NOPULL;
   
4.   HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
   
5.   HAL_NVIC_SetPriority(EXTI15_10_IRQn,
   
6.   HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);

复制代码

3 中断处理函数

中断处理函数中，实习点亮LED

1. void EXTI15_10_IRQHandler(void)
   
2. {
   
3.   /* USER CODE BEGIN EXTI15_10_IRQn 0 */
   
4.         HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, GPIO_PIN_SET); //PB7 = 1 LED_ON
   
5. 
   
6.   /* USER CODE END EXTI15_10_IRQn 0 */
   
7.   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
   
8.   /* USER CODE BEGIN EXTI15_10_IRQn 1 */
   
9. 
   
10.   /* USER CODE END EXTI15_10_IRQn 1 */
    
11. }</font></b></font></font>

复制代码

4 顶层测试函数

初始化MPU9250 后，配置Wake-on-Motion，在while循环中，关闭由中断点亮的LED，从而实现当运动时LED亮，1S内没有运动LED熄灭。

1. int Mpu9250_Test(void)
   
2. {
   
3.     Sw_I2C_Init();
   
4.     MPU9250_Init(&mpu9250, MPU9250_Device_0, ACCEL_SCALE_16G, GYRO_SCALE_2000dps, MAG_SCALE_16bit);
   
5.     Enable_MPU9250_MovetionDetection(&mpu9250);
   
6.     HAL_Delay(1000);
   
7. 
   
8.     while (1)
   
9.     {
   
10.         HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
    
11.         HAL_Delay(1000);
    
12. 
    
13.     }
    
14. }

复制代码

5 关于阈值设定说明

加速度阈值设定如下语句实现，这里设定值为1. 实际测试，当设定为1，人体微动可以检测。设置2-3，摆手可以检测，如果设置为F，用力甩臂可以检测。

1. writeByte(&hi2c1, MPU9250->I2C_Addr, WOM_THR, 0x01);
   

复制代码

固件 - UWB集成MPU9250

现固件代码，代码git 链接参见本文末尾。

我们的固件实现基础是之前的《TWR算法-多基站多标签固件》，

再此基础上将PU9250 Wake-on-Motion 模式 MPU9250 相关内容进行整合。

具体改动

1. 将MPU9250 相关驱动和初始化移植

2 定义全局变量isMpu9250_moved 用来保存是否在该定位周期内模块发生运动

1. if(Count_Anthor() < 4)
   
2.             {
   
3.                 gProcess_Dis = 0;
   
4.                 BPhero_TAG_Broadcast();
   
5.                 gSend_index = 0;
   
6.                  // HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_8);
   
7.                 isMpu9250_moved = 0;
   
8.                 HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
   
9. 
   
10.             }
    
11.             else
    
12.             {
    
13.                 if(gSend_index ==Count_Anthor())
    
14.                 {
    
15.                     gSend_index= 0;
    
16.                     Send_Dis_To_Anthor0();
    
17.                     isMpu9250_moved = 0;
    
18.                     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
    
19. 
    
20.                 } else
    
21.                 {
    
22.                     gProcess_Dis = 1;
    
23.                     BPhero_Distance_Measure_Specail_ANTHOR();// 从1 2 3 4发送
    
24.                   //        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);
    
25. 
    
26.                 }

复制代码

2 标签将isMpu9250_moved 打包并以广播的形式发送给各个模块

1. static void Send_Dis_To_Anthor0(void)
   
2. {
   
3.     static int framenum = 0 ;
   
4.     char crc_byte = 0;
   
5.     //only send this message to anthor0:short address equal 0x0001
   
6.     msg_f_send.destAddr[0] = 0xFF;
   
7.     msg_f_send.destAddr[1] = 0xFF;
   
8. 
   
9.     msg_f_send.seqNum = distance_seqnum;
   
10. 
    
11.     msg_f_send.messageData[0]='M';
    
12.         msg_f_send.messageData[1] = 0;//数据包长度
    
13.         
    
14.     uint8 *pAnthor_Str = &msg_f_send.messageData[2];
    
15.     int str_len = 0x20;
    
16.         sprintf(pAnthor_Str, "&&&:%02X$%04X:%d:%02X,str_len,SHORT_ADDR,isMpu9250_moved,msg_f_send.seqNum);//AA55 ANTHORID
    
17.    
    
18.         pAnthor_Str = pAnthor_Str + 15+2;
    
19.         isMpu9250_moved = 0;
    
20.     for(uint8 index = 0 ; index < MAX_ANTHOR; index++)
    
21.     {
    
22.         if(anthor_info[index].alive == 1)
    
23.         {
    
24.             sprintf(pAnthor_Str, "%04X:%04X:%02X#",anthor_info[index].short_address,anthor_info[index].distance,anthor_info[index].rssi_info);
    
25.             pAnthor_Str = pAnthor_Str + 13;
    
26.         }
    
27.     }
    
28.     pAnthor_Str = pAnthor_Str - 1;
    
29.     sprintf(pAnthor_Str, "$AA##\r\n");
    
30. 
    
31.     while(msg_f_send.messageData[str_len] != '\n')
    
32.     {
    
33.         crc_byte =crc_byte^msg_f_send.messageData[str_len];
    
34.         str_len++;
    
35.     }
    
36.     str_len++;//字符串最后追加'\n'
    
37.     printf(&msg_f_send.messageData[2]);
    
38.     msg_f_send.messageData[1] = str_len - 2;//有用数据，其他模块需要传输到串口的数据
    
39.                
    
40.     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
    
41.     dwt_writetxdata(11 + str_len,(uint8 *)&msg_f_send, 0) ;  // write the frame data
    
42.     dwt_writetxfctrl(11 + str_len, 0);
    
43.     dwt_starttx(DWT_START_TX_IMMEDIATE);
    
44.     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
    
45.     { };
    
46.                 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
    
47.     framenum++;
    
48.     LCD_Display_Distance();
    
49. }

复制代码

3 基站收到‘M’信息，通过串口送出

1. case 'M':
   
2.                 //将收到的距离信息通过串口发送给电脑上位机
   
3.                 USART_puts(&msg_f->messageData[2],msg_f->messageData[1]);
   
4.                 break;

复制代码

标签发送‘M’信息，第一个字节是‘M’，第二个字节保存了数据长度，在处理‘M’信息，直接使用msg_f->messageData[1] 定义送到串口的数据长度。

上位机测试效果：

源码链接:
固件源码已经放到git上，V1.0 版本开发完成，请详细看下面的描述
https://tuzhuke@bitbucket.org/tuzhuke/bp30_multianthor.git

Hash:3ec3d27cabc914365549fb0d907e034f3caa21ec

上位机：
这个部分实现上位机代码，上位机使用我们之前开源Python版本TWR上位机，代码可以在末尾论坛链接下载

直接上代码，代码主要是在《[开源项目] 蓝点无限 UWB Python版本上位机》基础上修改，这里列出代码更改部分。

1 解析数据包中的运动变量'acc'，并存放到字典中

1. result_dict = {'tag': 0x1005, 'acc':0, 'seq': 7, 'time': 1234, 'anthor_count': 4,'anthor': []}
   
2. # 数据包以&&& 开头
   
3. res = re.findall(r'&&&', string)
   
4. flag = 1
   
5. if len(res) > 0:
   
6.     # step1 print message length,ex 76
   
7.     temp_string = string.split(":")[0]  # &&&:80$
   
8.     data_len = int(temp_string.split(":")[1], 16)
   
9.     # tag info
   
10.     temp_string = string.split()[1]  # 000A:20
    
11.     tag_id = int(temp_string.split(":")[0], 16)  # 000A
    
12.     tag_acc = int(temp_string.split(":")[1], 16)
    
13.     tag_seq = int(temp_string.split(":")[2], 16)  # 20
    
14.     # print("标签ID: %02X  Seq: %X" % (tag_id, tag_seq))
    
15.     result_dict['tag'] = tag_id
    
16.     result_dict['acc'] = tag_acc
    
17.     result_dict['seq'] = tag_seq

复制代码

2 在返回结果中，将运动信息一并返回给上层

1. def twr_main(input_string):
   
2.     print(input_string)
   
3.     error_flag, result_dic = Process_String_Before_Udp(input_string)
   
4.     if error_flag == 0:
   
5.         [location_result, location_seq, location_addr, location_x, location_y] = Compute_Location(result_dic)
   
6.         return location_result, location_seq, location_addr, location_x, location_y, result_dic['acc']
   
7.     return 0, 0, 0, 0, 0, 0

复制代码

3 顶层收到定位结果和运动信息，打印结果，并发送给处理函数

1.        [location_result, location_seq, location_addr, location_x, location_y, tag_acc] = twr_main(msg)
   
2.                 print(tag_acc)
   
3.                 if location_result == 1:
   
4.                     self.data_result.emit(
   
5.                         '%d %d %0.2f %0.2f %d' % (location_seq, location_addr, location_x, location_y, tag_acc))

复制代码

4 UWB和运动信息进行简单融合，当模块静止，不更新坐标信息

1. def insert_result(self, input_str):
   
2.      strlist = input_str.split(' ')
   
3.      location_addr = int(strlist[1])
   
4.      location_x = float(strlist[2])
   
5.      location_y = float(strlist[3])
   
6.      tag_acc = int(strlist[4])
   
7.      print("acc = %d"%tag_acc)
   
8.      print("insert result")
   
9.      if tag_acc == 1:#只有模块移动的时候更新坐标
   
10.          self.Insert_Tag_Result(location_addr,
    
11.                                 {"x": location_x, "y": location_y, "z": 0, "qt": QGraphicsEllipseItem(-10, -10, 10, 10)})
    

复制代码

　其他代码，为了调试方便，在上位机增加了串口接收功能

1. class ComThread(QtCore.QThread):
   
2.     data_result = QtCore.pyqtSignal(object)
   
3.     data_draf = QtCore.pyqtSignal(object)
   
4. 
   
5.     def __init__(self):
   
6.         super(ComThread, self).__init__()
   
7.         self.l_serial = None
   
8.         self.alive = False
   
9.         self.waitEnd = None
   
10.         self.ID = None
    
11.         self.data = None
    
12.         self.port = None
    
13. 
    
14.     def set_port(self,port):
    
15.         self.port = port
    
16.         print(self.port)
    
17. 
    
18.     def waiting(self):
    
19.         if not self.waitEnd is None:
    
20.             self.waitEnd.wait()
    
21. 
    
22.     def SetStopEvent(self):
    
23.         if not self.waitEnd is None:
    
24.             self.waitEnd.set()
    
25.         self.alive = False
    
26.         self.stop()
    
27. 
    
28.     def start(self):
    
29.         self.l_serial = serial.Serial()
    
30.         self.l_serial.port = self.port
    
31.         self.l_serial.baudrate = 115200
    
32.         self.l_serial.timeout = 2
    
33.         self.l_serial.open()
    
34.         if self.l_serial.isOpen():
    
35.             self.waitEnd = threading.Event()
    
36.             self.alive = True
    
37.             self.thread_read = None
    
38.             self.thread_read = threading.Thread(target=self.FirstReader)
    
39.             self.thread_read.setDaemon(1)
    
40.             self.thread_read.start()
    
41.             return True
    
42.         else:
    
43.             return False
    
44. 
    
45.     def SendDate(self, i_msg, send):
    
46.         lmsg = ''
    
47.         isOK = False
    
48.         if isinstance(i_msg):
    
49.             lmsg = i_msg.encode('gb18030')
    
50.         else:
    
51.             lmsg = i_msg
    
52.         try:
    
53.             # 发送数据到相应的处理组件
    
54.             self.l_serial.write(send)
    
55.         except Exception as ex:
    
56.             pass;
    
57.         return isOK
    
58. 
    
59.     def FirstReader(self):
    
60.         while self.alive:
    
61.             data = ''
    
62.             data = data.encode('utf-8')
    
63.             n = self.l_serial.inWaiting()
    
64.             if n:
    
65.                 data = self.l_serial.readline()
    
66.                 print(data)
    
67.                 msg =str(data, encoding="utf-8")
    
68.                 self.data_draf.emit(msg)  # for debug only
    
69. 
    
70.                 [location_result, location_seq, location_addr, location_x, location_y, tag_acc] = twr_main(msg)
    
71.                 print(tag_acc)
    
72.                 if location_result == 1:
    
73.                     self.data_result.emit(
    
74.                         '%d %d %0.2f %0.2f %d' % (location_seq, location_addr, location_x, location_y, tag_acc))
    
75.         # #                 bphero_dispose(str(data))
    
76. 
    
77.         self.waitEnd.set()
    
78.         self.alive = False
    
79. 
    
80.     def stop(self):
    
81.         self.alive = False
    
82.         self.thread_read.join()
    
83.         if self.l_serial.isOpen():
    
84.             self.l_serial.close()

复制代码

数据融合版本上位机源码下载：
Trilateration_PyQt5_MPU9250.rar (93.4 KB, 下载次数: 163)

2021-9-5 12:56 上传

点击文件名下载附件
阅读权限: 10

蓝点无限

UWB运动传感器融合相关视频：

视频1：UWB 和 运动传感器 数据融合定位 硬件实现示例

视频2：UWB 和 运动传感器 数据融合定位软件实现以及室内测试

vacabun

感谢分享！！！！！

Rika777

赞一个！！！！

760371303

感谢大佬分享

sd196821

感谢楼主分享！

xiao郭

带壳标签的内部安装图可以发一下吗？壳太小了有点塞不进去，而且上层壳有6个安装孔，下层只有两个

蓝点无限

xiao郭 发表于 2021-11-8 16:28
带壳标签的内部安装图可以发一下吗？壳太小了有点塞不进去，而且上层壳有6个安装孔，下层只有两个

由于需要拆开锂电池发货，所以模块都需要拆开。同时我们帮你焊接了下载接口4根线。

如果测试没有问题，需要安装外壳，首先要把四根下载线焊接下去。然后按照如下三个步骤组装

Step1 连接电源线


Step2 将底板 和上盖通过螺丝固定（提前嵌入开关拨码）


Step3 将结合的上半部分，和底部通过两个螺丝连接

xiao郭

蓝点无限 发表于 2021-11-9 08:03
由于需要拆开锂电池发货，所以模块都需要拆开。同时我们帮你焊接了下载接口4根线。

如果测试没有问题 ...

嗯嗯，先下载代码测试，如果没问题，去掉四根焊接线，然后才能顺利组装。这样理解可以吗？

蓝点无限

xiao郭 发表于 2021-11-9 08:56
嗯嗯，先下载代码测试，如果没问题，去掉四根焊接线，然后才能顺利组装。这样理解可以吗？

对的，模块它只有触点，目前发货的是给大家焊接好下载线的。
后期有配套转接板，有需要可以联系客服