[发明专利]基于环境几何与拓扑特征的无人系统快速重定位方法

说明书

本发明涉及基于环境几何与拓扑特征的无人系统快速重定位方法，包括如下步骤：利用激光SLAM算法得到的栅格地图，提取出环境中的几何特征，包括以环境中障碍物的端点、角点作为地图节点，以及节点间的距离作为邻接矩阵；建立环境几何特征拓扑地图；通过提取当前激光数据中障碍物的关键点以及几何信息，再配合环境几何特征拓扑地图进行数据关联匹配，获得关键点对应的地图节点；通过几何信息配合节点匹配结果进行无人系统的粗定位，最后对粗定位结果进行精确定位。本方法相对于粒子滤波在全局范围内随机生成粒子，再以激光数据匹配得分为导向的定位方法，本发明的方法更有针对性、更有效率地确定搜索区域，同时可以避免陷入局部最优值的情况。

技术领域

本发明涉及无人系统定位领域，更具体地，涉及基于环境几何与拓扑特征的无人系统快速重定位方法。

背景技术

无人系统的准确定位是无人系统能自主导航、执行任务的前提，但是有三种情况会影响到定位效果。首先是定位过程中无人车、机器人可能会遇到“绑架问题”，即被动位移到地图的任意位置，但是系统认为自己停留在原地的定位错误。其次，无人系统在运行过程中因为突发的传感器故障，如里程计数据丢失无法更新位姿，导致定位失败的问题。最后，由于目前SLAM算法的特点，系统首次启动的初始定位固定是在地图原点，与实际情况可能不符。上述的定位问题都会影响到无人系统的正常工作，因此用于恢复正常定位的重定位方法十分重要。

最常用的重定位方法为粒子滤波算法，粒子滤波算法通过初始化粒子群、更新粒子群、计算粒子评分、重采样粒子群的方法对无人车进行位置的推算。传统的粒子滤波算法通过在全局范围内随机初始化粒子群，配合观测数据计算粒子群的得分，逐渐向得分最高的位置收敛，最后推算出位姿。但是传统方法在遇到相似环境的时候会陷入局部最优值，无法收敛正确的位姿。

公开号为“CN110006432A”的一种基于几何先验信息下的室内机器人快速重定位的方法，根据环境信息辅助粒子滤波算法进行快速定位，但是粒子滤波利用环境信息在全局范围内随机生成粒子的过程中，粒子更新的随机性使算法的收敛速度无法保证，容易令算法陷入局部最优值的问题。

发明内容

本发明为克服上述现有技术中现有的粒子滤波算法收敛速度难以保证的问题，提供基于环境几何与拓扑特征的无人系统快速重定位方法，解决无人系统的快速重定位的时候，具有良好的收敛速度且保证准确性。

为解决上述技术问题，本发明采用的技术方案是：基于环境几何与拓扑特征的无人系统快速重定位方法，包括如下步骤：

步骤一：利用激光SLAM算法得到的栅格地图，提取出环境中的几何特征，所述几何特征包括以环境中障碍物的端点、角点作为地图节点，以及节点间的距离作为邻接矩阵；

步骤二：将栅格地图和环境中的几何特征结合，建立环境几何特征拓扑地图；

步骤三：通过提取当前激光数据中障碍物的关键点以及几何信息，再配合环境几何特征拓扑地图进行数据关联匹配，获得关键点对应的地图节点；

步骤四：通过几何信息配合节点匹配结果进行无人系统的粗定位，最后对粗定位结果使用分枝定界法完成无人系统的精确定位。

优选的，在所述步骤一中，将栅格地图转化为简化地图；栅格地图的栅格有3种状态：“被占用”、“空闲”和“未知”，像素值用0、255、205表示，分别代表该栅格有障碍物、无障碍物和未知状态；简化地图是障碍物边缘的集合，提取相邻栅格是“空闲”状态的“被占用”栅格作为简化地图。

优选的，在所述步骤一中地图节点提取的步骤为：

S1.1:对简化地图进行曲线分离，分离出不连通的曲线l_i，最后将简化地图整理为曲线集合L＝{l₁,l₂,..,l_n}；