CN111998846A - 基于环境几何与拓扑特征的无人系统快速重定位方法 - Google Patents

Abstract

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

Description

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

优选的，在所述步骤一中的邻接矩阵生成的步骤为：

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的坐标已知为(x₁,y₁)、(x₂,y₂)；k为AB的斜率；交点C(x_c，y_c)、D(x_d，y_d)、M(x_m，y_m)、Y(x_n，y_n)。

优选的，两圆交点的表达式具体为：

其中，

式子中，P中的匹配节点为圆心，坐标已知为(x₁,y₁)、(x₂,y₂)；交点的连线与匹配节点的连线相交于点E(x₀,y₀)，K₂为两圆交点的连线的斜率，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是本发明的基于环境几何与拓扑特征的无人系统快速重定位方法的流程图；

图2是本发明的粗定位下一个关键点与无人系统的位置示意图；

图3是本发明的粗定位点的位置示意图；

图4是本发明的粗定位下两个关键点与无人系统的位置示意图；

图5是本发明的粗定位点的位置示意图；

图6是本发明的实施例3的简化地图的示意图；

图7是本发明的实施例3的节点提取结果示意图；

图8是本发明的实施例3的关键点提取结果示意图；

图9是本发明放入实施例3的关键点选取示意图；

图10是本发明放入实施例3的粗定位结果示意图；

图11是本发明放入实施例3的低分辨率地图的示意图；

图12是本发明放入实施例3的精定位结果的示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

下面通过具体实施例，并结合附图，对本发明的技术方案作进一步的具体描述：

实施例1

如图1所示为基于环境几何与拓扑特征的无人系统快速重定位方法的实施例，包括如下步骤：

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

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

地图节点提取的步骤为：

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

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

优选的，在所述步骤一中的邻接矩阵生成的步骤为：

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

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

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

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

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

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

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

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

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

数据关联匹配的方法为：

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

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

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

粗定位的方法为：

在本实施例中，只有一个关键点，如图2所示，无人系统与关键点之间的直线距离为半径r，无人系统与关键点所在的直线的竖直距离为h；如图3所示，以P中的匹配节点A为圆心，半径r为半径作圆，再作两条与圆心距离为h的直线，直线与圆的四个交点即为粗定位点，四个交点分别是C、D、M、N；直线与圆的四个交点表达式为：

其中，

式子中，匹配节点A、B的坐标已知为(x₁,y₁)、(x₂,y₂)；k为AB的斜率；交点C(x_c，y_c)、D(x_d，y_d)、M(x_m，y_m)、Y(x_n，y_n)。

分枝定界法进行精确定位的步骤为：

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

S4.2:在本实施例中，在每个粗定位结果处，以6°为步长，将360°分为60个朝向角，生成60个分枝，并用低分辨率地图进行匹配得分，选取粗定位所有结果中最高分的位姿(x,y,θ)作为本次最优结果；

S4.3:对S4.2得到结果进行角度精定位，以1°为步长，在区间[θ-3°,θ+3°]中生成7个分枝，并用原始栅格地图进行匹配得分，选取最高得分的角度为最优角度θ_best；

S4.4:取(x,y,θ_best)作为搜索起点，分别向上下左右4个方向移动与膨胀系数相同的长度，加上原始位置，生成5个分枝，并用对应膨胀系数的低分辨地图进行匹配得分，选取最高得分的位姿作为下一次搜索的起点，同时膨胀系数降低；

S4.5:循环S4.4的操作，直到膨胀系数降为0，取最高得分的位姿为精定位结果；

S4.6:判断该得分是否高于搜索停止阈值，如果高于阀值，则该结果为最终精定位结果；如果低于阀值，跳转至步骤S3.3重新选取关键点。

本实施例的有益效果：通过障碍物几何特征的关联匹配有导向性地确定搜索区域，再以激光数据匹配得分为导向进行精定位。相对于粒子滤波在全局范围内随机生成粒子，再以激光数据匹配得分为导向的定位方法，本发明的方法更有针对性、更有效率地确定搜索区域，同时可以避免陷入局部最优值的情况。

实施例2

基于环境几何与拓扑特征的无人系统快速重定位方法的另一实施例，与实施例1的区别在于，选取的关键点数量为两个，粗定位的方法为：如图4所示，无人系统与关键点之间的直线距离分别为半径r1和r2，如图5所示，以P中的匹配节点组合为圆心，分别以r1和r2为半径构造出两个圆，以两圆的交点C、D为粗定位点。

两圆交点的表达式具体为：

其中，

式子中，P中的匹配节点为圆心，坐标已知为(x₁,y₁)、(x₂,y₂)；交点的连线与匹配节点的连线相交于点E(x₀,y₀)，K₂为两圆交点的连线的斜率，L为匹配节点的连线的长度。

实施例3

基于环境几何与拓扑特征的无人系统快速重定位方法的另一实施例，本实施以直线特征为主的环境。

步骤一：采用Turtlebot机器人搭配RPlidarA3激光进行激光SLAM建图，为本方法提供原始栅格地图以及测试用的激光数据。

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

地图节点提取的步骤为：

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

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

优选的，在所述步骤一中的邻接矩阵生成的步骤为：

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

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

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

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

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

S3.1：对激光数据进行聚类，分割为多个点集。激光数据如图7所示。

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

S3.3：选取如图9所示的两个关键点，计算关键点间的距离；为两个关键点不属于同一条线段，因此两点间距离记为可达距离。

数据关联匹配的方法为：

根据步骤S3.3得到的关键点间的距离，与邻接矩阵中的数据进行对比，从邻近矩阵中找到与关键点距离相近的数据对应的节点组合，将所有符合要求的匹配节点记入P；

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

粗定位的方法为：

关键点有两个，因此计算雷达与关键点之间的距离r1、r2；以r1、r2为半径，以匹配的节点组合为圆心，构造出两个圆，取两圆交点为粗定位结果。粗定位结果如图10蓝色十字标示。

分枝定界法进行精确定位的步骤为：

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

S4.2:在每个粗定位结果处，以6°为步长，将360°分为60个朝向角，生成60个分枝，并用低分辨率地图进行匹配得分，选取粗定位所有结果中最高分的位姿(x,y,θ)作为本次最优结果；

S4.3:对S4.2得到结果进行角度精定位，以1°为步长，在区间[θ-3°,θ+3°]中生成7个分枝，并用原始栅格地图进行匹配得分，选取最高得分的角度为最优角度θ_best；

S4.4:取(x,y,θ_best)作为搜索起点，分别向上下左右4个方向移动与膨胀系数相同的长度，加上原始位置，生成5个分枝，并用对应膨胀系数的低分辨地图进行匹配得分，选取最高得分的位姿作为下一次搜索的起点，同时膨胀系数降低；

S4.5:循环S4.4的操作，直到膨胀系数降为0，取最高得分的位姿为精定位结果；

S4.6:因为最高得分满足停止搜索条件，因此该位姿返回为重定位结果，最后结果如图12所示。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

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

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

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

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

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

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

5.根据权利要求1-4任一所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，在所述步骤三中，激光数据特征提取的具体步骤为:

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

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

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

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

6.根据权利要求5所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，数据关联匹配的方法为：

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

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

7.根据权利要求6所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，对无人系统进行粗定位的方法为：

若只有一个关键点，无人系统与关键点之间的直线距离为半径r，无人系统与关键点所在的直线的竖直距离为h；以P中的匹配节点为圆心，半径r为半径作圆，再作两条与圆心距离为h的直线，直线与圆的四个交点即为粗定位点；

若选取的关键点数量为两个，无人系统与关键点之间的直线距离分别为半径r1和r2，以P中的匹配节点组合为圆心，分别以r1和r2为半径构造出两个圆，以两圆的交点为粗定位点。

8.根据权利要求7所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，直线与圆的四个交点分别为C、D、M、N，表达式为：

其中，

式子中，匹配节点A、B的坐标已知为(x₁,y₁)、(x₂,y₂)；k为AB的斜率；交点C(x_c，y_c)、D(x_d，y_d)、M(x_m，y_m)、Y(x_n，y_n)。

9.根据权利要求7所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，两圆交点的表达式具体为：

其中，

式子中，P中的匹配节点A、B分别为圆心，坐标已知为(x₁,y₁)、(x₂,y₂)；两圆的交点分别为C(x_c，y_c)、D(x_d，y_d)；CD与匹配节点的连线相交于点E(x₀,y₀)，K₂为CE的斜率，L为匹配节点的连线的长度。

10.根据权利要求7所述的基于环境几何与拓扑特征的无人系统快速重定位方法，其特征在于，分枝定界法进行精确定位的步骤为：

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重新选取关键点。

Images