Considérons un plan 2D avec trois points de référence dont les coordonnées sont (x 1 x_1 x 1 y 1 y_1 y 1 x 2 x_2 x 2 y 2 y_2 y 2 x 3 x_3 x 3 y 3 y_3 y 3 x x x y y y d 1 d_1 d 1 d 2 d_2 d 2 d 3 d_3 d 3
d 1 2 = ( x − x 1 ) 2 + ( y − y 1 ) 2 d 2 2 = ( x − x 2 ) 2 + ( y − y 2 ) 2 d 3 2 = ( x − x 3 ) 2 + ( y − y 3 ) 2 {d_1}^2 = (x-x_1)^2 + (y-y_1)^2 \\ {d_2}^2 = (x-x_2)^2 + (y-y_2)^2 \\ {d_3}^2 = (x-x_3)^2 + (y-y_3)^2 d 1 2 = ( x − x 1 ) 2 + ( y − y 1 ) 2 d 2 2 = ( x − x 2 ) 2 + ( y − y 2 ) 2 d 3 2 = ( x − x 3 ) 2 + ( y − y 3 ) 2 En développant :
d 1 2 = x 2 − 2 x x 1 + x 1 2 + y 2 − 2 y y 1 + y 1 2 d 2 2 = x 2 − 2 x x 2 + x 2 2 + y 2 − 2 y y 2 + y 2 2 d 3 2 = x 2 − 2 x x 3 + x 3 2 + y 2 − 2 y y 3 + y 3 2 {d_1}^2 = x^2 - 2xx_1 + {x_1}^2 + y^2 - 2yy_1 + {y_1}^2 \\ {d_2}^2 = x^2 - 2xx_2 + {x_2}^2 + y^2 - 2yy_2 + {y_2}^2 \\ {d_3}^2 = x^2 - 2xx_3 + {x_3}^2 + y^2 - 2yy_3 + {y_3}^2 \\ d 1 2 = x 2 − 2 x x 1 + x 1 2 + y 2 − 2 y y 1 + y 1 2 d 2 2 = x 2 − 2 x x 2 + x 2 2 + y 2 − 2 y y 2 + y 2 2 d 3 2 = x 2 − 2 x x 3 + x 3 2 + y 2 − 2 y y 3 + y 3 2 L'objectif est d'éliminer les termes au carré. On va alors soustraite (1) à (2) et (1) à (3).
d 2 2 − d 1 2 = − 2 x ( x 2 − x 1 ) + x 2 2 − x 1 2 − 2 y ( y 2 − y 1 ) + y 2 2 − y 1 2 d 3 2 − d 1 2 = − 2 x ( x 3 − x 1 ) + x 3 2 − x 1 2 − 2 y ( y 3 − y 1 ) + y 3 2 − y 1 2 {d_2}^2 - {d_1}^2 = - 2x(x_2 - x_1) + {x_2}^2 - {x_1}^2 - 2y(y_2 - y_1) + {y_2}^2 - {y_1}^2 \\ {d_3}^2 - {d_1}^2 = - 2x(x_3 - x_1) + {x_3}^2 - {x_1}^2 - 2y(y_3 - y_1) + {y_3}^2 - {y_1}^2 \\ d 2 2 − d 1 2 = − 2 x ( x 2 − x 1 ) + x 2 2 − x 1 2 − 2 y ( y 2 − y 1 ) + y 2 2 − y 1 2 d 3 2 − d 1 2 = − 2 x ( x 3 − x 1 ) + x 3 2 − x 1 2 − 2 y ( y 3 − y 1 ) + y 3 2 − y 1 2 Et en réagencent :
x ( x 1 − x 2 ) + y ( y 1 − y 2 ) = d 2 2 − d 1 2 − x 2 2 + x 1 2 − y 2 2 + y 1 2 2 x ( x 1 − x 3 ) + y ( y 1 − y 3 ) = d 3 2 − d 1 2 − x 3 2 + x 1 2 − y 3 2 + y 1 2 2 x(x_1 - x_2) + y(y_1 - y_2) = \frac{{d_2}^2 - {d_1}^2 - {x_2}^2 + {x_1}^2 - {y_2}^2 + {y_1}^2}{2} \\ x(x_1 - x_3) + y(y_1 - y_3) = \frac{{d_3}^2 - {d_1}^2 - {x_3}^2 + {x_1}^2 - {y_3}^2 + {y_1}^2}{2} x ( x 1 − x 2 ) + y ( y 1 − y 2 ) = 2 d 2 2 − d 1 2 − x 2 2 + x 1 2 − y 2 2 + y 1 2 x ( x 1 − x 3 ) + y ( y 1 − y 3 ) = 2 d 3 2 − d 1 2 − x 3 2 + x 1 2 − y 3 2 + y 1 2 On cherche à mettre ces équations sous la forme suivante :
( A B D E ) ( x y ) = ( C F ) \begin{pmatrix} A & B \\ D & E \end{pmatrix} \begin{pmatrix} x \\ y \end{pmatrix} = \begin{pmatrix} C \\ F \end{pmatrix} ( A D B E ) ( x y ) = ( C F ) Par identification, on obtient :
{ A = x 1 − x 2 B = y 1 − y 2 C = d 2 2 − d 1 2 − x 2 2 + x 1 2 − y 2 2 + y 1 2 2 D = x 1 − x 3 E = y 1 − y 3 F = d 3 2 − d 1 2 − x 3 2 + x 1 2 − y 3 2 + y 1 2 2 \left\{\begin{matrix} A = x_1 - x_2 \\ B = y_1 - y_2 \\ C = \frac{{d_2}^2 - {d_1}^2 - {x_2}^2 + {x_1}^2 - {y_2}^2 + {y_1}^2}{2} \\ D = x_1 - x_3 \\ E = y_1 - y_3 \\ F = \frac{{d_3}^2 - {d_1}^2 - {x_3}^2 + {x_1}^2 - {y_3}^2 + {y_1}^2}{2} \end{matrix}\right. ⎩ ⎨ ⎧ A = x 1 − x 2 B = y 1 − y 2 C = 2 d 2 2 − d 1 2 − x 2 2 + x 1 2 − y 2 2 + y 1 2 D = x 1 − x 3 E = y 1 − y 3 F = 2 d 3 2 − d 1 2 − x 3 2 + x 1 2 − y 3 2 + y 1 2 Il suffit alors d'inverser la matrice pour obtenir la position (x x x y y y
Considérons un plan 2D avec trois points de référence dont les coordonnées sont ( x 1 = 0 , y 1 = 0 ) (x_1=0, y_1=0) ( x 1 = 0 , y 1 = 0 ) ( x 2 = 1 , y 2 = 0 ) (x_2=1, y_2=0) ( x 2 = 1 , y 2 = 0 ) ( x 3 = 0 , y 3 = 1 ) (x_3=0, y_3=1) ( x 3 = 0 , y 3 = 1 ) ( x , y ) (x, y) ( x , y ) d 1 d_1 d 1 d 2 d_2 d 2 d 3 d_3 d 3 ( 1 , 1 ) (1,1) ( 1 , 1 )
Nos différents capteurs nous renvoient les 3 distances suivantes :
d 1 2 = 1 2 − 2 ∗ 1 ∗ 0 + 0 2 + 1 2 − 2 ∗ 1 ∗ 0 + 0 2 = 2 d 2 2 = 1 2 − 2 ∗ 1 ∗ 1 + 1 2 + 0 2 − 2 ∗ 1 ∗ 0 + 0 2 = 1 d 3 2 = 1 2 − 2 ∗ 1 ∗ 0 + 0 2 + 1 2 − 2 ∗ 1 ∗ 1 + 1 2 = 1 {d_1}^2 = 1^2 - 2*1*0 + 0^2 + 1^2 - 2*1*0 + 0^2 = 2 \\ {d_2}^2 = 1^2 - 2*1*1 + 1^2 + 0^2 - 2*1*0 + 0^2 = 1\\ {d_3}^2 = 1^2 - 2*1*0 + 0^2 + 1^2 - 2*1*1 + 1^2 = 1\\ d 1 2 = 1 2 − 2 ∗ 1 ∗ 0 + 0 2 + 1 2 − 2 ∗ 1 ∗ 0 + 0 2 = 2 d 2 2 = 1 2 − 2 ∗ 1 ∗ 1 + 1 2 + 0 2 − 2 ∗ 1 ∗ 0 + 0 2 = 1 d 3 2 = 1 2 − 2 ∗ 1 ∗ 0 + 0 2 + 1 2 − 2 ∗ 1 ∗ 1 + 1 2 = 1 En remplaçant les valeurs dans l'équation matricielle finale :
( − 1 0 0 − 1 ) ( x y ) = ( − 1 − 1 ) \begin{pmatrix} -1 & 0 \\ 0 & -1 \end{pmatrix} \begin{pmatrix} x \\ y \end{pmatrix} = \begin{pmatrix} -1 \\ -1 \end{pmatrix} ( − 1 0 0 − 1 ) ( x y ) = ( − 1 − 1 ) D'où la solution :
( x y ) = ( 1 1 ) \begin{pmatrix} x \\ y \end{pmatrix} = \begin{pmatrix} 1 \\ 1 \end{pmatrix} ( x y ) = ( 1 1 ) 