一种GNSS增强的激光惯性SLAM闭环检测与坐标校准方法及系统与流程

本发明涉及机器人同步定位与建图领域。更具体地，本发明涉及一种gnss增强的激光惯性slam闭环检测与坐标校准方法及系统。

背景技术：

1、在gnss增强的激光惯性slam闭环检测与坐标校准领域，城市峡谷、隧道等gnss退化场景下的连续高精度定位建图已成为核心需求。lio紧耦合lidar与imu可实现实时位姿估计，但长距离行驶存在累积漂移，闭环检测是消除漂移的关键；gnss/rtk虽能提供全局观测，但其enu坐标系与lio世界坐标系存在未知航向偏差，若不校准，gnss位置无法直接用于lio坐标系下的闭环检测与修正其中，lio指紧耦合融合lidar点云与imu数据实现实时位姿估计的激光惯性里程计，gnss/rtk指全球导航卫星系统/实时动态差分定位，enu指东-北-天坐标系。

2、现有技术中，以公告号cn114659514b的中国发明专利为例，其虽采用体素化精配准、gnss置信度加权和体素化回环因子，但未解决gnss-lio航向在线校准问题，闭环检测也未在描述符匹配失败后利用校准gnss位置进行回退触发，坐标校准与回环优化未形成紧耦合。同样的以公开号为cn115479598a的中国专利申请为例，其虽通过雷达虹膜二进制特征图和汉明距离实现回环检测，并以因子图融合gps因子，但gps仅作为后端绝对测量因子，不参与回环候选触发，且无gnss-lio航向校准，位置回退依赖轮速/里程计，长距离漂移下可靠性不足。

3、因此，现有技术主要存在gnss-lio航向在线校准缺失、单一描述符闭环召回不足、主检测失败后缺乏gnss位置回退、回环验证与坐标校准协同不足等技术问题。

技术实现思路

1、为解决上述gnss-lio航向在线校准缺失、单一描述符闭环召回不足等技术问题，本发明在如下的多个方面中提供方案。

2、在第一方面中，本发明公开了一种gnss增强的激光惯性slam闭环检测与坐标校准方法，包括：

3、s10：获取全球导航卫星系统与激光惯性里程计世界坐标系的位移向量，并进行质量筛选与加权；

4、s20：根据加权后的位移向量构建法方程求解航向旋转角，并根据位置偏移建立全球导航卫星系统与激光惯性里程计世界坐标系之间的坐标变换；

5、s30：根据关键帧点云建立全局描述符并进行相似度匹配的主检测；

6、s40：响应于主检测失败，将当前全球导航卫星系统位置经所述坐标变换映射至激光惯性里程计世界坐标系，根据映射位置与历史关键帧位置之间的距离确定候选闭环；

7、s50：对所述候选闭环执行多级迭代最近点配准，在配准过程中根据旋转偏差拒绝镜像解，并根据最终配准适应度确认闭环，通过位姿图优化更新关键帧位姿及所述位置偏移。

8、与现有的技术相比，通过对gnss与激光惯性里程计位移进行质量筛选和加权求解，在线消除两坐标系航向及位置偏差，无需人工标定或在滤波状态中增维；在点云描述符主检测失败后，再利用校准后的gnss位置回退搜索历史关键帧，可补偿单一描述符因视角变化、场景相似而产生的闭环漏检，并避免长期漂移的里程计位置误导候选搜索；候选经多级配准、镜像解拒绝和适应度检验后才进入位姿图，既容纳初始漂移，又抑制错误闭环，适用于机器人长距离、重复场景下的建图与定位。

9、优选地，步骤s10中根据全球导航卫星系统定位质量设置所述位移向量的权重，具体为：

10、对于实时动态差分定位固定解，采用对应的单步激光惯性里程计位移距离作为基础权重；

11、对于实时动态差分定位浮点解，将所述基础权重按预设的比例降低；

12、对于单点解，不参与所述位移向量对的累积；

13、且仅当所述单步激光惯性里程计位移距离大于预设的距离时，累积对应的位移向量对。

14、优选地，步骤s10还包括根据加权后的位移向量构建法方方程，表示为：

15、

16、其中，a表示由各组全球导航卫星系统水平位移构成的系数矩阵；表示为a的转置矩阵；w表示为位移权重构成的权重矩阵；b表示由各组激光惯性里程计水平位移构成的观测向量；c表示为航向旋转角的余弦分量；s表示为航向旋转角的正弦分量。

17、优选地，根据所述法方程求解航向旋转角，并对所述航向旋转角对应的余弦分量和正弦分量进行归一化。

18、优选地，步骤s10还包括：

19、根据多个激光惯性里程计世界坐标系位置与航向旋转后的全球导航卫星系统位置之差的平均值确定所述位置偏移；

20、根据所述航向旋转角和所述位置偏移建立所述全球导航卫星系统坐标系与所述激光惯性里程计世界坐标系之间的坐标变换；

21、校准完成后，将全球导航卫星系统位置经所述坐标变换映射至所述激光惯性里程计世界坐标系；

22、在映射后的全球导航卫星系统位置与激光惯性里程计位置之间的残差不大于预设最大残差时，对激光惯性状态进行位置修正。

23、优选地，步骤s20还包括：

24、根据相邻位姿之间的相对变换累计平移量和旋转量，当累计平移量大于预设距离或累计旋转量大于预设角度时创建关键帧，并重置所述累计平移量和所述累计旋转量；

25、将所述关键帧的点云投影至由多个环和扇区构成的极坐标网格，根据各网格的最大高度和平均强度形成高度描述符和强度描述符，并根据各环的非空网格占比形成环键。

26、优选地，所述相似度匹配具体为：

27、根据所述环键之间的距离对历史关键帧进行预筛，并排除时间上邻近的历史关键帧；

28、分别对预筛后的历史关键帧对应的高度描述符和强度描述符进行列移余弦距离计算，并对所得高度描述符距离和强度描述符距离进行加权融合；

29、当最小融合距离小于预设最小融合距离时确定主检测匹配成功，当不存在满足所述条件的历史关键帧时确定主检测失败，其中最小融合距离设置为0.3。

30、优选地，还包括对所述候选闭环执行多级迭代最近点配准，具体为：

31、以所述候选闭环对应的历史关键帧为中心，将其前后相邻关键帧的点云变换并合并为目标点云，以当前查询关键帧点云作为源点云；

32、分别对所述源点云和所述目标点云进行体素下采样，以所述历史关键帧位姿与当前查询关键帧位姿之间的相对位姿作为初始估计，依次采用递减的对应距离执行多级迭代最近点配准。

33、优选地，还包括：

34、计算当前配准结果相对于所述初始估计的旋转偏差；

35、当所述旋转偏差大于60°时拒绝所述候选闭环。

36、在第二方面中，本发明公开了一种gnss增强的激光惯性slam闭环检测与坐标校准系统，包括处理器和存储器，所述存储器存储有计算机程序指令，当所述计算机程序指令被所述处理器执行时实现第一方面记载的gnss增强的激光惯性slam闭环检测与坐标校准方法。

37、本发明的有益效果在于：以gnss与lio位移差分的加权法方程在线校准航向及位置偏移，并仅在点云描述符主检测失败后调用校准gnss位置回退搜索，再以递减对应距离的多级icp和旋转偏差判据确认闭环。闭环优化结果反向更新位置偏移，使校准、补漏检和全局轨迹修正形成协同，适用于长距离及重复结构环境中的持续定位建图。