TL:DR

Vitesses

\[\left\{ \begin{aligned} \dot{\theta} &= \frac{D}{2L}(\dot{\phi_3} - \dot{\phi_2}) \\ \underline{V} &= -\frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\underline{y_1} \\ \end{aligned} \right.\] \[\left\{ \begin{aligned} \dot{\theta} &= \frac{D}{2L}(\dot{\phi_3} - \dot{\phi_2}) \\ V_x &= \frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\sin{\theta} \\ V_y &= -\frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\cos{\theta} \\ \end{aligned} \right.\]

Positions

\(\left\{ \begin{aligned} \theta &= \frac{D}{2L}(\phi_3 - \phi_2) + \theta_0 \\ x &= \int{\frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\sin(\theta)dt} \\ y &= -\int{\frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\cos(\theta)dt} \\ \end{aligned} \right.\)

Implémentation

Forward Euler

\(\left\{ \begin{aligned} \theta &= \frac{D}{2L}(\phi_3 - \phi_2) + \theta_0 \\ x_{n+1} &= x_{n} + \frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\sin(\theta).\Delta t \\ y_{n+1} &= y_{n} -\frac{D}{4}(\dot{\phi_2} + \dot{\phi_3}).\cos(\theta).\Delta t \\ \end{aligned} \right.\)

Sans mesure directe de \(\dot{\phi}_2\) et \(\dot{\phi}_3\) on peut les approximer par respectivement:

\(\frac{\Delta \phi_2}{\Delta T} = \frac{\phi_{2_{n+1}} - \phi_{2_{n}}}{\Delta t}\) et \(\frac{\Delta \phi_3}{\Delta T} = \frac{\phi_{3_{n+1}} - \phi_{3_{n}}}{\Delta t}\).

Il vient:

\[\left\{ \begin{aligned} \theta &= \frac{D}{2L}(\phi_3 - \phi_2) + \theta_0 \\ x_{n+1} &= x_{n} + \frac{D}{4}(\Delta \phi_2 + \Delta \phi_3).\sin(\theta) \\ y_{n+1} &= y_{n} - \frac{D}{4}(\Delta \phi_2 + \Delta \phi_3).\cos(\theta) \\ \end{aligned} \right.\]

Définition

Figures de changement de base

On caractérise le robot par deux paramètres géométriques \(L\) la longueur entre les deux points de contact des roues au sol et \(D\) le diamètre des roues au sol. On cherche à exprimer ses coordonnés dans le référentiel global (Ou pose dans la littérature) \((x,y,\theta)\) en fonction de l’angle des roues motrices \((\phi_2, \phi_3)\).

Kinematics top Kinematics wheels Avec:

  • \[\underline{O_1O_2} = \frac{L}{2}.\underline{x_1}\]
  • \[\underline{O_1O_3} = -\frac{L}{2}.\underline{x_1}\]
  • \[\underline{O_2I_2} = -\frac{D}{2}.\underline{z_1}\]
  • \[\underline{O_3I_3} = -\frac{D}{2}.\underline{z_1}\]

Remarquez que pour conserver une cohérence de notation, les figures de changement de base des roues ont la même normale \(\underline{x_1} = \underline{x_2} = \underline{x_3}\), ainsi si \(\dot{\phi_2} > 0\) et \(\dot{\phi}_3 > 0\), les roues tournent dans le même sens. Cela ne correspondra probablement pas avec la façon dont sont montés vos moteurs sur le robot et donc selon votre montage il vous faudra inverser le signe de \(\phi_2\) ou \(\phi_3\) dans votre programme.

Cinématique

On cherche à exprimer le tenseur cinématique \(\{\nu_{1/0}\}\) en \(O_1\) de résultante \(\Omega_{1/0}\) et de moment \(V_{O_1 \in 1/0}\), la vitesse de \(O_1\) appartenant au robot 1 par rapport à la table 0.
Décomposons les mouvements par relation de Chasles selon (1/2/0) et (1/3/0):

\[\{\nu_{1/0}\}_{0_1} = \{\nu_{1/2}\}_{0_1} + \{\nu_{2/0}\}_{0_1}\quad(1)\] \[\{\nu_{1/0}\}_{0_1} = \{\nu_{1/3}\}_{0_1} + \{\nu_{3/0}\}_{0_1}\quad(2)\]

On remarque la relation \((1) = (2) \iff \{\nu_{1/2}\}_{0_1} + \{\nu_{2/0}\}_{0_1} = \{\nu_{1/3}\}_{0_1} + \{\nu_{3/0}\}_{0_1}\quad(3)\)

Expression de \(\{\nu_{1/2}\}_{0_1}\)

\(O_1\) étant situé sur l’axe de rotation de la roue, \(\underline{V_{0_1 \in 1/2}}\) est nul.
Et à partir des figures de changement de base, on établi par définition \(\underline{\Omega_{1/2}} = -\dot{\phi_2}\underline{x_1}\)

Démonstration

Pour s’en convaincre transportons le moment cinématique là où il est connu, c’est à dire sur le centre de rotation en \(O_2\). Par définition:

\[\underline{V_{0_1 \in 1/2}} \equiv \underline{V_{0_2 \in 1/2}} + \underline{O_1O_2} \wedge \underline{\Omega_{1/2}}\]

Puisque \(O_2\) est le centre de la liaison robot/roue (1/2), \(\underline{V_{O_2 \in 1/2}} = 0\).
Par ailleurs \(\underline{O_1O_2} = \frac{L}{2}\underline{x_1}\) et \(\underline{\Omega_{1/2}}= -\dot{\phi}_2\underline{x_1}\)
Donc \(\underline{O_1O_2} \wedge \underline{\Omega_{1/2}} = 0\).
Finalement \(\underline{V_{0_1 \in 1/2}} = 0\)

\[\{\nu_{1/2}\}_{0_1}= \begin{Bmatrix} V_{0_1\in 1/2}\\ \Omega_{1/2} \end{Bmatrix} = \begin{Bmatrix} 0\\ -\dot{\phi_2}\underline{x_1} \end{Bmatrix}\]

Par analogie, on admet que le résultat est le même pour \(\{\nu_{1/3}\}_{0_1}\) (Suffit de remplacer 2 par 3 …)
Il vient la relation:

\[(3) \Rightarrow \underline{V_{0_1\in1/0}} = \underline{V_{0_1\in2/0}} = \underline{V_{0_1\in3/0}}\quad(4)\]

Hypothèse du Roulement Sans Glissement

L’égalité (4) établie, nous allons pouvoir établir une relation entre les entrées \((\dot{\phi}_2 , \dot{\phi}_3)\) et \(\{\nu_{1/0}\}_{0_1}\)
L’astuce réside dans le transport des moments \(\underline{V_{O_1 \in 2/0}}\) et \(\underline{V_{O_1 \in 3/0}}\) en respectivement \(I_2\) et \(I_3\).
Il vient:

\(\underline{V_{O_1 \in 2/0}} = \underline{V_{I_2 \in 2/0}} + \underline{O_1I_2} \wedge \underline{\Omega_{2/0}}\) Par hypothèse, la roue roule sur la table sans glisser.
On parle alors de roulement sans glissement. Donc on suppose \(\underline{V_{I_2 \in 2/0}} = 0\) i.e le point de contact \(I2\) appartenant à la roue 2 ne se déplace pas par rapport à la table 0.

\[\underline{O_1I_2} \wedge \underline{\Omega_{2/0}} =(\underline{O_1O_2} + \underline{O_2I_2}) \wedge (\underline{\Omega_{2/1}} +\underline{\Omega_{1/0}})\] \[(\frac{L}{2}\underline{x_1} - \frac{D}{2}\underline{z_1}) \wedge (\dot{\phi_2}\underline{x_1} + \dot{\theta}\underline{z_1})\] \[\begin{pmatrix} \frac{L}{2}\\ 0\\ -\frac{D}{2}\\ \end{pmatrix} \wedge \begin{pmatrix} \dot{\phi_2}\\ 0\\ \dot{\theta}\\ \end{pmatrix} = \begin{pmatrix} 0\\ -\frac{D}{2}\dot{\phi_2} - \frac{L}{2}\dot{\theta}\\ 0\\ \end{pmatrix}\]

Ainsi:

\[\underline{V_{0_1\in2/0}} =-(\frac{D}{2}\dot{\phi_2} + \frac{L}{2}\dot{\theta}) \underline{y_1}\quad(5)\]

Notez que par hypothèse, la nature des liaisons entre le robot et la table impose que \(\underline{\Omega_{1/0}} = \dot{\theta}\underline{z_1}\).
On développe le même raisonnement pour \(\underline{V_{0_1\in3/0}}\), seul un signe change lors du développement:

\[\underline{O_1I_3} \wedge \underline{\Omega_{3/0}} =(\underline{O_1O_3} + \underline{O_3I_3}) \wedge (\underline{\Omega_{3/1}} +\underline{\Omega_{1/0}})\] \[(-\frac{L}{2}\underline{x_1} - \frac{D}{2}\underline{z_1}) \wedge (\dot{\phi_3}\underline{x_1} + \dot{\theta}\underline{z_1})\] \[\begin{pmatrix} -\frac{L}{2}\\ 0\\ -\frac{D}{2}\\ \end{pmatrix} \wedge \begin{pmatrix} \dot{\phi_2}\\ 0\\ \dot{\theta}\\ \end{pmatrix} = \begin{pmatrix} 0\\ -\frac{D}{2}\dot{\phi_3} + \frac{L}{2}\dot{\theta}\\ 0\\ \end{pmatrix}\]

Ainsi:

\[\underline{V_{O_1\in3/0}} =(-\frac{D}{2}\dot{\phi_3} + \frac{L}{2}\dot{\theta}) \underline{y_1}\quad(6)\]

Or d’après l’égalité \((4) \Rightarrow (5) = (6)\), donc:

\[-(\frac{D}{2}\dot{\phi_2} + \frac{L}{2}\dot{\theta}) = -\frac{D}{2}\dot{\phi_3} + \frac{L}{2}\dot{\theta}\] \[\dot{\theta} = \frac{D}{2L}(\dot{\phi_3} - \dot{\phi_2})\quad(7)\]

Il suffit alors de réinjecter l’égalité (7) dans (5) ou (6) pour en déduire \(V_{O_1 \in 1/0}\) à partir de l’égalité (4).

\[\underline{V_{O_1\in1/0}}= -\frac{D}{2}\dot{\phi_3} + \frac{L}{2}(\frac{D}{2L}(\dot{\phi_3} - \dot{\phi_2}))\underline{y_1}\] \[\underline{V_{O_1\in1/0}}= -\frac{D}{4}(\dot{\phi_3}+\dot{\phi_2})\underline{y_1}\quad(8)\]

Le tenseur cinématique dans la base (1) en \(O_1\) s’exprime donc ainsi:

\[\{\nu_{1/0}\}_{0_1, \mathcal{B}_1} = \begin{Bmatrix} \underline{V_{O_1\in1/0}}&= -\frac{D}{4}(\dot{\phi_3}+\dot{\phi_2})\underline{y_1}\\ \underline{\Omega_{1/0}} &= \frac{D}{2L}(\dot{\phi_3} - \dot{\phi_2})\underline{z_1} \end{Bmatrix}_{O_1}\]

On notera \(V_{O_1\in1/0}\) simplement \(V\) par la suite.
On projette \(\underline{V}\) dans la base 0.

\[\left\{ \begin{aligned} \dot{\theta} &= \frac{D}{2L}(\dot{\phi_3} - \dot{\phi_2})\\ V_x &= \frac{D}{4}(\dot{\phi_3}+\dot{\phi_2})\sin{\theta}\\ V_y &= -\frac{D}{4}(\dot{\phi_3}+\dot{\phi_2})\cos{\theta}\\ \end{aligned} \right.\]

Intégration

Nous avons l’expression de la vitesse du robot mais pas de sa pose, nous devons donc intégrer le torseur cinématique. N’existant pas de primitive pour l’expression de \(V\), il nous faut calculer son intégrale numériquement.
Il existe de nombreux schémas d’intégration numérique. Les schémas numériques d’intégration présenterait un chapitre à eux seuls, on se limitera ici à développer le plus simple d’entre eux à savoir Forward Euler (Euler Explicite en Français), géométriquement cela correspond à la méthode de calcul d’air des rectangles. On pourrait également citer la méthode de Tustin, Runge-Kutta 4 (ou RK4 pour les intimes, RockStar des schémas d’intégrations pour les simulations et calculs différentiels).

Forward Euler

L’intégration numérique est une forme particulière de résolution d’équation différentielle.
Un problème d’équation différentielle se pose dans sa forme générale de la façon suivante:

\(\underline{\dot{x}} = \underline{f}(\underline{x},\underline{u},t)\) Avec:

  • \(f\) une fonction connue, pouvant être un vecteur ou non
  • \(x\) les états du système pouvant être un vecteur ou non.
  • \(u\) les entrées du système pouvant être un vecteur ou non.
  • \(t\) le temps si le système dépend explicitement du temps.

En particulier dans le cadre d’une simple intégration numérique comme la notre,
si \(V\) correspond à \(\dot{x}\), \(f\) correspond à \(-\frac{D}{4}(\dot{\phi_3}+\dot{\phi_2})\). \(f\) ne dépend ni explicitement du temps,ni d’états \(x\), cela revient à déterminer \(x\) tel que:

\[\dot{x} = f(u) = f(\dot{\phi_2}, \dot{\phi_3})\]

Le schéma d’Euler explicite s’énonce comme suit:

\[x(n+1) = f(u(n))\Delta t + x(n)\]

En appliquant ce schéma à notre problème et en remarquant \(\dot{\theta}\) s’intègre analytiquement:

\[\left\{ \begin{aligned} \theta(n) &= \frac{D}{2L}(\phi_3(n) - \phi_2(n)) + \theta_0\\ x(n+1) &= \frac{D}{4}(\dot{\phi_3} + \dot{\phi_2})\sin({\theta})\Delta t + x(n)\\ y(n+1) &= -\frac{D}{4}(\dot{\phi_3} + \dot{\phi_2})\cos({\theta})\Delta t + y(n)\\ \end{aligned} \quad(9) \right.\]

Vers une reconstitution d’état

Votre système ne comportant probablement pas de capteur de vitesse de rotation des roues mais sans doute des encodeurs/résolveurs, vous ne disposez pas de la mesure de \(\dot{\phi_2}\) et \(\dot{\phi_2}\).
En automatique et dans un cadre plus général, il nous faudrait développer un modèle et ce qu’on appelle un observateur.
Dans notre cas devant la simplicité du problème, on supposera que votre encodeur ait une résolution suffisante pour dériver approximer la dérivée \(\dot{\phi} \approx \frac{\Delta \phi}{\Delta t}\)

Ainsi:

\[\dot{\phi}(n+1) = \frac{\Delta \phi (n)}{\Delta t} = \frac{\phi(n+1) - \phi(n)}{\Delta t}\quad(10)\]

Le schéma devient finalement:

\[\left\{ \begin{aligned} \theta(n) &= \frac{D}{2L}(\phi_3(n) - \phi_2(n)) + \theta_0\\ x(n+1) &= \frac{D}{4}(\Delta \phi_3 + \Delta \phi_2)\sin({\theta}) + x(n)\\ y(n+1) &= -\frac{D}{4}(\Delta \phi_3 + \Delta \phi_2)\cos({\theta}) + y(n)\\ \end{aligned} \quad(9) \right.\]

Notez que nos choix de schémas à la fois pour l’intégration et la dérivation ont conduit à la simplification des termes temporels. Cette simplification bien qu’arrangeante est complètement fortuite et n’aurait pas eu lieu pour tout type de schéma.

Conclusion

Nous avons réussi à obtenir un schéma numérique permettant d’estimer la pose du robot seulement au moyen de technique d’odométrie moyennant les hypothèses suivantes:

  • Roulement sans glissement
  • Terrain plan
  • Zone de contact roue/table assimilable à un point (\(L\) constant / \(O_1\) immobile dans le robot )
  • Résolution d’encodeurs suffisamment fine

Plusieurs autres schémas numériques sont disponibles et peuvent améliorer la précisions mais rien ne remplacera une fusion de données propre avec la mise en place d’un observateur et l’utilisation de d’autres capteurs.