本发明涉及激光定位，具体为一种激光定位与建图闭环检测方法及系统。
背景技术：
1、在自动化和机器人技术的快速发展中，激光雷达(lidar)定位和建图技术(slam)扮演了重要角色。slam技术允许机器人或自动化设备在未知环境中进行定位和地图构建，是实现全自动化导航的关键。现有的激光slam技术主要依赖于高精度的激光雷达传感器，通过不断扫描环境并收集点云数据来定位自身位置并绘制环境地图。然而，这一过程存在几个挑战：首先，环境噪声和雷达误差可能导致数据质量下降，影响定位准确性；其次，数据量庞大导致处理和存储成本高昂；最后，环境变化可能导致地图过时，需要有效的闭环检测机制以实时更新地图。
技术实现思路
1、鉴于上述存在的问题，提出了本发明。
2、因此，本发明解决的技术问题是：目前激光slam的定位精度低、数据处理和存储的成本高以及地图实时更新的能力弱。
3、为解决上述技术问题，本发明提供如下技术方案：一种激光定位与建图闭环检测方法，包括：采集激光雷达点云数据，进行预处理，进行初始位姿估计计算。
4、进行后端优化，改进位姿估计。
5、将优化后的位姿点云累积到激光雷达子地图，进行闭环检测。
6、更新位姿和地图。
7、作为本发明所述的激光定位与建图闭环检测方法的一种优选方案，其中：所述预处理包括，对所述激光雷达点云数据进行噪声过滤和数据压缩，所述噪声过滤为将原始点云数据集与噪声过滤阈值比较，在阈值内的数据保留，其余数据丢弃。
8、数据压缩表示为：
9、
10、其中，c’表示侯选压缩数据集，c表示原始数据集，c表示原始数据集中的一个点，c’表示侯选压缩数据集中的一个点。
11、作为本发明所述的激光定位与建图闭环检测方法的一种优选方案，其中：所述估进行初始位姿估计计算包括，将预处理后的点云与激光雷达子地图进行匹配，以估计初始位姿：
12、
13、其中，n表示点云数量，ci表示激光雷达点云中第i个点的坐标，wi表示第i个点的权重，t(p,li)表示根据位姿p变换后的子地图中第i个点的坐标，p表示激光雷达的位姿参数，表示正则化参数，vi(p)表示根据位姿p计算得到的第i个点的速度。
14、作为本发明所述的激光定位与建图闭环检测方法的一种优选方案，其中：所述进行后端优化包括，从激光slam前端获取初始位姿估计，构建位姿图，包括每个位姿节点以及节点间的相对关系。
15、使用位姿误差函数公式评估当前位姿图与实际激光雷达测量之间的偏差，表示为：
16、
17、其中，nw表示位姿节点总数，αi表示调整系数，pi表示第i个位姿估计，li表示与pi对应的激光雷达测量位姿，βi表示调整系数，angleerror(pi,li)表示为：
18、angleerror(pi,li)＝min(||θ(pi)-θ(li)||,2π-||θ(pi)-θ(li)||)
19、其中，θ(pi)表示从位姿pi提取的角度部分，θ(li)表示从激光雷达测量位姿li提取的角度部分。
20、利用g20因子图算法优化位姿图，得到精确的位姿估计：
21、
22、其中，edges表示位姿图中的边集合，γij表示边(i,j)的权重，tij表示从位姿pj到pi的理想变换，transform(pj,tij)表示pj与tij矩阵之间的乘法操作。
23、作为本发明所述的激光定位与建图闭环检测方法的一种优选方案，其中：所述将优化后的位姿点云累积到激光雷达子地图包括，使用改进后的位姿估计信息，将新采集的激光雷达点云数据整合到当前的激光雷达子地图中，并且实时更新当前子地图的内容。
24、若当前子地图的点云数据密度超过地图点云密度阈值，则创建新的子地图，用于后续闭环检测和地图优化。
25、作为本发明所述的激光定位与建图闭环检测方法的一种优选方案，其中：所述进行闭环检测包括，基于当前位姿和激光雷达子地图历史数据，生成闭环假设位姿：
26、
27、其中，hi表示第i个闭环假设位姿，m表示历史子地图，σp表示空间匹配误差的标准差，p表示地图点，ffeat(hi,p)表示为：
28、
29、其中，f(hi)表示从位姿hi提取的特征向量，f(p)表示从地图点p提取的特征向量，w表示特征匹配的权重。
30、基于假设位姿的匹配程度和先验概率，更新后每个假设位姿的后验概率：
31、
32、其中，pprior表示先验概率，表示为：
33、
34、其中，pprev表示前一时刻的位姿估计，σtrans表示位姿变换的预期标准差，z表示归一化因子。
35、基于后验概率，选择概率最高的闭环假设位姿作为闭环事件的位置。
36、作为本发明所述的激光定位与建图闭环检测方法的一种优选方案，其中：所述更新位姿和地图包括，将得到的闭环位姿信息，对slam系统中记录的所有历史位姿进行调整。
37、将调整后的位姿用于更新地图上的特征，包括点云和地标，确保地图特征在空间上与新的位姿信息一致。
38、根据新的位姿信息更新子地图了保持子地图在全局地图中的正确位置和方向。
39、一种激光定位与建图闭环检测系统，其特征在于：包括，
40、初始位姿估计模块，采集激光雷达点云数据，进行预处理，进行初始位姿估计计算。
41、改进位姿估计模块，进行后端优化，改进位姿估计。
42、闭环检测模块，将优化后的位姿点云累积到激光雷达子地图，进行闭环检测。
43、更新模块，更新位姿和地图。
44、一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现如上所述的方法的步骤。
45、一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上所述的方法的步骤。
46、本发明的有益效果：通过先进的位姿估计和后端优化，本方法能够更准确地确定机器人或设备的位置，提高了定位的精度。此外，闭环检测的引入保证了地图与实际环境的一致性，提高了地图的实时更新能力。这些改进使得本方法在复杂和动态环境中的应用更加可靠和有效。
1.一种激光定位与建图闭环检测方法，其特征在于，包括：
2.如权利要求1所述的激光定位与建图闭环检测方法，其特征在于：所述预处理包括，对所述激光雷达点云数据进行噪声过滤和数据压缩，所述噪声过滤为将原始点云数据集与噪声过滤阈值比较，在阈值内的数据保留，其余数据丢弃；
3.如权利要求2所述的激光定位与建图闭环检测方法，其特征在于：所述估进行初始位姿估计计算包括，将预处理后的点云与激光雷达子地图进行匹配，以估计初始位姿：
4.如权利要求3所述的激光定位与建图闭环检测方法，其特征在于：所述进行后端优化包括，从激光slam前端获取初始位姿估计，构建位姿图，包括每个位姿节点以及节点间的相对关系；
5.如权利要求4所述的激光定位与建图闭环检测方法，其特征在于：所述将优化后的位姿点云累积到激光雷达子地图包括，使用改进后的位姿估计信息，将新采集的激光雷达点云数据整合到当前的激光雷达子地图中，并且实时更新当前子地图的内容；
6.如权利要求5所述的激光定位与建图闭环检测方法，其特征在于：所述进行闭环检测包括，基于当前位姿和激光雷达子地图历史数据，生成闭环假设位姿：
7.如权利要求6所述的激光定位与建图闭环检测方法，其特征在于：所述更新位姿和地图包括，将得到的闭环位姿信息，对slam系统中记录的所有历史位姿进行调整；
8.一种采用如权利要求1-7任一所述方法的一种激光定位与建图闭环检测系统，其特征在于：
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述的方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
