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

权利要求书

1.基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，包括如下步骤：

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

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

步骤三：通过提取当前激光数据中障碍物的关键点以及几何信息，再配合环境几何特征拓扑地图进行数据关联匹配，获得关键点对应的地图节点；若关键点数量大于1，则根据步骤S3.3得到的关键点间的距离，与邻接矩阵中的数据进行对比，从邻近矩阵中找到与关键点距离相近的数据对应的节点组合，将所有符合要求的匹配节点记入P；

若关键点数量等于1，则根据步骤S3.4得到的连通距离，与邻接矩阵中的数据进行对比，从邻近矩阵中找到连通距离大于该距离的节点组合，将所有符合要求的匹配节点记入P；

激光数据特征提取的具体步骤为：

S3.1：对激光数据进行聚类，分割为多个点集；

S3.2：对点集进行端点提取，并进行断点判断，如无人系统到端点距离小于激光雷达最大扫描半径，则认为是断点并记入关键点；利用IEPF算法对点集提取角点，并将角点记入关键点；

S3.3：统计关键点的数量，如果只有一个，则直接到下一个步骤S3.4；否则选取两个关键点，计算关键点间的距离；如果两个关键点是属于同一条曲线并且是相邻节点，取距离的绝对值记为连通距离；否则，取距离负值并记为可达距离；

S3.4：选取关键点所在的线段长度作为连通距离

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

S4.1:将原始栅格地图的障碍物膨胀，构造低分辨率地图；

S4.2:在每个粗定位结果处，以至少2°为步长，将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重新选取关键点。

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

3.根据权利要求2所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤一中地图节点提取的步骤为：

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

S1.2:对每一条曲线l_i进行端点、角点提取，并将其记入为地图节点。

4.根据权利要求3所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤一中的邻接矩阵生成的步骤为：

S1.3计算地图节点间的距离；

S1.4:判断节点的空间关系，如果两个节点是属于同一条曲线并且是相邻节点，则将该距离称为连通距离，对应距离直接记入邻接矩阵；如果两个节点属于其他情况，则该距离称为可达距离，取距离的负值记入邻接矩阵。