【技术实现步骤摘要】
本专利技术涉及车辆定位与建图技术，尤其是涉及一种用于激光雷达同步定位与建图的点云地图管理方法。
技术介绍
1、在激光雷达同步定位与建图(slam)中，前端里程计设计是实现定位目标不可或缺的重要一环，目前主流里程计通过帧与点云地图间的匹配来推算物体的运动位置，并实现位姿估计。在点云地图中，点云信息丰富密集，覆盖范围广，每一帧点云数据都包含了大量信息，如何快速有效地构建、管理点云地图，是提高里程计位姿估计效果的关键。此外，对激光雷达点云地图的高效管理也有助于实时地更新和维护地图，以确保系统车辆能够准确认知当前行驶环境。
2、尽管点云地图的存储管理方式灵活多变，但都旨在提高点云匹配的准确性和效率。专利文献号为cn116608873a的中国专利技术公开了一种自动驾驶车辆的多传感器融合定位建图方法，其基于划分规则体素下的ikd-tree数据结构，建立局部地图来进行点云配准，该方法的增量更新与动态再平衡功能相比于k-d tree而言，极大提高了算法效率，但局部地图只维护原始点云，无法区分不同尺度大小的特征信息，从而在匹配准确度上会有所损失。专利文献号为cn117237428a的中国专利技术公开了一种面向三维点云的数据配准方法，该方法将八叉树结构应用于规则化的非空体素，这种结构对空间的表达效率较高，可以视点的密集程度合理分配节点资源，但是在索引规则体素时会随着点云规模增大而逐渐增大时间复杂度，进而影响里程计的点云配准速度。
3、综上，管理地图中多分辨率的体素以及不同尺度大小的特征，可以达到更好的匹配效果，有助于提高定位
技术实现思路
1、本专利技术的目的就是为了进一步克服上述现有技术的缺陷，提供一种用于激光雷达同步定位与建图的点云地图管理方法，具有高分辨率管理和更新地图、显示维护地图中不同尺度的平面特征、地图搜索访问的计算量少、定位与建图精度高等优点。
2、本专利技术的目的可以通过以下技术方案来实现：
3、本专利技术提供了一种用于激光雷达同步定位与建图的点云地图管理方法，该方法包括以下步骤：
4、步骤1：综合考虑点云地图规模和硬件存储与计算能力，确定初始体素尺寸，将空间划分为规则的三维体素，舍弃不包含点云的体素；
5、步骤2：为剩余每个体素建立一棵八叉树，规则体素作为八叉树的根节点，利用哈希表建立体素三维索引与八叉树的对应关系；
6、步骤3：计算体素中点云簇的协方差矩阵，若其最小特征值小于阈值，则生成拟合平面。若其最小特征值超过阈值，将该体素再均分为八个子体素，舍弃无点云的子体素，重新在剩余各子体素内计算协方差矩阵和最小特征值，直至其最小特征值小于阈值或体素达到最大深度；
7、步骤4：根据体素分割情况同步生成八叉树子节点，在子节点中存储子体素的点云和拟合平面参数，至此完成点云地图构建；
8、步骤5：对于新一帧点云簇，在哈希表中查询每个点最相近的规则体素，再在体素中查找最相近的八叉树子节点，确定最佳匹配子体素，建立点-面约束，完成位姿估计；
9、步骤6：根据位姿估计结果，由点云簇中的各个点云在世界系下的三维坐标计算出各点云所对应体素的索引，将各点云分别添加到对应的哈希-八叉树地图的体素中并更新体素的平面参数，若某点云处无体素，新建体素及八叉树，添加至哈希表，至此完成点云地图更新。
10、优选地，所述步骤1中将划分的三维体素在三个方向的数量分别记为i,j,k，任一给定点pn所在体素的索引计算具体公式为：
11、
12、其中，i,j,k为给定点pn所在体素的三维索引，i∈[0,i]，j∈[0,j]，k∈[0,k]，xn,yn,zn为pn的三维坐标，dx,dy,dz分别为所选体素在x,y,z方向上的尺寸，且dx＝dy＝dz。
13、优选地，所述步骤2中的哈希表只存储非空体素而舍弃空体素，哈希表中存储了多个“键-值”对，“键”为规则体素三维索引g(i,j,k)对应的哈希值id，计算公式为：
14、id＝(i·p1 xor j·p2 xor k·p3)mod n
15、其中，p1,p2,p3为选定的质数，n为哈希表的长度。“值”为规则体素所对应的八叉树结构。
16、优选地，所述步骤3包括以下子步骤：
17、步骤3-1：将每个体素内所有的点云表示为一个集合其中，n为体素中点云的总数量；
18、步骤3-2：计算中心点具体公式为：
19、
20、步骤3-3：计算点云协方差矩阵m，具体公式为：
21、
22、步骤3-4：计算m的特征值，如果最小特征值小于某一阈值，则生成拟合平面；如果最小特征值大于某一阈值，认为该体素中的点云不能构成一个平面，此时，对其进行体素细分，即将其等分为八个更小的体素(以下简称小体素)，舍弃不含点云的小体素，重新在剩余各子体素内计算协方差矩阵和最小特征值，继续进行平面拟合判断以及可能的进一步体素细分，直到所有小体素均实现平面拟合，或者达到指定最大分辨率；
23、步骤3-5：计算拟合平面协方差矩阵的最小特征值对应的特征向量n，将其作为拟合平面的法向量。
24、优选地，所述步骤4中同步生成八叉树子节点的过程具体为：八叉树的深度与体素细分次数相等，初始规则体素每细分一次，其对应的八叉树增加一层深度；细分出的小体素中每一个包含点云的非空小体素，对应于八叉树新增深度中的一个子节点。
25、优选地，所述步骤4中子节点存储的信息具体包括：体素中点云的三维空间坐标psubw、体素中拟合平面的参数(代表能否构成平面的标识fplane、所构成平面的中心点及法向量nsub)，以及是否包含子体素的标识fsub。
26、优选地，所述步骤5包括以下子步骤：
27、步骤5-1：在获取到新一帧点云簇时，先计算各扫描点在初始规则体素划分下的三维索引，利用哈希表查找与该扫描点距离最近的初始体素，后在八叉树结构中查找距离最近的子节点，将该子节点代表的子体素作为该扫描点的最佳匹配体素；
28、步骤5-2：将新扫描点pnw放入集合pw，利用八叉树中查询最佳匹配体素的拟合平面信息，建立扫描点与拟合平面之间的点-面约束，具体公式如下：
29、
30、其中，di为第i个扫描点的点面距离，ni为与第i个扫描点匹配的平面的法向量，为激光雷达的估计位姿，fh为点-面距离函数；
31、将所构建的所有点-面匹配对排列为矩阵形式，公式如下：
32、
33、步骤5-3：利用列文伯格-马夸尔特(lm)迭代求解方法，估计激光雷达的位姿。
34、优选地，所述步骤5-1包括以下子步骤：
35、步骤5-1-1：通过如下坐标变换公式将新扫描点变本文档来自技高网...
【技术保护点】
1.一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，该方法包括以下步骤：
2.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤1中将划分的规则三维体素在三个方向的数量分别记为I,J,K，任一给定点Pn所在体素的索引计算具体公式为：
3.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤2中的哈希表只存储含有点云的非空体素而舍弃空体素，哈希表中存储了多个“键-值”对，“键”为规则体素三维索引G(i,j,k)对应的哈希值id，计算公式为：
4.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤3包括以下子步骤：
5.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤4中同步生成八叉树子节点的过程具体为：八叉树的深度与体素细分次数相等，初始规则体素每细分一次，其对应的八叉树增加一层深度；细分出的小体素中每一个包含点云的非空小体素，对应于八叉树新增深度中的一个子节点。
6.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤4中子节点存储的信息具体包括：体素中点云的三维空间坐标PsubW、体素中拟合平面的参数：包括代表能否构成平面的标识Fplane、所构成平面的中心点及法向量nsub，以及是否包含子体素的标识Fsub。
7.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤5包括以下子步骤：
8.根据权利要求7所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤5-1包括以下子步骤：
9.根据权利要求7所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤5-3的位姿估计过程具体包括以下子步骤：
...【技术特征摘要】
1.一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，该方法包括以下步骤：
2.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤1中将划分的规则三维体素在三个方向的数量分别记为i,j,k，任一给定点pn所在体素的索引计算具体公式为：
3.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤2中的哈希表只存储含有点云的非空体素而舍弃空体素，哈希表中存储了多个“键-值”对，“键”为规则体素三维索引g(i,j,k)对应的哈希值id，计算公式为：
4.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤3包括以下子步骤：
5.根据权利要求1所述的一种用于激光雷达同步定位与建图的点云地图管理方法，其特征在于，所述步骤4中同步生成八叉树子节点的过程具体为：八叉树的深度与体素细分...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。