algorithm - triangulacion - trilateracion ejemplos

Trilateración y localización del punto(x, y, z)

Question

Quiero encontrar la coordenada de un nodo desconocido que se encuentra en algún lugar del espacio que tiene su distancia de referencia de 3 o más nodos que todos ellos tienen coordenadas conocidas.

Este problema es exactamente como Trilateration como se describe aquí Trilateration .

Sin embargo, no entiendo la parte sobre "Cálculos preliminares y finales" (consulte el sitio de wikipedia). ¿No llego a donde pude encontrar P1, P2 y P3 solo para poner esa ecuación?

Gracias

Answer 1

Aquí están los cálculos de Wikipedia, presentados en un script de OpenSCAD, que creo que ayuda a entender el problema de una manera visual y proporciona una manera fácil de verificar que los resultados son correctos. Ejemplo de salida del 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);
      }
  }
}

Answer 2

Este es el algoritmo que utilizo en un firmware de impresora 3D. Evita rotar el sistema de coordenadas, pero puede que no sea el mejor.

Existen 2 soluciones al problema de la trilateración. Para obtener el segundo, reemplaza "- sqrtf" por "+ sqrtf" en la solución de ecuación cuadrática.

Obviamente, puede usar dobles en lugar de flotadores si tiene suficiente potencia de procesador y memoria.

// 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;

Answer 3

La trilateración es el proceso de encontrar el centro del área de intersección de tres esferas. El punto central y el radio de cada una de las tres esferas deben ser conocidos.

Consideremos los tres puntos centrales de ejemplo P1 [-1,1], P2 [1,1] y P3 [-1, -1]. El primer requisito es que P1 ''esté en el origen, así que ajustemos los puntos en consecuencia agregando un vector de compensación V [1, -1] a los tres:

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

Nota: Los puntos ajustados se denotan mediante la anotación ''(principal).

P2 ''también debe estar en el eje x. En este caso ya lo hace, por lo que no es necesario ningún ajuste.

Asumiremos que el radio de cada esfera es 2.

Ahora tenemos 3 ecuaciones (dadas) y 3 incógnitas (X, Y, Z del punto de centro de intersección).

Resuelve para 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

Resuelve para 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

Ignora z para problemas en 2D.

P4 ''= [1, -1]

Ahora volvemos a traducir al espacio de coordenadas original restando el vector de desplazamiento V:

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

El punto de solución, P4, se encuentra en el origen como se esperaba.

La segunda mitad del artículo describe un método para representar un conjunto de puntos en los que P1 no está en el origen o P2 no está en el eje x, por lo que se ajustan a esas restricciones. Prefiero considerarlo como una traducción, pero ambos métodos darán como resultado la misma solución.

Edición: Girando P2 ''al eje x

Si P2 ''no se encuentra en el eje x después de traducir P1 al origen, debemos realizar una rotación en la vista.

Primero, creemos algunos vectores nuevos para usar como ejemplo: P1 = [2,3] P2 = [3,4] P3 = [5,2]

Recuerda, primero debemos traducir P1 al origen. Como siempre, el vector de desplazamiento, V, es -P1. En este caso, 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]

Para determinar el ángulo de rotación, debemos encontrar el ángulo entre P2 ''y [1,0] (el eje x).

Podemos usar la igualdad del producto punto :

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

Cuando B es [1,0], esto se puede simplificar: un punto B es siempre el componente X de A, y || B || (la magnitud de B) es siempre una multiplicación por 1, y por lo tanto puede ignorarse.

Ahora tenemos Ax = || A || cos (theta), que podemos reorganizar en nuestra ecuación final:

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

o en nuestro caso:

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

Calculamos la magnitud de P2 ''utilizando || A || = sqrt (Ax + Ay + Az)

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

Enchufando eso podemos resolver por theta.

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

Ahora usemos la matriz de rotación para rotar la escena en -45 grados. Dado que P2''y es positivo, y la matriz de rotación gira en sentido contrario a las agujas del reloj, usaremos una rotación negativa para alinear P2 con el eje x (si P2''y es negativo, no se debe negar theta).

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

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

Usaremos la notación de primos dobles, '''', para denotar vectores que se han traducido y rotado.

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]

Ahora puedes usar P1 '''', P2 '''' y P3 '''' para resolver P4 ''''. Aplique la rotación inversa a P4 '''' para obtener P4 '', luego la traducción inversa para obtener P4, su punto central.

Para deshacer la rotación, multiplique P4 '''' por R (-teta), en este caso R (45). Para deshacer la traducción, reste el vector de desplazamiento V, que es lo mismo que agregar P1 (suponiendo que usó -P1 como su V originalmente).