三边测量（Trilateration）定位方法

参考链接：https://blog.csdn.net/chengde6896383/article/details/76474358

基本原理

已知条件：

• 三个锚节点（Anchor，A）的坐标(x1,y1,z1),(x2,y2,z2),(x3,y3,z3)
• 标签节点(Tag，T)到达三个A节点的距离分别为r1,r2,r3

求解标签节点的坐标(x,y,z)。

根据勾股定理，可以计算得到T节点的位置：

$$ (x_{1}-x)^2+(y_{1}-y)^2+(z_{1}-z)^2=r_{1}^2 $$
$$ (x_{2}-x)^2+(y_{2}-y)^2+(z_{2}-z)^2=r_{2}^2 $$
$$ (x_{3}-x)^2+(y_{3}-y)^2+(z_{3}-z)^2=r_{3}^2 $$

首先设：A₁的坐标为(0,0,0)，A₂的y坐标与A₁相同，坐标为(d,0,0)，A₃的坐标为(i,j,0)，三个球形的半径（到T节点的距离）分别为r1,r2,r3，z为T节点到A节点构成的平面的距离，则有：

$$ x^2+y^2+z^2=r_{1}^2\quad① $$
$$ (x-d)^2+y^2+z^2=r_{2}^2\quad② $$
$$ (x-i)^2+(y-j)^2+z^2=r_{3}^2\quad③ $$

由①-②求得 x ：

$$ x = (r_{1}^2-r_{2}^2+d^2)/(2*d)\quad④ $$

由②-③并将④带入求得 y ：

$$ y = (r_{1}^2-r_{3}^2-x^2+(x-i)^2+j^2)/(2*j)\quad⑤ $$

然后由①④⑤求得 z ：

$$ z = \sqrt{r_{1}^2-x^2-y^2}\quad⑥ $$

完毕。

注意：这种方法需要三个A节点在同一水平面上，并且无法区分z是在水平面上面或者下面。