【(续)关于开发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)坐标生成的公式,以此类推:
( 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 (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 (x−x1)2+(y−