Geekly articles weekly

Tumpukan navigasi sendiri. Lebih baik daripada ROS?

Ini adalah artikel kedua oleh tim setUP tentang pengalaman kami dalam membuat robot otonom untuk kompetisi Eurobot Open dan menggunakan ROS untuk ini.

Artikel pertama adalah tentang mekanika dan arsitektur umum robot.

Robot melakukan perjalanan di bidang datar dan sebagian besar hambatan diketahui sebelumnya, namun, lawan yang jahat dapat mencoba mencuri sumber daya kami (dan kami terkadang ingin memakan beberapa lusin poin tambahan), sementara kami ingin berkendara ke titik yang diinginkan secepat mungkin dan tidak menyentuh rintangan. Dari kamera eksternal di lapangan, kami mendapatkan data tentang posisi musuh dan tahu di mana dia sekarang. Namun, tidak cukup mengetahui posisinya - Anda harus dapat menggunakan informasi ini.

Hari ini kami akan mencoba mengemudi dari titik A ke titik B tanpa melakukan perjalanan di sepanjang ekor kucing yang tertidur di lapangan. Secara khusus, kami akan menjelaskan bagaimana kami membangun rute dan mengontrol kecepatan robot, serta memberi tahu bagaimana memulai semuanya di komputer kami.



Mencoba bertahan dengan sedikit darah


Saat memecahkan masalah ini, Anda bisa mengambil materi siap pakai yang ditulis oleh para profesional, maka tidak akan ada siksaan dan penemuan "sepeda" berikutnya. Kami menggunakan platform roda omni, jadi meskipun ada upaya untuk menggunakan planer lokal yang sudah jadi dari ROS, karena sejumlah alasan mereka menemukan ini tidak menjanjikan. Di bawah ini Anda dapat melihat berapa banyak barang yang dibutuhkan oleh perancang glider standar:

/** * @class TrajectoryPlanner * @brief Computes control velocities for a robot given a costmap, a plan, and the robot's position in the world. */ class TrajectoryPlanner{ friend class TrajectoryPlannerTest; //Need this for gtest to work public: /** * @brief Constructs a trajectory controller * @param world_model The WorldModel the trajectory controller uses to check for collisions * @param costmap A reference to the Costmap the controller should use * @param footprint_spec A polygon representing the footprint of the robot. (Must be convex) * @param inscribed_radius The radius of the inscribed circle of the robot * @param circumscribed_radius The radius of the circumscribed circle of the robot * @param acc_lim_x The acceleration limit of the robot in the x direction * @param acc_lim_y The acceleration limit of the robot in the y direction * @param acc_lim_theta The acceleration limit of the robot in the theta direction * @param sim_time The number of seconds to "roll-out" each trajectory * @param sim_granularity The distance between simulation points should be small enough that the robot doesn't hit things * @param vx_samples The number of trajectories to sample in the x dimension * @param vtheta_samples The number of trajectories to sample in the theta dimension * @param pdist_scale A scaling factor for how close the robot should stay to the path * @param gdist_scale A scaling factor for how aggresively the robot should pursue a local goal * @param occdist_scale A scaling factor for how much the robot should prefer to stay away from obstacles * @param heading_lookahead How far the robot should look ahead of itself when differentiating between different rotational velocities * @param oscillation_reset_dist The distance the robot must travel before it can explore rotational velocities that were unsuccessful in the past * @param escape_reset_dist The distance the robot must travel before it can exit escape mode * @param escape_reset_theta The distance the robot must rotate before it can exit escape mode * @param holonomic_robot Set this to true if the robot being controlled can take y velocities and false otherwise * @param max_vel_x The maximum x velocity the controller will explore * @param min_vel_x The minimum x velocity the controller will explore * @param max_vel_th The maximum rotational velocity the controller will explore * @param min_vel_th The minimum rotational velocity the controller will explore * @param min_in_place_vel_th The absolute value of the minimum in-place rotational velocity the controller will explore * @param backup_vel The velocity to use while backing up * @param dwa Set this to true to use the Dynamic Window Approach, false to use acceleration limits * @param heading_scoring Set this to true to score trajectories based on the robot's heading after 1 timestep * @param heading_scoring_timestep How far to look ahead in time when we score heading based trajectories * @param meter_scoring adapt parameters to costmap resolution * @param simple_attractor Set this to true to allow simple attraction to a goal point instead of intelligent cost propagation * @param y_vels A vector of the y velocities the controller will explore * @param angular_sim_granularity The distance between simulation points for angular velocity should be small enough that the robot doesn't hit things */ TrajectoryPlanner(WorldModel& world_model, const costmap_2d::Costmap2D& costmap, std::vector<geometry_msgs::Point> footprint_spec, double acc_lim_x = 1.0, double acc_lim_y = 1.0, double acc_lim_theta = 1.0, double sim_time = 1.0, double sim_granularity = 0.025, int vx_samples = 20, int vtheta_samples = 20, double pdist_scale = 0.6, double gdist_scale = 0.8, double occdist_scale = 0.2, double heading_lookahead = 0.325, double oscillation_reset_dist = 0.05, double escape_reset_dist = 0.10, double escape_reset_theta = M_PI_2, bool holonomic_robot = true, double max_vel_x = 0.5, double min_vel_x = 0.1, double max_vel_th = 1.0, double min_vel_th = -1.0, double min_in_place_vel_th = 0.4, double backup_vel = -0.1, bool dwa = false, bool heading_scoring = false, double heading_scoring_timestep = 0.1, bool meter_scoring = true, bool simple_attractor = false, std::vector<double> y_vels = std::vector<double>(0), double stop_time_buffer = 0.2, double sim_period = 0.1, double angular_sim_granularity = 0.025);

Ini adalah contoh inisialisasi parameter untuk pengaturan kecepatan dan lintasan secara keseluruhan.

Parameter penting untuk perhitungan lengkap:

  1. Parameter world_model.
  2. Parameter peta biaya: tautan ke peta yang berisi rintangan, serta "ekstensi maya" mereka, dengan mempertimbangkan potensi tabrakan.

Dari kelebihan tumpukan standar, Anda dapat menyoroti ketersediaan dokumentasi dan kemampuan untuk menemukan informasi di forum. Anda dapat membaca lebih lanjut di situs web resmi dengan dokumentasi

Penting untuk menyebutkan bahwa paket ROS ditulis untuk platform roda dua dan di bawah Omni mereka dioptimalkan dengan meningkatkan sudut rotasi yang tersedia ketika bergerak hingga 360 derajat, yang tentunya merupakan penopang.

Setelah menganalisis proyek, kami menyadari bahwa akan ada kesulitan dalam mempelajari dan menambah, serta ada banyak chip berat yang tidak kita butuhkan. Kelihatannya, biarkan saja, tetapi kami menggunakan Odroid xu4 (prosesor yang masih menggunakan Samsung s5), dan hasil kinerja yang menyedihkan, dan ruang untuk sesuatu yang lebih kuat (dan raspberry 4 dan prosesor jetson nano dengan gugup mengeluarkan asap dengan gugup dibandingkan dengan dengan dia) tidak. Saya harus meninggalkan tumpukan standar dan mencoba membuat perencana global, perencana lokal, dan pengatur lintasan sendiri



Perencana global, perencana lokal, pengatur lintasan, dan semuanya


Glider global dan lokal diperlukan untuk mendapatkan arah ke tujuan. Mengapa pemisahan itu perlu? Mengapa Anda tidak bisa mengambil A * dan mengendarainya? Sebagai aturan, perencana global, ketika membangun rute, dapat menggunakan seluruh peta dalam pekerjaannya, sehingga algoritma harus secepat mungkin, bahkan mungkin dengan beberapa penyederhanaan. Untuk memperlancar penyederhanaan ini, mereka juga menggunakan perencana lokal, yang, berdasarkan hasil perencana global (atau hanya beberapa area terbatas di sekitar robot), mencoba memperhitungkan semua nuansa.

Setelah kami membangun rute, kami tahu ke mana robot harus pergi, tetapi bagaimana dia bisa diberitahu tentang ini? Untuk melakukan ini, ada regulator lintasan. Ini menghitung pada kecepatan apa dan ke arah mana robot harus bergerak saat ini agar tidak menyimpang dari lintasan. Dalam banyak hal, paket ini bertanggung jawab atas seberapa cepat dan cantik robot itu.

Selain ketiga entitas ini, ada keempat - server peta, yang memungkinkan Anda untuk dengan mudah memproses keadaan dunia. Ini mengatur bagaimana kita menggambarkan peta, kemungkinan apa yang kita miliki saat bekerja dengan peta, dan, dalam banyak hal, menentukan kecepatan pesawat layang.

Sebelum melanjutkan dengan deskripsi tumpukan navigasi, alangkah baiknya menguraikan alasan mengapa cost_map dipilih sebagai server peta. Secara umum, kami mencoba berbagai opsi untuk penangan peta: Occupancy_grid , Grid_map , Cost_map , tetapi memilih yang terakhir.

Alasan:

  1. Mudah berinteraksi dengan peta.
  2. Ada berbagai iterator yang kita butuhkan dalam berbagai bentuk (lingkaran, linier, persegi panjang, dll.).
  3. Anda dapat menyimpan banyak lapisan peta dengan parameter berbeda.
  4. Manajemen memori yang baik.
  5. Dan yang terpenting, kecepatan. Grid map bekerja dengan tipe ganda dan karena ini beberapa kali lebih lambat daripada server peta yang menggunakan int8 untuk bekerja.

Terlepas dari kenyataan bahwa grid Occupancy juga bekerja dengan int8, itu tidak dapat membanggakan kegunaan yang sama, jadi saya harus meninggalkannya.

Dari peta, kita perlu tahu di mana zona bebas, berbahaya dan tak tertahankan. Untuk setiap objek yang ada di lapangan, kita bisa menyesuaikan bidang inflasi - nilai yang, tergantung pada jarak ke objek, mencirikan permeabilitas sel. Inflasi adalah ekor kucing, mudah untuk tidak menyadarinya, tetapi kemudian Anda akan menyesalinya untuk waktu yang sangat lama. Kami memetakan robot musuh dan menambahkan zona bahaya yang hanya diperhitungkan perencana lokal. Perencana global mengabaikan semua poin, jika itu bukan halangan.

Perencana global


Hal pertama yang mereka tulis dalam navigasi adalah perencana global. Ini didasarkan pada algoritma theta *. Singkatnya, ini adalah A * yang dimodifikasi, di mana penekanannya adalah pada menemukan simpul induk, yang dapat dihubungi secara langsung, mis. tidak ada hambatan baginya. Ini memungkinkan kita untuk membangun jalur yang nyaman dan mulus yang digunakan dalam perencana lokal.


Perbandingan A * dan theta *

Untuk perencana global, kami memiliki file dengan parameter (params / path_planner.yaml) yang menjelaskan topik peta dan topik dengan lokasi semua robot (untuk keempat robot di lapangan, di mana "null" adalah topik dengan data tentang robot saat ini).

 # 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

Ini juga menunjukkan:

  1. salah satu algoritma yang dapat Anda pilih untuk membangun rute,
  2. nama lapisan di mana kita akan membangun rute itu sendiri,
  3. topik tentang posisi kami, tempat data yang disaring dikeluarkan (dalam kasus kami ini adalah kombinasi data lokasi dari kamera dan odometri).

Algoritma pencarian jalur itu sendiri - Theta Star - disorot dalam file terpisah (src / global_planner.cpp) untuk kenyamanan menambahkan algoritma baru:

 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; }

Algoritma untuk menghapus poin tambahan pada jalur RamerDouglasPeucker juga dialokasikan ke file terpisah.
Ini menghilangkan titik dari jalan jika berada di luar jarak tertentu dari garis yang menghubungkan dua titik tetangga.



Perencana lokal


Dia bekerja untuk kita berdasarkan gradient descent di bidang potensial. Sebagai masukan, jalan dari perencana global. Namun, ini tidak semua yang dia mampu. Di local_planner, ada layanan internal untuk memilih mode pembangunan jalur. Ada dua mode operasi secara total: mode titik bergeser di sepanjang gradien, menggunakan beberapa lintasan di peta, serta mode pergeseran, di mana kami segera menghitung kenaikan dalam dua koordinat dan memindahkan titik ke tepi zona aman. Jika titik tersebut jatuh ke beberapa zona seperti itu, maka kita bergeser ke tempat-tempat persimpangan mereka, karena ada yang paling aman.

Mode operasi adalah sebagai berikut: jika tidak ada halangan di jalur dari iterasi sebelumnya, maka kita memecah jalur setiap 2 cm, dan kemudian menggesernya sepanjang gradien, jika tidak kita menggunakan mode operasi perencana lokal kedua.

Opsi kedua cukup ringan dan, seperti perencana global, tidak banyak memuat prosesor. Kami menggunakan beberapa versi dari algoritma ini dan berbagai manipulasi dengan peta. Sebagai contoh, kami mencoba untuk menulis grafik di mana simpul ditemukan setiap 10 cm dan digeser maksimal 4 cm, setelah itu algoritma Dijkstra digunakan pada grafik yang diperoleh untuk menemukan jarak terkecil. Titik akhir dalam kasus ini menggunakan titik offset terdekat. Tetapi algoritma seperti itu lebih cocok untuk perencana global dan kami memutuskan untuk meninggalkan implementasi ini.

Kami juga mencoba menggunakan pembangunan jalur dari awal menggunakan metode gradient descent. Metode ini adalah yang pertama yang kami putuskan untuk ditulis. Ternyata tidak efisien dalam memori (ini menghabiskan lebih dari 400 mb RAM murni, karena menggunakan peta biaya dengan setiap pass) dan lambat. Kontrol frekuensi dimatikan karena optimasi yang buruk dan kecepatannya kurang dari 30 kali per detik, yang tidak cocok untuk kami.

Sebagai hasilnya, kami memutuskan untuk menggunakan gradient descent di bidang potensial berdasarkan jalur planer global. Itu ternyata merupakan algoritma yang ringan dan relatif sederhana, yang sepenuhnya sesuai dengan kami dalam hal kualitas jalur, waktu kerja dan jumlah RAM yang dikonsumsi di wilayah 100-150 mb, yang beberapa kali lebih sedikit daripada di iterasi pertama pengembangan.


Contoh jalur diimbangi oleh planer lokal

Tidak seperti global_planner, parameter dalam local_planner sangat kecil, yang disebabkan oleh kesederhanaannya, karena semua tugas yang paling penting terletak pada 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"

Dalam hal ini, kami mengonfigurasi:

  • Jari-jari zona aman untuk masing-masing robot.
  • Jalur maksimum diimbangi oleh planer lokal.
  • Nama layer peta yang sedang kami kerjakan.

Di kelas yang terpisah semua fungsi yang paling penting dialokasikan. Dalam hal ini, itu adalah kerusakan jalur rekursif, penciptaan glider dan konstruktor virtual dan destruktor.

 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: // virtual double Calculate_Path_Len(); nav_msgs::Path* recursive_path(nav_msgs::Path *path, std::pair<double, double> start, std::pair<double, double> end, double epsilon, int &index) { if (CalcDistance(start, end) < (epsilon) || path->poses.size() > 200) return path; double start_x = (start.first + end.first) / 2.0; double start_y = (start.second + end.second) / 2.0; index = find_out(path, start); geometry_msgs::PoseStamped pose; pose.pose.position.x = start_x; pose.pose.position.y = start_y; path->poses.insert(path->poses.begin() + index, pose); recursive_path(path, start, std::pair<double, double>{start_x, start_y}, epsilon, index); recursive_path(path, std::pair<double, double>{start_x, start_y}, end, epsilon, index); } int find_out(nav_msgs::Path *path, pair_double point) { int index = 0; for (int i = 0; i < path->poses.size(); i++) if (path->poses[i].pose.position.x == point.first && path->poses[i].pose.position.y == point.second) return ++i; return index; } void add_in_path(nav_msgs::Path* Path, geometry_msgs::PoseStamped pose, int& Index) { Path->poses[Index % max_size] = pose; Index++; } double inflation_radius; double our_map[300][200]; int max_size = 49; std::string frame_id; nav_msgs::Path *path = new nav_msgs::Path; nav_msgs::Path *OurPath = new nav_msgs::Path; };

Kelas LocalPlanning diwarisi darinya, di mana seluruh inti glider berada, yaitu, menggeser poin ke tepi zona aman dan memutuskan apa yang harus dilakukan dengan jalur secara khusus.

Semua fungsi lainnya disorot dalam file terpisah fichi.hpp, dan gradient descent di potential_field.hpp. Di bawah ini adalah snapshot dari file ini, yang menunjukkan fungsi untuk membuat bidang potensial pada peta 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]) { // Use some magic for normalisation of Field double max_distance = (CalcAttractivePotential(grid_map::Index(startx, starty), goalx, goaly) + 15); if (max_distance == 0.0) { max_distance = 0.01; } for (cost_map::CircleIterator iterator(cost_map, grid_map::Position(startx / 100.0, starty / 100.0), max_distance / 100.); !iterator.isPastEnd(); ++iterator) { try { grid_map::Index index(*iterator); double uf; uf = cost_map.at("obstacle_inflation_layer", *iterator); // if we on free podouble - field is more less then if it not free podouble if ( uf >= 10) { uf = 3000.0; // CP - is const variable } else uf += CalcAttractivePotential(index, goalx, goaly)/max_distance * 256; our_map[299-index(0)][199-index(1)] = uf; } catch(std::exception& e) { ROS_INFO("Exception! %s", e.what() ); } } }

Regulator lintasan


Terakhir, namun tidak kalah pentingnya, adalah regulator lintasan. Dia bertanggung jawab untuk mengubah jalur perencana lokal ke jalur dan memberikan kecepatan untuk langkah saat ini.

Versi pertamanya, yang kami gunakan pada final Eurobot 2018, adalah campuran dari pengontrol percepatan dan pengereman, di mana vektor dinormalisasi ke titik berikutnya di jalur, relatif terhadap jarak ke titik akhir.

Pengontrol PID, singkatnya, adalah jumlah dari tiga status sistem yang membantu memperbaiki kesalahan sistem dan acak yang kadang-kadang terjadi.

Fungsi-fungsi ini dipilih secara empiris dan tergantung pada jarak ke titik akhir di jalan (bisa kuadratik, kubik, terbalik, tetapi kemudian kita menetap di kuadratik). Ini berhasil, tetapi satu-satunya hal yang tidak sesuai dengan kami adalah bahwa robot tidak dapat mengerem dalam waktu dengan kecepatan di atas 0,7 meter per detik. Karena itu, ketika saatnya tiba, kami memutuskan untuk membangun kembali seluruh algoritma.

Iterasi pertama dalam perjalanan ke lintasan baru adalah penggantian vektor yang akan kami tuju. Sekarang jumlah vektor untuk ketiga berikutnya dengan koefisien yang berbeda. Iterasi kedua adalah menulis Jerk Minimum. Singkatnya, ini adalah konstruksi polinomial derajat 5, di mana koordinat x dan y tergantung pada waktu kedatangan ke setiap titik.


Gambar menunjukkan grafik dari salah satu koordinat versus waktu, serta kecepatan di sepanjang koordinat ini

Regulator lintasan jenis ini lebih cocok untuk kita, karena memerlukan manipulasi yang lebih sedikit dengan pemilihan koefisien yang berbeda, karena semua koefisien adalah nilai dalam polinomial, yang dihitung berdasarkan waktu kedatangan, kecepatan dan percepatan saat ini, kecepatan saat ini dan percepatan, kecepatan keluaran dan percepatan.

Hasil penulisan ulang lintasan adalah bahwa kami berhasil menggandakan kecepatan rata-rata robot.

Seperti dalam dua kasus sebelumnya, semua fungsi utama disorot dalam file terpisah untuk memudahkan interaksi. Kali ini, kelas PlannerTrajectory bertanggung jawab untuk membangun lintasan berdasarkan 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; // Defualt or get from param // double max_velocity = 100; double coef_x[6]; double coef_y[6]; int frequency = 30; // Defualt or get from param // int index; public: PlannerTrajectory(cost_map::CostMap *costmap_, const int &frequency_, const double &max_velocity_, const double &average_velocity_) { average_velocity = average_velocity;; max_velocity = max_velocity_; costmap_ptr = costmap_; frequency = frequency_; Goal_pred.pose.position.x = 0.0; Goal_pred.pose.position.y = 0.0; }

Foto menunjukkan semua variabel yang dideklarasikan yang kami gunakan.

Segala sesuatu yang lain disorot dalam file lain (termasuk / trajectory_regulator.h): menerima poin dari topik, memutuskan apakah akan pergi ke titik berikutnya (jika itu dalam hambatan, maka kita tidak akan pergi) dan banyak lagi.

Bermigrasi ke ROS Melodic


Sampai tahun lalu, kami menggunakan RTE - ROS Kinetic lte. Dia umumnya cocok untuk kita, tetapi dukungannya akan berakhir tahun depan, dan banyak paket yang kita butuhkan mulai keluar secara eksklusif untuk ROS Melodic. Dan kemudian ternyata costmap_server yang kami gunakan tidak di bawah Melodic.

Terjadi masalah saat memproses data dari kartu.

Kami memilih peta Grid, karena tumpukannya mirip, tetapi awal peta berada di tempat yang berbeda, dan nilai peta itu sendiri bervariasi dari 0 hingga 1. Ini telah menjadi masalah besar di seluruh tumpukan navigasi. Jika sebelumnya perencana global diluncurkan 50 kali per detik (ada batasan frekuensi, dan karena itu prosesor tidak digunakan terlalu banyak, bahkan dalam setengah dari satu utas), sekarang ia membuka jalan setiap dua detik dan menganggapnya buruk: benar-benar memuat satu inti. Dalam 2 detik, robot bisa melintasi seluruh peta. Ini tidak sesuai dengan kami, dan upaya untuk memparalelkan proses ini berakhir dengan kegagalan, karena dengan demikian tidak ada kinerja yang tersisa untuk proyek lain (dengan mempertimbangkan biaya paralelisasi).

Kami memutuskan untuk mengubah tumpukan lagi, meninggalkan peta kisi demi kisi hunian. Masalah baru telah muncul - ketidakmampuan untuk menyimpan beberapa versi peta pada saat yang sama (misalnya, lengkap, dengan semua hambatan, dan peta statis, dengan hanya hambatan dinamis). Saya harus mengubah setengah dari kode, yang tidak terlalu dapat diandalkan. Oleh karena itu, kami memutuskan untuk mencari solusi alternatif untuk masalah ini.

Server Peta Biaya


Setelah pencarian yang panjang, kami menemukan peta fork costmap_serverr: https://github.com/lelongg/cost_map.git - sangat berguna untuk sistem fork kami.



Dan sekarang, alih-alih hanya melayani peta kisi, untuk pengiriman, kami berhasil menghitung kemungkinan lokasi musuh berdasarkan prediksi filter Kalman.



Salah satu hal terpenting untuk server peta adalah file peta, yang digunakan untuk pembuatan awal semua lapisan, yang selanjutnya hanya diperbarui. Ini adalah gambar png biner, di mana hitam adalah hambatan dan putih adalah zona bebas.



Ada juga file pengaturan untuk konfigurasi cost_map_server. Ini berisi topik dengan poin musuh, zona inflasi dan ukuran alun-alun, yang juga dapat digunakan untuk menempatkan zona berbahaya musuh di peta.

 ########################### # 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

Semua lapisan diterbitkan hanya jika seseorang berlangganan:
 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); } }

Jalankan di komputer Anda


Untuk memulai seluruh tumpukan, Anda harus:

  • Masukan ROS
  • roslaunch cost_map_server cost_map_server_alone.launch - untuk memulai peta
  • roslaunch global_planner global_planner.launch - luncurkan planer global dengan parameter
  • rosparam memuat $ (temukan local_planner) /param/param.yaml
  • rosrun local_planner local_planning
  • rosrun trajectory_regulator trajectory_regulator
  • rosrun global_planner penggerak
  • rosrun rviz
  • tambahkan inflation_layer
  • Sekarang mengirim pesan ke topik / gp / tujuan, kami mengirim robot ke titik yang diinginkan

Sebagai hasil dari peluncuran semua item, Anda akan memiliki simulasi yang siap untuk meluncurkan tumpukan kami di komputer Anda. Garpu yang diperlukan


Awalnya, kami membutuhkan navigasi yang akan membantu robot kami berkendara dengan indah, cepat dan akurat pada roda omni. Selama persiapan kompetisi, tidak ada satupun kucing yang terluka, dan robotnya tampan. Sebagai hasilnya, kami memiliki tumpukan navigasi ringan untuk kontes yang mirip dengan eurobot, yang kami cukup puas.

Bagi kami, tumpukan ini lebih baik daripada yang standar, tapi ...

Telegram kami: t.me/SetUpSber
Repositori semua kreativitas kami

Source: https://habr.com/ru/post/id479636/

More articles:


All Articles