uwb最详细的DS-TWR测距

李肖遥

共 7511字,需浏览 16分钟

 ·

2022-05-10 23:52

    关注、星标公众号,直达精彩内容

来源:小昭debug

作者:小昭


简介


•测距、定位和数据传输。 


•利用双向测距(TOF)测量或单向到达时间差(TDOA)到达时间差,误差在10cm,经过一定的滤波误差可以达到更低。


•跨越 3.5 GHz 至 6.5 GHz 的 6 个 RF 频段。


•支持 110 kbps, 850 kbps 和 6.8 Mbps 的数据速率。


•高数据传输速率,可以保持播放时间短,从而节省电量并延长电池的使用寿命。 


•它能够处理严重的多路径环境,因此非常适合高度反射的射频环境。

 

 

测距原理

两个uwb设备测距为例


    设备A在a时刻发送消息(消息内容包含a时间戳),设备B接收在c时刻接收到消息,设备B在接收和发送消息,都可以记录当前的时间戳。


那计算a-c是不是就是电磁波在空气中传输的时间?

    答案不是。因为它们的时钟并不同步


a-b是本地时钟,那是不是电磁波往返的时间?

    答案不是,这里还需要考虑到Trely,设备A发送给设备B到设备B发送给设备A的时间(设备B接收到消息后,做消息处理和硬件天线延迟的问题),这个不能不计,1 ns(纳秒) 的误差相当于测量距离的 30 cm 误差(光速)。

 

 

什么是时钟同步?

    在某一个时刻,设备A和设备B的节拍相同,假设从0计数到65536,设备从一上电就开始计数,但是你能保证所有设备一上电都是同时从0开始计数吗?显然不行的。


哪有什么办法可以计算电磁波在空气中飞行的时间?

    单向双向测距(SS-TWR)涉及单个消息从一个节点到另一个节点的往返延迟的简单测量,以及将响应发送回原始节点。(a-b)-(c-d):表示电磁波在空气中飞行的往返时间。


如果不能理解,举个例子:

    地球和火星的距离。来自地球的老王,打电话给在火星上的老马并看了现在的时间12:00:00(时分秒),老马接到电话,时间是18:00:00,多了一会,在18:05:00老马打了电话给老王,老王在12:13:00接到电话。因为他们的频率是一致都是以秒为单位(假设),通过这些数据可以计算信号在地球和火星间飞行的时间,往返的时间是13-5=8分钟;这里我们还要考虑到老马或者老王的表是否快几秒的误差可能。

 

    可以看出,随着 Treply 增加,并且随着时钟偏移增加,飞行时间估计中的误差增加到误差使得估计非常不准确的程度。由于这个原因, SS-TWR 不常用。

 

    DS-TWR 双向双向测距(DS-TWR)是基本的单向双向测距的延伸,其中使用了两个往返时间测量,并将其组合在一起,从而得到飞行时间结果 即使相当长的响应延迟也是错误的。极度降低Treply 的时间因为一些障碍物或者噪声的干扰,测距变化较大,最好多次取平均值,DS-TWR就是这种。

 测距公式:

    


 

 

代码讲解

 

 

uwb硬件实物(开源)


  uwb硬件原理图


主控与uwb通信,只使用了UWB_RST和SPI引脚。

//消息头//设备B 基站static uint8 rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'H', 'A', 'V', 'E', 0x21, 0, 0, 0, 0};static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'L', 'O', 'V', 'E', 0x10, 0x02, 0, 0, 0, 0};static uint8 rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'O', 'P', 'E', 'N', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};//设备A 标签static uint8 tx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'H', 'A', 'V', 'E', 0x21, 0, 0, 0, 0};static uint8 rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'L', 'O', 'V', 'E', 0x10, 0x02, 0, 0, 0, 0,0};static uint8 tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'O', 'P', 'E', 'N', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};//只定义消息 数据通信static uint8 tx_distance_msg[] = {0x41, 0x88, 0, 0xAA, 0xDE, 'W', 'A', 'V', 'E', 0xAA, 0, 0,0, 0, 0};#define DISTAN_INDEX  9#define TURE_LENGTH 5

 

通信流程

    //重启DW1000     reset_DW1000();/* Target specific drive of RSTn line into DW1000 low for a period. */    //降低SPI频率    spi_set_rate_low();    //初始化DW1000    if(dwt_initialise(DWT_LOADUCODE) == -1)    {          while(1){    GPIO_SetBits(GPIOA,GPIO_Pin_2);    GPIO_SetBits(GPIOB,GPIO_Pin_14);      deca_sleep(100);                GPIO_ResetBits(GPIOA,GPIO_Pin_2);        GPIO_ResetBits(GPIOB,GPIO_Pin_14);      deca_sleep(100);              }          }    //回复SPI频率    spi_set_rate_high();      /* Configure DW1000. See NOTE 6 below. */    //配置DW1000 传输速率 通道 包长 包格式    dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 1 below. */ //设置接收天线延迟 dwt_setrxantennadelay(RX_ANT_DLY);    //设置发射天线延迟     dwt_settxantennadelay(TX_ANT_DLY); /* Set expected response's delay and timeout. See NOTE 4 and 5 below. * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */ //设置发送后开启接收,并设定延迟时间 dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); // 接收器 设置接收超时时间 dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);



1、设备A作为标签,主动发送消息(poll T1 时间戳保留在寄存器中)DS-TWR测距

tx_poll_msg[ALL_MSG_ANTHOR_IDX] = dest_anthor;  //UWB POLL 包数据rx_resp_msg[ALL_MSG_ANTHOR_IDX] = dest_anthor;  //UWB RESPONSE 包数据tx_final_msg[ALL_MSG_ANTHOR_IDX] = dest_anthor;//UWB Fianl 包数据        /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//将Poll包数据传给DW1000,将在开启发送时传出去dwt_writetxfctrl(sizeof(tx_poll_msg), 0);//设置超宽带发送数据长度
/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay * set by dwt_setrxaftertxdelay() has elapsed. */dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送,发送完成后等待一段时间开启接收,等待时间在dwt_setrxaftertxdelay中设置//发送完 打开接收器 uwb寄存器会记录发射的时间戳/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误{ };//等待接收



设备B作为基站,接收poll消息 并发送resp (T2 T3时间戳)

dwt_setrxtimeout(0);//设定接收超时时间,0位没有超时时间/* Activate reception immediately. */dwt_rxenable(0);//打开接收/* Poll for reception of a frame or error/timeout. See NOTE 7 below. */while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到接收成功或者出现错误{ };if (status_reg & SYS_STATUS_RXFCG)//成功接收{               /* Clear good RX frame event in the DW1000 status register. */    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);    //清楚标志位
/* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023; //获得接收数据长度
if (frame_len <= RX_BUFFER_LEN) { dwt_readrxdata(rx_buffer, frame_len, 0); } /*判断数据*/ if(rx_buffer[ALL_MSG_ANTHOR_IDX]!=ANCHOR_ID) continue; /* Check that the frame is a poll sent by "DS TWR initiator" example. * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0;
if (rx_buffer[Frame_type]==0x21)//判断是否是poll包数据 { uint32 resp_tx_time; /* Retrieve poll reception timestamp. */ poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
/* Set send time for response. See NOTE 8 below. */ resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。 dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
/* Set expected delay and timeout for final message reception. */ dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS); //设置发送完成后开启接收延迟时间 dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间
/* Write and send the response message. See NOTE 9 below.*/ memcpy(&tx_resp_msg[11],&dis,2); tx_resp_msg[ALL_MSG_TAG_IDX] = rx_buffer[ALL_MSG_TAG_IDX]; tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb; dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0); //写入发送数据 dwt_writetxfctrl(sizeof(tx_resp_msg), 0); //设定发送长度 dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED); //延迟发送,等待接收 while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///不断查询芯片状态直到接收成功或者出现错误 { }; //等待接收


设备A 接收resp 发送final()T4 T5

if (status_reg & SYS_STATUS_RXFCG)//如果成功接收{              uint32 frame_len;
/* Clear good RX frame event and TX frame sent in the DW1000 status register. */ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位
/* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度
dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据 /*数据帧类型判断*/ if(rx_buffer[ALL_MSG_TAG_IDX] != TAG_ID)//检测TAG_ID continue;
/* Check that the frame is the expected response from the companion "DS TWR responder" example. * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0; if (rx_buffer[Frame_type]==0x10)//判断接收到的数据是否是response数据 { uint32 final_tx_time;
/* Retrieve poll transmission and response reception timestamp. */ poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1 resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4 //距离发送 memcpy(&dist[dest_anthor],&rx_buffer[11],2); /* Compute final message transmission time. See NOTE 9 below. */ final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间,T5=T4+Treply2 dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */ final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay
/* Write all timestamps in the final message. See NOTE 10 below. */ final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1,T4,T5写入发送数据 final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts); tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb; dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将发送数据写入DW1000 dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度 dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送


设备B接收final,final消息包(包含了设备A的T1 T4 T5),汇总后开始计算距离。

if (status_reg & SYS_STATUS_RXFCG)//接收成功  {      /* Clear good RX frame event and TX frame sent in the DW1000 status register. */      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚标志位
/* A frame has been received, read it into the local buffer. */ frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//数据长度
dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据 /* Check that the frame is a final message sent by "DS TWR initiator" example. * As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */ rx_buffer[ALL_MSG_SN_IDX] = 0; if (rx_buffer[Frame_type]==0x23)//判断是否为Fianl包 { uint32 poll_tx_ts, resp_rx_ts, final_tx_ts; uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32; double Ra, Rb, Da, Db; int64 tof_dtu; /* Retrieve response transmission and final reception timestamps. */ resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3 final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6
/* Get timestamps embedded in the final message. */ final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1,T4,T5 final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts); final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */ poll_rx_ts_32 = (uint32)poll_rx_ts;//使用32位数据计算 resp_tx_ts_32 = (uint32)resp_tx_ts; final_rx_ts_32 = (uint32)final_rx_ts; Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1 Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3 Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4 Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2 tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式
tof = tof_dtu * DWT_TIME_UNITS; distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间 dist2 = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//距离减去矫正系数 dis = dist2*1000;//dis 为单位为mm的距离


自定义数据包

//发送tx_distance_msg[ALL_MSG_ANTHOR_IDX] = 2;tx_distance_msg[DISTAN_INDEX] = dist[0]/100;tx_distance_msg[DISTAN_INDEX+1] = dist[0]%100;tx_distance_msg[DISTAN_INDEX+2] = dist[1]/100;tx_distance_msg[DISTAN_INDEX+3] = dist[1]%100;  dwt_writetxdata(sizeof(tx_distance_msg), tx_distance_msg, 0);dwt_writetxfctrl(sizeof(tx_distance_msg), 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)){ };

 

//接收判断 接收用户数据if(rx_buffer[ALL_MSG_ANTHOR_IDX]!=ANCHOR_ID)              continue;else if (memcmp(rx_buffer, tx_distance_msg, TURE_LENGTH) == 0){                                               dist[0] = rx_buffer[DISTAN_INDEX]*100+rx_buffer[DISTAN_INDEX+1];    dist[1] = rx_buffer[DISTAN_INDEX+2]*100+rx_buffer[DISTAN_INDEX+3];}



如果是多基站,标签做个轮询每个基站测距,就可以完成。

这种遍历方式,不是最优的,可以采用中断的方式,检测UWB_IRQ引脚电平,判断接收消息类型,做对应的数据处理和数据回复。

 //中断设置 /* Register RX call-back. */ //接收消息的回调函数  dwt_setcallbacks(&tx_conf_cb, &rx_ok_cb, &rx_to_cb, &rx_err_cb);  /* Enable wanted interrupts (TX confirmation, RX good frames, RX timeouts and RX errors). */  //设置对应的中断标志  dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | DWT_INT_RFTO | DWT_INT_RXPTO | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFSL | DWT_INT_SFDT, 1);
 


    如果是采用这种方式做定位,也不是最优,还有更优的操作,TDOA定位,TDOA不需要UWB定位标签与定位基站之间进行往复通信,只需要定位标签发射一次UWB信号,工作时长缩短了,功耗也就大大降低了,故能做到更高的定位动态和定位容量。


免责声明:本文素材来源网络,版权归原作者所有。如涉及作品版权问题,请与我联系删除。

‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧  END  ‧‧‧‧‧‧‧‧‧‧‧‧‧‧‧

关注我的微信公众号,回复“加群”按规则加入技术交流群。

点击下面图片,有星球具体介绍,新用户有新人优惠券,老用户半价优惠,期待大家一起学习一起进步。


点击“阅读原文”查看更多分享,欢迎点分享、收藏、点赞、在看。

浏览 38
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报
评论
图片
表情
推荐
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报