【（续）关于开发UWB模块所使用的二维和三维定位算法】

前程提要

在上一篇文章中， 提及了开发UWB模块中所使用到的三点定位，这篇文章将会和大家讨论四点定位算法。
相关链接：二维空间的构建（三点定位算法）

需要的材料

三维空间：四个base，一个label

构建三维空间

构建三维空间的base和label的空间图：
base = a,b,c,d
label = target

构建三维空间的base和label的平面图：

注意事项：如果四个base的z坐标都是相同的，那么第四个点的x,y,z可以直接由前三个基站推算出来，所以第四个base的高度要设置成于其他三个base不同，形成一个3D感知的环境。
比如: base a,b,c 一米；base d 一米五

公式

基于欧式距离Euclidean distance，下方为a,b,c,d base的(x,y,z)坐标生成的公式，以此类推：
$$\begin{gathered} (x-x_1{)}^2+(y-y_1{)}^2+(z-z_1{)}^2=R_1^2 \\ (x-x_2{)}^2+(y-y_2{)}^2+(z-z_2{)}^2=R_2^2 \\ (x-x_3{)}^2+(y-y_3{)}^2+(z-z_3{)}^2=R_3^2 \\ (x-x_4{)}^2+(y-y_4{)}^2+(z-z_4{)}^2=R_4^2 \end{gathered}$$

算法

利用四个基站的坐标和距离数据计算出标签的坐标：

'''
a,b,c,d 代表base
x,y,z 为坐标
d 为距离
'''
import sympy 
def quartPosition(xa,ya,za,da,xb,yb,zb,db,xc,yc,zc,dc,xd,yd,zd,dd): 
    x,y,z = sympy.symbols('x y z')

    # 基于欧式距离得出公式 List of equations based on Euclidean distance
    f1 = x*(2*xa-2*xb)+y*(2*ya-2*yb)+z*(2*za-2*zb)-(np.square(xa)-np.square(xb)+np.square(ya)-np.square(yb)+np.square(za)-np.square(zb)-da*da+db*db)
    f2 = x*(2*xa-2*xc)+y*(2*ya-2*yc)+z*(2*za-2*zc)-(np.square(xa)-np.square(xc)+np.square(ya)-np.square(yc)+np.square(za)-np.square(zc)-da*da+dc*dc)
    f3 = x*(2*xa-2*xd)+y*(2*ya-2*yd)+z*(2*za-2*zd)-(np.square(xa)-np.square(xd)+np.square(ya)-np.square(yd)+np.square(za)-np.square(zd)-da*da+dd*dd)
    
    # 用python(sympy)包裹解方程 Solve the equations by sympy package
    out, = sympy.linsolve([f1,f2,f3],[x,y,z]) # tuple
    return list(out)
    # 有输入值后，输出 [x.xx, y.yy, z,zz]

优化 (Optimisation)

对于z 坐标的不稳定，阅读资料后，可概括为三种情况：
1/ base无法区分label在x轴上的位置，返回默认值。

2/ 四个base的高度差不足。
-> 多尝试几个基站高度，形成三维空间所需要的base 高度差。

3/ 在x,y平面上的累积误差值在z轴上呈现出来。
-> 需要优化算法
-> 根据距离来解方程，由于x,y轴误差到累积（即便是细微的，透过公式二次方的累积也会变成大的误差值），最后解出的z值的误差大、不稳定。我们需要用到数学方法来解决，比如在3D定位的论文中提到，利用梯度下降法来寻找最优解。

～～～这一部分我们还在学习中，见下回分解～～～

Credit

MIT Hong Kong Innovation Node
Mike Zhang
Xavier Pan

https://github.com/lighTag-UWB

持续学习，欢迎大家留言讨论，谢谢。