一种基于激光雷达强度主动视觉与多源观测融合的定位建图方法

摘要: 本发明属于同时定位与地图构建、多传感器融合及激光雷达主动视觉感知,具体涉及一种把激光雷达回波强度作为主动视觉观测,并把真实径向距离、惯性测量和可选相机观测统一用...
一种基于激光雷达强度主动视觉与多源观测融合的定位建图方法

本发明属于同时定位与地图构建、多传感器融合及激光雷达主动视觉感知,具体涉及一种把激光雷达回波强度作为主动视觉观测,并把真实径向距离、惯性测量和可选相机观测统一用于位姿与地图估计的定位建图方法。

背景技术:

1、同时定位与地图构建需要移动载体在未知或部分未知环境中估计自身位姿并构建环境地图。相机能够利用纹理和外观完成特征关联、重定位与回环检测,但其属于被动成像传感器,在黑暗、过曝、光照快速变化或弱纹理区域中可能难以持续提供稳定观测。

2、激光雷达通过主动发射激光并测量回波获得真实尺度三维几何,其测距不直接依赖环境可见光;但是在长走廊、隧道、开阔区域或几何结构单一的环境中,点云几何配准可能沿部分方向约束不足。惯性测量能够提供高频短时运动信息,但长期积分会累积漂移,需要外部观测持续校正。

3、现有激光雷达—视觉—惯性定位建图方案通常把激光雷达用于几何约束、把相机用于视觉约束、把惯性测量用于运动传播,并分别维护点云配准、视觉跟踪、惯性传播及多模态融合过程。当相机退化时,视觉约束会显著减少,系统又主要依赖激光雷达几何与惯性约束。

4、激光雷达除输出三维坐标外还记录回波强度,回波强度的空间变化包含可用于图像式关联的材料、边缘和结构纹理信息。wei gao等公开的《super lidar intensity forrobotic perception》,ieee robotics and automation letters,第11卷第4期,第4275—4282页,2026年,doi:10.1109/lra.2026.3664533,公开了面向非重复扫描激光雷达的强度稠密化模型。该公开模型属于已有技术,本发明不主张其原始网络结构、损失函数或预训练权重。

5、强度补全像素表示推断得到的二维外观,不能视为真实激光测距。若直接把补全位置反投影为三维点或写入距离约束,补全误差会被错误转化为几何误差。因此,仍需要一种在共享状态与共享地图中利用稠密强度外观,同时严格隔离真实径向距离与补全位置,并按照观测质量融合各类残差的方法。

技术实现思路

1、要解决的技术问题

2、本发明要解决的技术问题是:在相机视觉可能退化的条件下,如何把激光雷达强度转化为可供视觉前端处理的主动视觉观测,并在不把补全像素误作真实测距的前提下,与径向距离、惯性测量及可选相机观测共同完成定位建图。

3、本发明还要解决固定融合权重不能反映局部对比度、匹配质量、真实测距有效性及距离不确定性的问题,使低质量观测能够被门控或降低其对状态估计的影响。

4、技术方案

5、为解决上述技术问题,本发明提供一种基于激光雷达强度主动视觉与多源观测融合的定位建图方法,包括以下步骤:

6、s1、获取带时间戳的激光雷达点云和惯性测量,并在相机可用时获取相机图像;由激光雷达点云生成像素对齐的稠密强度图、稀疏径向距离图、距离方差图和有效观测掩码,其中稀疏径向距离图仅在真实有效激光返回位置存储径向距离。

7、s2、从稠密强度图提取和关联图像特征,把各特征关联至最近球面像素;仅在有效观测掩码、距离方差和有效测距范围均通过门控时读取真实径向距离。具有真实径向距离的特征能够直接初始化共享三维地图点;不具有真实径向距离的补全位置只保留二维轨迹,直至后续获得有效径向距离或满足三角化条件。

8、s3、在共享载体状态和共享三维地图点下建立激光雷达强度重投影因子、径向距离一致性因子和惯性预积分因子,并在存在有效相机观测时建立相机重投影因子;激光雷达强度描述子与相机描述子分别在各自模态内匹配。

9、s4、先依据视场、遮挡、匹配几何及对应测量有效条件形成二值门控,再由有效观测数量、跟踪内点率、局部对比度、描述子匹配质量、补全位置标志、径向距离连续性和距离方差中的至少一种形成置信度权重;所述动态置信度作用于激光雷达强度、径向距离和相机观测,惯性因子采用预积分噪声传播所得信息矩阵。

10、s5、在滑动窗口内对通过门控的各观测因子执行带鲁棒核的非线性最小二乘优化,输出载体六自由度位姿轨迹和稀疏三维地图。

11、有益效果

12、本发明把激光雷达强度转换为主动获取的视觉式图像,使激光雷达能够同时提供外观关联与真实尺度径向距离,降低定位建图对被动相机单一视觉来源的依赖。

13、本发明利用有效观测掩码和距离方差隔离补全强度与真实测距,避免以补全像素直接构造三维点或距离因子,并使真实径向距离能够用于地图点初始化和距离一致性约束。

14、本发明使激光雷达主动视觉、真实径向距离、惯性测量和可选相机观测共享位姿状态与地图点,减少不同里程计子系统之间重复状态和重复地图维护。

15、本发明在硬门控之后进一步计算模态级和观测级置信度,并以预积分协方差单独确定惯性信息量,从而避免不确定观测在联合优化中产生过大影响。

1.一种基于激光雷达强度主动视觉与多源观测融合的定位建图方法,其特征在于,包括以下步骤:

2.根据权利要求1所述的方法,其特征在于,所述s1和s2包括:以id,t、ds,t、vs,t和mt分别表示稠密强度图、稀疏径向距离图、距离方差图和真实有效激光返回掩码,并按下式确定距离有效标志:

3.根据权利要求1所述的方法,其特征在于,所述s3包括:对地图点计算连续球面预测像素和预测径向距离,分别建立采用水平最短周期差的激光雷达强度重投影残差和径向距离一致性残差;相机重投影残差由相机标定投影函数和固定外参计算;惯性预积分因子包括相邻关键帧的位置、速度、旋转、加速度计偏置和陀螺仪偏置残差,并由误差状态转移矩阵、噪声注入矩阵和惯性噪声协方差递推预积分协方差。

4.根据权利要求1所述的方法,其特征在于,所述s4对候选观测先计算二值门控γi,tm,并对通过门控的激光雷达强度、径向距离和相机观测按下式确定权重:

5.根据权利要求1所述的方法,其特征在于,所述s5采用高斯—牛顿法或列文伯格—马夸尔特法迭代求解;对激光雷达强度、径向距离和相机观测残差采用huber鲁棒核,对惯性残差和边缘化先验不采用所述观测鲁棒核;固定窗口第一状态或加入等价先验消除规范自由度,在增量范数不大于收敛阈值或达到最大迭代次数时停止,并输出优化后的位姿轨迹和共享稀疏三维地图。