基于环境几何与拓扑特征的无人系统快速重定位方法技术方案

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

【技术实现步骤摘要】
基于环境几何与拓扑特征的无人系统快速重定位方法
本专利技术涉及无人系统定位领域，更具体地，涉及基于环境几何与拓扑特征的无人系统快速重定位方法。
技术介绍
无人系统的准确定位是无人系统能自主导航、执行任务的前提，但是有三种情况会影响到定位效果。首先是定位过程中无人车、机器人可能会遇到“绑架问题”，即被动位移到地图的任意位置，但是系统认为自己停留在原地的定位错误。其次，无人系统在运行过程中因为突发的传感器故障，如里程计数据丢失无法更新位姿，导致定位失败的问题。最后，由于目前SLAM算法的特点，系统首次启动的初始定位固定是在地图原点，与实际情况可能不符。上述的定位问题都会影响到无人系统的正常工作，因此用于恢复正常定位的重定位方法十分重要。最常用的重定位方法为粒子滤波算法，粒子滤波算法通过初始化粒子群、更新粒子群、计算粒子评分、重采样粒子群的方法对无人车进行位置的推算。传统的粒子滤波算法通过在全局范围内随机初始化粒子群，配合观测数据计算粒子群的得分，逐渐向得分最高的位置收敛，最后推算出位姿。但是传统方法在遇到相似环境的时候会陷入局部最优值，无法收敛正确的位姿。公开号为“CN110006432A”的一种基于几何先验信息下的室内机器人快速重定位的方法，根据环境信息辅助粒子滤波算法进行快速定位，但是粒子滤波利用环境信息在全局范围内随机生成粒子的过程中，粒子更新的随机性使算法的收敛速度无法保证，容易令算法陷入局部最优值的问题。
技术实现思路
本专利技术为克服上述现有技术中现有的粒子滤波算法收敛速度难以保证的问题，提供基于环境几何与拓扑特征的无人系统快速重定位方法，解决无人系统的快速重定位的时候，具有良好的收敛速度且保证准确性。为解决上述技术问题，本专利技术采用的技术方案是：基于环境几何与拓扑特征的无人系统快速重定位方法，包括如下步骤：步骤一：利用激光SLAM算法得到的栅格地图，提取出环境中的几何特征，所述几何特征包括以环境中障碍物的端点、角点作为地图节点，以及节点间的距离作为邻接矩阵；步骤二：将栅格地图和环境中的几何特征结合，建立环境几何特征拓扑地图；步骤三：通过提取当前激光数据中障碍物的关键点以及几何信息，再配合环境几何特征拓扑地图进行数据关联匹配，获得关键点对应的地图节点；步骤四：通过几何信息配合节点匹配结果进行无人系统的粗定位，最后对粗定位结果使用分枝定界法完成无人系统的精确定位。优选的，在所述步骤一中，将栅格地图转化为简化地图；栅格地图的栅格有3种状态：“被占用”、“空闲”和“未知”，像素值用0、255、205表示，分别代表该栅格有障碍物、无障碍物和未知状态；简化地图是障碍物边缘的集合，提取相邻栅格是“空闲”状态的“被占用”栅格作为简化地图。优选的，在所述步骤一中地图节点提取的步骤为：S1.1:对简化地图进行曲线分离，分离出不连通的曲线li，最后将简化地图整理为曲线集合L＝{l1,l2,..,ln}；S1.2:对每一条曲线li进行端点、角点提取，并将其记入为地图节点。优选的，在所述步骤一中的邻接矩阵生成的步骤为：S1.3计算地图节点间的距离；S1.4:判断节点的空间关系，如果两个节点是属于同一条曲线并且是相邻节点，则将该距离称为连通距离，对应距离直接记入邻接矩阵；如果两个节点属于其他情况，则该距离称为可达距离，取距离的负值记入邻接矩阵。优选的，在所述步骤三中，激光数据特征提取的具体步骤为:S3.1：对激光数据进行聚类，分割为多个点集。S3.2：对点集进行端点提取，并进行断点判断，如无人系统到端点距离小于激光雷达最大扫描半径，则认为是断点并记入关键点；利用IEPF算法对点集提取角点，并将角点记入关键点。S3.3：统计关键点的数量，如果只有一个，则直接到下一个步骤S3.4；否则选取两个关键点，计算关键点间的距离；如果两个关键点是属于同一条曲线并且是相邻节点，取距离的绝对值记为连通距离；否则，取距离负值并记为可达距离。S3.4：选取关键点所在的线段长度作为连通距离。优选的，数据关联匹配的方法为：若关键点数量大于1，则根据步骤S3.3得到的关键点间的距离，与邻接矩阵中的数据进行对比，从邻近矩阵中找到与关键点距离相近的数据对应的节点组合，将所有符合要求的匹配节点记入P；若关键点数量等于1，则根据步骤S3.4得到的连通距离，与邻接矩阵中的数据进行对比，从邻近矩阵中找到连通距离大于该距离的节点组合，将所有符合要求的匹配节点记入P。优选的，对无人系统进行粗定位的方法为：若只有一个关键点，无人系统与关键点之间的直线距离为半径r，无人系统与关键点所在的直线的竖直距离为h；以P中的匹配节点为圆心，半径r为半径作圆，再作两条与圆心距离为h的直线，直线与圆的四个交点即为粗定位点；若选取的关键点数量为两个，无人系统与关键点之间的直线距离分别为半径r1和r2，以P中的匹配节点组合为圆心，分别以r1和r2为半径构造出两个圆，以两圆的交点为粗定位点。优选的，直线与圆的四个交点表达式为：直线与圆的四个交点为C、D、M、N，表达式为：其中，式子中，匹配节点A、B的坐标已知为(x1,y1)、(x2,y2)；k为AB的斜率；交点C(xc，yc)、D(xd，yd)、M(xm，ym)、Y(xn，yn)。优选的，两圆交点的表达式具体为：其中，式子中，P中的匹配节点为圆心，坐标已知为(x1,y1)、(x2,y2)；交点的连线与匹配节点的连线相交于点E(x0,y0)，K2为两圆交点的连线的斜率，L为匹配节点的连线的长度。优选的，分枝定界法进行精确定位的步骤为：S4.1:将原始栅格地图的障碍物膨胀，构造低分辨率地图；S4.2:在每个粗定位结果处，以至少2°为步长，将360°均分为若干个朝向角，生成若干个分枝，能够对全角度进行覆盖，其中步长角度以可以被360度整除为优，并用低分辨率地图进行匹配得分，选取粗定位所有结果中最高分的位姿(x,y,θ)作为本次最优结果；S4.3:对S4.2得到结果进行角度精定位，以1°为步长，在粗定位步长区间中生成若干个分枝，并用原始栅格地图进行匹配得分，选取最高得分的角度为最优角度θbest；S4.4:取(x,y,θbest)作为搜索起点，分别向上下左右4个方向移动与膨胀系数相同的长度，加上原始位置，生成5个分枝，并用对应膨胀系数的低分辨地图进行匹配得分，选取最高得分的位姿作为下一次搜索的起点，同时膨胀系数降低；S4.5:循环S4.4的操作，直到膨胀系数降为0，取最高得分的位姿为精定位结果；S4.6:判断该得分是否高于搜索停止阈值，如果高于阀值，则该结果为最终精定位结果；如果低于阀值，跳转至步骤S3.3重新选取关键点。与现有技术相比，本专利技术的有益效果是：通过障碍物几何特征的关联匹配有本文档来自技高网...

【技术保护点】
1.基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，包括如下步骤：/n步骤一：利用激光SLAM算法得到的栅格地图，提取出环境中的几何特征，所述几何特征包括以环境中障碍物的端点、角点作为地图节点，以及节点间的距离作为邻接矩阵；/n步骤二：将栅格地图和环境中的几何特征结合，建立环境几何特征拓扑地图；/n步骤三：通过提取当前激光数据中障碍物的关键点以及几何信息，再配合环境几何特征拓扑地图进行数据关联匹配，获得关键点对应的地图节点；/n步骤四：通过几何信息配合节点匹配结果进行无人系统的粗定位，最后对粗定位结果使用分枝定界法完成无人系统的精确定位。/n

【技术特征摘要】
1.基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，包括如下步骤：
步骤一：利用激光SLAM算法得到的栅格地图，提取出环境中的几何特征，所述几何特征包括以环境中障碍物的端点、角点作为地图节点，以及节点间的距离作为邻接矩阵；
步骤二：将栅格地图和环境中的几何特征结合，建立环境几何特征拓扑地图；
步骤三：通过提取当前激光数据中障碍物的关键点以及几何信息，再配合环境几何特征拓扑地图进行数据关联匹配，获得关键点对应的地图节点；
步骤四：通过几何信息配合节点匹配结果进行无人系统的粗定位，最后对粗定位结果使用分枝定界法完成无人系统的精确定位。


2.根据权利要求1所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤一中，将栅格地图转化为简化地图；栅格地图的栅格有3种状态：“被占用”、“空闲”和“未知”，像素值用0、255、205表示，分别代表该栅格有障碍物、无障碍物和未知状态；简化地图是障碍物边缘的集合，提取相邻栅格是“空闲”状态的“被占用”栅格作为简化地图。


3.根据权利要求2所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤一中地图节点提取的步骤为：
S1.1:对简化地图进行曲线分离，分离出不连通的曲线li，最后将简化地图整理为曲线集合L＝{l1,l2,..,ln}；
S1.2:对每一条曲线li进行端点、角点提取，并将其记入为地图节点。


4.根据权利要求3所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤一中的邻接矩阵生成的步骤为：
S1.3计算地图节点间的距离；
S1.4:判断节点的空间关系，如果两个节点是属于同一条曲线并且是相邻节点，则将该距离称为连通距离，对应距离直接记入邻接矩阵；如果两个节点属于其他情况，则该距离称为可达距离，取距离的负值记入邻接矩阵。


5.根据权利要求1-4任一所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤三中，激光数据特征提取的具体步骤为:
S3.1：对激光数据进行聚类，分割为多个点集；
S3.2：对点集进行端点提取，并进行断点判断，如无人系统到端点距离小于激光雷达最大扫描半径，则认为是断点并记入关键点；利用IEPF算法对点集提取角点，并将角点记入关键点；
S3.3：统计关键点的数量，如果只有一个，则直接到下一个步骤S3.4；否则选取两个关键点，计算关键点间的距离；如果两个关键点是属于同一条曲线并且是相邻节点，取距离的绝对值记为连通距离；否则，取距离负值并记为可达距离；
S3.4：选取关键点所在的线段长度作为连通距离。


6.根据权利要求5所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，数据关联匹配的方法为：
若关键点数量大于1，则根据步骤S3.3得到的关键点间的距离，与邻接矩阵中的数据进行对比，从邻近矩阵中找到与关键点距离相近的数据对应的节点组合，将所有...

【专利技术属性】
技术研发人员：熊会元，杜继能，梁凡，钱沛聪，
申请(专利权)人：中山大学，
类型：发明
国别省市：广东;44