Hexapode (Partie 5: Algorithme)

Pour cette partie, on va faire pas mal de maths… Et surtout de la trigo.

Liste des algorithmes

L’objectif que je me suis fixé pour les déplacements est de créer un minimum de fonctions généralistes qui permettent de remplir des fonctions simples. Voici une liste des algorythmes que je vais détailler ici:

  • Permettre de calculer l’angle des servomoteurs en fonction de la position de l’extrêmité de la patte par rapport au centre de l’hexapode dans un espace défini.
  • Permettre à l’extremité de la patte de suivre une trajectoire parabolique dont les paramètres seront la hauteur du pas, sa longueur, le point de départ et le point d’arrivée.
  • Découper cette courbe en un certain nombre de points dans le but d’avoir une trajectoire précise sous forme de coordonnées discrètes. Les paramètres seront le nombre de sections que nous souhaitons pour cette courbe.
  • Permettre une trajectoire rectiligne de « retour » sur un même plan et présenter la sortie sous forme de suite de coordonées discrètes.
  • Permettre un déplacement en fonction d’instructions simples qui seront transmises via le réseau depuis les applications clientes.

Pourquoi ce choix? J’aurais pu me contenter de définir une série de mouvements , mais cela n’aurais pas permis au code d’évoluer par la suite. Avec une structure comme celle-ci, on pourra envisager plus tard de compenser des irrégularité de terrain, de lui faire apprendre des mouvements différents, de se déplacer avec une assiette non parallèle au sol ou encore d’effectuer des mouvements de déplacement du corps avec les 6 pattes au sol.

Avant de détailler les algos, on va définir les nom des éléments angles etc… avec ce dessin:

Quelques précisions:

  • Les droites (OA) et (AB) appartiennent au plan XY
  • L’angle θ est défini et fixe ensuite
  • L’angle α se déplace autour d’un axe perpendiculaire au plan XY
  • Les angles β et λ se déplacent autour d’une droite parallèle au plan XY
  • Les points A, B, C et D sont coplanaires
  • Les longueurs a, b,c et o sont définies et fixe ensuite
  • Le point O est concidéré comme le centre de l’hexapode

Algo1: Angles des servos

Cet algo permet donc le calcul de l’angle des servos en fonction de la position du point D. On va travailler plan par plan.

Les données sont les suivantes:
Les longeurs o, a, b et c sont des constantes.
L’angle θ est fixe.
Les angles α, β et λ sont les variables à exprimer en fonction des constantes.
Tout le reste est inconnu.

Plan XY
(fig.1) :

Plan XZ
(fig.2) :

Calcul des position du point A:

 A: \left\lbrace\begin{matrix} x_{A}=o \cos (\theta ) \\ y_{A}=o \sin(\theta) \\ z_{A}= 0  \end{matrix}\right.

Calcul de la longueur l :

 l= \sqrt{ \left(x_{D}-x_{A}\right)^{2}+ \left(y_{D}-y_{A}\right)^{2}}

Calcul de l’angle \alpha:

 \alpha= \cos^{-1} \left\{ \frac{x_{D}-x_{A}}{l} \right\}

On calcule la longueur [BD]:

 \left[BD\right]=\sqrt{l^{2}+z_{D}^{2}}

On passe dans le plan local (B,i,j) qui correspond à la coupe du plan BCD
(fig.3) :

D’après ce schéma on a j_{D}=z_{D}.

On calcule d’abord l’angle \mu:

 \mu=\cos^{-1}\left\{\frac{l-a}{\left[BD\right]}\right\}

Puis V et U:

 U=\frac{\left( b^{2}-c^{2}+\left[BD\right]^{2}\right)}{2\left[BD\right]}

 V=\left[BD\right]-U

Et enfin les angles des servomoteurs:

 \lambda=\phi+\mu

 \beta=\cos^{-1}\left(\frac{U}{b} \right)-\mu

Algo2: Trajectoire parabolique

Les figures suivantes montrent la trajectoire que doit suivre une patte lors d’un déplacement.

plan XY fig 4:

Plan local fig 5:

Les données sont les suivantes:
La distance entre les points S et P est nommée l
La distance entre les points S et H est nommée h
les longueurs l et h sont connues.
La position des points S et P dans l’espace du plan XYZ sont connues.
On cherche à exprimer la fonction f en fonction l et h.

 f(u)=au^{2}+bu+c

On résout le système d’équation composé de f(l)=0 et f\left(frac{l}{2}\right)=h:

 \begin{cases} al^{2}+bl+c=0\\ a\left(\frac{l}{2}\right)^{2}+b\left(\frac{l}{2}\right)+c=h \end{cases}

Dans notre cas on a c=0 donc:

 \begin{cases} al^{2}+bl=0\\ a\left(\frac{l}{2}\right)^{2}+b\left(\frac{l}{2}\right)=h \end{cases}

On résout et on trouve:

 f(u)=\frac{-4h}{l^{2}}u^{2}+\frac{4h}{l}u

il suffira de remplacer u par la valeur et d’ajouter la coordonnée en Z du point S et on aura la coordonnée en Z qui nous intéresse pour une position donnée.

Algo3: Découpe de trajectoire

Pour pouvoir déplacer la patte, on va déterminer quels points de la courbe obtenue précédemment nous intéresse. Par faire cela, on va découper la courbe de cette manière:

découpage trajectoire fig6:

On voit grâce à ce schéma que si les mises à jour des positions sont faites à des intervalles non réguliers. On a un mouvement qui commence lentement, accélère, puis ralenti avant de toucher le sol.

On va s’intéresser dans cette partie au découpage de l’abscisse u.

On va chercher une fonction qui prends en entrée le nombre de sections que l’on souhaite faire de cette courbe et en sortie la coordonée en abscisse du n ème point du découpage de l’axe.

La fonction sera donc de cette forme fig7:

Je ne vais pas le démontrer ici mais la fonction que j’ai trouvé est en deux parties.

Pour n<\frac{m}{2}:

\sqrt{l^{2} \frac{x}{2m} - \fra{l^2}{4}} + \frac{l}{2}

Pour n \geq \frac{m}{2}:

-\sqrt{-l^{2} \frac{x}{2m} + \fra{l^2}{4}} + \frac{l}{2}

Le « l » est la longueur du pas et correspond à l’ordonnée du point F. La variable « m » est le nombre de point que l’on souhaite pour la découpe, et « n » le numéro du point dont on souhaite l’ordonnée pour g (ou l’abssice pour f).

En gros, je l’ai trouvée en passant par des racines carrées et cubique et en ayant comme contraintes le passage par le point M au milieu. Cette équation est perfectible et pourrait être changée à l’avenir. Cependant elle fait plutôt bien ce que je lui demande.

Voici ce que j’obtiens en sortie de la classe XML pour un pas:

C’est cohérent avec ce que je souhaitais.

On peut améliorer ce tracé et faire varier l’accélération du mouvement en passant par un algorythme à base de courbes elliptiques. Je détaillerai cela ici-même d’ici peu.

Algo4: Trajectoire de retour

Pour la trajectoire de retour au sol, je souhaite que le mouvement se fasse de manière régulière, pas comme pour l’algo précédent. Avec ce choix, j’espère avoir avoir une vitesse relativement continue et sans accélérations brusques.

En paramètres de cette fonction on a: la position des points S et P ainsi que le nombre de divisions m du segment.

Trajectoire de retour fig8:

On va donc découper le segment \left[SP\right] en m sections égales.

On commence par calculer la longueur des coordonnées en X et en Y:

  x_{SP}=(x_{P}-x_{S})

  y_{SP}=(y_{P}-y_{S})

Puis on divise y_{SP} par m et on l’ajoute à la position en Y du point d’origine:

  y_{n}=y_{S}+n\times\frac{y_{SP}}{m}

Idem pour la coordonée en X:

  x_{n}=x_{S}+n\times\frac{x_{SP}}{m}

Voilà, on  peut calculer toutes les coordonnées des points du segment en chageant le paramètre n.

Algo5: Déplacement directionnel

à venir…

Sommaire du projet:

Partie 1
Partie 2
Partie 3
Partie 4
Partie 5
Partie 6
Partie 7

Laisser un commentaire

Votre adresse de messagerie ne sera pas publiée. Les champs obligatoires sont indiqués avec *