【关于开发UWB模块所使用的二维和三维定位算法】
本文将介绍开发UWB模块中所使用到的二维定位算法（三点定位算法）
链接：
三维空间的构建（四点定位算法）
概念介绍
UWB室内定位系统，需要：
1/ 定位标签(label)，需要电池供电，能够发射UWB信号来确定位置。
2/ 定位基站(base)是固定在一个地方的發射站，接收从label发送过来的信号；二维和三维需要的基站数量会不同。
3/ 综合并解读位置信息的算法和平台，可通过wifi模块实现无线传输，
本文只集中讨论在电脑上解读传输信息的算法
。
需要的材料
比如 BP-TWR-50 UWB 模块：
二维空间：三个base，一个label
三维空间：四个base，一个label
label会有一个显示屏，用以接收并计算base传送过来的距离信息。
算法
欧几里得距离 (Euclidean distance)
在数学中，Euclidean distance是欧氏空间中两点间的直线距离。使用这个距离，欧氏空间成为度量空间。
公式为：
d
(
p
,
q
)
=
∑
i
=
1
n
(
q
i
−
p
i
)
2
d(p,q) = \sqrt{\sum_{i=1}^n(q_i-p_i)^2}
d
(
p
,
q
)
=
i
=
1
∑
n
(
q
i
−
p
i
)
2
或者
d
=
(
q
2
−
q
1
)
2
+
(
p
2
−
p
1
)
2
d = \sqrt{(q_2-q_1)^2+(p_2-p_1)^2}
d
=
(
q
2
−
q
1
)
2
+
(
p
2
−
p
1
)
2
使用公式，可得出距离d 的直线距离为(q2,p2)和(q1,p1)两点之间的长度：
方程求解
三角定位(trilateration)假设一个测量的目标点(object/target)和两个已知坐标的点可形成一个三角形，因此透过计算距离长度，可推论出target的距离和坐标。
计算二维空间的label数值，最理想情况：三圆交于一点
使用python(sympy)包裹可帮助我们解方程：
import sympy
'''
a,b,c 代表base
q,p 为坐标x,y
d 为距离
'''
def triPosition(qa,pa,da,qb,pb,db,qc,pc,dc):
q,p = sympy.symbols('x y')
# 基于欧式距离得出公式 List of equations based on Euclidean distance
f1 = 2*q*(qa-qc)+np.square(qc)-np.square(qa)+2*p*(pa-pc)+np.square(pc)-np.square(pa)-(np.square(dc)-np.square(da))
f2 = 2*q*(qb-qc)+np.square(qc)-np.square(qb)+2*p*(pb-pc)+np.square(pc)-np.square(pb)-(np.square(dc)-np.square(db))
# 用python(sympy)包裹解方程 Solve the equations by sympy package
result = sympy.solve([f1,f2],[x,y])
locx,locy = result[x],result[y]
return [locx,locy]
# 最后得出[x.xx, y.yy]
Credit
MIT Hong Kong Innovation Node
Mike Zhang
Xavier Pan
https://github.com/lighTag-UWB
持续学习，欢迎大家留言讨论，谢谢。