技术分享 | Prometheus避障—A_star算法代码阅读

在使用P系列无人机过程中，相信大家都知道P系列无人机的避障功能，在之前的技术分享中：Prometheus（P450）-室内外避障，给看官们详细讲解了避障的算法原理以及实际飞行操作演示的文章。今天，为了方面更加快速的进行二次开发，着重解读一下A星算法代码的实现。

prometheus项目中，A_star算法功能包存放在Prometheus/Modules/planning/global_planning路径下，如图所示。进入global_planning功能包的src文件夹下，我们可以看到5个cpp文件，如下图所示，A_star.cpp:实现A_star规划的功能的主要部分；global_planner.cpp:主要定义了一些消息的接收端，如飞机状态、点云数据、目标点状态等消息的接收，为A_star算法做准备工作的文件；global_planner_node.cpp:主函数，创建一个global_planner的对象来调用a_star算法；occupy_map.cpp:它主要承担将传感器获得的障碍物的点云数据转化为栅格地图中障碍物的信息，为A_star规划算法做准备；tools.cpp:主要用于向终端发布，用户所关心的一些消息，或是反应程序进程的消息；下面我们来看看A_star规划具体是怎么实现的。首先我们进入主函数会发现它只有短短的几行，如下图所示。其中最主要的代码，已用红框圈出，它申明了一个Global_Planner类的对象叫global_planner,然后调用global_planner对象的init函数；接下来我们进入init函数，init函数是定义在global_planner.cpp这个文件下的，如图所示。它首先是读取一些必要的参数，每个参数所代表的含义已在图中给出；然后它会订阅目标点、无人机状态、地图的更新方式等等，订阅端收到消息，会将收到的消息传入回调函数，对数据进行处理；然后还初始化了一些发布者，用来发布对应的消息，每个函数对应什么功能已在图中给出；接下来我们需要先初始化一个A_star规划器，代码如图所示，我们申明了一个叫Astar的对象，和指向它的指针Astar_ptr;然后将句柄传入A_star对象的init函数中；这个函数是申明在A_star.cpp文件下的，如图所示。首先也是先读取一些必要的参数，具体的参数信息已在图上给出；申明了一个tie_breaker_,这个参数主要是为了防止出现对称性的路径，所以在算出的某条路径的代价的基础上，放大一个极小量，以减少对称路径出现的概率；path_node_pool_是用来存放规划器已经访问过的节点的容器，容器中的每一个成员表示栅格地图上的每个点，它包含了地图点的位置、状态等等；然后使用Occupy_map_ptr对象的init函数初始化地图；

如图，为初始化地图的代码，其中主要的代码已用红框标出，表示x,y,z三个维度计算公式为大小除以它的分辨率，上图即为0.2，也就是说1米的长度会在栅格地图中分解为5个栅格，然后我们计算出每个维度的具体栅格数的大小，就可以计算出整个栅格地图的大小；然后将occupancy_buffer_这个容器申明为栅格地图的大小，它是用来存放地图中障碍物信息的容器，全部由0,1组成，0表示这个栅格点没被占据，1表示该栅格点上有障碍物，将它初始化全为0；

然后回到A_star.cpp，读取初始化后的地图的参数，如下图所示；

执行完A_star对象的init函数，返回到函数调用点，即Global_Planner::init函数中，如图所示；

接着往下执行初始化规划器的状态和指令，无论是仿真还是实际飞行，程序都是一样的，只是我们输入的参数有一些差别，参数输入完成之后，程序进入红框部分，ros的消息回调处理函数，前面我们申明的一些回调函数，如下图，

他们不会收到消息立马就回调，而是一直在后台接收消息，直到程序运行到ros::spinOnce()才会统一执行一次回调函数；goal_cb函数，主要功能就是接收用户给定的目标点；drone_state_cb函数，读取飞控返回的飞机状态；mainloop_cb主循环回调函数，负责最主要的规划问题；进入mainloop_cb函数，

以上代码是对状态进行检查，确保当前状态符合规划的要求；以上代码，检查exec_state状态，我们重点阅读一下红框部分的代码，首先是重置一下A_star规划器，代码如下，清空了扩展节点集、路径节点集，申明一个空白的优先级序列，将空白优先级序列，与open_set_集交换，即清空开集；把之前处理过的节点挨个取出，恢复初始化状态，最后把use_node_num_和iter_num_置0，回归初始化状态，执行完毕。返回mainloop_cb函数，执行下一句，即进入Astar::search函数，如下，首先检查，目标点处是否有障碍物，无障碍物才会继续执行下面的程序；然后初始化起始点，申明一个node结构体的对象，用来存放起点的数据，具体代码如上；然后把起点对象放入开集中，准备进入主循环！主循环代码如上，首先检查open_set不为空，则取出开集中f值最小的节点，然后判断取出的节点是否达到了终止条件。如果达到了终止条件，就将当前节点赋给terminate_node,然后将其传入retrievePath函数，往回搜寻，得到路径存入path_node_容器中，代码如下；如果没达到终止条件，则将当前节点弹出开集放入闭集中，然后对当前节点进行扩展，代码如下；一直这样不断搜索，直到开集为空，或是达到终止条件，或是达到最大搜索次数，才结束循环。以上就是Prometheus项目中，A_star算法的代码具体实现流程。最后附上A_star仿真运行截图，以上就是P系列无人机激光避障功能的代码研读了，相信大家看完之后，对代码有了深一层的理解，也欢迎大家能够参与进来优化代码，提交issues，在此开源项目Prometheus小组成员感谢支持，感谢贡献。

• End -

技术发展的日新月异，阿木实验室将紧跟技术的脚步，不断把机器人行业最新的技术和硬件推荐给大家。看到经过我们培训的学员在技术上突飞猛进，是我们培训最大的价值。如果你在机器人行业，就请关注我们的公众号，我们将持续发布机器人行业最有价值的信息和技术。

阿木实验室致力于前沿IT科技的教育和智能装备，让机器人研发更高效！