Dynamique d'un robot non holonomique à entrainement différentiel.
Modèle dynamique d’un robot non holonomique (Differential drive)
Motivations
On cherche ici à développer un modèle dynamique du robot dans une optique de développement d’un estimateur d’état à l’aide de la mécanique Lagrangienne et le module
de calcul symbolique de Python (Sympy).
De plus, un tel modèle peut servir à réaliser, dans une certaine mesure, du Software In the Loop (SIL), c’est-à-dire à simuler la physique du robot en cohabitation avec le firmware du contrôleur, ce qui facilite et avance les campagnes de test. À cette fin nous avons au préalable essayé de simuler le comportement du robot
via des logiciels dédiés tels que Webots, seulement malgré la sophistication et les fonctionnalités que peuvent offrir ce type de logiciel,
il nous a semblé que la physique des roulements assez mal représentée et/ou difficile à régler.
Après plusieurs essais et des résultats alarmants, nous en avons conclu que nous avions intérêt à développer notre propre modèle, qui servirait de toute façon pour
notre estimateur
Enfin le développement d’un modèle physique et son exploitation dans le développement d’un controlleur sont des compétences importantes
pour un ingénieur qui nous semblent important de développer.
On reprendra en partie les résultats obtenus dans l’article Cinématique d’un robot de classe unicycle.
Mécanique Lagrangienne
Un peu d’histoire
Il est pertinent de prendre un bref moment pour explorer l’origine de la mécanique Lagrangienne afin de mieux comprendre l’intuition sous-jacente
aux relations que nous utiliserons par la suite. De plus, le principe fondamental de la loi de Lagrange a une portée qui s’étend à l’ensemble
de la physique, permettant ainsi de relier différents domaines de cette discipline. Je m’appuie ici sur le livre
Les principes variationnels en physique de Jean-Louis Basdevant.
À partir du XVIIème siècle, l’idée à commencer à émerger dans l’esprit des philosophes et physiciens l’idée selon laquelle le monde est régit
selon des critères d’optimalité. Ainsi de Leibniz naquit l'optimisme, courant de pensée selon lequel parmi tous les mondes possibles et
imaginables, notre monde est le fruit d’une machination divine, et que le chemin qu’il suit est le meilleur au regard du créateur.
Diderot résumait dans son Encyclopédie, la pensée Leibnizienne :
Il avait encore sur la physique générale une idée particulière: c’est que Dieu a fait avec la plus grande économie possible ce qu’il y avait de plus parfait et de meilleur; il est le fondateur de l’optimisme, ou de ce système qui semble faire de Dieu un automate dans ses décrets et dans ses actions […] Cependant comme il y a une infinité de combinaisons et de mondes possibles dans les idées de Dieu, et que de ces mondes il n’en peut exister qu’un, faut qu’il y ait une certaine raison suffisante de son choix: or cette raison ne peut être que dans le différent degré de perfection; d’où il s’ensuit que le monde qui est, est le plus parfait. Dieu l’a choisi dans sa sagesse, connu dans sa bonté, produit dans la plénitude de sa puissance.
C’est de ces idées dont se moque Voltaire dans sa célèbre satire Candide et son acide Tout est pour le mieux dans le meilleur des mondes.
La genèse de l’optimisme et des principes variationnels dans les sciences physiques remonte aux travaux de Pierre Fermat (1601-1665) qui irrité
par le manque de rigueur mathématique de René Descartes dans sa démonstration des lois de la réfraction, redémontra proprement les résultats
de Descartes, et en déduira un principe plus général dit des moindres temps.
Je profite de ce paragraphe pour rendre hommage à mon professeur de PCSI M. Colson qui nous avait subtilement introduit ce principe au hasard d’un exercice de DS, dans lequel un maître nageur au maillot de bain bleu (mais c’est un détail), cherchait le chemin le plus court pour sauver une victime de la noyade. Considérant une vitesse \(v_1\) sur le sable et \(v_2\) dans la mer, nous redécouvrions sous nos yeux ébaillis la fameuse loi de la réfraction \(v_1.sin(i_1) = v_2.sin(i_2)\).
En 1744 Maupertuis, physicien Français importateur des théories de Newton en France, introduisait le principe de moindre quantité d'action.
En reprenant les travaux de Fermat, il est parvenu retrouver les équations de Newton dans certaines conditions, à partir d’un problème
d’optimisation d’une quantité qu’il nomma l'Action.
Je passe ici plusieurs exemples mais vous devriez commencer à saisir où nous voulons en venir, ces domaines à priori bien distincts
(l’optique géométrique et la mécanique, mais également la thermodynamique, l’électronique …) ont en commun qu’ils peuvent être exprimés comme
un problème d’optimalité. Il s’agit à chaque fois de mimiser une quantité exprimée sous la forme d’une intégrale et qui pour ce faire,
nécessite de considérer l’ensemble des chemins possibles, et de choisir celui qui minimise notre quantité i.e choisir le meilleur, le plus
parfait des mondes cf Leibniz.
Ce principe a depuis été rafiné succéssivement par Leonhard Euler (1707-1783), Joseph-Louis Lagrange (1736-1813) et
William R. Hamilton (1805-1865).
La physique moderne utilise désormait ce principe sous la forme énoncée par Lagrange ou Hamilton, aussi on trouvera aussi bien des cours de
mécanique Lagrangienne, Hamiltonienne ou de mécanique dite analytique (formulation plus générale). Pour la suite on s’intéressera particulièrement
à la mécanique Lagrangienne, non pas par chauvinisme, mais il semble que bien qu’elle soit antérieure, la mécanique Lagrangienne convienne mieux à
l’ingénieur que son pendant Irlandais qui semble plus adapté à la mécanique céleste.
Démonstration avec les mains
Je m’attache à présenter dans cette section des éléments de réflexions qui permettent d’aboutir aux équations d’Euler-Lagrange tout en se permettant volontairement une certaine légèreté sur la rigueur mathématique. Cela permet malgré tout de saisir les grands principes de cette démonstration et je m’éforcerai plus tard de la développer de façon plus rigoureuse.
Le problème se pose donc sous la forme d’un calcul variationel.
\[I(y) = \int_A^B{\mathcal{L}{(y,\dot{y},x)}}dx\]\(I\) est ce que l’on appelle une fonctionnelle, c’est à dire une application qui prend comme paramètre non pas un scalaire mais une fonction et retourne un scalaire,
c’est la quantité à minimiser.
On suppose que \(y\) existe et minimise \(I\) et on pose \(\tilde{y}(x) = y(x) + \epsilon.\eta(x)\) avec \(\epsilon \in \mathcal{R}\) et \(\eta\) une
fonction arbitraire doublement dérivable qui respecte les conditions aux limites \(\eta(A)=\eta(B)=0\).
On construit donc de cette façon une famille de variation par rapport à la solution optimale \(y\).
Puisque \(\tilde{y}\) est définie pour une fonction \(y\) spécifique (càd la fonction particulière qui minimise la fonctionnelle), et une fonction \(\eta\)
arbitrairement donnée, alors \(I(\tilde{y})\) devient une simple fonction de \(\epsilon\), \(I(\epsilon)\). Or puisque par définition, \(y\) minimise \(I\), alors
\(y\) est un extremum.
Ainsi \(\frac{dI}{d\epsilon}\rvert_{\epsilon=0}=0\)
Plusieurs commentaires, tout d’abord remarquez qu’on insiste bien sur le fait que l’on applique la dérivée de la fonctionnelle par rapport à \(\epsilon\) en 0 car
on se place sur son extremum, c’est à dire là où sa dérivée s’annule (D’où l’égalité nulle). Par ailleurs \(L\) est une fonction composée multivariable, donc pour
calculer sa dérivée nous allons devoir utiliser la règle de dérivation en chaine.
Par ailleurs, l’intégrale portant sur \(x\), on peut passer l’opérateur dérivée dans l’intégrande.
On calcule donc \(\frac{dL}{d\epsilon}\) en utilisant la règle de dérivation en chaine.
Donc:
\[\frac{dL}{d\epsilon}(\tilde{y},\dot{\tilde{y}},x) = \frac{\partial L}{\partial \tilde{y}}\eta(x) + \frac{\partial L}{\partial \dot{\tilde{y}}}\dot{\eta}(x)\] \[\frac{dL}{d\epsilon}\rvert_{\epsilon = 0} (\tilde{y},\dot{\tilde{y}},x) = \frac{\partial L}{\partial y}\eta(x) + \frac{\partial L}{\partial \dot{y}}\dot{\eta}(x)\]Ainsi:
\[\int_A^B{\frac{\partial L}{\partial y}\eta(x) + \frac{\partial L}{\partial \dot{y}}\dot{\eta}(x)dx} = 0\]Accrochez vous, nous sommes presques arrivés, il reste deux étapes avant de développer les célèbres équations d’Euler-Lagrange.
Il faut tout d’abord user d’une ruse et intégrer par partie le 2nd terme de l’intégrande en posant \(u(x) = \frac{\partial L}{\partial \dot{y}}\) et \(\dot{v} = \dot{\eta}(x)\).
Par définition \(\eta(A) = \eta(B) = 0\) Donc \([\frac{\partial L}{\partial \dot{y}}\eta(x)]_A^B = 0\)
\[\int_A^B{\frac{\partial L}{\partial y}\eta(x) - \frac{d}{dt}\frac{\partial L}{\partial \dot{y}}\eta(x)dx} = 0\] \[\int_A^B{(\frac{\partial L}{\partial y} - \frac{d}{dt}\frac{\partial L}{\partial \dot{y}})\eta(x)dx} = 0\]La dernière étape consiste à utiliser le lemme fondamental du calcul variationnel qui nous permet d’écrire que pour toute fonction \(f\) de classe \(C^k\) sur
l’interval \([A,B]\) telle que \(\int_A^Bf(x)h(x)dx=0\) pour toute fonction \(h\) de classe \(C^k\) sur \([A,B]\) avec \(h(A)=h(B)=0\), alors
\(f\) est identiquement nulle sur l’inteval \([A,B]\).
Donc:
S’arrête ici le travail du mathématicien philosophe, et commence celui du physicien dans le choix de la fonction à minimiser.
Les travaux de Lagrange montrent que le choix de \(L\) comme la différence entre l’énergie cinétique et l’énergie potentielle \(L = E - V\) permettent non seulement
de retrouver les lois de Newton, mais également nous le vérons d’introduire nativement des contraintes dans les équations.
Nous avons donc fait apparaître de façon peu rigoureuse, l’intuition derrière les équations d’Euler-Lagrange, il est normal à ce stade de lecture, de ne pas pouvoir
tout à fait saisir ni l’intérêt ni comment se servir de ces équations dans un cas pratique, néanmoins ce développement était nécessaire pour donner une intuition
derrière ces calculs.
Coordonnés généralisés
En mécanique analytique, les \(n\) variables supposées indépendantes permettant de caractériser entièrement la pose (position et orientation) d’un système mécanique
s’appellent les coordonnées généralisées regroupées dans un vecteur usuellement dénoté \(q\).
Dans notre cas, le système est composé de trois solides, le robot 1 et les roues (2 et 3).
Nous faisons le choix de prendre en compte entièrement ou en partie de la dynamique des roues dans cette étude, il nous faudra donc également
introduire des coordonnées généralisées pour caractériser la configuration des roues. Nous aurions pu également retirer complètement les roues de l’étude
et supposer que leur contribution se résume à une force selon l’axe \(y_1\) et un couple selon l’axe \(z_1\).
On introduit donc les coordonnées généralisées suivantes et leur dérivées respectives.
\[q = \left(\begin{matrix}x\\y\\\theta\\\phi_{2}\\\phi_{3}\end{matrix}\right) \quad \dot{q}= \left(\begin{matrix}\dot{x}\\\dot{y}\\\dot{\theta}\\\dot{\phi}_{2}\\\dot{\phi}_{3}\end{matrix}\right)\]Lagrangien
\[L = \frac{J \dot{\theta}^{2}}{2} + \frac{m \left(\dot{x}^{2} + \dot{y}^{2}\right)}{2}\]On remarque le lagrangien ne dépend pas des paramètres des roues que l’on néglige pour l’instant.
Mais il suffirait d’ajouter la masse et l’inertie des roues pour les prendre en compte, ce qui complexifierait grandement les équations du mouvement.
Par ailleurs, cela ne nous empêche pas d’intégrer les angles des roues comme coordonnées généralisées.
Équations du mouvement
On admet ici qu’un système comprenant \(n\) coordonnées généralisées soumis à \(n\) efforts extérieurs (ceux-ci pouvant être tous ou en partie nuls) et à \(p\) contraintes non holonomiques (c’est à dire qui ne peuvent pas s’exprimer sous la forme \(f(q)=0\)) dérivent de la forme suivante:
\[\begin{align*} \frac{d}{dt}(\frac{\partial L}{\partial \dot{q_i}}) - \frac{\partial L}{\partial q_i} &= A_{ij}(q)\lambda_j + fe_i\\ \nonumber\\ M(q).\ddot{q} &= A(q).\Lambda + f_e\\ \nonumber\\ \left(\begin{matrix}m \ddot{x}\\m \ddot{y}\\J \ddot{\theta}\\0\\0\end{matrix}\right)&=\left(\begin{matrix}- \alpha_{t} \dot{x} + \lambda_{1}\\- \alpha_{t} \dot{y} + \lambda_{2}\\- \alpha_{r} \dot{\theta} + \lambda_{3}\\- \frac{D \lambda_{1} \sin{\left(\theta \right)}}{4} + \frac{D \lambda_{2} \cos{\left(\theta \right)}}{4} + \frac{D \lambda_{3}}{2 L} + T_{2} - \rho \dot{\phi}_{2}\\- \frac{D \lambda_{1} \sin{\left(\theta \right)}}{4} + \frac{D \lambda_{2} \cos{\left(\theta \right)}}{4} - \frac{D \lambda_{3}}{2 L} + T_{3} - \rho \dot{\phi}_{3}\end{matrix}\right) \end{align*}\]Les \(\lambda_i\) traduisents les forces des ‘\(p\)’ contraintes et doivent encore être déterminées. On cherche à exprimer les contraintes en fonction des \(q\) et \(\dot{q}\). Pour cela on dérive les équations de contraintes \(S(q).\dot{q} = 0\), il vient:
\[\dot{S}(q)\dot{q} + S(q).\ddot{q} = 0\]On obtient ainsi ‘\(p\)’ équations, que l’on ajoute aux \(n\) équations aux équations du mouvement pour former le système étendu.
Définition du système étendu
\[\tilde{M}\ddot{\tilde{q}} = \tilde{f_e} \quad \text{avec } \tilde{M}=\begin{pmatrix}M(q)&-A(q)\\S(q)&0\end{pmatrix} \quad \ddot{\tilde{q}}=\begin{pmatrix}\ddot{q}\\ \Lambda\end{pmatrix} \quad \tilde{f_e} = \begin{pmatrix}f_e\\-\dot{S}(q)\dot{q}\end{pmatrix}\] \[\begin{align*} M(q) &= \left(\begin{matrix}m & 0 & 0 & 0 & 0\\0 & m & 0 & 0 & 0\\0 & 0 & J & 0 & 0\\0 & 0 & 0 & 0 & 0\\0 & 0 & 0 & 0 & 0\end{matrix}\right)\\ \nonumber\\ S(q) = A^\intercal (q) &= \left(\begin{matrix}1 & 0 & 0 & - \frac{D \sin{\left(\theta \right)}}{4} & - \frac{D \sin{\left(\theta \right)}}{4}\\0 & 1 & 0 & \frac{D \cos{\left(\theta \right)}}{4} & \frac{D \cos{\left(\theta \right)}}{4}\\0 & 0 & 1 & \frac{D}{2 L} & - \frac{D}{2 L}\end{matrix}\right)\\ \nonumber\\ f_e &= \left(\begin{matrix}- \alpha_{t} \dot{x}\\- \alpha_{t} \dot{y}\\- \alpha_{r} \dot{\theta}\\T_{2} - \rho \dot{\phi}_{2}\\T_{3} - \rho \dot{\phi}_{3}\end{matrix}\right) \end{align*}\]On introduit des frottements fluides dits de translation et rotation \(\alpha_t\), \(\alpha_r\) et un frottement sans glissement au roulement \(\rho\). Ces valeurs sont à déterminer expérimentalement. Par la suite on considèrera arbitrairement des frottements fluide de l’ordre de 1e-4 (USI) et de 1e-3 (USI) pour le roulement.
Résolution du système étendu
\[\ddot{\tilde{q}} = \tilde{M}^{-1}.\tilde{f_e}\] \[\begin{align*} \left(\begin{matrix}\ddot{x}\\\ddot{y}\\\ddot{\theta}\\\ddot{\phi}_{2}\\\ddot{\phi}_{3}\end{matrix}\right) &= \left(\begin{matrix}\frac{D^{2} m \cos{\left(\theta \right)} \dot{\phi}_{2} \dot{\theta} + D^{2} m \cos{\left(\theta \right)} \dot{\phi}_{3} \dot{\theta} - 4 D \alpha_{t} \sin^{2}{\left(\theta \right)} \dot{x} + 4 D \alpha_{t} \sin{\left(\theta \right)} \cos{\left(\theta \right)} \dot{y} + 8 T_{2} \sin{\left(\theta \right)} + 8 T_{3} \sin{\left(\theta \right)} - 8 \rho \sin{\left(\theta \right)} \dot{\phi}_{2} - 8 \rho \sin{\left(\theta \right)} \dot{\phi}_{3}}{4 D m}\\\frac{D^{2} m \sin{\left(\theta \right)} \dot{\phi}_{2} \dot{\theta} + D^{2} m \sin{\left(\theta \right)} \dot{\phi}_{3} \dot{\theta} + 4 D \alpha_{t} \sin{\left(\theta \right)} \cos{\left(\theta \right)} \dot{x} - 4 D \alpha_{t} \cos^{2}{\left(\theta \right)} \dot{y} - 8 T_{2} \cos{\left(\theta \right)} - 8 T_{3} \cos{\left(\theta \right)} + 8 \rho \cos{\left(\theta \right)} \dot{\phi}_{2} + 8 \rho \cos{\left(\theta \right)} \dot{\phi}_{3}}{4 D m}\\\frac{- D \alpha_{r} \dot{\theta} - L T_{2} + L T_{3} + L \rho \dot{\phi}_{2} - L \rho \dot{\phi}_{3}}{D J}\\\frac{- 2 D J \alpha_{t} \sin{\left(\theta \right)} \dot{x} + 2 D J \alpha_{t} \cos{\left(\theta \right)} \dot{y} + D L \alpha_{r} m \dot{\theta} + 4 J T_{2} + 4 J T_{3} - 4 J \rho \dot{\phi}_{2} - 4 J \rho \dot{\phi}_{3} + L^{2} T_{2} m - L^{2} T_{3} m - L^{2} m \rho \dot{\phi}_{2} + L^{2} m \rho \dot{\phi}_{3}}{D^{2} J m}\\\frac{- 2 D J \alpha_{t} \sin{\left(\theta \right)} \dot{x} + 2 D J \alpha_{t} \cos{\left(\theta \right)} \dot{y} - D L \alpha_{r} m \dot{\theta} + 4 J T_{2} + 4 J T_{3} - 4 J \rho \dot{\phi}_{2} - 4 J \rho \dot{\phi}_{3} - L^{2} T_{2} m + L^{2} T_{3} m + L^{2} m \rho \dot{\phi}_{2} - L^{2} m \rho \dot{\phi}_{3}}{D^{2} J m}\end{matrix}\right)\\ \nonumber\\ \Lambda &= \left(\begin{matrix}\frac{D^{2} m \cos{\left(\theta \right)} \dot{\phi}_{2} \dot{\theta} + D^{2} m \cos{\left(\theta \right)} \dot{\phi}_{3} \dot{\theta} + 4 D \alpha_{t} \sin{\left(\theta \right)} \cos{\left(\theta \right)} \dot{y} + 4 D \alpha_{t} \cos^{2}{\left(\theta \right)} \dot{x} + 8 T_{2} \sin{\left(\theta \right)} + 8 T_{3} \sin{\left(\theta \right)} - 8 \rho \sin{\left(\theta \right)} \dot{\phi}_{2} - 8 \rho \sin{\left(\theta \right)} \dot{\phi}_{3}}{4 D}\\\frac{D^{2} m \sin{\left(\theta \right)} \dot{\phi}_{2} \dot{\theta} + D^{2} m \sin{\left(\theta \right)} \dot{\phi}_{3} \dot{\theta} + 4 D \alpha_{t} \sin^{2}{\left(\theta \right)} \dot{y} + 4 D \alpha_{t} \sin{\left(\theta \right)} \cos{\left(\theta \right)} \dot{x} - 8 T_{2} \cos{\left(\theta \right)} - 8 T_{3} \cos{\left(\theta \right)} + 8 \rho \cos{\left(\theta \right)} \dot{\phi}_{2} + 8 \rho \cos{\left(\theta \right)} \dot{\phi}_{3}}{4 D}\\\frac{- L T_{2} + L T_{3} + L \rho \dot{\phi}_{2} - L \rho \dot{\phi}_{3}}{D}\end{matrix}\right)\\ \end{align*}\]Après résolution on redécompose les coordonnés généralisés étendu \(\tilde{\ddot{q}}\) en deux vecteurs \(\ddot{q}\) et \(\Lambda\), ce dernier étant d’intérêt limité sauf si l’on souhaite connaître les contraintes dans le mécanisme.
Simulation
Simulation offline de bon fonctionnement
Dans un premier temps, on vérifie la cohérence numérique du modèle à partir d’une simulation dite “offline”, c’est à dire une simulation dont ne se déroulant pas en temps réel et donc dont toutes les entrées sont connues à l’avance. Cela permet une intégration numérique plus précise et donc de vérifier si le problème est bien posé sans douter de la méthode de résolution. On s’attachera à vérifier les points suivants:
- Sans frottement et avec un couple T2 et T3 positifs, le robot doit former un cercle parfait
- Les équations de contraintes doivent être respectées
- L’énergie mécanique doit être conservée
Injection de couple constante sans frottement (\(T_2 = 0.2\) N.m \(T_3 = 0.1\) N.m)
On trouve ci-dessus respectivement:
- La trajectoire formant un cercle parfait
- L’angle \(\theta(t)\) et sa dérivée \(\dot{\theta}(t)\), linéaire car absence de frottement.
- Les trois composantes du vecteur \(S(q)\dot{q}\), nulles à \(1e-6\) près. Naturellement, une erreur d’intégration existe sur les deux premières composantes qui contiennent des termes qui ne s’intègrent pas \((cos(\theta)/sin(\theta))\), l’erreur va donc être fonction de \(\dot{q}\), or sur cette maneuvre, on atteint déjà des vitesses absurdes pour ce système (40 rad/s correspondant environ à 6 tr/s !)
Injection de couple constante avec frottement (\(T_2 = 0.2\) N.m \(T_3 = 0.1\) N.m)
- La trajectoire converge désormais sur un cercle parfait lorsque la vitesse terminale est atteinte
- La vitesse converge vers une vitesse terminale
- L’erreur semble être bornée dans le temps
Le code sympy est disponible ici, à ce jour (15/11/2023) les bases sont là mais l’organisation, la création d’exemple et la séparation en module distincts est encore à prévoir.