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

Posted 2023-03-27

【中文标题】三边测量和定位点 (x,y,z)

【英文标题】：Trilateration and locating the point (x,y,z)

【发布时间】：2013-04-17 02:22:33

【问题描述】：

我想找到一个未知节点的坐标，它位于空间中的某个位置，其参考距离远离 3 个或更多节点，这些节点都具有已知坐标。

这个问题与Trilateration 中描述的三边测量完全相同。

但是，我不明白关于“初步和最终计算”的部分（请参阅***网站）。我不知道在哪里可以找到 P1、P2 和 P3，以便我可以代入这些等式？

谢谢

【参考方案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 轴上，我们必须对视图执行旋转。

首先，让我们创建一些新向量作为示例： 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] 时，可以简化为：A 点 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）。

【讨论】：

我不知道我是如何得到这些点 P1、P2 和 P3 的。我的意思是对于每个参考已知节点，我都有它的 x,y,z 坐标。

P1 是第一个参考节点中心的点（由一对 X,Y 坐标组成）。 P2 和 P3 也是如此。如果有参考坐标，则有 P1、P2 和 P3。它们是一回事。

我设置了一个测试坐标只是为了验证数学，它没有给我正确的结果，这就是我感到困惑的原因。这是我的设置： p1 = (-1,1) p2 = (1,1) p3 = (-1,-1) 从我找到的等式中： ex = (1,0) i = 0 ey = (0 ,-1) d = 2 等找到 x 和 yi 使用其他方程，我得到 x = 1 y = 1 这是错误的。我希望看到 x = 0 和 y = 0。注意：在这种情况下，我使用 r1 = sqrt(2) = r2 = r3 你能发现计算中的问题吗？

我重写了我的答案，希望对您有帮助。如果仍然没有意义，请告诉我。

@Dan 我又读了一遍帖子，发现 x,y 点是“中心”点。我不知道我们得到了中心。现在没关系。谢谢

【参考方案2】：

这是我在 3D 打印机固件中使用的算法。它避免了旋转坐标系，但它可能不是最好的。

三边测量问题有两种解决方案。要获得第二个，请将二次方程解中的“- sqrtf”替换为“+ sqrtf”。

如果你有足够的处理器能力和内存，显然你可以使用双精度而不是浮点数。

// Primary parameters
float anchorA[3], anchorB[3], anchorC[3];               // XYZ coordinates of the anchors

// Derived parameters
float Da2, Db2, Dc2;
float Xab, Xbc, Xca;
float Yab, Ybc, Yca;
float Zab, Zbc, Zca;
float P, Q, R, P2, U, A;

...

inline float fsquare(float f)  return f * f; 

...

// Precompute the derived parameters - they don't change unless the anchor positions change.
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]);
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]);
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]);
Xab = anchorA[0] - anchorB[0];
Xbc = anchorB[0] - anchorC[0];
Xca = anchorC[0] - anchorA[0];
Yab = anchorA[1] - anchorB[1];
Ybc = anchorB[1] - anchorC[1];
Yca = anchorC[1] - anchorA[1];
Zab = anchorB[2] - anchorC[2];
Zbc = anchorB[2] - anchorC[2];
Zca = anchorC[2] - anchorA[2];
P = (  anchorB[0] * Yca
     - anchorA[0] * anchorC[1]
     + anchorA[1] * anchorC[0]
     - anchorB[1] * Xca
    ) * 2;
P2 = fsquare(P);
Q = (  anchorB[1] * Zca
     - anchorA[1] * anchorC[2]
     + anchorA[2] * anchorC[1]
     - anchorB[2] * Yca
    ) * 2;  

R = - (  anchorB[0] * Zca
       + anchorA[0] * anchorC[2]
       + anchorA[2] * anchorC[0]
       - anchorB[2] * Xca
      ) * 2;
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P);
A = (P2 + fsquare(Q) + fsquare(R)) * 2;

...

// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc)
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
// P, Q and R depend only on the anchor positions, so they are pre-computed
const float S = - Yab * (fsquare(Lc) - Dc2)
                - Yca * (fsquare(Lb) - Db2)
                - Ybc * (fsquare(La) - Da2);
const float T = - Xab * (fsquare(Lc) - Dc2)
                + Xca * (fsquare(Lb) - Db2)
                + Xbc * (fsquare(La) - Da2);

// Calculate quadratic equation coefficients
const float halfB = (S * Q) - (R * T) - U;
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2;

// Solve the quadratic equation for z
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;

// Substitute back for X and Y
float x = (Q * z + S)/P;
float y = (R * z + T)/P;

【参考方案3】：

以下是 Wikipedia 计算，以 OpenSCAD 脚本呈现，我认为这有助于以视觉方式理解问题，并提供一种简单的方法来检查结果是否正确。 Example output from the script

// Trilateration example
// from Wikipedia
// 
// pA, pB and pC are the centres of the spheres
// If necessary the spheres must be translated
// and rotated so that:
// -- all z values are 0
// -- pA is at the origin
pA = [0,0,0];
// -- pB is on the x axis
pB = [10,0,0];
pC = [9,7,0];

// rA , rB and rC are the radii of the spheres
rA = 9;
rB = 5;
rC = 7;


if ( pA != [0,0,0])
   echo ("ERROR: pA must be at the origin");
   assert(false);


if ( (pB[2] !=0 ) || pC[2] !=0)
   echo("ERROR: all sphere centers must be in z = 0 plane");
   assert(false);


if (pB[1] != 0)
   echo("pB centre must be on the x axis");
   assert(false);


// show the spheres
module spheres()
   translate (pA)
      sphere(r= rA, $fn = rA * 10);
   

   translate(pB)
      sphere(r = rB, $fn = rB * 10);
   

   translate(pC)
      sphere (r = rC, $fn = rC * 10);
   


function unit_vector( v) = v / norm(v);

ex = unit_vector(pB - pA) ;
echo(ex = ex);

i = ex * ( pC - pA);
echo (i = i);

ey = unit_vector(pC - pA - i * ex);
echo (ey = ey);

d = norm(pB - pA);
echo (d = d);

j =  ey * ( pC - pA);
echo (j = j);

x = (pow(rA,2) - pow(rB,2) + pow(d,2)) / (2 * d);
echo( x = x);

// size of the cube to subtract to show 
// the intersection of the spheres
cube_size = [10,10,10];

if ( ((d - rA) >= rB) || ( rB >= ( d + rA)) )
   echo ("Error Y not solvable");
else
   y = (( pow(rA,2) - pow(rC,2) + pow(i,2) + pow(j,2)) / (2 * j))
      - ( i / j) * x;
   echo(y = y);
   zpow2 = pow(rA,2) - pow(x,2) - pow(y,2);
   if ( zpow2 < 0)
      echo ("z not solvable");
   else
      z = sqrt(zpow2);
      echo (z = z);
      // subtract a cube with one of its corners 
      // at the point where the sphers intersect
      difference()
         spheres();
         translate ([x,y - cube_size[1],z])
           cube(cube_size);
         
      
      translate ([x,y - cube_size[1],z])
           %cube(cube_size);