Les systèmes de positionnements sont importants en robotique pour un grand nombre d’applications. L’un des projets de 2023 était un système de positionnement basé sur un lidar et un ensemble de balises.

Définition du problème

On peut formuler le problème de positionnement absolu comme la détermination de la position et de l’angle du robot dans un repère global. On représente cette position et cet angle par le tuple $(x_r, y_r, \theta_r)$.

Lidar et suivi de balises

En utilisant notre propre librairie de tracking, nous sommes capable de suivre plusieurs balises en utilisant notre RPLidar A2M8. Comme le suivi de balises est hors du champ de cet article, nous pouvons simplifier et considérer qu’à chaque instant $t$, nous mesurons la distance et l’angle de toutes les balises par rapport au robot.

Calcul de la position du robot par trilatération

Estimation de la position

Nous pouvons calculer la position de notre robot en utilisant le principe de trilatération. Supposons dans un premier temps que la mesure des distance des balises est parfaite et reflète complètement la réalité. Afin de déterminer la position du robot sans ambiguïté, nous avons besoin de 3 balises. Nous les nommons $B_1$, $B_2$ et $B_3$. Nous définissons leurs positions respectives $(x_1, y_1)$, $(x_2, y_2)$ et $$ (x_3, y_3).

Au moment où nous mesurons la distance $d_1$ entre le robot et la balise $B_1$, nous savons que le robot ne peut qu’être autour d’un cercle de centre $(x_1, y_1)$ et de rayon $d_1$. Ajouter $d_2$, la mesure de la distance de la balise $B_2$, nous donne deux positions possibles pour notre robot (les deux intersections des deux cercles de centre $B_1$ et $B_2$). Enfin, avec la distance $d_3$ à la balise $B_3$, la position de notre robot peut être entièrement déterminée (voir schéma).

Rappelons que l’équation d’un cercle de centre $(x_c, y_c)$ et de rayon $r$ est donnée par $r^2 = (x - x_c)^2 + (y - y_c)^2$. Nous avons donc :

\[d_1^2 = (x_r - x_1)^2 + (y_r - y_1)^2\] \[d_2^2 = (x_r - x_2)^2 + (y_r - y_2)^2\] \[d_3^2 = (x_r - x_3)^2 + (y_r - y_3)^2\]

Nous pouvons linéariser ce système en soustrayant les lignes 2 et 3 à la ligne 1 :

\[d_1^2 - d_2^2 = x (-2x_1 + 2x_2) + y (-2y_1 + 2y_2) + x_1^2 + y_1^2 -x_2^2 -y_2^2\] \[d_1^2 - d_3^2 = x (-2x_1 + 2x_3) + y (-2y_1 + 2y_3) + x_1^2 + y_1^2 -x_3^2 -y_3^2\]

Enfin :

\[2 (-x_1 + x_2) x_r + 2 (-y_1 + y_2) y_r = d_1^2 - d_2^2 - x_1^2 - y_1^2 + x_2^2 + y_2^2\] \[2 (-x_1 + x_3) x_r + 2 (-y_1 + y_3) y_r = d_1^2 - d_3^2 - x_1^2 - y_1^2 + x_3^2 + y_3^2\]

Ce qui est solvable avec votre outil numérique favori (en espérant qu’il s’agisse de scipy et non pas de Matlab !).

Prise en compte des imperfections de mesure

Le système présenté précédemment ne peut être résolu que dans des conditions parfaites. Dans la réalité, les mesures de distance sont sujettes à du bruit, ce qui amène à une situation où le système d’équations ci-dessus n’aura pas de solution.

Heureusement, nous pouvons utiliser le concept de pseudo-inverse pour résoudre ce problème. Tout d’abord, rappelons que la résolution du système suivant pour $x_1$ et $x_2$ :

\[a_{1,1} x_1 + a_{1,2} x_2 = b_1\] \[a_{2, 1} x_1 + a_{2, 2} x_2 = b_2\]

Revient à trouver l’inverse de la matrice A dans l’équation suivante :

\[Ax = b\]

Puisque nous pouvons trouver $x$ en connaissant $A^{-1}$:

\[A^{-1}Ax = A^{-1}b\] \[x = A^{-1}b\]

Mais dans notre cas, $A$ n’est peut-être pas inversible puisque nous n’avons peut-être pas de solution… cependant, nous pouvons tout de même trouver sa pseudo-inverse $A^+$ !

La pseudo-inverse d’une matrice est une généralisation de l’inverse ayant certaines propriétés. Nous pouvons notamment l’utiliser pour calculer une approximation de $x$ “suffisamment bonne”, $\hat{x} = A^+b$. L’approximation est “suffisamment bonne” dans le sens où elle minimise le carré de de l’erreur :

\[E_{\hat{x}} = min_{x} || Ax - b ||_2\]

Là encore, $A^+$ peut être calculé à l’aide de votre logiciel favori.

Calcul de l’angle de l’avant du robot

Estimation de l'angle

Une fois la position du robot calculée, on peut facilement calculer l’angle $\theta$ de l’avant du robot. Nous désignons l’angle par rapport à la balise $B_1$ par $\varphi_1$, et le vecteur entre le robot et la balise $B_1$ par $\vec{v}$. Comme $\theta$ est l’angle complet entre $\vec{x}$ et $\vec{v}$ moins l’angle par rapport à la balise, nous avons simplement :

\[\theta = atan2(v_y, v_x) - \varphi_1\]