Pure Pursuit
Le pure pursuit est un algorithme de contrôle à haute vitesse. Le principe est simple, il permet au robot de suivre une ligne de course de manière très précise. Cette ligne de course est en fait une liste de waypoint. À chaque itération, l'algorithme choisi un waypoint devant le robot de manière stratégique et oriente le robot dans cette direction.
Cette lecture du cours F1TENTH explique très bien le fonctionnement de l'algorithme d'un point de vue mathématique.
-
liens vers la lecture
L'implémentation utilise des formules légèrement différentes qui sont disponibles ici.
L'algorithme pure pursuit du VAUL dépend de 2 noeuds:
-
Le waypoint_publisher_node qui s'occupe de charger la liste de waypoints en mémoire et la publier sur un topic.
-
Le pure_pursuit qui contient la logique de contrôle.
Pour utiliser le pure pursuit, il est nécessaire de faire une carte de l'environnement. Par la suite, on peut générer une raceline pour le robot à l'aide du gui de génération de racelines. Finalement, on peut utiliser l'algorithme pour suivre cette raceline.
C'est 3 fonctions utilitaires.
Cette fonction calcule le lookahead en fonction de la vitesse. Le lookahead
est une fonction linéaire de la vitesse qui peut être ajusté à l'aide des paramètres m
et q
.
lookahead_distance = speed * m + q
Récupère le waypoint qui est le plus près du lookahead_distance
. Cette opération se fait en 2 temps:
- On trouve le waypoint le plus près du robot.
- On itère vers l'avant jusqu'à ce que l'on trouve le waypoint qui se rapproche le plus de la distance
lookahead_distance
du robot.
Permet de transformer un waypoint du référenciel monde vers le référenciel du robot.
Permet de calculer le steering_angle
depuis le goal point selon les maths décrites plus haut.
Permet de limiter la vitesse lorsqu'on démarre le véhicule à un endroit où la vitesse serait normalement élevé. Peut être ajusté à l'aide des paramètres smooth_start_threshold
et smooth_start_factor
.
Récupère la vitesse de la ligne de course à la position du robot et la multiplie par le velocity_percentage
.
Publie une commande de vitesse et d'angle de direction sur le topic drive.
Permet la mise à jour de la vitesse mesuré du robot.
C'est la fonction qui est appelée à chaque fois que la position du robot change. C'est ici qu'on calcule le steering_angle et la vitesse à envoyer comme commande au robot.
Permet la mise à jour de la ligne de course.
Visualise le goal point dans Rviz et foxglove.
Visualise l'angle de braquage dans Rviz et foxglove.
Mets à jour périodiquement tous les paramètres du noeud.