Ceci est le deuxième article de l'équipe setUP sur notre expérience dans la création de robots autonomes pour les compétitions Eurobot Open et l'utilisation de ROS pour cela.
Le premier article concerne la mécanique et l'architecture générale des robots.Les robots voyagent sur un terrain plat et la plupart des obstacles sont connus à l'avance, cependant, des adversaires insidieux peuvent essayer de voler nos ressources (et nous voulons parfois manger quelques dizaines de points supplémentaires), tandis que nous voulons conduire au point souhaité le plus rapidement possible et ne pas toucher les obstacles. À partir d'une caméra externe sur le terrain, nous obtenons des données sur la position de l'ennemi et savons où il se trouve actuellement. Cependant, il ne suffit pas de connaître sa position - vous devez pouvoir utiliser ces informations.
Aujourd'hui, nous allons essayer de conduire du point A au point B sans avoir voyagé le long de la queue d'un chat qui s'est endormi sur le terrain. En particulier, nous expliquerons comment nous construisons un itinéraire et contrôlons la vitesse du robot, ainsi que comment démarrer tout sur notre ordinateur.
Essayer de s'en sortir avec un peu de sang
En résolvant ce problème, vous pouvez prendre du matériel prêt à l'emploi écrit par des professionnels, puis il n'y aura pas de tourment et d'invention du prochain «vélo». Nous avons utilisé une plate-forme omnidirectionnelle, donc bien qu'il y ait eu une tentative d'utiliser une raboteuse locale prête à l'emploi de ROS, pour un certain nombre de raisons, ils ont trouvé que cela n'était pas prometteur. Ci-dessous, vous pouvez voir combien de choses le concepteur d'un planeur standard a besoin:
class TrajectoryPlanner{ friend class TrajectoryPlannerTest;
Il s'agit d'un exemple d'initialisation des paramètres de régulation des vitesses et de la trajectoire dans son ensemble.Paramètres importants pour un calcul complet:
- Paramètre world_model.
- Paramètre de carte de coût: un lien vers une carte contenant des obstacles, ainsi que leur "extension virtuelle", en tenant compte de la collision potentielle.
Parmi les avantages de la pile standard, vous pouvez souligner la disponibilité de la documentation et la possibilité de trouver des informations sur les forums.
Vous pouvez en savoir plus sur le site officiel avec la documentationIl est important de mentionner que les packages ROS ont été écrits pour les plates-formes à deux roues et sous Omni, ils ont été optimisés en augmentant l'angle de rotation disponible lors d'un déplacement jusqu'à 360 degrés, ce qui est certainement une béquille.
Après avoir analysé le projet, nous avons réalisé qu'il y aurait une difficulté à étudier et à compléter, ainsi que de nombreuses puces lourdes dont nous n'avons pas besoin. Il semblerait que ce soit le cas, mais nous avons utilisé Odroid xu4 (dont le processeur était toujours sur Samsung s5), et les résultats de performance étaient déprimants, et l'espace pour quelque chose de plus puissant (et la framboise 4 et le processeur nets jetson fument nerveusement de côté par rapport à avec lui) non. J'ai dû abandonner la pile standard et essayer de créer nous-même le planificateur global, le planificateur local et le régulateur de trajectoire
Planificateur global, planificateur local, régulateur de trajectoire et tous tous tous
Des planeurs mondiaux et locaux sont nécessaires pour obtenir les directions vers la destination. Pourquoi la séparation est-elle nécessaire? Pourquoi ne pouvez-vous pas simplement prendre
A * et le monter? En règle générale, un planificateur global, lors de la construction d'un itinéraire, peut utiliser la carte entière dans son travail, donc l'algorithme doit être aussi rapide que possible, peut-être même avec quelques simplifications. Pour lisser ces simplifications, ils utilisent également le planificateur local, qui, basé sur le résultat du planificateur global (ou juste une zone limitée autour du robot), essaie de prendre en compte toutes les nuances.
Après avoir construit l'itinéraire, nous savons où le robot doit aller, mais comment peut-il être informé de cela? Pour ce faire, il existe un régulateur de trajectoire. Il calcule à quelle vitesse et dans quelle direction le robot doit se déplacer en ce moment pour ne pas dévier de la trajectoire. À bien des égards, ce package est responsable de la vitesse et de la beauté du robot.
En plus de ces trois entités, il y en a une quatrième - un serveur de carte, qui vous permet de traiter facilement l'état du monde. Il définit comment nous décrivons la carte, quelles possibilités nous avons lorsque nous travaillons avec la carte et, à bien des égards, détermine la vitesse des planeurs.
Avant de procéder à la description de la pile de navigation, il serait intéressant de décrire les raisons pour lesquelles cost_map a été choisi comme serveur de carte. En général, nous avons essayé différentes options pour le gestionnaire de carte:
Occupancy_grid ,
Grid_map ,
Cost_map , mais nous nous sommes installés sur ce dernier.
Raisons:
- Interagissez facilement avec la carte.
- Il existe différents itérateurs dont nous avons besoin sous diverses formes (circulaire, linéaire, rectangulaire, etc.).
- Vous pouvez stocker plusieurs couches de carte avec différents paramètres.
- Bonne gestion de la mémoire.
- Et surtout, la vitesse. La carte de grille fonctionne avec un type double et pour cette raison, elle est plusieurs fois plus lente que les serveurs de carte qui utilisent int8 pour fonctionner.
Malgré le fait que la grille d'occupation fonctionne également avec int8, elle ne peut pas se vanter de la même convivialité, j'ai donc dû l'abandonner.
À partir de la carte, nous devons savoir où se trouvent les zones libres, dangereuses et irrésistibles. Pour chaque objet qui se trouve sur le terrain, nous pouvons ajuster le champ de gonflage - une valeur qui, en fonction de la distance à l'objet, caractérise la perméabilité de la cellule. L'inflation est la queue du chat, il est facile de ne pas le remarquer, mais vous le regretterez très longtemps. Nous cartographions les robots ennemis et ajoutons une zone de danger que seul le planificateur local prend en compte. Le planificateur global ignore tous les points, s'ils ne sont pas un obstacle.
Planificateur mondial
La première chose qu'ils ont écrite dans la navigation est le planificateur global. Il est basé sur l'algorithme thêta *. En bref, il s'agit d'un
A * modifié, où l'accent est mis sur la recherche du nœud parent, qui peut être atteint directement, c'est-à-dire il n'y a aucun obstacle pour elle. Cela nous permet de construire des chemins pratiques et lisses qui sont utilisés dans le planificateur local.
Comparaison de A * et thêta *Pour le planificateur global, nous avons un fichier avec des paramètres (params / path_planner.yaml) qui décrit les sujets de la carte et les sujets avec l'emplacement de tous les robots (pour les quatre robots sur le terrain, où "null" est le sujet avec des données sur le robot actuel).
# small robot debug param list robot_id: "small" ########################### # cost_map_server params ## ########################### cost_map_server/big_robot_size: 0.45 cost_map_server/little_robot_size: 0.45 cost_map_server/cube_size: 0.11 cost_map_server/inscribed_radius: 0.3 cost_map_server/inflation_radius: 0.3 cost_map_server/inflation_exponential_rate: 0.6 cost_map_server/big_robot1: "/aruco/big_robot" cost_map_server/big_robot2: "/aruco/enemy_robot1" cost_map_server/small_robot1: "null" cost_map_server/small_robot2: "/aruco/enemy_robot2" cost_map_server/collision: "collision" cost_map_server/image_resource_name: "$(find cost_map_server)/param/image_resource.yaml" cost_map_server/min_diff_points: 0.01 ########################### ### path_planner params ### ########################### global_planner/path_layer: "inflation_layer" global_planner/path_force_layer: "obstacle_inflation_layer" global_planner/frame_id: "map" global_planner/current_position: "real_corr" global_planner/path_filter_epsilon: 0
Il indique également:
- l'un des algorithmes que vous pouvez choisir pour créer un itinéraire,
- noms des couches sur lesquelles nous allons construire l'itinéraire lui-même,
- un sujet sur notre position, où des données filtrées sont émises (dans notre cas, il s'agit d'une combinaison de données de localisation de la caméra et de l'odométrie).
L'algorithme de recherche de chemin lui-même - Theta Star - est mis en évidence dans un fichier séparé (src / global_planner.cpp) pour la commodité d'ajouter de nouveaux algorithmes:
float cost_so_far[300][200]; PriorityPoint neighbors[8]; PriorityPoint current; int come_from[300][200][2]; double makePath_ThetaStar(std::vector<cost_map::Index> &path, PriorityPoint start, PriorityPoint goal, std::string layer, cost_map::CostMap &cost_map, double grid_cost, bool only_cost) { std::priority_queue<PriorityPoint, std::vector<PriorityPoint>, myCompare> openSet; size_t max_x = cost_map.getSize()[0]; size_t max_y = cost_map.getSize()[1]; std::fill_n(*cost_so_far, max_x * max_y, std::numeric_limits<float>::max()); cost_so_far[start.x][start.y] = 0; come_from[start.x][start.y][0] = start.x; come_from[start.x][start.y][1] = start.y; openSet.push(start); grid_cost=5; while (!openSet.empty()) { current = openSet.top(); openSet.pop(); if (current == goal) { break; } current.GetNeighbors(neighbors); float current_cost = cost_so_far[current.x][current.y]; int parent_carent[2] ={come_from[current.x][current.y][0], come_from[current.x][current.y][1]}; for (int i = 0; i < 8; i++) { if (!neighbors[i].OnMap(max_x, max_y)) { continue; } bool onLine = lineOfSight(parent_carent[0], parent_carent[1], neighbors[i].x, neighbors[i].y, cost_map, layer, grid_cost + 1); if (onLine) { float new_cost = cost_so_far[parent_carent[0]][parent_carent[1]] + HeuristicEvclid(parent_carent, neighbors[i], grid_cost*10); if (new_cost < cost_so_far[neighbors[i].x][neighbors[i].y]) { cost_so_far[neighbors[i].x][neighbors[i].y] = new_cost; neighbors[i].priority = HeuristicEvclid(neighbors[i], goal, grid_cost*10) + new_cost; openSet.push(neighbors[i]); come_from[neighbors[i].x][neighbors[i].y][0] = parent_carent[0]; come_from[neighbors[i].x][neighbors[i].y][1] = parent_carent[1]; } } else { float neighbor_price = cost_map.at(layer, cost_map::Index({neighbors[i].x, neighbors[i].y})) + neighbors[i].priority; float new_cost = current_cost + neighbor_price; if (new_cost < cost_so_far[neighbors[i].x][neighbors[i].y]) { cost_so_far[neighbors[i].x][neighbors[i].y] = new_cost; neighbors[i].priority =HeuristicEvclid(neighbors[i], goal, grid_cost*10) + new_cost; openSet.push(neighbors[i]); come_from[neighbors[i].x][neighbors[i].y][0] = current.x; come_from[neighbors[i].x][neighbors[i].y][1] = current.y; } } } } if (only_cost) { return cost_so_far[current.x][current.y]; } path.clear(); PriorityPoint temp_point; while (current != start) { path.push_back({current.x, current.y}); temp_point.x = come_from[current.x][current.y][0]; temp_point.y = come_from[current.x][current.y][1]; current = temp_point; } path.push_back({current.x, current.y}); return 0; }
Un algorithme pour supprimer des points supplémentaires sur le chemin RamerDouglasPeucker est également alloué à un fichier distinct.
Il supprime des points du chemin s'il est au-delà d'une distance donnée de la ligne qui relie deux points voisins.
Planificateur local
Il travaille pour nous sur la base d'une descente de gradient dans un champ potentiel. En entrée, le chemin du planificateur global. Mais ce n'est pas tout ce dont il est capable. Dans local_planner, il existe des services internes pour choisir le mode de construction du chemin. Il existe deux modes de fonctionnement au total: le mode de déplacement des points le long du gradient, en utilisant plusieurs passes sur la carte, ainsi que le mode de décalage, dans lequel nous calculons immédiatement l'incrément en deux coordonnées et déplaçons le point au bord de la zone de sécurité. Si le point tombe dans plusieurs de ces zones, alors nous nous déplaçons vers les lieux de leur intersection, car là, il est le plus sûr.
Le mode de fonctionnement est le suivant: s'il n'y a pas d'obstacle sur le chemin depuis l'itération précédente, alors nous cassons le chemin tous les 2 cm, puis le décalons le long du gradient, sinon nous utilisons le deuxième mode de fonctionnement du planificateur local.
La deuxième option est assez légère et, comme le planificateur global, ne charge pas beaucoup le processeur. Nous avons utilisé plusieurs versions de cet algorithme et diverses manipulations avec la carte. Par exemple, nous avons essayé d'écrire un graphique dans lequel les sommets se trouvent tous les 10 cm et sont décalés d'un maximum de 4 cm, après quoi l'algorithme de Dijkstra a été utilisé sur le graphique obtenu pour trouver la plus petite distance. Dans ce cas, le point d'extrémité utilise le point de décalage le plus proche. Mais un tel algorithme était plus adapté au planificateur global et nous avons décidé d'abandonner cette implémentation.
Nous avons également essayé d'utiliser la construction de chemin à partir de zéro en utilisant la méthode de descente en gradient. Cette méthode a été la première que nous avons décidé d'écrire. Il s'est avéré ne pas être efficace en mémoire (il occupait plus de 400 Mo de RAM pure, car il utilisait une carte de coût à chaque passage) et lent. Le contrôle de fréquence a été désactivé en raison d'une mauvaise optimisation et la vitesse était inférieure à 30 fois par seconde, ce qui ne nous convenait pas.
En conséquence, nous avons décidé d'utiliser la descente de gradient dans un champ potentiel basé sur la trajectoire du planificateur global. Il s'est avéré être un algorithme léger et relativement simple, qui nous convient parfaitement en termes de qualité de chemin, de temps de travail et de quantité de RAM utilisée dans la région de 100-150 Mo, ce qui est plusieurs fois moins que dans les premières itérations de développement.
Un exemple de décalage de trajectoire de rabotage localContrairement à global_planner, les paramètres de local_planner sont extrêmement petits, ce qui est dû à sa simplicité, car toutes les tâches les plus importantes incombent à global_planner:
grid_map_server/big_robot1 : 0.4 grid_map_server/big_robot2 : 0.4 grid_map_server/small_robot1 : 0.4 grid_map_server/small_robot2 : 0.4 local_planner/radius : 0.15 global_planner/frame_id : "map"
Dans ce cas, nous configurons:
- Rayons de zones de sécurité pour chacun des robots.
- Déport maximal décalé par la raboteuse locale.
- Le nom de la couche de carte avec laquelle nous travaillons.
Dans une classe séparée, toutes les fonctions les plus importantes ont été attribuées. Dans ce cas, il s'agit d'une rupture récursive du chemin, de la création d'un planeur et de constructeurs et destructeurs virtuels.
class Planner { public: Planner(double inflation_radius_, std::string frame_id_) { inflation_radius = inflation_radius_; for (int i = 0; i < 300; i++) { for(int j = 0; j < 200; j++) our_map[i][j] = 3000.0; } OurPath->poses.resize(50); geometry_msgs::Pose pose; pose.position.x = 0.0; pose.position.y = 0.0; for( int i = 0; i < 50; i++) { OurPath->poses[i].pose = pose; } frame_id = frame_id_; } virtual ~Planner() { } virtual void UpdatePath() = 0; ros::Publisher move_pub; ros::Publisher BigEnemyPub; ros::Publisher LittleEnemyPub; ros::Publisher local_path_publisher; protected:
La classe LocalPlanning en a été héritée, où se trouve tout le noyau du planeur, c'est-à-dire déplacer les points vers le bord de la zone de sécurité et décider quoi faire du chemin en particulier.
Toutes les autres fonctions sont mises en évidence dans un fichier séparé fichi.hpp et la descente de gradient dans potential_field.hpp. Ci-dessous est un instantané de ce fichier, qui montre les fonctions pour créer un champ potentiel sur la carte cost_map:
double CalcAttractivePotential(grid_map::Index index, double goalx, double goaly) { return sqrt(pow(index[0] - goalx, 2) + pow(index[1] - goaly, 2)); } void CalcPotential(double startx, double starty, double goalx, double goaly, cost_map::CostMap &cost_map, double (&our_map)[300][200]) {
Régulateur de trajectoire
Le dernier mais non le moindre est le régulateur de trajectoire. Il est responsable de la conversion du chemin du planificateur local en chemin et donne la vitesse de l'étape en cours.
Sa première version, que nous avons utilisée lors de la finale de l'Eurobot 2018, est un mélange du contrôleur PID d'accélération et de freinage, où le vecteur est normalisé au point suivant sur la trajectoire, par rapport à la distance jusqu'au point final.
Le contrôleur PID, en bref, est la somme des trois états du système qui aident à corriger le système et les erreurs aléatoires qui se produisent parfois.
Ces fonctions ont été choisies empiriquement et dépendent de la distance jusqu'au point final du chemin (elle peut être quadratique, cubique, inverse, mais ensuite nous nous sommes installés sur le quadratique). Cela a fonctionné, mais la seule chose qui ne nous convenait pas était que le robot ne pouvait pas freiner à temps à des vitesses supérieures à 0,7 mètre par seconde. Par conséquent, le moment venu, nous avons décidé de reconstruire l'ensemble de l'algorithme.
La première itération sur le chemin de la nouvelle trajectoire a été le remplacement du vecteur vers lequel nous allions. Maintenant, c'était la somme des vecteurs pour les trois suivants avec des coefficients différents. La deuxième itération écrivait Minimum Jerk. En bref, il s'agit d'une construction d'un polynôme du 5e degré, où les coordonnées x et y dépendent de l'heure d'arrivée à chaque point.
La figure montre un graphique de l'une des coordonnées en fonction du temps, ainsi que la vitesse le long de cette coordonnéeCe type de contrôleur de trajectoire nous convenait mieux, car il nécessitait moins de manipulation avec la sélection de différents coefficients, car tous les coefficients sont les valeurs du polynôme, qui ont été calculées en fonction du temps pour y arriver, de la vitesse et de l'accélération actuelles, de la vitesse de sortie et de l'accélération.
Le résultat de la réécriture de la trajectoire a été que nous avons réussi à doubler la vitesse moyenne du robot.
Comme dans les deux cas précédents, toutes les fonctions principales sont mises en évidence dans un fichier séparé pour une interaction facile. Cette fois, la classe PlannerTrajectory est responsable de la construction d'une trajectoire basée sur MinimumJerk
typedef struct State { double velocity; double acceleration; State(double v_, double a_) : velocity(v_), acceleration(a_) {} State() { velocity = 0; acceleration = 0; } }; class PlannerTrajectory { private: nav_msgs::Path global_path; cost_map::CostMap *costmap_ptr; geometry_msgs::PoseStamped Goal_pred; std::vector<double> trajectory_x; std::vector<double> trajectory_y; double average_velocity = 80;
La photo montre toutes les variables déclarées que nous utilisons.Tout le reste est mis en évidence dans un autre fichier (include / trajectory_regulator.h): recevoir des points des sujets, décider d'aller au point suivant (s'il est dans un obstacle, alors nous n'y allons pas) et bien plus encore.
Migration vers ROS Melodic
Jusqu'à l'année dernière, nous utilisions la version lte de ROS - ROS Kinetic. Il nous convenait généralement, mais son soutien prend fin l'année prochaine, et bon nombre des packages dont nous avons besoin ont commencé à sortir exclusivement pour ROS Melodic. Et puis il s'est avéré que le costmap_server que nous avons utilisé n'est pas sous Melodic.
Un problème est survenu lors du traitement des données des cartes.
Nous avons choisi la carte Grid, car la pile est similaire, mais le début de la carte est à un endroit différent, et les valeurs de la carte elle-même varient de 0 à 1. Cela est devenu un gros problème dans toute la pile de navigation. Si auparavant le planificateur global était lancé 50 fois par seconde (il y avait des restrictions de fréquence, et donc le processeur n'était pas trop utilisé, même dans la moitié d'un thread), maintenant il ouvrait la voie toutes les deux secondes et le considérait comme mauvais: il chargeait complètement un cœur. En 2 secondes, le robot a pu traverser toute la carte. Cela ne nous convenait pas et les tentatives de parallélisation de ce processus se sont soldées par un échec, car il n'y avait alors plus de performances pour les autres projets (en tenant compte des coûts de la parallélisation).
Nous avons décidé de changer à nouveau la pile, d'abandonner la carte de la grille au profit de la grille d'occupation. Un nouveau problème est apparu - l'incapacité de stocker plusieurs versions de cartes en même temps (par exemple, une carte complète avec tous les obstacles et une carte statique avec uniquement des obstacles dynamiques). Je devrais changer la moitié du code, ce qui n'était pas particulièrement fiable. Par conséquent, nous avons décidé de rechercher des solutions alternatives à ce problème.
Serveur de cartographie des coûts
Après une longue recherche, nous avons trouvé des cartes de fourches costmap_serverr:
https://github.com/lelongg/cost_map.git - extrêmement utiles pour notre système de fourches.
Et maintenant, au lieu de ne desservir que la carte de la grille, pour la livraison, nous parvenons à calculer l'emplacement probable de l'ennemi en fonction de la prédiction du filtre de Kalman.
Un des éléments les plus importants pour un serveur de carte est un fichier de carte, qui est utilisé pour la création initiale de toutes les couches, qui sont ensuite uniquement mises à jour. C'est une image binaire png, où le noir est un obstacle et le blanc est une zone libre.
Il existe également un fichier de paramètres pour la configuration cost_map_server. Il contient des sujets avec des points ennemis, une zone de gonflage et la taille du carré, qui peuvent également être utilisés pour mettre la zone dangereuse de l'ennemi sur la carte.
########################### # cost_map_server params ## ########################### cost_map_server/big_robot_size: 0.4 cost_map_server/little_robot_size: 0.4 cost_map_server/cube_size: 0.11 cost_map_server/inscribed_radius: 0.25 cost_map_server/inflation_radius: 0.25 cost_map_server/inflation_exponential_rate: 0.6 cost_map_server/big_robot1: "/aruco/robot1" cost_map_server/big_robot2: "/aruco/robot2" cost_map_server/small_robot1: "/aruco/robot3" cost_map_server/small_robot2: "/aruco/robot4" cost_map_server/collision: "collision" cost_map_server/image_resource_name: "$(find cost_map_server)/param/image_resource.yaml" cost_map_server/min_diff_points: 0.01
Toutes les couches sont publiées uniquement si quelqu'un s'y abonne:
void PublishChanges() { nav_msgs::OccupancyGrid msg; cost_map_msgs::CostMap cost_map_msg; if (obstacle_inflation_publisher.getNumSubscribers() > 0) { cost_map::toOccupancyGrid(costmap, "obstacle_layer", msg); obstacle_inflation_publisher.publish(msg); } if (inflation_publisher.getNumSubscribers() > 0) { cost_map::toOccupancyGrid(costmap, "inflation_layer", msg); inflation_publisher.publish(msg); } if (cost_map_publisher.getNumSubscribers() > 0) { cost_map::toMessage(costmap, cost_map_msg); cost_map_publisher.publish(cost_map_msg); } }
Exécutez sur votre ordinateur
Pour démarrer la pile entière, vous devez:
- Mettez ROS
- roslaunch cost_map_server cost_map_server_alone.launch - pour démarrer la carte
- roslaunch global_planner global_planner.launch - lancer un planificateur global avec des paramètres
- rosparam charger $ (trouver local_planner) /param/param.yaml
- rosrun local_planner local_planning
- rosrun trajectory_regulator trajectory_regulator
- rosrun global_planner mover
- rosrun rviz
- ajouter inflation_layer
- Maintenant, en envoyant un message au sujet / gp / objectif, nous envoyons le robot au point souhaité
À la suite du lancement de tous les éléments, vous aurez une simulation prête à lancer notre pile sur votre ordinateur. Fourche nécessaireAu départ, nous avions besoin d'une navigation qui aiderait notre robot à rouler magnifiquement, rapidement et avec précision sur des roues omnidirectionnelles. Pendant la préparation de la compétition, pas un chat n'a été blessé et le robot est beau. En conséquence, nous avons une pile de navigation légère pour les concours similaires à eurobot, dont nous sommes très satisfaits.Pour nous, cette pile est meilleure que la pile standard, mais ...Notre télégramme: t.me/SetUpSberLe référentiel de toute notre créativité