【点】三角定位

三角定位的基本原理很简单,可以抽象成如下问题:已知A(x1,y1)、B(x2,y2)、C(x3,y3)三个点的坐标,以及该三点至未知点D点的距离(分别是d1,d2,d3),求D点的坐标。

可以列出以下公式,从而解出未知点坐标。

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
# -*- coding: utf-8 -*-
import numpy as np

def distance(x1, y1, x2, y2):
dist = np.sqrt((x1-x2)**2 + (y1-y2)**2)
return dist

def solve_equation(x1, y1, x2, y2, x3, y3, d1, d2, d3):
from scipy.optimize import leastsq

def func(i):
x, y = i[0], i[1]
return [
(x1 - x)**2 + (y1 - y)**2 - d1**2,
(x2 - x)**2 + (y2 - y)**2 - d2**2,
(x3 - x)**2 + (y3 - y)**2 - d3**2
]

r = leastsq(func, [0, 0, 0])
return r[0][0], r[0][1]

if __name__=='__main__':

# 给定三个点
x1 = 10
y1 = 10
x2 = 30
y2 = 20
x3 = 40
y3 = 50

# 目标点
x = 5
y = 44

# 给定三个点到目标点的距离
d1 = distance(x1, y1, x, y)
d2 = distance(x2, y2, x, y)
d3 = distance(x3, y3, x, y)

# 解方程组
x_esti, y_esti = solve_equation(x1, y1, x2, y2, x3, y3, d1, d2, d3)
print(x_esti, y_esti)
0%