一种融合算法的移动机器人自主导航方法

1.本发明属于移动机器人技术领域，具体涉及一种融合算法的移动机器人自主导航方法。

背景技术：

2.移动机器人在智能家居，安防等领域等到越来越多的应用，移动机器人的导航问题一直是移动机器人领域研究热点之一，特别是自主性导航。实现自主导航有三个方面，即定位与地图构建，以及路径规划。目前移动机器人的自主导航存在鲁棒性，避障的实时性不足，以及规划的轨迹难以跟随等问题。

技术实现要素：

3.本发明的目的是提供一种融合算法的移动机器人自主导航方法，分别采用改进a*算法和teb算法对全局路径和局部动态路径进行优化，提高了整体路径寻优效率，解决现有技术中的移动机器人导航效果不好的技术问题。
4.为了实现上述目的，本发明所采用的技术方案是：一种融合算法的移动机器人自主导航方法，包括如下步骤：s1，移动机器人获取作业环境的格栅地图；s2，使用纯定位模式将雷达点与局部地图点进行匹配以此获得移动机器人的当前位置，并将移动机器人的当前位置作为规划起始点；s3，将移动机器人的当前位置和目标位置从大地坐标系上转化到格栅地图的地图坐标系上；s4，改进a*算法，并使用改进后的a*算法进行全局路径规划，规划出能够避开静态障碍物的路径；具体方法为：s41，将移动机器人的前轮转角控制量进行离散化处理，并对前进方向的速度进行离散化处理，并将速度设定为固定值，得到符合运动学约束的6个搜索方向；s42，按照a*算法的搜索规则进行搜索，保证每个格栅内只保留一个代价最小的节点；s43，在规划出的节点连接线上使用三次样条插值对曲率变化较大的节点进行平滑处理；s44，对规划的路径进行评价，从所有的路径中选出评价得分最高的曲线作为全局路径规划轨迹；s5，结合步骤s4规划的全局路径规划轨迹，利用teb算法对移动机器人进行局部路径规划，以避开动态障碍物；s6，控制移动机器人跟随规划的局部路径；s7，检测移动机器人是否到达目标位置，若移动机器人到达目标位置，则转动移动机器人达到正确的朝向，若没有达到目标位置，重复步骤s4及之后的步骤，直至移动机器人
移动到目标位置。
5.步骤s1中，移动机器人使用激光雷达进行建图，再将地图进行栅格化。
6.步骤s1中，移动机器人在工作环境行走以此来遍历整个作业环境，在此过程中使用cartogtapher算法接收单线激光雷达、惯性导航系统、轮速计的数据，并对数据进行处理，输出二维栅格占用率，根据二维格栅占用率判断栅格占用情况，并将表示格栅占用情况的数据存储在一个二维数组中。
7.在步骤s2中，移动机器人当前位置的获取方法为：先将当前定位前后数帧的地图构建成子图，再将当前激光雷达的数据与子图进行匹配，以获取移动机器人的当前位置。
8.在步骤s43中，计算移动机器人最小转弯半径，计算规划路径上每一个节点的曲率，若规划的曲率小于移动机器人最小转弯半径，就剔除该节点，否则，保留该节点；对保留的节点进行三次样条插值获得平滑轨迹。
9.步骤s5的方法为：s51，通过move_base框架将步骤s4得到的全局路径规划轨迹发送给局部路径规划器；s52，局部路径规划器接收全局路径规划器的参数，并对控制量以及行进方向进行采样，得出多条可以绕开障碍物的路径；s53，设计评价函数对上一步规划的路径进行评价，评价函数参数包含轨迹与障碍物之间的距离、轨迹曲率、与全局路径的跟随程度；s54，从多条路径中选出一条评价函数评分最高的路径作为避开动态障碍物的局部路径轨迹。
10.本发明的有益效果是：本发明的一种融合算法的移动机器人自主导航系统，在基于cartographer算法的基础上，利用基于图优化理论的算法后端优化，并根据激光点云子图和全局地图进行匹配定位，而后采用占据栅格地图法构建地图，所构建出的地图具有较好的全局一致性。进而使用改进a*算法进行全局路径规划，同时使用teb算法进行局部动态避障，提高了系统整体寻优效率和避障的鲁棒性。
具体实施方式
11.下面结合实施例对本发明作进一步的详细说明，但并不作为对发明做任何限制的依据。
12.本发明所提出的一种融合算法的移动机器人自主导航方法，主要包括如下几个步骤：s1，移动机器人获取作业环境的格栅地图；s2，使用纯定位模式将雷达点与局部地图点进行匹配以此获得移动机器人的当前位置，并将移动机器人的当前位置作为规划起始点；s3，将移动机器人的当前位置和目标位置从大地坐标系上转化到格栅地图的地图坐标系上；s4，改进a*算法，并使用改进后的a*算法进行全局路径规划，规划出能够避开静态障碍物的路径；s5，结合步骤s4规划的全局路径规划轨迹，利用teb算法对移动机器人进行局部路
径规划，以避开动态障碍物；s6，控制移动机器人跟随规划的局部路径；s7，检测移动机器人是否到达目标位置，若移动机器人到达目标位置，则转动移动机器人达到正确的朝向，若没有达到目标位置，重复步骤s4及之后的步骤，直至移动机器人移动到目标位置。
13.上述步骤为本发明的基本步骤，更为具体的步骤如下。
14.s1，获取移动机器人作业环境栅格地图；s11，将移动机器人在工作环境行走以此来遍历整个作业环境，在此过程中使用cartogtapher算法接收单线激光雷达、惯性导航系统、轮速计的数据，对数据进行处理，输出二维栅格占用率p(x,y)，根据栅格占用率p(x,y)判断栅格占用情况，用m来表示：m值只有0和1，数值0表示栅格未被占用，数值1表示当前栅格处于占用状态。并将所有数据存储在一个二维数组中。
15.s12，对雷达数据进行最远最近滤波，保留中间值。
16.s13，获得整个移动机器人作业环境的占据栅格地图，其中栅格地图的分辨率一般是0.05 m。占据栅格地图是.pgm格式，其中包含256级灰度图像。
17.s2，使用纯定位模式将雷达点与局部地图点进行匹配以此获得当前的位置；s21，将当前定位前后n帧的地图构建成子图，其中n的数值应根据当前机器人激光雷达的扫描频率进行确定；s22，将当前激光雷达的数据与子图进行匹配。为了使得匹配的速度加快，使用特征匹配的方法。将具体的一团点云聚类成一个特征点，对此一系列特征点进行匹配，来获得当前地图坐标系的坐标。
18.s3，将移动机器人的当前位置和目标位置从大地坐标系上转化到格栅地图的地图坐标系上。
19.s4，对传统a*算法进行改进，并使用改进后的a*算法进行全局路径规划，规划出能够避开静态障碍物的路径；具体方法为：s41，将移动机器人的前轮转角控制量进行离散化处理，并对前进方向的速度进行离散化处理，并将速度设定为固定值，得到符合运动学约束的6个搜索方向；s42，按照a*算法的搜索规则进行搜索，判断一个栅格内是否有两个或两个以上的点，如果有的话保留代价最小的点，剔除代价较大的点，保证每个栅格内只保留一个点；每次扩展之后将扩展的节点进行标记，如果下次搜索再次扩展上标记的节点的时候则重新计算计算的节点代价值，选取最小值保留，对其它的节点进行剔除；s43，在规划出的节点连接线上使用三次样条插值对曲率变化较大的节点进行平滑处理，具体为：计算移动机器人的最小转弯半径，计算上述节点连接线上每个节点的曲率，若曲率小于移动机器人的最小转弯半径，则剔除相应的节点，若曲率不小于移动机器人的最小转弯半径，则保留相应节点，并对所保留的节点进行三次样条插值获得平滑轨迹；s44，从所有的路径中选出评价函数得分最高的曲线作为全局路径规划轨迹。
20.s5，结合步骤s4规划的全局路径规划轨迹，利用teb算法对移动机器人进行局部路径规划，以避开动态障碍物；具体方法为：s51，通过move_base框架将步骤s4得到的全局路径规划轨迹发送给局部路径规划器；s52，局部路径规划器接收全局路径规划器的参数，并对控制量以及行进方向进行采样，得出多条可以绕开障碍物的路径；s53，设计评价函数对上一步规划的路径进行评价，评价函数参数包含轨迹与障碍物之间的距离、轨迹曲率、与全局路径的跟随程度，评价函数表示如下：其中di是第n个节点与障碍物之间的距离，ci是第n个节点的轨迹曲率，ui代表了轨迹与全局路径的跟随程度；ki、li、ri分别代表三个评价函数影响因子的权重；s54，从多条路径中选出一条评价函数评分最高的路径作为避开动态障碍物的局部路径轨迹。
21.s6，通过控制器控制移动机器人跟踪步骤s5规划出来的局部路径轨迹，并检测移动机器人是否到达目标位置，若移动机器人到达目标位置，则转动移动机器人达到正确的朝向，若没有达到目标位置，重复步骤s4及之后的步骤，直至移动机器人移动到目标位置。
22.以上实施例仅用以说明本发明的技术方案而非对其进行限制，所属领域的普通技术人员应当理解，参照上述实施例可以对本发明的具体实施方式进行修改或者等同替换，这些未脱离本发明精神和范围的任何修改或者等同替换均在申请待批的权利要求保护范围之内。

技术特征：

1.一种融合算法的移动机器人自主导航方法，其特征在于，包括如下步骤：s1，移动机器人获取作业环境的格栅地图；s2，使用纯定位模式将雷达点与局部地图点进行匹配以此获得移动机器人的当前位置，并将移动机器人的当前位置作为规划起始点；s3，将移动机器人的当前位置和目标位置从大地坐标系上转化到格栅地图的地图坐标系上；s4，改进a*算法，并使用改进后的a*算法进行全局路径规划，规划出能够避开静态障碍物的路径；具体方法为：s41，将移动机器人的前轮转角控制量进行离散化处理，并对前进方向的速度进行离散化处理，并将速度设定为固定值，得到符合运动学约束的6个搜索方向；s42，按照a*算法的搜索规则进行搜索，保证每个格栅内只保留一个代价最小的节点；s43，在规划出的节点连接线上使用三次样条插值对曲率变化较大的节点进行平滑处理；s44，对规划的路径进行评价，从所有的路径中选出评价得分最高的曲线作为全局路径规划轨迹；s5，结合步骤s4规划的全局路径规划轨迹，利用teb算法对移动机器人进行局部路径规划，以避开动态障碍物；s6，控制移动机器人跟随规划的局部路径；s7，检测移动机器人是否到达目标位置，若移动机器人到达目标位置，则转动移动机器人达到正确的朝向，若没有达到目标位置，重复步骤s4及之后的步骤，直至移动机器人移动到目标位置。2.根据权利要求1所述的一种融合算法的移动机器人自主导航方法，其特征在于：步骤s1中，移动机器人使用激光雷达进行建图，再将地图进行栅格化。3.根据权利要求1所述的一种融合算法的移动机器人自主导航方法，其特征在于：步骤s1中，移动机器人在工作环境行走以此来遍历整个作业环境，在此过程中使用cartogtapher算法接收单线激光雷达、惯性导航系统、轮速计的数据，并对数据进行处理，输出二维栅格占用率，根据二维格栅占用率判断栅格占用情况，并将表示格栅占用情况的数据存储在一个二维数组中。4.根据权利要求1所述的一种融合算法的移动机器人自主导航方法，其特征在于：在步骤s2中，移动机器人当前位置的获取方法为：先将当前定位前后数帧的地图构建成子图，再将当前激光雷达的数据与子图进行匹配，以获取移动机器人的当前位置。5.根据权利要求1所述的一种融合算法的移动机器人自主导航方法，其特征在于：在步骤s43中，计算移动机器人最小转弯半径，计算规划路径上每一个节点的曲率，若规划的曲率小于移动机器人最小转弯半径，就剔除该节点，否则，保留该节点；对保留的节点进行三次样条插值获得平滑轨迹。6.根据权利要求1所述的一种融合算法的移动机器人自主导航方法，其特征在于：步骤s5的方法为：s51，通过move_base框架将步骤s4得到的全局路径规划轨迹发送给局部路径规划器；s52，局部路径规划器接收全局路径规划器的参数，并对控制量以及行进方向进行采
样，得出多条可以绕开障碍物的路径；s53，设计评价函数对上一步规划的路径进行评价，评价函数参数包含轨迹与障碍物之间的距离、轨迹曲率、与全局路径的跟随程度；s54，从多条路径中选出一条评价函数评分最高的路径作为避开动态障碍物的局部路径轨迹。

技术总结

本发明涉及一种融合算法的移动机器人自主导航方法，包括S1：移动机器人获取作业环境的格栅地图；S2：获取移动机器人当前位置；S3：移动机器人位置的坐标转换；S4：使用改进后的A*算法进行全局路径规划；S5：结合全局路径规划轨迹，利用TEB算法对移动机器人进行局部路径规划；S6：控制移动机器人跟随规划的局部路径；S7：检测移动机器人是否到达目标位置。本发明分别采用改进A*算法和TEB算法对全局路径和局部动态路径进行优化，提高了整体路径寻优效率，解决现有技术中的移动机器人导航效果不好的技术问题。的技术问题。