这是setUP团队的第二篇文章，介绍了我们在为Eurobot Open竞赛创建自动机器人并为此使用ROS方面的经验。
第一篇文章介绍了机器人的力学和总体架构。机器人在平坦的地面上行驶，大多数障碍是事先知道的，但是，阴险的对手可以尝试窃取我们的资源（我们有时想吃掉几十个额外的积分），同时我们希望尽快驶向所需的地点而不碰障碍物。 从现场的外部摄像机，我们可以获得敌人的位置数据，并知道他现在的位置。 但是，仅仅知道他的位置还不够-您需要能够使用此信息。
今天，我们将尝试从A点驶向B点，而不必沿着在赛场上睡着了的猫尾巴走。 特别是，我们将说明如何构建路线并控制机器人的速度，并说明如何启动计算机上的所有内容。
试图一点点流血
解决此问题时，您可以获取专业人士编写的现成材料，这样就不会有下一个“自行车”的折磨和发明。 我们使用的是全轮式平台，因此尽管尝试使用ROS提供的现成的本地刨床，但由于多种原因，他们认为这是没有希望的。 在下面，您可以看到标准滑翔机的设计师需要多少东西：
class TrajectoryPlanner{ friend class TrajectoryPlannerTest;
这是用于调节速度和整体轨迹的参数初始化的示例。完整计算的重要参数：
- 参数world_model。
- 成本图参数：到包含障碍物及其“虚拟延伸”的地图的链接，其中考虑了潜在的碰撞。
在标准堆栈的优点中，您可以突出显示文档的可用性以及在论坛上查找信息的能力。
您可以在官方网站上阅读更多文档值得一提的是，ROS软件包是为两轮平台编写的，在Omni之下，它们通过增加到360度时的可用旋转角度进行了优化，这无疑是一个拐杖。
对项目进行分析后，我们意识到学习和补充存在困难，并且不需要很多繁重的芯片。 看起来，让它们成为现实，但是我们使用了Odroid xu4（该处理器仍在三星s5上使用），性能结果令人沮丧，而更强大的设备的空间（与之相比，树莓4和jetson nano处理器则紧张地抽烟了）和他在一起） 我不得不放弃标准堆栈，尝试自己创建全局计划程序，本地计划程序和轨迹管理器
全球规划师，本地规划师，轨迹管理器以及所有这些
需要全球和本地滑翔机才能获得前往目的地的路线。 为什么需要分隔？ 您为什么不能只乘
A *乘它呢？ 通常，全局规划人员在构造路线时可以在其工作中使用整个地图，因此算法应该尽可能快，甚至可以进行一些简化。 为了简化这些简化过程，他们还使用了本地计划程序，该程序根据全局计划程序的结果（或仅在机器人周围的有限区域内）尝试考虑所有细微差别。
构建路线后，我们知道机器人应该去哪里，但是如何得知他呢？ 为此，有一个轨迹调节器。 它计算机器人此时应以什么速度和朝哪个方向运动，以免偏离轨迹。 在许多方面，此软件包负责机器人运行的速度和美观程度。
除了这三个实体之外，还有第四个实体-地图服务器，它使您可以方便地处理世界状况。 它设置了我们描述地图的方式，使用地图时有什么可能性，并在许多方面决定了滑翔机的速度。
在继续介绍导航堆栈之前，最好概述一下为什么选择cost_map作为地图服务器的原因。 通常，我们为地图处理程序尝试了不同的选项：
Occupancy_grid ，
Grid_map ，
Cost_map ，但选择了后者。
原因：
- 方便地与地图互动。
- 我们需要各种形式（圆形，线性，矩形等）的迭代器。
- 您可以使用不同的参数存储多个地图图层。
- 良好的内存管理。
- 最重要的是速度。 网格地图可使用双精度类型，因此，它比使用int8起作用的那些地图服务器要慢几倍。
尽管Occupancy网格也可以与int8一起使用，但是它不能拥有相同的可用性，因此我不得不放弃它。
从地图上，我们需要知道自由，危险和不可抗拒的区域在哪里。 对于场上的每个对象，我们都可以调整充气场-该值取决于与对象的距离，该值表示单元的渗透性。 通货膨胀是猫的尾巴，很容易注意到，但您会后悔很长一段时间。 我们为敌方机器人绘制地图，并添加仅本地计划人员考虑的危险区域。 全球规划师将忽略所有要点，如果它们不是障碍。
全球规划师
他们在导航中写的第一件事是全球规划师。 它基于theta *算法。 简而言之，这是一个修改后的
A * ，其重点是找到可以直接到达的父节点，即 她没有任何障碍。 这使我们能够构建在本地计划程序中使用的方便且平滑的路径。
A *和theta *的比较对于全局规划师，我们有一个带有参数的文件（params / path_planner.yaml），该文件描述地图主题和所有机器人的位置的主题（对于现场的所有四个机器人，其中“ null”是带有有关当前机器人数据的主题）。
# 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
它还表明：
- 您可以选择的一种算法来构建路线，
- 我们将在其自身上构建路线的图层的名称，
- 一个关于我们位置的主题，在该位置发布已过滤的数据（在我们的情况下，它是来自照相机和里程表的位置数据的组合）。
路径搜索算法本身-Theta Star-在单独的文件（src / global_planner.cpp）中突出显示，以方便添加新算法：
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; }
删除RamerDouglasPeucker路径上多余点的算法也分配给单独的文件。
如果距连接两个相邻点的线的距离超出给定距离，它将从路径中删除点。
当地规划师
他在势场的梯度下降基础上为我们工作。 作为输入，来自全局计划程序的路径。 但是，这还不是他所能做到的。 在local_planner中，有用于选择路径构造模式的内部服务。 总共有两种操作模式：在地图上使用多次通过沿梯度平移点的模式以及平移模式，在平移模式下，我们立即在两个坐标中计算增量并将该点移动到安全区的边缘。 如果该点落入多个此类区域，则我们将移至其交点的位置，因为在那里最安全。
操作模式如下：如果上一次迭代的路径中没有障碍物，则我们每2 cm断开路径，然后沿梯度移动它，否则我们使用第二个局部规划器操作模式。
第二个选项非常轻巧，并且像全局计划器一样，不会过多地加载处理器。 我们对地图使用了该算法的多个版本以及各种操作。 例如，我们尝试编写一个图形，在该图形中，每隔10 cm会找到一个顶点，并且顶点之间的最大偏移量为4 cm，然后在获得的图形上使用Dijkstra算法查找最小的距离。 在这种情况下，端点使用最近的偏移点。 但是这种算法更适合全局规划人员，因此我们决定放弃此实现。
我们还尝试使用梯度下降法从头开始使用路径构建。 该方法是我们决定编写的第一个方法。 结果证明内存效率不高（它占用了超过400 mb的纯RAM，因为它每次都使用了成本映射）并且速度很慢。 由于优化不佳而关闭了频率控制，并且速度低于每秒30次，这不适合我们。
结果，我们决定根据整体刨床的路径在势场中使用梯度下降。 事实证明，这是一种轻量级且相对简单的算法，在路径质量，工作时间和100-150 mb区域内使用的RAM量方面完全适合我们，这比开发的第一次迭代少了几倍。
本地平面铣刀偏移路径的示例与global_planner不同，local_planner中的参数非常小，这是因为它很简单，因为所有最重要的任务都由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"
在这种情况下，我们配置：
- 每个机器人的安全区域半径。
- 本地刨床的最大路径偏移量。
- 我们正在使用的地图图层的名称。
在一个单独的类中，分配了所有最重要的功能。 在这种情况下，这是路径的递归分解，创建滑翔机以及虚拟构造函数和析构函数。
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:
LocalPlanning类是从该类继承而来的，该类是滑翔机整个核心所在的位置，即，将点移动到安全区的边缘，并具体决定如何处理该路径。
所有其他功能在单独的文件fichi.hpp中突出显示，并且梯度下降在potential_field.hpp中。 以下是此文件的快照，其中显示了在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]) {
轨迹调节器
最后但并非最不重要的是轨迹调节器。 他负责将本地计划程序路径转换为路径，并给出当前步骤的速度。
我们在Eurobot 2018决赛中使用的第一个版本是加速和制动pid控制器的混合体，其中矢量相对于到终点的距离归一化为沿途的下一个点。
简而言之，PID控制器是有助于修复系统的系统三个状态和有时会出现的随机错误的总和。
这些函数是根据经验选择的，并且取决于到路径终点的距离（它可以是二次方，三次方，逆方，但是我们定为二次方）。 这行得通，但唯一不适合我们的是机器人无法以每秒0.7米以上的速度及时制动。 因此，当时间到来时，我们决定重建整个算法。
通往新轨迹的第一个迭代是替换我们要去的向量。 现在它是下三个系数不同的向量的总和。 第二次迭代是编写最小混蛋。 简而言之，这是第5次多项式的构造，其中x和y坐标取决于到达每个点的时间。
该图显示了坐标之一与时间的关系图，以及沿该坐标的速度这种类型的轨迹调节器更适合我们，因为它需要较少的操作来选择不同的系数，因为所有系数都是多项式中的值，这些值是根据到达时间，当前速度和加速度，输出速度和加速度来计算的。
轨迹重写的结果是我们设法使机器人的平均速度提高了一倍。
与前两种情况一样，所有主要功能都在单独的文件中突出显示，以便于交互。 这次，PlannerTrajectory类负责基于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;
照片显示了我们使用的所有已声明的变量。其他所有内容都在另一个文件（包括/ trajectory_regulator.h）中突出显示：从主题接收点，确定是否要到达下一个点（如果有障碍，那么我们就不走），等等。
迁移到ROS Melodic
直到去年，我们才使用ROS的LTE版本-ROS Kinetic。 他通常适合我们，但他的支持将在明年结束，并且我们需要的许多软件包开始专门针对ROS Melodic提供。 然后事实证明，我们使用的costmap_server不在Melodic之下。
处理卡中的数据时出现问题。
我们选择了Grid地图，因为堆栈是相似的，但是地图的开头在不同的位置，并且地图本身的值从0到1不等。这已成为整个导航堆栈中的大问题。 如果早些时候全局规划器每秒启动50次（存在频率限制，因此即使在一个线程的一半中也不会使用太多处理器），现在每两秒钟铺路并认为它不好：它完全加载了一个内核。 在2秒内，机器人可以越过整个地图。 这不适合我们，并且尝试并行化此过程的尝试均以失败告终，因为这样一来，其他项目就没有性能可言（考虑到并行化的成本）。
我们决定再次更改堆栈，放弃网格图，转而使用占用网格。 出现了一个新问题-无法同时存储多个版本的地图（例如，具有所有障碍的完整地图以及仅具有动态障碍的静态地图）。 我将不得不更改一半的代码，这并不是特别可靠。 因此，我们决定为该问题寻找替代解决方案。
成本图服务器
经过长时间的搜索，我们发现了分叉地图costmap_serverr：
https :
//github.com/lelongg/cost_map.git-对我们的分叉系统非常有用。
现在，我们不再仅仅为栅格地图提供服务，而是为了交付，我们设法根据卡尔曼滤波器的预测来计算敌人的可能位置。
地图服务器最重要的事情之一就是地图文件，该文件用于初始创建所有图层，随后仅对其进行更新。 这是一个二进制png图片，其中黑色是障碍物，白色是自由区。
还有一个用于cost_map_server配置的设置文件。 它包含带有敌人要点，通货膨胀区和正方形大小的主题，还可以用于在地图上放置敌人的危险区。
########################### # 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
仅当有人订阅所有图层时，才会发布所有图层：
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); } }
在电脑上运行
要启动整个堆栈，您必须：
- 放ROS
- roslaunch cost_map_server cost_map_server_alone.launch-启动地图
- roslaunch global_planner global_planner.launch-使用参数启动全局刨床
- rosparam加载$（找到local_planner）/param/param.yaml
- rosrun local_planner local_planning
- rosrun trajectory_regulator rosjectory_regulator
- rosrun global_planner移动器
- 罗斯伦·维兹
- 添加通胀层
- 现在向主题/ gp /目标发送消息，我们将机器人发送至所需位置
启动所有项目的结果是，您将进行仿真以准备在计算机上启动我们的堆栈。必要的叉子最初，我们需要导航，以帮助我们的机器人在全向轮上精美，快速，准确地行驶。在比赛的准备过程中，没有一只猫受伤，而且机器人很帅。因此，我们拥有一个类似于Eurobot的轻量级导航堆栈，我们对此感到非常满意。对于我们来说，此堆栈比标准堆栈要好，但是...我们的电报：t.me/SetUpSber我们所有创造力的存储库