4 septembre 2022
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\} $$
Et la règle de Cramer indique que, dans le cas où la matrice A est inversible, les solutions sont :
$$ x_k = \frac{\det(A_k)}{\det(A)} $$
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.
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} $$
$$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}}$$
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_{BF} \\ 0 \end{array} \right\} $$
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 ?