51uwb.cn

 找回密码
 立即注册
查看: 31115|回复: 67
打印 上一主题 下一主题

【蓝点开源】UWB 运动传感器融合定位

[复制链接]

35

主题

1040

帖子

4469

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
4469
跳转到指定楼层
#
发表于 2021-9-5 12:13:15 | 只看该作者 回帖奖励 |正序浏览 |阅读模式
论坛开源项目,不作为淘宝订单资料,任何疑问不走淘宝旺旺交流。有问题直接发帖

【蓝点开源】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. #include "mpu9250.h"

  8. /* Sensor Handler */
  9. MPU9250_t mpu9250;
  10. HAL_StatusTypeDef whoAmI_Check(MPU9250_t *mpu9250);

  11. int Mpu9250_Test(void)
  12. {
  13.         Sw_I2C_Init();
  14.         MPU9250_Init(&mpu9250, MPU9250_Device_0, ACCEL_SCALE_16G, GYRO_SCALE_2000dps, MAG_SCALE_16bit);
  15.         while (1)
  16.         {
  17.                 MPU9250_ReadAcc(&mpu9250);
  18.                 printf("acc.x = %0.2f acc.y = %0.2f acc.z= %0.2f\n",mpu9250.acc[0],mpu9250.acc[1],mpu9250.acc[2]);
  19.                 MPU9250_ReadGyro(&mpu9250);
  20.                 HAL_Delay(300);

  21.                 printf("gyro.x = %0.2f gyro.y = %0.2f gyro.z= %0.2f\n",mpu9250.gyro[0],mpu9250.gyro[1],mpu9250.gyro[2]);
  22.                
  23.                 MPU9250_ReadMag(&mpu9250);
  24.                 printf("mag.x = %0.2f mag.y = %0.2f mag.z = %0.2f\n",mpu9250.mag[0],mpu9250.mag[1],mpu9250.mag[2]);
  25.                
  26.                 HAL_Delay(300);
  27.                 MPU9250_ReadTemperature(&mpu9250);
  28.                 printf("Temp = %0.2f\n",mpu9250.temp);
  29.                 HAL_Delay(300);

  30.         }
  31. }
复制代码
测试结果:


固件- 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. //        writeByte(&hi2c1, MPU9250->I2C_Addr, INT_STATUS, 0xFF);
  24. }
复制代码
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.   /* USER CODE END EXTI15_10_IRQn 0 */
  6.   HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
  7.   /* USER CODE BEGIN EXTI15_10_IRQn 1 */

  8.   /* USER CODE END EXTI15_10_IRQn 1 */
  9. }</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.     while (1)
  8.     {
  9.         HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
  10.         HAL_Delay(1000);

  11.     }
  12. }
复制代码
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.             else
  11.             {
  12.                 if(gSend_index ==Count_Anthor())
  13.                 {
  14.                     gSend_index= 0;
  15.                     Send_Dis_To_Anthor0();
  16.                     isMpu9250_moved = 0;
  17.                     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON

  18.                 } else
  19.                 {
  20.                     gProcess_Dis = 1;
  21.                     BPhero_Distance_Measure_Specail_ANTHOR();// 从1 2 3 4发送
  22.                   //        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);

  23.                 }
复制代码
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.     msg_f_send.seqNum = distance_seqnum;

  9.     msg_f_send.messageData[0]='M';
  10.         msg_f_send.messageData[1] = 0;//数据包长度
  11.         
  12.     uint8 *pAnthor_Str = &msg_f_send.messageData[2];
  13.     int str_len = 0x20;
  14.         sprintf(pAnthor_Str, "&&&:%02X$%04X:%d:%02X,str_len,SHORT_ADDR,isMpu9250_moved,msg_f_send.seqNum);//AA55 ANTHORID
  15.    
  16.         pAnthor_Str = pAnthor_Str + 15+2;
  17.         isMpu9250_moved = 0;
  18.     for(uint8 index = 0 ; index < MAX_ANTHOR; index++)
  19.     {
  20.         if(anthor_info[index].alive == 1)
  21.         {
  22.             sprintf(pAnthor_Str, "%04X:%04X:%02X#",anthor_info[index].short_address,anthor_info[index].distance,anthor_info[index].rssi_info);
  23.             pAnthor_Str = pAnthor_Str + 13;
  24.         }
  25.     }
  26.     pAnthor_Str = pAnthor_Str - 1;
  27.     sprintf(pAnthor_Str, "$AA##\r\n");

  28.     while(msg_f_send.messageData[str_len] != '\n')
  29.     {
  30.         crc_byte =crc_byte^msg_f_send.messageData[str_len];
  31.         str_len++;
  32.     }
  33.     str_len++;//字符串最后追加'\n'
  34.     printf(&msg_f_send.messageData[2]);
  35.     msg_f_send.messageData[1] = str_len - 2;//有用数据,其他模块需要传输到串口的数据
  36.                
  37.     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
  38.     dwt_writetxdata(11 + str_len,(uint8 *)&msg_f_send, 0) ;  // write the frame data
  39.     dwt_writetxfctrl(11 + str_len, 0);
  40.     dwt_starttx(DWT_START_TX_IMMEDIATE);
  41.     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
  42.     { };
  43.                 dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
  44.     framenum++;
  45.     LCD_Display_Distance();
  46. }
复制代码
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.     def __init__(self):
  5.         super(ComThread, self).__init__()
  6.         self.l_serial = None
  7.         self.alive = False
  8.         self.waitEnd = None
  9.         self.ID = None
  10.         self.data = None
  11.         self.port = None

  12.     def set_port(self,port):
  13.         self.port = port
  14.         print(self.port)

  15.     def waiting(self):
  16.         if not self.waitEnd is None:
  17.             self.waitEnd.wait()

  18.     def SetStopEvent(self):
  19.         if not self.waitEnd is None:
  20.             self.waitEnd.set()
  21.         self.alive = False
  22.         self.stop()

  23.     def start(self):
  24.         self.l_serial = serial.Serial()
  25.         self.l_serial.port = self.port
  26.         self.l_serial.baudrate = 115200
  27.         self.l_serial.timeout = 2
  28.         self.l_serial.open()
  29.         if self.l_serial.isOpen():
  30.             self.waitEnd = threading.Event()
  31.             self.alive = True
  32.             self.thread_read = None
  33.             self.thread_read = threading.Thread(target=self.FirstReader)
  34.             self.thread_read.setDaemon(1)
  35.             self.thread_read.start()
  36.             return True
  37.         else:
  38.             return False

  39.     def SendDate(self, i_msg, send):
  40.         lmsg = ''
  41.         isOK = False
  42.         if isinstance(i_msg):
  43.             lmsg = i_msg.encode('gb18030')
  44.         else:
  45.             lmsg = i_msg
  46.         try:
  47.             # 发送数据到相应的处理组件
  48.             self.l_serial.write(send)
  49.         except Exception as ex:
  50.             pass;
  51.         return isOK

  52.     def FirstReader(self):
  53.         while self.alive:
  54.             data = ''
  55.             data = data.encode('utf-8')
  56.             n = self.l_serial.inWaiting()
  57.             if n:
  58.                 data = self.l_serial.readline()
  59.                 print(data)
  60.                 msg =str(data, encoding="utf-8")
  61.                 self.data_draf.emit(msg)  # for debug only

  62.                 [location_result, location_seq, location_addr, location_x, location_y, tag_acc] = twr_main(msg)
  63.                 print(tag_acc)
  64.                 if location_result == 1:
  65.                     self.data_result.emit(
  66.                         '%d %d %0.2f %0.2f %d' % (location_seq, location_addr, location_x, location_y, tag_acc))
  67.         # #                 bphero_dispose(str(data))

  68.         self.waitEnd.set()
  69.         self.alive = False

  70.     def stop(self):
  71.         self.alive = False
  72.         self.thread_read.join()
  73.         if self.l_serial.isOpen():
  74.             self.l_serial.close()
复制代码
数据融合版本上位机源码下载:
Trilateration_PyQt5_MPU9250.rar (93.4 KB, 下载次数: 140)














回复

使用道具 举报

0

主题

6

帖子

13

积分

新手上路

Rank: 1

积分
13
67#
发表于 2024-7-8 09:41:29 | 只看该作者
多谢分享!!!
回复

使用道具 举报

0

主题

4

帖子

9

积分

新手上路

Rank: 1

积分
9
66#
发表于 2024-6-11 18:29:54 | 只看该作者
感谢分享!!!!!
回复

使用道具 举报

0

主题

2

帖子

4

积分

新手上路

Rank: 1

积分
4
65#
发表于 2024-2-22 17:09:05 | 只看该作者
学习学习……
回复

使用道具 举报

0

主题

9

帖子

18

积分

新手上路

Rank: 1

积分
18
64#
发表于 2024-1-31 18:12:33 | 只看该作者
回帖子,攒积分
回复

使用道具 举报

0

主题

24

帖子

369

积分

中级会员

Rank: 3Rank: 3

积分
369
63#
发表于 2024-1-18 05:32:18 | 只看该作者
学习学习。。。。。。
回复

使用道具 举报

0

主题

8

帖子

18

积分

新手上路

Rank: 1

积分
18
62#
发表于 2024-1-15 19:21:19 | 只看该作者
谢谢楼主分享
回复

使用道具 举报

9

主题

40

帖子

144

积分

注册会员

Rank: 2

积分
144
61#
发表于 2023-11-23 11:18:56 | 只看该作者
1111111111111
回复

使用道具 举报

0

主题

3

帖子

5

积分

新手上路

Rank: 1

积分
5
60#
发表于 2023-10-20 11:34:03 | 只看该作者
厉害!融合算法太需要了
回复

使用道具 举报

0

主题

2

帖子

117

积分

官方会员

Rank: 8Rank: 8

积分
117
59#
发表于 2023-10-10 14:02:50 | 只看该作者
感谢分享!
回复

使用道具 举报

0

主题

11

帖子

14

积分

新手上路

Rank: 1

积分
14
58#
发表于 2023-6-15 14:56:15 | 只看该作者

感谢分享!!!!!
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

bphero Inc.  

GMT+8, 2024-11-1 08:14 , Processed in 0.024489 second(s), 6 queries , File On.

Powered by Discuz! X3.3

© 2001-2017 Comsenz Inc. Template By 【未来科技】【 www.wekei.cn 】

快速回复 返回顶部 返回列表