|
论坛开源项目，不作为淘宝订单资料，任何疑问不走淘宝旺旺交流。有问题直接发帖
【蓝点开源】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 对应接口 - #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[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);
- }
- }
固件- 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);
- }
我们提供了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);
现固件代码，代码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[0] = 0xFF;
- msg_f_send.destAddr[1] = 0xFF;
- msg_f_send.seqNum = distance_seqnum;
- msg_f_send.messageData[0]='M';
- msg_f_send.messageData[1] = 0;//数据包长度
-
- uint8 *pAnthor_Str = &msg_f_send.messageData[2];
- 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[index].alive == 1)
- {
- sprintf(pAnthor_Str, "%04X:%04X:%02X#",anthor_info[index].short_address,anthor_info[index].distance,anthor_info[index].rssi_info);
- pAnthor_Str = pAnthor_Str + 13;
- }
- }
- pAnthor_Str = pAnthor_Str - 1;
- sprintf(pAnthor_Str, "$AA##\r\n");
- while(msg_f_send.messageData[str_len] != '\n')
- {
- crc_byte =crc_byte^msg_f_send.messageData[str_len];
- str_len++;
- }
- str_len++;//字符串最后追加'\n'
- printf(&msg_f_send.messageData[2]);
- msg_f_send.messageData[1] = 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[2],msg_f->messageData[1]);
- break;
标签发送‘M’信息，第一个字节是‘M’，第二个字节保存了数据长度，在处理‘M’信息，直接使用msg_f->messageData[1] 定义送到串口的数据长度。 上位机测试效果：
Hash:3ec3d27cabc914365549fb0d907e034f3caa21ec
上位机：
这个部分实现上位机代码，上位机使用我们之前开源Python版本TWR上位机，代码可以在末尾论坛链接下载
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(":")[0] # &&&:80$
- data_len = int(temp_string.split(":")[1], 16)
- # tag info
- temp_string = string.split()[1] # 000A:20
- tag_id = int(temp_string.split(":")[0], 16) # 000A
- tag_acc = int(temp_string.split(":")[1], 16)
- tag_seq = int(temp_string.split(":")[2], 16) # 20
- # print("标签ID: %02X Seq: %X" % (tag_id, tag_seq))
- result_dict['tag'] = tag_id
- result_dict['acc'] = tag_acc
- result_dict['seq'] = tag_seq
- def twr_main(input_string):
- print(input_string)
- error_flag, result_dic = Process_String_Before_Udp(input_string)
- if error_flag == 0:
- [location_result, location_seq, location_addr, location_x, location_y] = Compute_Location(result_dic)
- return location_result, location_seq, location_addr, location_x, location_y, result_dic['acc']
- return 0, 0, 0, 0, 0, 0
3 顶层收到定位结果和运动信息，打印结果，并发送给处理函数 - [location_result, location_seq, location_addr, location_x, location_y, tag_acc] = 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[1])
- location_x = float(strlist[2])
- location_y = float(strlist[3])
- tag_acc = int(strlist[4])
- 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
- [location_result, location_seq, location_addr, location_x, location_y, tag_acc] = 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()
Trilateration_PyQt5_MPU9250.rar
(93.4 KB, 下载次数: 140)
|
发表于 2021-9-5 12:13:15
收藏
楼主
发表于 2021-9-29 19:27:55
