简单学习：微波定位模块的测距算法 - noox
UWB是一种无线载波通信技术，利用纳秒级的非正弦波窄脉冲传输数据，工作频段在3.25GHZ~6.75GHZ，频宽典型值为500MHZ或者1GHZ，所以可以获取亚纳米的精确时间（1ns约等于750px）。UWB也可用于传输数据，普通帧大约在128字节，扩展帧则能传输K级别的数据。
与普通的通信不同，UWB直接使用数字波形来传输数据，而一般的无线通信使用了载波。
通过二者通信的时间戳，我们能利用TOF的方法，来获得两个模块之间的距离。这个算法就是测距算法。
1. 单边算法：
我们有两个模块，Device A ,DeviceB，他们按照如下流程工作：
- deviceA 首先发送一个带时间戳的帧至 deviceB
- deviceB收到帧的同时开始启动定时T_replay。
- DeviceB 发送回应帧，并把定时间隔T_replay附加在帧中。
- DeviceA 收到回应帧的同时，计算出发送接收间隔T_round ,并从帧中得到T_replay。
这样，DeviceA 就得到了计算需要的关键数据：T_round ，T_replay。
而通过计算，就能得到微波在空间中的传输时间T_prop
由于微波是以光速C运行的，进而通过C*T_prop得到传输距离，完成测距。
T_prop算法如下：
T_prop = (Tround – T_replay) /2
T_prop的误差：
由于模块自身的时钟是有误差的，所以时间采集也有误差。我们假定，DeviceA的时钟误差为e_a, DeviceB的时钟误差为e_b.
按照资料上看，误差计算公式是：
Error≈ 1/2 * (e_b-e_a) / T_replay
按照我自己的计算，误差应该是:
Error = ( e_a*Troud*(1+e_b) – e_b*T_replay*(1+e_a) ) / (Tround(1+e_b) -T_replay*(1+e_a))
现在还不明白这个误差公式是怎么推导出来的。
二. 双边算法：
这里用最常用的三次消息法来说明：
计算距离时，公式如下图：
T_prop = (T_ound1*T_ound2 – Treplay1*Treplay2) / ( Tround1 + Tound2 +Treplay1 + Treplay2)
我不严谨的分析了下，当T1,T2是一个确定的时间，误差出现在时间读数上时：
T_round1 * T_round2 = T1^2 * e_a*e_b
Treplay1 * Treplay2 = T2^2 * e_a*e_b
资料上，其误差公式如下：
Error = T_prop *(1- (ka+kb)/2)
其中，ka, kb 时设备A，设备B的运行频率，接近于1.具体什么含义我也不清楚。
由于确实买看懂误差公式，所以我使用python来暴力列举了下，在当设备误差在(+-0.01)，也就是百分之1之内，使用不同的算法，误差有多大。
这里涉及到两个设备，我让E_a，E_b互相独立，设定Tround=2400，Treplay = 1800，计算其误差。
得到如下对比的误差图。，其中X轴是e_a误差， Y轴是e_b误差。Z轴是在设备处于e_a,e_b误差下，测距算法的误差百分比。
可见，使用DTR（双边双向） 算法，误差保存在+-1%之间， 而使用STR（单边）算法，误差在+-6%之间。
最后时代码：
import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D ax = plt.figure().add_subplot(111, projection = \'3d\') bx = plt.figure().add_subplot(111, projection = \'3d\') #单边算法 求各E_a ,E_b 下的测距误差,输入为真实时间 def STR_E(T_round=2400,T_replay=1800): for x in range(990,1010): for y in range(990,1010): x_f = x/1000 y_f = y /1000 z_r = (T_round - T_replay)/2 z_e = (T_round*x_f - T_replay*y_f)/2 error = (z_e - z_r)/z_r *100 point = [x_f,y_f,error]; yield point #双边算法 求各E_a ,E_b 下的测距误差 ，输入为真实时间 def DTR_E(T_round=2400,T_replay=1800): for x in range(990,1010): for y in range(990,1010): x_f = x/1000 y_f = y/1000 T1_e = T_round*x_f T2_e = T_replay*y_f T3_e = T_round*y_f T4_e = T_replay*x_f muti = T1_e*T3_e-T2_e*T4_e summ = T1_e+T2_e+T3_e+T4_e z_e = muti / summ z_r = (T_round - T_replay)/2 error = (z_e - z_r)/z_r *100 point = [x_f,y_f,error]; yield point #使用生成器，速度很慢 #for pp in DTR_E(): #ax.scatter(pp[0],pp[1],pp[2], c = \'r\', marker = \'.\') #点为红色三角形 #先把数据写入列表，然后再显示，速度快 px=[] py=[] pz=[] for point in DTR_E(): px.append(point[0]) py.append(point[1]) pz.append(point[2]) ax.scatter(px,py,pz, c = \'r\', marker = \'.\') #点为红色三角形 pbx=[] pby=[] pbz=[] for point in STR_E(): pbx.append(point[0]) pby.append(point[1]) pbz.append(point[2]) bx.scatter(pbx,pby,pbz, c = \'r\', marker = \'.\') #点为红色三角形 #设置坐标轴 ax.set_xlabel(\'X Label\') ax.set_ylabel(\'Y Label\') ax.set_zlabel(\'Z Label\') bx.set_xlabel(\'X Label\') bx.set_ylabel(\'Y Label\') bx.set_zlabel(\'Z Label\') #显示图像 plt.show()