Robot holonome - localisation (partie 2 - les équations)

Dans l’épisode précédent, nous avons montré qu’il était possible de déterminer la vitesse du robot à partir des vitesses de rotation des moteurs. Nous avons obtenu les équations et la matrice permettant de trouver la solution.

Maintenant, il nous reste à appliquer la règle de Cramer pour obtenir la solution. Pour rappel, notre vecteur d’inconnues est le suivant :

$$ X = \left\{ \begin{array}{c} V_{AL}\\ V_{BL}\\ V_{CL}\\ \omega_z\\ \end{array} \right\} $$

 Expression de la rotation

Pour ωz :

$$\omega_z = x_4 = \frac{\det(A_4)}{\det(A)}$$

Nous substituons la 4e colonne par le vecteur $\Lambda$ dans la matrice A :

$$ A_4 = \left\{ \begin{array}{c} \cos(\alpha)& -\cos(\beta) & 0 & \sin(\beta)\times V_{BF} - \sin(\alpha)\times V_{AF} \\ \sin(\alpha)& -\sin(\beta) & 0 & \cos(\alpha)\times V_{AF}-\cos(\beta)\times V_{BF} \\ \cos(\alpha)& 0 & -\cos(\gamma) & \sin(\gamma)\times V_{CF} - \sin(\alpha)\times V_{AF}\\ \sin(\alpha)& 0 & -\sin(\gamma) & \cos(\alpha)\times V_{AF} -\cos(\gamma)\times V_{CF}\\ \end{array} \right\} $$

Puis nous exprimons le déterminant :

$$ \begin{array}{cl} \det(A_4) = & \cos(\gamma)\cos(\alpha)\sin(\beta)\times[\cos(\alpha)V_{AF}-\cos(\gamma)V_{CF}] \\ & -\cos(\gamma)\sin(\alpha)\cos(\beta)\times[\cos(\alpha)V_{AF}-\cos(\gamma)V_{CF}] \\ & +\cos(\gamma)\sin(\alpha)\cos(\beta)\times[\cos(\alpha)V_{AF}-\cos(\beta)V_{BF}] \\ & -\cos(\gamma)\sin(\alpha)\sin(\beta)\times[\sin(\gamma)V_{CF}-\sin(\alpha)V_{AF}] \\ & \\ & -sin(\gamma)\cos(\alpha)\sin(\beta)\times[\sin(\gamma)V_{CF}-\sin(\alpha)V_{AF}] \\ & +sin(\gamma)\sin(\alpha)\cos(\beta)\times[\sin(\gamma)V_{CF}-\sin(\alpha)V_{AF}] \\ & -sin(\gamma)\cos(\alpha)\cos(\beta)\times[\cos(\alpha)V_{AF}-\cos(\beta)V_{BF}] \\ & +sin(\gamma)\cos(\alpha)\sin(\beta)\times[\sin(\beta)V_{BF}-\sin(\alpha)V_{AF}] \end{array} $$

En factorisant par VAF, VBF et VCF :

$$ \begin{array}{ccl} \det(A_4) = & V_{AF} \times[ & \cos(\gamma)\cos(\alpha)\sin(\beta)\cos(\alpha) \\ & & \color{green}-\cos(\gamma)\sin(\alpha)\cos(\beta)\cos(\alpha) \\ & & \color{green}+\cos(\gamma)\sin(\alpha)\cos(\beta)\cos(\alpha) \\ & & +\cos(\gamma)\sin(\alpha)\sin(\beta)\sin(\alpha) \\ & & \color{green}+\sin(\gamma)\sin(\alpha)\cos(\beta)\sin(\alpha) \\ & & -\sin(\gamma)\sin(\alpha)\cos(\beta)\sin(\alpha) \\ & & -\sin(\gamma)\cos(\alpha)\cos(\beta)\cos(\alpha) \\ & & \color{green}-\sin(\gamma)\cos(\alpha)\sin(\beta)\sin(\alpha) ] \\ & +V_{BF}\times[ & -\cos(\gamma)\sin(\alpha)\cos(\beta)\cos(\beta) \\ & & +\sin(\gamma)\cos(\alpha)\cos(\beta)\cos(\beta) \\ & & +\sin(\gamma)\cos(\alpha)\sin(\beta)\sin(\beta)] \\ & +V_{CF}\times[ & \color{green}-\cos(\gamma)\cos(\alpha)\sin(\beta)\cos(\gamma) \\ & & \color{green}+\cos(\gamma)\sin(\alpha)\cos(\beta)\cos(\gamma) \\ & & -\cos(\gamma)\sin(\alpha)\sin(\beta)\sin(\gamma) \\ & & -\sin(\gamma)\cos(\alpha)\sin(\beta)\sin(\gamma) \\ & & +\sin(\gamma)\sin(\alpha)\cos(\beta)\sin(\gamma) ] \end{array} $$

On peut être surpris de voir plus de termes pour certaines vitesses, laissant supposer qu’une vitesse à plus d’impact sur la rotation qu’une autre, mais les lignes en vert s’annulent deux à deux, donnant une formule plus équilibrée, mais pas tout à fait... :

$$ \begin{array}{ccl} \det(A_4) = & V_{AF} \times[ & \cos(\gamma)\cos(\alpha)\sin(\beta)\cos(\alpha) \\ & & +\cos(\gamma)\sin(\alpha)\sin(\beta)\sin(\alpha) \\ & & -\sin(\gamma)\sin(\alpha)\cos(\beta)\sin(\alpha) \\ & & -\sin(\gamma)\cos(\alpha)\cos(\beta)\cos(\alpha) ] \\ & +V_{BF}\times[ & -\cos(\gamma)\sin(\alpha)\cos(\beta)\cos(\beta) \\ & & +\sin(\gamma)\cos(\alpha)\cos(\beta)\cos(\beta) \\ & & +\sin(\gamma)\cos(\alpha)\sin(\beta)\sin(\beta)] \\ & +V_{CF}\times[ & -\cos(\gamma)\sin(\alpha)\sin(\beta)\sin(\gamma) \\ & & -\sin(\gamma)\cos(\alpha)\sin(\beta)\sin(\gamma) \\ & & +\sin(\gamma)\sin(\alpha)\cos(\beta)\sin(\gamma) ] \end{array} $$

Pour la suite, il n’y a pas de grand intérêt à continuer à se trimballer des formules longues comme le bras.

Valeurs utilisées
Angle (°) Angle (rad) sinus cosinus
α 30 π / 6 1/2 $\frac{\sqrt{3}}{2}$
β 150 π 5 / 6 1/2 $-\frac{\sqrt{3}}{2}$
γ 270 π 3 / 2 -1 0

$$\det(A_4) = -\frac{\sqrt{3}}{2} \times V_{AF} - \frac{\sqrt{3}}{2} \times V_{BF} -\frac{\sqrt{3}}{2} \times V_{BF}$$

$$\omega_z = -[V_{AF} + V_{BF} + V_{CF}] \times \frac{\sqrt{3}}{2\times\det(A)}$$

$$\det(A) = \frac{3\sqrt{3}}{2}\times d$$

D’où :

$$\omega_z = -\frac{[V_{AF} + V_{BF} + V_{CF}]}{3\times d} $$

 Vitesse libre de la roue A - VAL

$$V_{AL} = x_1 = \frac{\det(A_1)}{\det(A)}$$

La matrice A1 :

$$ A_1 = \left\{ \begin{array}{c} \sin(\beta)\times V_{BF} - \sin(\alpha)\times V_{AF} & -\cos(\beta) & 0 & d \times [\sin(\alpha)-\sin(\beta)]\\ \cos(\alpha)\times V_{AF}-\cos(\beta)\times V_{BF} & -\sin(\beta) & 0 & d \times [\cos(\beta)-\cos(\alpha)] \\ \sin(\gamma)\times V_{CF} - \sin(\alpha)\times V_{AF} & 0 & -\cos(\gamma)& d \times[\sin(\alpha)-\sin(\gamma)]\\ \cos(\alpha)\times V_{AF} -\cos(\gamma)\times V_{CF}& 0 & -\sin(\gamma) & d \times [\cos(\gamma) -\cos(\alpha)]\\ \end{array} \right\} $$

Le déterminant obtenu en utilisant la première colonne comme "axe" pour le déterminer :

$$ \begin{array}{rl} \det(A_1) = & [\sin(\beta)V_{BF}-\sin(\alpha)V_{AF}]\times -\sin(\beta)\times -\cos(\gamma)\times d[\cos(\gamma)-\cos(\alpha)] \\ & + [\sin(\beta)V_{BF}-\sin(\alpha)V_{AF}]\times -\sin(\beta)\times \sin(\gamma)\times d[\sin(\alpha)-\sin(\gamma)] \\ & - [\cos(\alpha)V_{AF}-\cos(\beta)V_{BF}]\times -\cos(\beta)\times -\cos(\gamma)\times d[\cos(\gamma)-\cos(\alpha)] \\ & - [\cos(\alpha)V_{AF}-\cos(\beta)V_{BF}]\times -\cos(\beta)\times \sin(\gamma)\times d[\sin(\alpha)-\sin(\gamma)] \\ & + [\sin(\gamma)V_{CF}-\sin(\alpha)V_{AF}]\times -\cos(\beta)\times \sin(\gamma)\times d[\cos(\beta)-\cos(\alpha)] \\ & + [\sin(\gamma)V_{CF}-\sin(\alpha)V_{AF}]\times \sin(\beta)\times \sin(\gamma)\times d[\sin(\alpha)-\sin(\beta)] \\ & - [\cos(\alpha)V_{AF}-\cos(\gamma)V_{CF}]\times -\cos(\beta)\times \cos(\gamma)\times d[\cos(\beta)-\cos(\alpha)] \\ & - [\cos(\alpha)V_{AF}-\cos(\gamma)V_{CF}]\times \sin(\beta)\times \cos(\gamma)\times d[\sin(\alpha)-\sin(\beta)] \end{array} $$

Ça commence pas très beau et ça continue en pire. Voici son expression développée. À côté de chaque ligne, nous indiquons la valeur du produit des fonctions trigonométrique (cos et sin) dans notre cas :

$$ \begin{array}{rlr} \det(A_1) = & V_{BF}\times d \times \sin(\beta)\sin(\beta)\cos(\gamma)\cos(\gamma) & 0 \\ & -V_{BF}\times d \times \sin(\beta)\sin(\beta)\cos(\gamma)\cos(\alpha) & 0 \\ & -V_{AF}\times d \times \sin(\alpha)\sin(\beta)\cos(\gamma)\cos(\gamma) & 0 \\ & +V_{AF}\times d \times \sin(\alpha)\sin(\beta)\cos(\gamma)\cos(\alpha) & 0 \\ & \\ & -V_{BF}\times d \times \sin(\beta)\sin(\beta)\sin(\gamma)\sin(\alpha) & 1/8 \\ & +V_{BF}\times d \times \sin(\beta)\sin(\beta)\sin(\gamma)\sin(\gamma) & 1/4 \\ & +V_{AF}\times d \times \sin(\alpha)\sin(\beta)\sin(\gamma)\sin(\alpha) & -1/8 \\ & -V_{AF}\times d \times \sin(\alpha)\sin(\beta)\sin(\gamma)\sin(\gamma) & -1/4 \\ & \\ & -V_{AF}\times d \times \cos(\alpha)\cos(\beta)\cos(\gamma)\cos(\gamma) & 0 \\ & +V_{AF}\times d \times \cos(\alpha)\cos(\beta)\cos(\gamma)\cos(\alpha) & 0 \\ & +V_{BF}\times d \times \cos(\beta)\cos(\beta)\cos(\gamma)\cos(\gamma) & 0 \\ & -V_{BF}\times d \times \cos(\beta)\cos(\beta)\cos(\gamma)\cos(\alpha) & 0 \\ & \\ & +V_{AF}\times d \times \cos(\alpha)\cos(\beta)\sin(\gamma)\sin(\alpha) & 3/8 \\ & -V_{AF}\times d \times \cos(\alpha)\cos(\beta)\sin(\gamma)\sin(\gamma) & 3/4 \\ & -V_{BF}\times d \times \cos(\beta)\cos(\beta)\sin(\gamma)\sin(\alpha) & 3/8 \\ & +V_{BF}\times d \times \cos(\beta)\cos(\beta)\sin(\gamma)\sin(\gamma) & 3/4 \\ & \\ & -V_{CF}\times d \times \sin(\gamma)\cos(\beta)\sin(\gamma)\cos(\beta) & -3/4 \\ & +V_{CF}\times d \times \sin(\gamma)\cos(\beta)\sin(\gamma)\cos(\alpha) & -3/4 \\ & +V_{AF}\times d \times \sin(\alpha)\cos(\beta)\sin(\gamma)\cos(\beta) & -3/8 \\ & -V_{AF}\times d \times \sin(\alpha)\cos(\beta)\sin(\gamma)\cos(\alpha) & -3/8 \\ & \\ & +V_{CF}\times d \times \sin(\gamma)\sin(\beta)\sin(\gamma)\sin(\alpha) & 1/4 \\ & -V_{CF}\times d \times \sin(\gamma)\sin(\beta)\sin(\gamma)\sin(\beta) & -1/4\\ & -V_{AF}\times d \times \sin(\alpha)\sin(\beta)\sin(\gamma)\sin(\alpha) & 1/8\\ & V_{AF}\times d \times \sin(\alpha)\sin(\beta)\sin(\gamma)\sin(\beta) & -1/8\\ & \\ & +V_{AF}\times d \times \cos(\alpha)\cos(\beta)\cos(\gamma)\cos(\beta) & 0 \\ & -V_{AF}\times d \times \cos(\alpha)\cos(\beta)\cos(\gamma)\cos(\alpha) & 0 \\ & -V_{CF}\times d \times \cos(\gamma)\cos(\beta)\cos(\gamma)\cos(\beta) & 0 \\ & +V_{CF}\times d \times \cos(\gamma)\cos(\beta)\cos(\gamma)\cos(\alpha) & 0 \\ & \\ & +V_{AF}\times d \times \cos(\alpha)\sin(\beta)\cos(\gamma)\sin(\alpha) & 0 \\ & -V_{AF}\times d \times \cos(\alpha)\sin(\beta)\cos(\gamma)\sin(\beta) & 0 \\ & -V_{CF}\times d \times \cos(\gamma)\sin(\beta)\cos(\gamma)\sin(\alpha) & 0 \\ & +V_{CF}\times d \times \cos(\gamma)\sin(\beta)\cos(\gamma)\sin(\beta) & 0 \\ \end{array} $$

Ce qui nous permet de déterminer une forme simple du déterminant applicable à notre cas :

$$ \det(A_1) = (V_{BF}-V_{CF})\times d \times \frac{3}{2} $$

Et d’en déduire la vitesse VAL :

$$V_{AL} = \frac{\det(A_1)}{\det(A)} = (V_{BF}-V_{CF})\times d \times \frac{3}{2} \times \frac{2}{3\sqrt{3}\times d}$$


$$V_{AL} = \frac{(V_{BF}-V_{CF})}{\sqrt{3}}$$

 Vitesse du point O

Nous avons enfin tous les termes pour définir la vitesse au point O, le centre du robot. Rappelons que nous exprimions la vitesse du point O en déplaçant le vecteur cinématique du point A :

$$\vec{V}_{OA} = \left\{\begin{array}{ccc} \sin(\alpha)\times V_{AF} + \cos(\alpha)\times V_{AL} &+& \omega_z \times \sin(\alpha)\times d \\ -\cos(\alpha)\times V_{AF} + \sin(\alpha)\times V_{AL} &+& -\omega_z \times \cos(\alpha)\times d \\ 0 \end{array} \right\}$$

Ce qui, en remplaçant les fonctions trigonométriques par les valeurs de notre robot et en utilisant les expressions trouvées pour VAL et ωz, donne :

$$ \vec{V}_{OA} = \left\{\begin{array}{ccc} \frac12\times V_{AF} + \frac{\sqrt{3}}{2}\times \frac{(V_{BF}-V_{CF})}{\sqrt{3}} -\frac{[V_{AF} + V_{BF} + V_{BF}]}{3\times d} \times \frac12 \times d \\ -\frac{\sqrt{3}}{2}\times V_{AF} + \frac12 \times \frac{(V_{BF}-V_{CF})}{\sqrt{3}} +\frac{[V_{AF} + V_{BF} + V_{BF}]}{3\times d} \times \frac{\sqrt{3}}{2}\times d \\ 0 \end{array} \right\} $$

Qui se simplifie en :

$$\vec{V}_{OA} = \left\{\begin{array}{c} \frac13\times V_{AF} + \frac13\times V_{BF} - \frac23\times V_{CF}\\ -\frac{\sqrt{3}}{3}\times V_{AF} +\frac{\sqrt{3}}{3}\times V_{AB} \\ 0 \end{array} \right\} $$

 Conclusion

Avec ces équations nous pouvons estimer la position du robot grâce à la rotation de ses roues.

Grace aux formules de ωz et VOA, nous allons pouvoir trouver nos lois de commande du robot, mais ce sera l’objet d’un prochain article !

Note sur les simplifications : une longue route pour des formules finales simples. Ce qui amène une petite question, est-ce que notre cas particulier (120° entre chaque roue, le cosinus de γ nul et des roues équidistantes du centre) simplifie énormément les équations où existe-t-il une solution plus simple ?

Commentaires

Il n'y a pas de commentaires

Ajouter un commentaire

Pseudo :
Mail :

Texte :

Copyright "POIVRON" 2011-2021, tous droits réservés
Administration du site