Triangulation

29 octobre 2023

Voici l’étude d’une localisation en se basant sur les angles que mesure un robot par rapport à des points connus (balises).

 Les principes

La logique se base sur deux principes mathématiques :

• La somme des angles d’un triangle vaut 180°
• L’équation d’une droite dans le plan

 Convention pour les angles

Nous utiliserons le sens conventionnel pour les angles, comme indiqué dans l’image ci-dessous

Signe des angles

 La somme des angles d’un triangle

 L’équation d’une droite

Un point $M$ de coordonnées $(x, y)$ décrit une droite s’il répond à certaines équations.

La première équation qui vient à l’esprit est généralement :

$$ y = ax+b$$

Cette équation est imparfaite dans la mesure où elle ne permet pas de représenter une droite verticale. Nous lui préférons généralement cette forme :

$$ ax + by + c = 0 $$

Mais dans notre cas, nous allons vouloir représenter des droites qui ont un certain angle $\alpha$ et qui passent par un point connu (notre balise - de coordonnées $(P_x, P_y)$).

Équation d’une droite

Travailler avec l’équation ci-dessus devrait simplifier la démarche :

$$ \left\{ \begin{array}{c} x = P_x + cos(\alpha) \times t\\ y = P_y + sin(\alpha) \times t\\ \end{array} \right. $$

 La logique

 Intersections de droites

Notre point est à l’intersection de 3 droites, passant chacune par une balises et par le robot.

Notons $\alpha$, l’angle de la droite qui passe par la balise $A$ et notre robot. Puis $\beta$ et $\gamma$, les angles des droites qui passent respectivement par la balise $B$ et le robot et la balise $C$ et le robot.

Angles recherchés

Notre robot est le point qui appartient à ces trois droites, il faut donc résoudre les équations :

$$ \left\{ \begin{array}{c} x = A_x + cos(\alpha) \times t_a\\ y = A_y + sin(\alpha) \times t_a\\ \\ x = B_x + cos(\beta) \times t_b\\ y = B_y + sin(\beta) \times t_b\\ \\ x = C_x + cos(\gamma) \times t_c\\ y = C_y + sin(\gamma) \times t_c\\ \end{array} \right. $$

Ce qui nous fait un total de 8 inconnues : $x$, $y$, $\alpha$, $\beta$, $\gamma$, $t_a$, $t_b$, $t_c$, pour 6 équations indépendantes. Pour trouver les deux équations restantes, nous allons travailler sur les angles

 Réduction des inconnues par les angles

Notre robot mesure l’angle entre les balises $A$ et $B$ et les balises $A$ et $C$. Notons les $\theta_1$ et $\theta_2$.

Angles mesurés

Sachant que la somme des angles d’un triangle vaut 180°, nous pouvons exprimer $\beta$ et $\gamma$ en fonction d’$\alpha$ et de données connues.

Exprimons les angles $\alpha$, $\beta$ et $\gamma$ en fonction d’angles connus :

$$ \left\{ \begin{array}{c} \alpha = \widehat{A_1} - \widehat{RAB} \\ \beta = \widehat{B} + \widehat{RBA} \\ \gamma = \widehat{C} - \widehat{ACR} \\ \end{array} \right. $$

Réductions des inconnues (1)

On peut exprimer tous ces angles en fonction d’un seul angle inconnu, choisissons $\widehat{RAB}$.

Considérons le triangle ABR, pour exprimer $\beta$ en fonction de $\widehat{RAB}$ et des angles connus :

Triangle ABR

$$\beta = \widehat{B} + (180 - \theta_1 - \widehat{RAB}) $$

Considérons le triangle ACR, pour exprimer $\gamma$ en fonction de $\widehat{RAB}$ et des angles connus :

Triangle ACR

$$ \begin{array}{l} \gamma = \widehat{C} - (180 - \theta_2 - \widehat{CAR}) \\ \gamma = \widehat{C} - (180 - \theta_2 - (\widehat{A_1} - \widehat{A_2} - \widehat{RAB}))\\ \gamma = \widehat{C} - 180 + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB} \end{array} $$

Nous obtenons alors les équations suivantes :

$$ \left\{ \begin{array}{lc} x = A_x + cos(\widehat{A_1} - \widehat{RAB}) \times t_a & (1)\\ y = A_y + sin(\widehat{A_1} - \widehat{RAB}) \times t_a & (2)\\ \\ x = B_x + cos(\widehat{B} + (180 - \theta_1 - \widehat{RAB})) \times t_b & (3)\\ y = B_y + sin(\widehat{B} + (180 - \theta_1 - \widehat{RAB})) \times t_b & (4)\\ \\ x = C_x + cos(\widehat{C} - 180 + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times t_c & (5)\\ y = C_y + sin(\widehat{C} - 180 + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times t_c & (6)\\ \end{array} \right. $$

Ce qui nous fait 6 inconnues $x$, $y$, $\widehat{RAB}$, $t_a$, $t_b$, $t_c$ pour 6 équation indépendantes. Le tour parait joué, "y’a plus qu’à" résoudre le système, et pourquoi pas avec une méthode de Cramer ?

La règle de Cramer ne s’applique qu’aux équations linéaires ! Or ici, nous avons des inconnues dans les fonctions sinus et cosinus.

 Résolution

La résolution analytique ne semble pas bien partie, mais essayons quand même de résoudre ce système d’équation au maximum, pour dépendre d’un minimum d’inconnues.

Exprimons $t_b$ et $t_c$ en fonction de $t_a$ à l’aide des équations (1) et (3) puis (1) et (5).

$$ t_b = \frac{A_x - B_x + \cos(\widehat{A_1} - \widehat{RAB}) \times t_a}{ \cos(\widehat{B} + 180 - \theta_1 - \widehat{RAB})} $$

$$ t_c = \frac{A_x - C_x + \cos(\widehat{A_1} - \widehat{RAB}) \times t_a}{cos(\widehat{C} - 180 + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB})} $$

Maintenant, utilisons ces expressions de $t_b$ et $t_c$ dans les équations en y, (4) et (6)

$$ \left\{ \begin{array}{lc} y = B_y + sin(\widehat{B} + (180 - \theta_1 - \widehat{RAB})) \times \frac{A_x - B_x + \cos(\widehat{A_1} - \widehat{RAB}) \times t_a}{ \cos(\widehat{B} + (180 - \theta_1 - \widehat{RAB})} \\ y = C_y + sin(\widehat{C} - 180 + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times \frac{A_x - C_x + \cos(\widehat{A_1} - \widehat{RAB}) \times t_a}{cos(\widehat{C} - 180 + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB})} \\ \end{array} \right. $$

Sachant que $\tan(x) = \frac{\sin(x)}{\cos(x)}$ et que $\tan(x+180) = \tan(x-180) = \tan(x)$, nous pouvons simplifier légèrement l’écriture.

$$ \left\{ \begin{array}{lc} y = B_y + \tan(\widehat{B} -\theta_1 - \widehat{RAB}) \times [A_x - B_x + \cos(\widehat{A_1} - \widehat{RAB}) \times t_a] \\ y = C_y + \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times [A_x - C_x + \cos(\widehat{A_1} - \widehat{RAB}) \times t_a] \\ \end{array} \right. $$

À partir de là, comme $y=y$ nous pouvons exprimer $t_a$ en fonction de $\widehat{RAB}$.

$$ \begin{array}{rl} C_y - B_y = & \tan(\widehat{B} -\theta_1 - \widehat{RAB}) \times [A_x - B_x]\\ & - \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times [A_x - C_x] \\ & + \tan(\widehat{B} -\theta_1 - \widehat{RAB}) \times \cos(\widehat{A_1} - \widehat{RAB}) \times t_a \\ & - \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times \cos(\widehat{A_1} - \widehat{RAB}) \times t_a \end{array} $$

En isolant les termes contenant $t_a$ :

$$ \begin{array}{rcl} C_y - B_y - \tan(\widehat{B} -\theta_1 - \widehat{RAB}) \times [A_x - B_x] \\ + \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times [A_x - C_x] & = & [\tan(\widehat{B} -\theta_1 - \widehat{RAB}) - \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB})] \\ & & \times \cos(\widehat{A_1} - \widehat{RAB}) \times t_a \end{array} $$

Soit :

$$ t_a = \frac{C_y - B_y - \tan(\widehat{B} -\theta_1 - \widehat{RAB}) \times [A_x - B_x] + \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB}) \times [A_x - C_x] }{ [\tan(\widehat{B} -\theta_1 - \widehat{RAB}) - \tan(\widehat{C} + \theta_2 + \widehat{A_1} - \widehat{A_2} - \widehat{RAB})] \times \cos(\widehat{A_1} - \widehat{RAB})} $$

 Solveur

Maintenant, nous avons toutes les équations, mais en fonction de l’angle $\widehat{RAB}$ choisi, nous trouvons plusieurs positions pour le robot en fonction des équations utilisées. La "bonne" valeur de $\widehat{RAB}$ est celle qui donne le même point qu’il soit calculé avec les équations (1) et (2) ou (3) et (4). Il faut maintenant travailler sur une résolution rapide de ce problème.

Voici un tableur, Calculs de triangulation, pour visualiser les calculs et les résultats.

Commentaires

• Suivez les commentaires de cet article par flux RSS

Ajouter un commentaire