algorithm - 三边测量和定位点 (x,y,z)

Question

我想找到位于空间中某处的未知节点的坐标，该节点的引用距离远离 3 个或更多节点，这些节点都具有已知坐标。

这个问题与这里描述的三边测量完全一样 Trilateration .

但是，我不明白有关“初步和最终计算”的部分(请参阅维基百科网站)。我不知道在哪里可以找到 P1、P2 和 P3，所以我可以计算这些等式？

谢谢

Answer 1

三边测量是寻找三个球体相交区域的中心的过程。必须知道三个球体的中心点和半径。

让我们考虑一下您的三个示例中心点 P1 [-1,1]、P2 [1,1] 和 P3 [-1,-1]。第一个要求是 P1' 位于原点，因此让我们通过向所有三个添加偏移向量 V [1,-1] 来相应地调整点:

P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]

注意:调整后的点由 '(素数)注释表示。

P2' 也必须位于 x 轴上。在这种情况下，它已经这样做了，因此无需进行任何调整。

我们假设每个球体的半径为 2。

现在我们有 3 个方程(给定)和 3 个未知数(交点中心的 X、Y、Z)。

求解 P4'x:

x = (r1^2 - r2^2 + d^2) / 2d  //(d,0) are coords of P2'
x = (2^2 - 2^2 + 2^2) / 2*2
x = 1

求解 P4'y:

y = (r1^2 - r3^2 + i^2 + j^2) / 2j - (i/j)x //(i,j) are coords of P3'
y = (2^2 - 2^2 + 0 + -2^2) / 2*-2 - 0
y = -1

对于 2D 问题忽略 z。

P4' = [1,-1]

现在我们通过减去偏移向量 V 转换回原始坐标空间:

P4 = P4' - V = [0,0]

解点 P4 正如预期的那样位于原点。

文章的后半部分描述了一种表示一组点的方法，其中 P1 不在原点或 P2 不在 x 轴上，因此它们符合这些约束。我更愿意将其视为一种翻译，但这两种方法都会产生相同的解决方案。

编辑:将 P2' 旋转到 x 轴

如果将 P1 平移到原点后 P2' 不在 x 轴上，我们必须对 View 执行旋转。

首先，让我们创建一些新的向量作为示例: P1 = [2,3] P2 = [3,4] P3 = [5,2]

请记住，我们必须先将 P1 平移到原点。与往常一样，偏移矢量 V 为 -P1。在这种情况下，V = [-2,-3]

P1' = P1 + V = [2,3] + [-2,-3] = [0, 0]
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1]
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1]

要确定旋转角度，我们必须找到 P2' 和 [1,0](x 轴)之间的角度。

我们可以使用 dot product平等:

A dot B = ||A|| ||B|| cos(theta)

当 B 为 [1,0] 时，这可以简化:点 B 始终只是 A 的 X 分量，并且 ||B|| (B 的大小)总是乘以 1，因此可以忽略。

我们现在有 Ax = ||A|| cos(theta)，我们可以将其重新排列为我们的最终方程:

theta = acos(Ax / ||A||)

或者在我们的例子中:

theta = acos(P2'x / ||P2'||)

我们使用 ||A|| 计算 P2' 的大小= sqrt(Ax + Ay + Az)

||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)

将其插入我们可以求解 theta

theta = acos(1 / sqrt(2)) = 45 degrees

现在让我们使用 rotation matrix将场景旋转 -45 度。 由于 P2'y 为正，并且旋转矩阵逆时针旋转，我们将使用负旋转将 P2 与 x 轴对齐(如果 P2'y 为负，则不要取反 theta)。

R(theta) = [cos(theta) -sin(theta)]
           [sin(theta)  cos(theta)]

  R(-45) = [cos(-45) -sin(-45)]
           [sin(-45)  cos(-45)]

我们将使用双素数符号“”来表示已平移和旋转的向量。

P1'' = [0,0] (no need to calculate this one)

P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414]
       [1 sin(-45) + 1 cos(-45)] = [0]       = [0]

P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)]    = [ 1.414]
       [3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828]

现在您可以使用 P1''、P2'' 和 P3'' 求解 P4''。对 P4'' 应用反向旋转得到 P4'，然后反向平移得到 P4，你的中心点。

要取消旋转，请将 P4'' 乘以 R(-theta)，在本例中为 R(45)。要撤消平移，减去偏移向量 V，这与添加 P1 相同(假设您最初使用 -P1 作为 V)。