本发明属于自动驾驶，尤其涉及一种基于道路拓扑的激光全局定位方法和系统。
背景技术：
1、随着社会经济和信息技术的迅速发展，无人驾驶的应用场景逐渐拓宽至物流运输、农业生产、军事等各个领域，对导航定位技术的需求也日益增多。全局定位在移动机器人和无人车导航定位中扮演着至关重要的角色。全局定位利用收到的传感器数据和先验地图信息，提供了一个相对可靠的全局姿态，并用于在slam的后端优化消除其累积误差。目前，视觉传感器由于其成本较低，常常被用于全局定位，然而，视觉传感器对光照和天气等环境相当敏感，鲁棒性较低。相比之下，激光雷达的探测范围广，相对于环境变化的适应性更强，因此激光全局定位方法也开始受到了更多的关注。
2、在早期的激光全局定位技术中，常用的直接点云配准方法虽然在理论上可行，但实际应用中却因其计算效率低下而受到限制。为了解决这一问题，后续研究提出了基于描述符的激光全局定位技术。这种技术通过利用高效的描述符来简化点云数据，从而显著减少了处理时间和计算资源消耗。尽管如此，当遇到外观相似的场景时，现有基于描述符的方法仍难以避免错误匹配的问题。这主要是因为这些方法依赖于点云数据的相似性计算，而在复杂或相似的环境中，这种计算往往缺乏足够的区分度。
3、针对上述方法中存在的各项问题，迫切需要一种计算高效、准确稳定的激光全局定位方法。
技术实现思路
1、有鉴于此，本发明提出了一种基于道路拓扑结构的激光全局定位方法，能够提高定位计算的精确性、鲁棒性和稳定性。
2、一种基于道路拓扑的激光全局定位方法，用于搭载多线激光雷达的无人自主移动平台的定位，该方法包括：
3、步骤一，根据当前时刻多线激光雷达获得的点云数据，构造障碍物二值平面图像；
4、步骤二，对所述障碍物二值平面图像提取道路中心线，得到当前时刻道路拓扑；
5、步骤三，如果步骤二所提取的当前时刻道路拓扑为路口或拐角，则在预先构造的环境拓扑地图中进行匹配，找到匹配节点，即无人自主移动平台当前所处节点；提取匹配节点对应的道路拓扑和位姿，位姿即为粗定位结果；其中，所述预先构造的环境拓扑地图为一张无向连通图，图中节点存储的信息包括无人自主移动平台的位姿和对应的道路拓扑；
6、步骤四，计算所述当前时刻道路拓扑和步骤三获得的匹配节点道路拓扑之间的相对位姿变换；将相对位姿变换补偿到粗定位结果上，获得无人自主移动平台的精准位姿，实现全局定位。
7、优选地，所述步骤一中，使用基于高度差算法构造所述障碍物二值平面图像。
8、优选地，所述步骤一中，进一步对构造的障碍物二值平面图像进行图像滤波，具体步骤为：
9、步骤s11，使用广度优先遍历，得到所述障碍物二值平面图像中所有的八邻域连通像素块；
10、步骤s12，计算步骤s11中所得八邻域连通像素块中的像素数量，如果所述像素数量小于设定第一阈值，则将该八邻连通像素块中的像素数值全部置零，以滤除噪声。
11、优选地，所述步骤二中，所述提取道路中心线的算法为分水岭算法。
12、优选地，所述步骤三中，所提取的当前时刻道路拓扑是否为路口或拐角的判断方式为：
13、步骤s301，确定步骤二所提取的当前时刻道路拓扑的分支数n；
14、步骤s302，如果分支数n大于2，初步判定当前时刻道路拓扑为路口，执行步骤s303；如果分支数n等于2，则执行步骤s304，进一步判断当前时刻道路拓扑是否为拐角；
15、步骤s303，提取路口的拓扑特征点，该拓扑特征点使得周围3x3邻域内的边缘点数量最多；执行步骤305；
16、步骤s304，通过最小化代价函数(i)找到当前时刻道路拓扑的起点和终点ps,pe；
17、
18、式中，i(i,j)为图像中(i,j)处像素的值；
19、设r(i,j)为(i,j),ps,pe三个点所确定的外接圆半径，计算当前时刻道路拓扑的最大曲率κmax，如果最大曲率κmax大于设定值c，则初步判定当前时刻道路拓扑为拐角，并取最大曲率的点为拐角的拓扑特征点，执行步骤s305；
20、步骤s305，如果当前时刻道路拓扑符合路口或拐角的道路特征，将当前时刻点云数据的接收时间，并与上一次出现同样道路特征的点云数据接收时间进行对比，如果相隔时间δt小于设定间隔，则最终判定当前时刻道路拓扑确实为路口或拐角。
21、优选地，所述步骤三中，所述在预先构造的环境拓扑地图中进行匹配，找到无人自主移动平台当前所处节点，提取当前所处节点对应的道路拓扑和位姿，作为粗定位结果的具体步骤包括：
22、步骤s311，给定所述环境拓扑地图中无人自主移动平台出发的初始节点，并记录经过的历史道路拓扑类型序列；
23、步骤s312，在所述环境拓扑地图中，通过深度优先搜索寻找符合历史道路拓扑类型序列的第一条路径；
24、步骤s313，输出s312所得路径中最后一个节点存储的位姿，得到粗定位结果。
25、优选地，所述步骤四具体包括如下步骤
26、步骤s41，利用拓扑特征点对齐当前时刻道路拓扑的拓扑特征点和匹配节点道路拓扑；
27、步骤s42，以拓扑特征点为中心，根据角度分辨率ρ，依次将当前时刻道路拓扑转动不同旋转角度θ＝kρ,k∈[0,m-1]，m为当前时刻道路拓扑旋转时的角度分辨率；
28、步骤s43，在每个旋转角度θ下，针对当前时刻道路拓扑中各拓扑点pi，搜索在匹配节点的道路拓扑中欧式距离最小的拓扑点qj；如果最小距离d小于设定第二阈值，则将pi,qj记为一对匹配拓扑点；
29、步骤s44，在每个旋转角度θ下，通过步骤s43的计算，获得当前时刻道路拓扑与匹配节点道路拓扑的匹配拓扑点数量nmatched，将nmatched相对于当前时刻道路拓扑中拓扑点总数n的占比作为旋转角度θ对应的道路拓扑匹配程度得分score；
30、步骤s45，将所有旋转角度θ的最高道路拓扑匹配程度得分scoremax作为当前时刻道路拓扑和匹配节点道路拓扑的最终匹配得分；
31、步骤s46，若步骤s45中所述最终匹配得分超过第三阈值，则认为全局定位成功；此时获取步骤s41中对齐的拓扑特征点的相对平移变换量，以及步骤s45中最高道路拓扑匹配程度得分scoremax对应的道路拓扑的相对旋转变换量，将所得相对平移变换量和相对旋转变换量补偿到步骤三所得的粗定位结果上，输出精定位结果t，计算公式为
32、
33、式中，δt与δr分别为补偿的相对平移变换量和相对旋转量，t′为粗定位结果。
34、本发明还提供了一种基于道路拓扑的激光全局定位系统，用于搭载多线激光雷达的无人自主移动平台的定位，包括：环境拓扑地图存储模块、道路拓扑获取模块、拓扑类型判断模块、粗定位模块和精定位模块；
35、所述环境拓扑地图存储模块，用于存储预先构造的环境拓扑地图，该环境拓扑地图为一张无向连通图，图中节点存储的信息包括无人自主移动平台的位姿和对应的道路拓扑；
36、所述道路拓扑获取模块，用于根据当前时刻多线激光雷达获得的点云数据，构造障碍物二值平面图像；对所述障碍物二值平面图像提取道路中心线，得到当前时刻道路拓扑；
37、所述拓扑类型判断模块，用于判断所述道路拓扑获取模块得到的所述当前时刻道路拓扑是否为路口或拐角，如果是则启动粗定位模块；
38、所述粗定位模块，用于被启动后，在所述环境拓扑地图存储模块存储的环境拓扑地图中进行匹配，找到匹配节点，即无人自主移动平台当前所处节点；提取匹配节点对应的道路拓扑和位姿，位姿即为粗定位结果，发送给精定位模块；
39、所述精定位模块，用于计算所述当前时刻道路拓扑和所述粗定位模块获得的匹配节点道路拓扑之间的相对位姿变换；将相对位姿变换补偿到粗定位结果上，获得无人自主移动平台的精准位姿，实现全局定位。
40、优选地，所述拓扑类型判断模块包括第一判断子模块、路口拓扑特征点确定子模块、拐角拓扑特征点确定子模块、第二判断子模块；
41、所述第一判断子模块，用于判断当前时刻道路拓扑的分支数n是否大于2，如果n大于2，则初步判定当前时刻道路拓扑为路口，启动路口拓扑特征点确定子模块；如果分支数n等于2，则启动拐角拓扑特征点确定子模块；
42、所述路口拓扑特征点确定子模块，用于提取路口的拓扑特征点，该拓扑特征点使得周围3x3邻域内的边缘点数量最多，通知第二判断子模块；
43、所述拐角拓扑特征点确定子模块，用于根据当前时刻道路拓扑的最大曲率判断是否为拐角，并获取拐角的拓扑特征点，通知第二判断子模块；
44、所述第二判断子模块，用于在当前时刻道路拓扑符合路口或拐角的道路特征时，将当前时刻点云数据的接收时间，并与上一次出现同样道路特征的点云数据接收时间进行对比，如果相隔时间δt小于设定间隔，则最终判定当前时刻道路拓扑确实为路口或拐角。
45、优选地，所述精定位模块包括对齐子模块、拓扑点匹配子模块、匹配程度打分子模块和变换子模块；
46、所述对齐子模块，用于利用拓扑特征点对齐当前时刻道路拓扑的拓扑特征点和匹配节点道路拓扑；
47、所述拓扑点匹配子模块，用于以拓扑特征点为中心，根据角度分辨率ρ，依次将当前时刻道路拓扑转动不同旋转角度θ；在每一旋转角度θ下，针对当前时刻道路拓扑中各拓扑点pi，搜索在匹配节点的道路拓扑中欧式距离最小的拓扑点qj，如果pi,qj距离小于设定第二阈值，则将pi,qj记为匹配拓扑点；
48、所述匹配程度打分子模块，用于在每个旋转角度θ下，利用拓扑点匹配子模块进行拓扑点匹配，获得当前时刻道路拓扑与匹配节点道路拓扑的匹配拓扑点数量nmatched；将nmatched相对于当前时刻道路拓扑中拓扑点总数n的占比作为旋转角度θ对应的道路拓扑匹配程度得分score；将所有旋转角度θ的最高道路拓扑匹配程度得分scoremax作为当前时刻道路拓扑和匹配节点道路拓扑的最终匹配得分；若所述最终匹配得分超过第三阈值，则认为全局定位成功，启动所述变换子模块；
49、所述变换子模块，被启动后，获取所述对齐子模块拓扑特征点的相对平移变换量，获取最高道路拓扑匹配程度得分scoremax对应的道路拓扑的相对旋转变换量，将所得相对平移变换量和相对旋转变换量补偿到所述粗定位结果上，输出精定位结果t，计算公式为
50、
51、式中，δt与δr分别为补偿的相对平移变换量和相对旋转量，t′为粗定位结果。
52、有益效果：
53、(1)本发明并不是基于描述符来简化点云数据，也并非依赖于点云数据的相似性计算，而是利用了环境中包含的丰富的道路结构特征，并配合点云的几何信息，这种方案避免了相似场景带来的误匹配问题，能够在复杂或相似的环境中，展现出足够的区分度，有效避免错误匹配的发生，保证了定位结果的准确性。同时，通过这种多维度的数据融合，有效提高了定位的精确性和鲁棒性。
54、(2)本发明采用道路中心线表现出来的道路拓扑，是抽象化的道路特征，去掉了道路中的无用特征，道路特征明显，不仅能够保证定位结果的准确性，而且拓扑运算大大降低了运算的复杂度，提升了计算效率。
55、(3)本发明不依赖视觉传感器采集的图像信息，对环境光照条件的变化更具鲁棒性。
56、(4)该方法还能够在不同环境下实现更为稳定的全局定位，特别是在具有重复或相似地理特征的城市环境中，有效避免错误匹配的发生，从而为自动驾驶及其他导航应用提供更为可靠的支持。
57、(5)在一具体实施方式中，在判定是否为路口时，通过该公式求和周围8个点的函数值，并减去中心点的函数值，确保了选取的中心点是其周围边缘点最多的点。通过在3x3邻域内寻找边缘点最多的点，从而能够准确定位路口的特征点。这样提取路口的拓扑特征点在准确性和鲁棒性方面均有优势，其中在准确性方面，通过考虑邻域内的边缘点分布，可以更准确地确定路口的中心位置；在鲁棒性方面，3x3邻域内的计算能够平滑掉一些噪声和误差，提高特征点提取的稳定性。
58、(6)在一优选实施例中，在进行补偿时，考虑从0度到360度的m个不同角度。通过这样分段，可以在特征点的周围，以不同的角度对道路拓扑进行分析和计算。这个方法有助于在不同方向上分析道路拓扑的变化，从而更加准确地确定道路的形状和特征。
1.一种基于道路拓扑的激光全局定位方法，用于搭载多线激光雷达的无人自主移动平台的定位，其特征在于，该方法包括：
2.根据权利要求1所述的一种基于道路拓扑的激光全局定位方法，其特征在于，所述步骤一中，使用基于高度差算法构造所述障碍物二值平面图像。
3.根据权利要求1所述的一种基于道路拓扑的激光全局定位方法，其特征在于，所述步骤一中，进一步对构造的障碍物二值平面图像进行图像滤波，具体步骤为：
4.根据权利要求1所述的一种基于道路拓扑的激光全局定位方法，其特征在于，所述步骤二中，所述提取道路中心线的算法为分水岭算法。
5.根据权利要求1所述的一种基于道路拓扑的激光全局定位方法，其特征在于，所述步骤三中，所提取的当前时刻道路拓扑是否为路口或拐角的判断方式为：
6.根据权利要求1所述的一种基于道路拓扑的激光全局定位方法，其特征在于，所述步骤三中，所述在预先构造的环境拓扑地图中进行匹配，找到无人自主移动平台当前所处节点，提取当前所处节点对应的道路拓扑和位姿，作为粗定位结果的具体步骤包括：
7.根据权利要求1所述的一种基于道路拓扑的激光全局定位方法，其特征在于，所述步骤四具体包括如下步骤
8.一种基于道路拓扑的激光全局定位系统，用于搭载多线激光雷达的无人自主移动平台的定位，其特征在于，包括：环境拓扑地图存储模块、道路拓扑获取模块、拓扑类型判断模块、粗定位模块和精定位模块；
9.根据权利要求8所述的一种基于道路拓扑的激光全局定位系统，其特征在于，所述拓扑类型判断模块包括第一判断子模块、路口拓扑特征点确定子模块、拐角拓扑特征点确定子模块、第二判断子模块；
10.根据权利要求8所述的一种基于道路拓扑的激光全局定位系统，其特征在于，所述精定位模块包括对齐子模块、拓扑点匹配子模块、匹配程度打分子模块和变换子模块；
