SLAM从入门到精通（三边测量法详解）

【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：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*x1
  B = 2*y2 - 2*y1
  C = r1**2 - r2**2 - x1**2 + x2**2 - y1**2 + y2**2
  D = 2*x3 - 2*x2
  E = 2*y3 - 2*y2
  F = r2**2 - r3**2 - x2**2 + x3**2 - y2**2 + y3**2
  x = (C*E - F*B) / (E*A - B*D)
  y = (C*D - A*F) / (B*D - A*E)
  return x,y