一种基于面元特征的激光雷达位姿估计方法与流程

本发明涉及激光雷达位姿估计，尤其涉及一种基于面元特征的激光雷达位姿估计方法。

背景技术：

1、面对危险复杂的地下环境，人们希望利用机器人技术为地下资源勘探、矿井生产数字化管理和地下军事行动等领域带来一场技术革新，以减少地下任务中的人员伤亡。随着全自主移动机器人应用场景越来越广泛，机器人技术也面临更大的挑战。实时定位与建图(slam)是自主移动机器人在未知环境中执行任务的关键技术，而在地下环境中部署一套精确鲁棒的slam系统仍然十分困难。多传感器异步数据的融合、设计在广泛场景中能够可靠检测和匹配的特征、构建合理的优化框架等模块设计是决定slam系统性能的关键。目前，大多数slam方法更多关注于处理一般场景，而在无gps信号、传感器视野受限、移动平台抖动剧烈、特征严重退化等挑战的地下环境中位姿估计精度下降，甚至是系统失效。

2、在多传感器融合slam系统中，通过前后两帧传感器数据进行数据关联，构建关于两帧时刻机器人位姿变换的代价函数，从而求解当前帧时刻到上一帧时刻(或全局坐标系)的机器人相对位姿变换。然后，利用估计的姿态将当前帧数据注册到全局地图中。但是错误数据关联的存在，位姿估计存在一定的误差，位姿估计的误差会降低地图的精度，进而在下一次数据关联的时候降低位姿估计的精度。如此反复迭代，造成恶性循环。设计一个稳健且多功能的激光雷达惯导slam系统仍然是一个挑战，尤其在广泛场景中能可靠检测和匹配的特征进行有效的数据关联是艰巨的任务。

3、地下环境中移动机器人的位姿估计主要面临的挑战有特征稀疏，环境复杂。近年来，有很多基于3d点云的位姿估计方法来实现移动机器人的定位，如中国专利“cn114004869a一种基于3d点云配准的定位方法”提出了一种基于3d点云的定位方法，在帧间匹配时使用最邻近搜索,搜索与当前帧点云邻近的点云数据。使用i cp匹配算法计算出当前机器人位姿与点云地图中机器人位姿的相对转换关系,将其作为约束添加到位姿图中对激光里程计进行优化,得到准确的机器人位姿信息。然而，基于icp的点云配准方法计算量大，同时在地下环境中不能取得好的定位精度。

4、也有很多基于学习的方法来实现点云的配准，如中国专利“cn117689702a一种基于几何注意力机制的点云配准方法和装置”提出了一种基于几何注意力机制的点云配准方法和装置，包括：真实激光雷达采集得到目标点云数据,虚拟激光雷达采集得到源点云数据；将点云数据进行体素下采样后输入局部特征提取网络得到局部特征描述符；提取点云数据中的几何结构信息,在基于几何注意力机制的transformer网络中,将几何结构信息嵌入局部特征描述符以关联全局上下文信息并增强局部几何结构信息；采用最近邻搜索、随机采样一致性算法以及最优欧式变换,完成点云配准。然而，基于学习的方法需要耗费额外的时间进行训练，同时模型的质量依赖于训练的数据，模型的泛化性也是一个问题。

技术实现思路

1、(一)发明目的

2、为解决背景技术中存在的技术问题，本发明提出一种基于面元特征的激光雷达位姿估计方法。

3、(二)技术方案

4、为解决上述问题，本发明提出了一种基于面元特征的激光雷达位姿估计方法，包括：

5、步骤s100，提取面元特征：

6、根据激光雷达对真实环境进行捕获，以获取目标点云；

7、利用金字塔网格对目标点云进行体素化；

8、使用八叉树的数据结构对体素化后的不同分辨率点云进行存储；

9、计算体素中点云形状参数，以使得在当前帧点云提取多分辨面元特征；

10、步骤s200，多分辨面元的数据关联：

11、分别在当前帧和局部地图中用七维向量空间最近邻搜索最近的面元特征，互为最近则构建一对数据关联；

12、步骤s300，激光雷达惯导紧耦合里程计：

13、将提取的面元特征和imu测量值输入到状态估计模块中进行状态估计，通过估计的姿态将点云注册到全局框架下，并将新注册的点云和地图中的点云映射合并，更新后的地图用于下一步数据关联，从而注册新一帧的点云。

14、优选的，在所述体素化过程中，记录每个网格中点云数量，一个体素中至少包含七个点的情景下，进行计算体素中点云形状参数。

15、优选的，每个体素以其内部点的平均测量时间标记该体素的时间，若一个体素中的点的平均测量时间晚于该体素中最早被测量的点的时间，表明这些点不包含于该体素中。

16、优选的，从每个体素中提取面元特征时，所述面元特征描述是由每个体素中点云的均值和协方差计算局部特征参数来拟合点的分布，对三维激光雷达扫描的点云，根据点的位置和时间戳进行聚类和椭圆体拟合生成多尺度面元特征。

17、优选的，所述对三维激光雷达扫描的点云，根据点的位置和时间戳进行聚类和椭圆体拟合生成多尺度面元特征，包括：

18、将空间划分为一组多尺度立方体单元，并将每个立方体单元内的具有相近时间戳的激光雷达点聚合在一起；

19、根据预先确定的阈值为每个足够大的点群拟合一个椭圆体；

20、每个椭圆体的中心和其形状分别由点群中三维点的样本平均值和协方差决定，根据椭球轴的长度确定局部点云结构是平面/圆柱形。

21、优选的，所述面元特征存储于surfelmap的数据结构中，基于八叉树的surfelmap的数据结构中的节点i对应一个体素中面元属性，节点深度表示体素的尺度，对于叶节点，面元的深度属性为0，父节点的深度属性为1，surfelmap中节点i上的面元由体素中包含的点集定义，记为构成面元的基本属性公式如下：ni＝|vi|,

22、上式中，ni、si、ci为每个面元在surfelmap的数据结构中的存储值，根据所述属性可快速计算vi的均值μi和协方差γi，其他属性由以下公式可得：

23、

24、式中，μi为均值，γi为与特征值λ0、λ1、λ2的协方差，ni＝λ0为与λ0相关的归一化特征向量；ρi为曲面的平面相似度；

25、根据相应金字塔网格内点云的均值和协方差从高分辨率向低分辨率网格的均值和协方差递归计算，以提高计算效率。

26、优选的，在所述步骤s200中，通过kd-tree进行最近邻搜索完成面元和面元之间的关联匹配；

27、沿着一个共同平面法线最小化两个面元质心之间距离的残差，以求解获取两帧之间传感器的位姿变换。

28、优选的，将计算得到的面元形状特征信息纳入i cp对应步骤中以实现数据关联，每个包含足够数量的体素形成七维空间向量：

29、

30、式中，μ为点的质心，v1为平面的法向量，对应的网格大小，通过在七维空间中获取每个体素的最近邻体素，在每次迭代中估计每层面元的对应关系；

31、还包括面元构建残差，表达式如下：

32、ey＝ξynij(μi(δt(τi))-μj)

33、其中，公共表面法线nij是匹配面元中点融合后重新计算的协方差最小特征值对应的法向量，μi是面元i的质心，μj是面元j的质心,系数取决于传感器测量噪声σr和特征值λ1，δt(τi)为当前帧到上一帧激光雷达的位姿估计。

34、优选的，所述面元特征之间的匹配受质心在其表面法线方向上的偏移量及其表面法线之间角度的约束，在七维向量空间距离上和相互匹配面元法向量夹角的大小限制面元匹配的结果，以剔除部分互为最近关联的外点。

35、优选的，在步骤s300中，所述激光雷达惯导紧耦合里程计中包括：

36、构建描述传感器观测、机器人真实状态和测量误差之间关系的观测模型；

37、计算误差的协方差矩阵，以使得在优化问题中正确应用每个最小二乘项的权重；

38、计算每个观测值与其预测值之间的残差；

39、计算雅可比行列式，以保障优化过程的快速实施。

40、本发明的上述技术方案具有如下有益的技术效果：

41、采用本发明的技术方案，能够实现全局定位、高精度鲁棒定位的定位。通过基于面元的点云特征提取、基于多分辨面元的数据关联、激光雷达惯导紧耦合里程计来实现高精度位姿估计。通过在非结构化地下矿井，结构化人工隧道，结构化建筑和非结构化植被的园区等多个挑战性场景进行评估，并与其它方法金星和比较，实验结果表明，本发明提出的方法能够实现更高的位姿估计精度。通过使用本发明提出的位姿估计方法可以在地下场景下移动机器人高精度鲁棒定位。