【技术实现步骤摘要】
本专利技术涉及导航，特别是涉及导航技术中的全覆盖导航方法。
技术介绍
1、通常导航机器人根据其工作内容需要设定导航方式，机器人导航方式其中包括在一些情景下经常出现需要机器人对所指定的区域进行覆盖式导航，现有的一些覆盖式导航的方法大多存在着对场景适应性不高、导航死区不稳定、实际导航流程繁琐和对机器人性能占用大等问题。还有，现有的机器人覆盖式导航的方法，在一次导航流程中大多只能使用一种类型的导航路径，如弓字形或回字形，这种方式会导致导航对多变或用户自主规划的场地适应性不高，可能出现导航死区过大的问题。此外，现有覆盖式导航的方法大多仅支持一次性导航路径部署，如新增区域规划则需要重新部署，无法实现导航路径的实时计算部署。有鉴于此，现有的导航机器人的覆盖式导航技术还有待改进提升，以适应一些导航工作的方便使用需要。
技术实现思路
1、本专利技术的目的在于提供一种能够实现导航路径的实时计算部署，自适应路径规划的一种基于nav2行为树逻辑的全覆盖导航方法。
2、为实现上述目的，本专利技术的技术方案是：一种基于nav2行为树逻辑的全覆盖导航方法，方法步骤如下：
3、s1、预构建好基于nav2行为树的导航逻辑行为树；
4、s2、在所述导航逻辑行为树的逻辑下，系统导航规划模块接收有需要规划区域的区域信息后进行规划区域划分，得到区域划分信息；
5、s3、在所述导航逻辑行为树的逻辑下，系统导航规划模块对区域划分信息进行区域代价地图确认和起点选择；
7、所述导航逻辑行为树包括分别自定义命名为area_des_fallback、area_des和wait的节点，所述area_des_fallback节点为一个顺序节点，其功能为请求area_des节点更新区域，所述有需要规划的区域包括接收用户通过远程设备自定义导航规划区域；在步骤s2中，若所述area_des_fallback节点请求area_des节点更新区域，所述area_des节点返回为失败状态，则进入wait节点等待服务器下发区域信息，所述系统导航规划模块在收到区域信息后，通过area_des_fallback节点将该区域信息的区域栅格化添加到系统导航规划模块当前所使用的栅格地图中，从而得到区域划分信息。
8、在所述area_des_fallback节点内通过costmap_2d的构造函数构造一个costmap对象库，假设area_des_fallback节点接收到区域信息中多边形区域的顶点坐标，所述area_des_fallback节点将依据该坐标使用costmap对象库中包括polygonoutlinecells的函数提取出区域信息中多边形区域以及该多边形区域的边界，然后获取的多边形区域从世界坐标系下的区域转换为当前栅格坐标下的区域，被转换为当前栅格坐标占用的多边形区域的边界将被代价赋予函数赋予致命障碍的代价值。
9、所述导航逻辑行为树还包括分别自定义命名为prepare_plan、update_costmap、start_point、des、presuppose和ergodic的节点，所述prepare_plan为一个条件节点，其功能为先判断update_costmap节点是否有更新地图，在步骤s3中，当先判断update_costmap节点是否有更新地图返回为成功状态后，执行start_point节点进行区域代价地图确认和起点选择。
10、所述update_costmap节点在首次中首先调用costmap对象库的getcostmap函数获取系统导航规划模块中的静态代价地图，后续仅更新区域划分信息，更新时，首先需要将已有栅格地图进行重栅格化，重栅格合成的栅格数根据机器人直径进行设定，如此进行区域代价地图确认。
11、所述prepare_plan节点在更新区域划分信息的同时进入start_point节点，该节点为一个顺序节点，首先进入由取反功能装饰的所述des节点修饰的presuppose节点，该节点将根据多边形区域的顶点坐标获取包括多边形区域的左上角、右上角、左下角、右下角和中心为预设初始位置存入起点备选列表中，所述预设初始位置中左上角、右上角、左下角和右下角的确定为各角点的坐标向中心移动一个机器人直径位置，计算各预设初始位置是否在多边形区域内部以及是否处于可以运行的栅格上，若各预设初始位置不在多边形区域内部或在不可运行的栅格上，则该预设初始位置判断为不可运行起点并从起点备选列表中删除，若所有预设初始位置均为不可运行起点，则会进入ergodic节点遍历栅格地图寻找未被障碍栅格覆盖且在多边形区域内部可以启动的新起点，并存入起点备选列表中，如此进行所述起点选择。
12、计算各预设初始位置是否在多边形区域内部的方法为引出由当前点的射线判断其穿过的多边形区域的边的数量是否为奇数，为奇数则不在多边形区域内部，判断是否处于可运行栅格上使用costmap的getcost函数实现，若获取的代价值表示处于障碍物附近或处于障碍物上，则该预设初始位置为不可运行起点。
13、所述导航逻辑行为树还包括分别自定义命名为plan和plan_des的节点，所述prepare_plan节点为plan的子节点，所述plan_des还包括有多个子节点，所述plan节点为一个顺序节点，当子节点prepare_plan返回为成功状态时表示步骤s3已完成区域代价地图确认与起点选择，接着进入plan_des节点获取通过该节点计算得出的最终决定的全覆盖规划路径。
14、所述全覆盖规划路径为通过所述plan_des节点的多个子节点将根据栅格法计算出由所述起点选择的起点开始的全覆盖导航路径；具体的，根据在步骤s3中所述区域代价地图确认得到的更新后的区域代价地图，以步骤s3中所述起点选择得到的起点为起点，并以地图坐标系x轴正方向对重栅格后的代价重栅格地图进行遍历，当所遍历的重栅格中包含代价值为致命障碍的栅格时，将该重栅格标记为致命障碍，并调整遍历方向；在遍历的过程中，每遍历一个重栅格便将该重栅格的中心栅格利用costmap对象库中的maptoworld函数将该栅格坐标转换为地图坐标并存储在一个点集p中，该点集p表示执行全覆盖导航流程时机器人导航所需要经过的点集；根据步骤s3起点备选列表中起点的类型不同，预选路径的计算方式有所不同，若所述起点选择的起点为左上角、右上角、左下角和右下角这4个角点，则当调整遍历方向时依据角点与中心点的相对位置关系旋转180度形成弓字形路径并计入点集p，若所述起点选择的起点为中心则旋转90度形成回字形路径并计入点集p；所述plan_des节点在获得不同起点的多个点集p后，需要对比各个点集p中包含的目标点的个数以及遍历重栅格地图时所调整遍历方向的次数，筛选出其中包含目标点最多且调整方向次数最少的点集p作为最终决定选取的本文档来自技高网...
【技术保护点】
1.一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，方法步骤如下：
2.如权利要求1所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述导航逻辑行为树包括分别自定义命名为Area_Des_Fallback、Area_Des和Wait的节点，所述Area_Des_Fallback节点为一个顺序节点，其功能为请求Area_Des节点更新区域，所述有需要规划的区域包括接收用户通过远程设备自定义导航规划区域；在步骤S2中，若所述Area_Des_Fallback节点请求Area_Des节点更新区域，所述Area_Des节点返回为失败状态，则进入Wait节点等待服务器下发区域信息，所述系统导航规划模块在收到区域信息后，通过Area_Des_Fallback节点将该区域信息的区域栅格化添加到系统导航规划模块当前所使用的栅格地图中，从而得到区域划分信息；
3.如权利要求2所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述导航逻辑行为树还包括分别自定义命名为Prepare_Plan、Update_Costmap、Start_Point
4.如权利要求3所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，计算各预设初始位置是否在多边形区域内部的方法为引出由当前点的射线判断其穿过的多边形区域的边的数量是否为奇数，为奇数则不在多边形区域内部，判断是否处于可运行栅格上使用costmap的getcost函数实现，若获取的代价值表示处于障碍物附近或处于障碍物上，则该预设初始位置为不可运行起点。
5.如权利要求3或4所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述导航逻辑行为树还包括分别自定义命名为Plan和Plan_Des的节点，所述Prepare_Plan节点为Plan的子节点，所述Plan_Des还包括有多个子节点，所述Plan节点为一个顺序节点，当子节点Prepare_Plan返回为成功状态时表示步骤S3已完成区域代价地图确认与起点选择，接着进入Plan_Des节点获取通过该节点计算得出的最终决定的全覆盖规划路径。
6.如权利要求5所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述全覆盖规划路径为通过所述Plan_Des节点的多个子节点将根据栅格法计算出由所述起点选择的起点开始的全覆盖导航路径；具体的，根据在步骤S3中所述区域代价地图确认得到的更新后的区域代价地图，以步骤S3中所述起点选择得到的起点为起点，并以地图坐标系x轴正方向对重栅格后的代价重栅格地图进行遍历，当所遍历的重栅格中包含代价值为致命障碍的栅格时，将该重栅格标记为致命障碍，并调整遍历方向；在遍历的过程中，每遍历一个重栅格便将该重栅格的中心栅格利用costmap对象库中的maptoworld函数将该栅格坐标转换为地图坐标并存储在一个点集P中，该点集P表示执行全覆盖导航流程时机器人导航所需要经过的点集；根据步骤S3起点备选列表中起点的类型不同，预选路径的计算方式有所不同，若所述起点选择的起点为左上角、右上角、左下角和右下角这4个角点，则当调整遍历方向时依据角点与中心点的相对位置关系旋转180度形成弓字形路径并计入点集P，若所述起点选择的起点为中心则旋转90度形成回字形路径并计入点集P；所述Plan_Des节点在获得不同起点的多个点集P后，需要对比各个点集P中包含的目标点的个数以及遍历重栅格地图时所调整遍历方向的次数，筛选出其中包含目标点最多且调整方向次数最少的点集P作为最终决定选取的全覆盖规划路径。
7.如权利要求1-6任意一项的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，该全覆盖导航方法替代nav2的Follow_Path原生行为树中的右侧树的直线路径规划方法应用，机器人的控制器对接入Follow_Path原生行为树中左侧树的运动部分，该运动包部分包括对运动频率的控制及运动目标点的发布更新。
...【技术特征摘要】
1.一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，方法步骤如下：
2.如权利要求1所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述导航逻辑行为树包括分别自定义命名为area_des_fallback、area_des和wait的节点，所述area_des_fallback节点为一个顺序节点，其功能为请求area_des节点更新区域，所述有需要规划的区域包括接收用户通过远程设备自定义导航规划区域；在步骤s2中，若所述area_des_fallback节点请求area_des节点更新区域，所述area_des节点返回为失败状态，则进入wait节点等待服务器下发区域信息，所述系统导航规划模块在收到区域信息后，通过area_des_fallback节点将该区域信息的区域栅格化添加到系统导航规划模块当前所使用的栅格地图中，从而得到区域划分信息；
3.如权利要求2所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述导航逻辑行为树还包括分别自定义命名为prepare_plan、update_costmap、start_point、des、presuppose和ergodic的节点，所述prepare_plan为一个条件节点，其功能为先判断update_costmap节点是否有更新地图，在步骤s3中，当先判断update_costmap节点是否有更新地图返回为成功状态后，执行start_point节点进行区域代价地图确认和起点选择；
4.如权利要求3所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，计算各预设初始位置是否在多边形区域内部的方法为引出由当前点的射线判断其穿过的多边形区域的边的数量是否为奇数，为奇数则不在多边形区域内部，判断是否处于可运行栅格上使用costmap的getcost函数实现，若获取的代价值表示处于障碍物附近或处于障碍物上，则该预设初始位置为不可运行起点。
5.如权利要求3或4所述的一种基于nav2行为树逻辑的全覆盖导航方法，其特征在于，所述导航逻辑行为树还包括分别自定义命名为plan和plan...
【专利技术属性】
技术研发人员：张仲清，王涛，梁培栋，李昊东，袁碧波，尉志军，赵亚军，王文平，
申请(专利权)人：中煤大同能源有限责任公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。