【 声明:版权所有,欢迎转载,请勿用于商业用途。 联系信箱:feixiaoxing @163.com】
三边测量,或者说叫三角定位,是定位系统中很常见的一种测量方法。它最主要的原理就是依靠已有的三个特征坐标和半径,就可以计算出当前自己的坐标信息。听上去这么做很玄乎,其实相关的计算,用初中数学的知识就可以完全掌握。
首先我们假设三个特征坐标分别是(x1,y1)、(x2,y2)、(x3,y3),它们的半径分别是r1、r2、r3。已知的条件就这么多,下面就要求出(x,y)的坐标了。
1、第一步,构建方程组
(x-x1)*(x-x1) + (y-y1)*(y-y1) = r1*r1
(x-x2)*(x-x2) + (y-y2)*(y-y2) = r2*r2
(x-x3)*(x-x3) + (y-y3)*(y-y3) = r3*r3
2、第二步,展开方程组
x*x - 2*x1*x + x1*x1 + y*y - 2*y1*y + y1*y1 = r1 * r1
x*x - 2*x2*x + x2*x2 + y*y - 2*y2*y + y2*y2 = r2 * r2
x*x - 2*x3*x + x3*x3 + y*y - 2*y3*y + y3*y3 = r3 * r3
3、第三步,两两相减
针对之前生成的公式,现在可以两两相减,这样就可以得到如下公式,这是目前为止最为关键的一步,
(-2*x1 + 2*x2)*x + (-2*y1 + 2*y2)*y = r1*r1 - r2*r2 -x1*x1 + x2*x2 -y1*y1 + y2*y2
(-2*x2 + 2*x3)*x + (-2*y2 + 2*y3)*y = r2*r2 - r3*r3 -x2*x2 + x3*x3 -y2*y2 + y3*y3
4、第四步,重写表达式
因为上面公式的内容比较多,我们可以换个符号重写一下,即,
A*x + B*y = C
D*x + E*y = F
注意,这里的A、B、C、D、E、F分别代表上面数学表达式中的子表达式,
A = -2*x1 + 2*x2
B = -2*y1 + 2*y2
C = r1*r1 - r2*r2 -x1*x1 + x2*x2 -y1*y1 + y2*y2
D = -2*x2 + 2*x3
E = -2*y2 + 2*y3
F = r2*r2 - r3*r3 -x2*x2 + x3*x3 -y2*y2 + y3*y3
5、第五步,获取最终的x和y
有了A/B/C/D/E/F,这样最终的x和y也就可以计算出来了,
x = (CE-FB)/(EA-BD)
y = (CD-AF)/(BD-AE)
6、python代码范例
最后,我们给出一份python代码,大家可以灵活地将他改成自己需要的语言。最后,非常感谢https://www.101computing.net/cell-phone-trilateration-algorithm/这个链接,有兴趣的同学可以直接阅读上面的内容。
def track(x1,y1,r1,x2,y2,r2,x3,y3,r3):A = 2*x2 - 2*x1B = 2*y2 - 2*y1C = r1**2 - r2**2 - x1**2 + x2**2 - y1**2 + y2**2D = 2*x3 - 2*x2E = 2*y3 - 2*y2F = r2**2 - r3**2 - x2**2 + x3**2 - y2**2 + y3**2x = (C*E - F*B) / (E*A - B*D)y = (C*D - A*F) / (B*D - A*E)return x,y