Trilatération et localisation du point (x, y, z)
Je veux trouver la coordonnée d’un nœud inconnu qui se situe quelque part dans l’espace dont la distance de référence s’éloigne de 3 nœuds ou plus dont tous ont la coordonnée connue.
Ce problème est exactement comme la trilatération décrite ici Trilatération .
Cependant, je ne comprends pas la partie concernant les "calculs préliminaires et finaux" (voir le site wikipedia). Je n'arrive pas où je pourrais trouver P1, P2 et P3 juste pour pouvoir en arriver à l'équation?
Merci
La trilatération est le processus de recherche du centre de la zone d'intersection de trois sphères. Le centre et le rayon de chacune des trois sphères doivent être connus.
Examinons vos trois exemples de points centraux P1 [-1,1], P2 [1,1] et P3 [-1, -1]. La première exigence est que P1 'soit à l'origine, ajustons donc les points en conséquence en ajoutant un vecteur de décalage V [1, -1] aux trois:
P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]
Remarque: les points ajustés sont désignés par l'annotation '(premier).
P2 'doit également se trouver sur l'axe des x. Dans ce cas, c'est déjà fait, aucun ajustement n'est nécessaire.
Nous supposerons que le rayon de chaque sphère est égal à 2.
Nous avons maintenant 3 équations (données) et 3 inconnues (X, Y, Z du centre des points d’intersection).
Résoudre pour 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
Résoudre pour 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
Ignorez z pour les problèmes 2D.
P4 '= [1, -1]
Maintenant, nous traduisons vers l'espace de coordonnées d'origine en soustrayant le vecteur de décalage V:
P4 = P4 '- V = [0,0]
Le point de solution, P4, se situe à l’origine comme prévu.
La seconde moitié de l'article décrit une méthode de représentation d'un ensemble de points où P1 n'est pas à l'origine ou P2 n'est pas sur l'axe des x de manière à ce qu'ils répondent à ces contraintes. Je préfère le considérer comme une traduction, mais les deux méthodes aboutiront à la même solution.
Edit: Rotation de P2 'sur l'axe des x
Si P2 'ne se trouve pas sur l'axe des x après la conversion de P1 en origine, vous devez effectuer une rotation sur la vue.
Premièrement, créons de nouveaux vecteurs à utiliser à titre d'exemple: P1 = [2,3] P2 = [3,4] P3 = [5,2]
Rappelez-vous, nous devons d'abord traduire P1 en Origine. Comme toujours, le vecteur de décalage, V, est -P1. Dans ce cas, 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]
Pour déterminer l'angle de rotation, il faut trouver l'angle entre P2 'et [1,0] (l'axe des x).
Nous pouvons utiliser le produit scalaire égalité:
A dot B = ||A|| ||B|| cos(theta)
Lorsque B vaut [1,0], ceci peut être simplifié: un point B est toujours juste la composante X de A et || B || (la magnitude de B) est toujours une multiplication par 1 et peut donc être ignoré.
Nous avons maintenant Ax = || A || cos (thêta), que nous pouvons réorganiser à notre équation finale:
theta = acos(Ax / ||A||)
ou dans notre cas:
theta = acos(P2'x / ||P2'||)
Nous calculons la magnitude de P2 'en utilisant || A || = sqrt (Ax + Ay + Az)
||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)
En branchant cela, nous pouvons résoudre pour thêta
theta = acos(1 / sqrt(2)) = 45 degrees
Maintenant, utilisons la matrice rotation pour faire pivoter la scène de -45 degrés . Puisque P2'y est positif et que la matrice de rotation tourne dans le sens inverse des aiguilles d’une montre, nous utiliserons une rotation négative pour aligner P2 sur le Axe des abscisses (si P2'y est négatif, n’annulez pas thêta).
R(theta) = [cos(theta) -sin(theta)]
[sin(theta) cos(theta)]
R(-45) = [cos(-45) -sin(-45)]
[sin(-45) cos(-45)]
Nous utiliserons la notation double prime, '', pour désigner les vecteurs traduits et pivotés.
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]
Vous pouvez maintenant utiliser P1 '', P2 '' et P3 '' pour résoudre P4 ''. Appliquez la rotation inverse à P4 '' pour obtenir P4 ', puis inversez-la pour obtenir P4, votre point central.
Pour annuler la rotation, multipliez P4 '' par R (-theta), dans ce cas R (45). Pour annuler la traduction, soustrayez le vecteur de décalage V, ce qui revient à ajouter P1 (en supposant que vous avez utilisé -P1 en tant que V à l'origine).
C'est l'algorithme que j'utilise dans un micrologiciel d'imprimante 3D. Cela évite de faire tourner le système de coordonnées, mais ce n’est peut-être pas le meilleur.
Il y a 2 solutions au problème de la trilatération. Pour obtenir le second, remplacez "- sqrtf" par "+ sqrtf" dans la solution d'équation quadratique.
Évidemment, vous pouvez utiliser des doublons au lieu des flottants si vous avez assez de puissance de processeur et de mémoire.
// 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;
Voici les calculs de Wikipedia, présentés dans un script OpenSCAD, qui, je pense, aident à comprendre le problème de manière visuelle et offrent un moyen simple de vérifier que les résultats sont corrects. Exemple de sortie du 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);
}
}
}