Les trajectoires complexes

Nous souhaitons que le PAMI parcourt des trajectoires variées :

• des lignes droites ;
• des arcs de cercles ;
• des courbes de Bézier ;
• des rotations pures.

Cette partie est la dépend directement de la partie « Asservissement en position ». Certaines fonctions ne sont utiles que si vous avez également suivi la partie « Le trajet et les trajectoires ».

Nous souhaitons impacter au minimum la partie « Trajet », nous allons reprendre les mêmes fonctions que celles définies précédemment, au détail près que ces fonctions gagnent un argument supplémentaire : une structure contenant les données de la trajectoire.

Les fonctions appelées par « Trajet » sont des fonctions aiguillages qui, en fonction du type de trajectoire, vont appeler les fonctions adéquates. Donc chaque type de trajectoire aura une fonction pour :

• calculer sa longueur ;
• convertir l’abscisse curviligne en position XY ;
• trouver l’abscisse curviligne qui permet de parcourir la distance demandée à partir d’une position donnée par une autre abscisse curviligne.

Voyons dans en détail les différentes trajectoires.

Elle se définit par deux points, D départ et A, arrivée.

La longueur se calcule avec : $Longueur = \sqrt{(A_x - D_x)² + (A_y - D_y)²}$

La position à partir de l’abscisse curviligne s’obtient à l’aide de la formule du barycentre :

$$ P(t) = D \cdot (1-t) + A \cdot t $$

L’abscisse permettant de parcourir la distance désirée s’obtient : $t' = t + \frac{distance}{longueur}$

L’arc de cercle se définit par un centre, un rayon et deux angles, celui de début et celui de fin.

La longueur se calcule avec : $Longueur = \lvert \textrm{angle_fin} - \textrm{angle_debut} \rvert \cdot rayon$

La position à partir de l’abscisse curviligne s’obtient avec : $$P(t) = \left\{ \begin{array}{l} x = \cos(\textrm{angle_debut} \cdot (t-1) + \textrm{angle_fin} \cdot t) \cdot rayon \\ y = \sin(\textrm{angle_debut} \cdot (t-1) + \textrm{angle_fin} \cdot t) \cdot rayon \\ \end{array} \right. $$

L’abscisse permettant de parcourir la distance désirée s’obtient : $t' = t + \frac{distance}{longueur}$

La courbe de Bézier cubique est probablement la trajectoire la plus pratique, car elle permet de relier deux points en tenant compte des contraintes d’orientation du PAMI en début et en fin de trajectoire.

Elle se définit par 4 points. Le point de départ (D) et le point d’arrivée (A) ainsi que deux poignées de contrôle que nous nommerons D’ pour celle du côté du départ et A’ pour celle du côté de l’arrivée.

Il n’y a pas de formule magique pour calculer la longueur d’une courbe de Bézier. Nous la calculons en discrétisant la courbe en segments. Le nombre de segment est un compris entre la vitesse de calcul et la précision souhaitée. Nous utilisons 500 segments pour calculer la longueur de nos courbes de Bézier.

$$ \sum_{i=0}^{n-1} Distance \left ( P(\frac{i}{n}),P(\frac{i+1}{n}) \right ) $$

La position à partir de l’abscisse curviligne s’obtient avec :

$$ P(t) = D \cdot (1-t)^3 + 3 \cdot D' \cdot t \cdot (1-t)^2 + 3 \cdot A' \cdot t^2 \cdot (1-t) + A \cdot t^3 $$

L’abscisse permettant de parcourir la distance désirée s’obtient par itérations. D’abord en calculant l’abscisse comme sur une ligne droite : $t' = t + \frac{distance}{longueur}$

Puis en calculant l’erreur entre la distance obtenue avec cette abscisse et celle désirée.

$$ \textrm{distance_obtenue} = Distance \left ( P(\frac{i}{n}),P(\frac{i+1}{n}) \right ) $$ $$ erreur = 1 - \frac{\textrm{distance_obtenue}}{\textrm{distance_désirée}}$$

Si l’erreur est supérieure au seuil souhaité (1 % par exemple), nous calculons la nouvelle abscisse :

$$ t" = t + (t-t') \cdot \frac{\textrm{distance_désirée}}{\textrm{distance_obtenue}}$$

Ainsi de suite jusqu’à avoir une erreur en dessous du seuil souhaité.

Pour plus d’information, l’article de Wikipédia permet de s’initier tandis que « A Primer on Bézier Curves » (en anglais) offre une étude approfondie de ces courbes.

Malgré les trajectoires précédentes vous aurez probablement besoin d’effectuer une rotation sans déplacer le PAMI, un changement d’orientation…

Votre trajectoire se définit par un angle de départ et un angle d’arrivé.

Là, vous avez le choix. Soit vous raisonnez en angle, votre longueur à parcourir sera un angle, votre vitesse, une vitesse angulaire et votre accélération, une accélération angulaire. Les calculs du module « Trajet » seront toujours valables, mais, si comme nous, vous précisez l’unité dans les noms de variables, vous aurez des écarts du type :

distance_mm = 3 ; // radians

Soit vous raisonnez en distance en considérant que la longueur à parcourir est celle que doit parcourir l'une des roues. Cette approche à l’avantage de rester cohérent au niveau des unités. C’est celle-ci que nous choisirons.

La longueur se calcule avec : $Longueur=\lvert \textrm{angle_fin} - \textrm{angle_debut} \rvert \cdot \textrm{distance_roue_centre}$

La position à partir de l’abscisse curviligne s’obtient avec :

$$P(t) = \textrm{orientation_début} \cdot (1-t) + \textrm{orientation_fin} \cdot t$$

L’abscisse permettant de parcourir la distance désirée s’obtient : $t' = t + \frac{distance}{longueur}$

Pour que toutes les trajectoires soient compatibles, elles doivent toute se servir des mêmes structures de données. En interface entre les modules, nous avons deux principales structures de données :

• Celle qui contient les informations de la trajectoire
• Celle qui contient les informations de la consigne de position

Si vous implémentez la rotation pure, votre fonction renvoie une orientation au lieu d’un couple (X, Y). Soit vous renvoyez un point orienté (X, Y, orientation) pour toutes vos trajectoires. Soit vous renvoyer un point (X, Y) et stockez la consigne d’orientation dans X… Nous préférons la première solution.

À l’initialisation, celui-ci reçoit et stocke une structure qui contient les données de la trajectoire. C’est cette structure qui sera passée en argument à chaque fonction Trajectoire.

Toutes les fonctions prennent comme argument une structure de données contenant les informations de la trajectoire. Nommons cette structure trajectoire_t. Elle est composée des champs suivants :

• type_de_trajectoire (enum type_de_trajectoire_t) : trajectoire_droite, trajectoire_arc_de_cercle, trajectoire_bezier, trajectoire_rotation_pure
• P1, P2, P3, P4 (struct point_t) : les points de la trajectoire – tous ne sont pas utiles pour tous les types de trajectoire.
• angle_debut, angle_fin (float) : contiennent les angles de début et de fin pour la trajectoire en arc de cercle ou la rotation
• longueur (float):initialement à -1, elle n’est calculée qu’une fois afin d’économiser des cycles de calcul
• Rayon (float) : uniquement pour l’arc de cercle

La structure point_t contient x_mm et y_mm, les coordonnées du point.

Le module trajectoire contient aussi les fonctions pour initialiser n’importe quelle trajectoire. L’initialisation d’une trajectoire consiste à stocker les données au bon endroit dans la structure. Par exemple, pour une ligne droite, nous allons stocker le point de début et celui de fin, dans les points P1 et P2. C’est notre convention arbitraire, comme cela aurait pu être P1 et P4.

Les fonctions « trajectoire_get_longueur », « trajectoire_get_position » et « trajectoire_avance » sont des fonctions aiguillages qui, dépendant du type de trajectoire, vont appeler la fonction définie pour le type de trajectoire en question, que nous stockons dans un fichier séparé.

Nous obtenons ainsi autant de sous-modules (couple de fichier .c et .h) que de types de trajectoire, chacun implémentant ces trois fonctions.

Créez un code de démonstration par type de trajectoire avec les paramètres donnés ci-dessous. Le PAMI part de (0,0) avec une orientation à 0° (selon le vecteur X du repère).

• Vitesse : 300 mm/s
• Accélération : 600 mm/s²
• Départ : (0,0)
• Arrivée : X= 500 mm, Y = 0 

• Vitesse : 200 mm/s
• Accélération : 400 mm/s²
• Centre : X = 0, Y = 200
• Angle de début : -π/2
• Angle de fin : π/2
• Rayon : 200 mm

• Vitesse : 200 mm/s
• Accélération : 400 mm/s²
• Centre : X = 0, Y = 0
• Angle début : 0
• Angle fin : π

• Vitesse : 200 mm/s
• Accélération : 600 mm/s²
• P1 : (0, 0)
• P2 : (280, 0)
• P3 : (390, 530)
• P4 : (475, 0)

# Insérer une image de la coube + vidéo des trajectoires