基于几何先验的二维激光惯性退化检测与视觉辅助方向选择性更新方法及系统
本发明涉及机器人导航定位技术,具体涉及一种基于几何先验的二维激光惯性退化检测与视觉辅助方向选择性更新方法及系统。
背景技术:
1、近年来,随着移动机器人技术的快速发展,机器人被广泛应用于室内清洁、仓储配送和巡检等场景。同时定位与地图构建(slam)是移动机器人实现自主导航的关键技术。在缺少全球定位系统(gps)信号的室内环境中,由二维激光雷达和惯性测量单元(imu)构成的激光惯性slam系统,具有成本较低、测距稳定和定位效率较高等优点(参见文献[1] zhangb., peng z., zeng b., lu j. 2dliw-slam: 2d lidar-inertial-wheel odometry withreal-time loop closure,arxiv:2404.07644,2024.)。然而,当机器人进入长走廊、空旷区域或旋转对称环境等弱几何约束场景时,激光扫描数据无法在全部位姿方向上提供充分约束,容易产生定位漂移、轨迹跳变和地图重影。因此,提高机器人在弱几何约束环境下的定位稳定性具有重要意义。
2、现有二维激光定位方法通常将当前激光帧与局部子地图进行扫描匹配,并根据匹配残差估计机器人在平面内的位置和航向角。为识别激光配准退化,部分方法通过分析配准信息矩阵的特征值或点云几何分布判断约束强弱,但其判断结果容易受到激光点数量、测量噪声和环境尺度变化的影响,难以准确确定具体退化方向。文献[2](姚二亮,宋海涛,赵婧,范晓婧.考虑点云结构和表观信息的激光雷达-惯性slam算法[j].机器人,2024,46(4).)公开的技术方案在检测到激光退化后切换至视觉惯性定位。公开号为cn118067108a的中国专利申请公开一种基于激光雷达-惯导-视觉融合的建图和定位方法及系统,该方案将激光雷达、imu与视觉信息进行整体融合。此类方法没有充分区分激光配准中的退化方向和非退化方向,可能舍弃激光雷达在非退化方向上的有效约束,或者将视觉误差引入原本约束充分的方向,从而影响定位精度和稳定性。
3、现有退化检测方法多依据激光配准信息矩阵的特征值、条件数或点云几何分布进行判断,检测结果容易受到激光点数量、测量噪声、错误匹配及环境尺度变化的影响,存在退化误判或漏判的情况。同时,仅通过数值指标难以确定弱约束方向是否与环境实际几何结构一致。
4、现有方法通常只能判断当前是否发生退化,难以进一步区分沿走廊轴向的平移退化、二维平移退化或航向角退化。此外,机器人位姿中的平移量与旋转量存在量纲差异,直接进行方向比较和子空间投影会影响退化方向判断的准确性。
5、部分方法在检测到激光退化后,直接切换至视觉惯性定位,或者在全部位姿方向上融合视觉信息,没有区分激光配准的退化方向和非退化方向。这可能舍弃激光雷达在非退化方向上的有效约束,或者将视觉误匹配、光照变化和运动模糊产生的误差引入原本约束充分的方向。
6、现有多传感器融合方法通常采用固定融合权重,难以根据激光退化程度和视觉观测质量的变化动态调整。当视觉特征不足或观测质量较差时,仍可能引入不可靠的视觉约束,进而影响机器人定位的精度和稳定性。
7、因此,需要一种能够结合环境几何结构与激光配准信息准确检测退化方向,并仅在退化方向引入视觉辅助约束的定位方法,以充分保留二维激光雷达在非退化方向上的有效信息,提高移动机器人在弱几何约束环境下的定位精度和鲁棒性。
技术实现思路
1、本发明针对现有技术中的不足,提供一种基于几何先验的二维激光惯性退化检测与视觉辅助方向选择性更新方法及系统。解决仅依据信息矩阵特征值或点云统计特征进行退化检测时容易误判的问题,实现环境几何先验方向与激光配准弱约束方向的联合核验,提高退化检测的准确性。解决现有方法难以识别具体退化自由度,以及平移量与旋转量量纲不一致的问题,实现对轴向平移、二维平移和航向角等退化方向的准确判定。解决整体切换定位模态或全方向融合视觉信息时,激光有效约束被舍弃或视觉误差被引入非退化方向的问题,实现视觉辅助约束的方向选择性融合。解决固定融合权重难以适应传感器观测质量变化的问题,根据激光退化程度和视觉观测质量动态调节融合权重,提高弱几何约束环境下机器人定位的精度和稳定性。
2、为实现上述目的,本发明采用以下技术方案:
3、一种基于几何先验的二维激光惯性退化检测与视觉辅助方向选择性更新方法,用于地面移动机器人在弱几何约束环境下的平面定位,所述方法包括以下步骤:
4、s1、获取二维激光雷达连续扫描数据、惯性测量单元imu测量数据以及同步图像序列,并根据imu测量数据对相邻激光帧之间的机器人运动状态进行传播,对二维激光雷达连续扫描数据进行扫描去畸变处理,得到当前激光帧的位姿预测信息;
5、s2、基于位姿预测信息,将当前激光帧与局部子地图进行二维激光前端配准,获得激光配准残差,并由激光配准残差构造二维激光配准信息矩阵;
6、s3、根据局部子地图提取环境几何先验信息,并根据环境几何先验信息构造候选退化子空间;
7、s4、对二维激光配准信息矩阵进行弱约束方向分析,获得hessian弱约束子空间,并通过候选退化子空间与hessian弱约束子空间之间的一致性核验确定最终退化子空间;
8、s5、在机器人位姿增量空间中引入加权度量矩阵,并根据最终退化子空间的基矩阵构造退化方向加权正交投影矩阵和非退化方向投影矩阵;
9、s6、根据退化方向加权正交投影矩阵和非退化方向投影矩阵,对二维激光配准信息、imu预积分先验信息、边缘化先验信息以及视觉辅助信息进行方向选择性融合,得到方向选择性更新信息矩阵;
10、s7、根据方向选择性更新信息矩阵求解机器人位姿增量,将位姿增量分解为退化方向分量和非退化方向分量,在非退化方向上保留二维激光雷达配准约束和imu约束作为主定位约束,在退化方向上衰减二维激光雷达弱约束并引入由所述同步图像序列构造的视觉辅助约束,实现机器人位姿的方向选择性更新。
11、为优化上述技术方案,采取的具体措施还包括:
12、进一步地,s2中,所述构造二维激光配准信息矩阵具体为:
13、
14、其中,表示二维激光配准信息矩阵,表示第个激光配准残差对机器人位姿增量的雅可比矩阵,表示第个激光配准残差的权重矩阵。
15、进一步地,s3中,所述根据环境几何先验信息构造候选退化子空间具体包括:
16、当所述环境几何先验信息表征当前环境为长走廊环境时,根据局部子地图中的平行墙面线特征提取走廊轴向单位向量,并将沿走廊轴向的平移方向确定为候选退化方向,其对应的候选退化子空间表示为:
17、
18、当所述长走廊环境中可见端部角点或横向结构不足,且航向角约束弱时,将航向角方向进一步加入候选退化子空间,其对应的候选退化子空间表示为:
19、
20、当所述环境几何先验信息表征当前环境为平移全向弱约束环境时,将二维平面内的两个平移方向确定为候选退化方向,其对应的候选退化子空间表示为:
21、
22、当所述环境几何先验信息表征当前环境为旋转对称环境时,将航向角方向确定为候选退化方向,其对应的候选退化子空间表示为:
23、
24、候选退化子空间根据当前环境类型在、、和中选择得到。
25、进一步地,s4中,所述对二维激光配准信息矩阵进行弱约束方向分析,获得hessian弱约束子空间具体为:
26、对二维激光配准信息矩阵进行特征分解:
27、
28、其中,表示由特征向量组成的矩阵,表示第n个特征向量,表示由特征值组成的对角矩阵,表示第n个特征值,且特征值满足:
29、
30、根据候选退化子空间的维数,选取二维激光配准信息矩阵中最小的前个特征值对应的特征向量,构造hessian弱约束子空间:
31、
32、其中,hessian弱约束子空间用于表示当前二维激光配准中约束强度最弱的位姿增量方向,是第k个特征向量。
33、进一步地,s4中,所述通过候选退化子空间与hessian弱约束子空间之间的一致性核验确定最终退化子空间具体包括:
34、在机器人位姿增量空间中引入加权度量矩阵,用于统一平移量与旋转量之间的量纲差异,所述加权度量矩阵表示为:
35、
36、其中,表示局部地图代表性尺度因子;
37、设候选退化子空间的基矩阵为,hessian弱约束子空间的基矩阵为,基于加权度量矩阵计算二者之间的加权主角:
38、
39、其中,表示矩阵的最大奇异值;
40、当所述加权主角满足如下条件时:
41、
42、判定所述候选退化子空间与所述hessian弱约束子空间具有方向一致性,并确认当前二维激光雷达配准在所述候选退化子空间对应方向上发生退化,将经一致性核验后的候选退化子空间确定为最终退化子空间:
43、
44、其中,表示预设主角阈值。
45、进一步地,所述局部地图代表性尺度因子根据局部子地图的空间尺度、激光匹配点分布或走廊几何尺度动态确定,计算公式如下:
46、
47、其中,表示局部子地图半径,表示有效匹配激光点相对于其质心的均方根距离,表示由平行墙面线特征估计得到的走廊宽度,表示二维激光雷达的有效匹配范围,、、、表示对应尺度项的权重系数,且满足:
48、
49、和分别表示预设的尺度因子下限和尺度因子上限,表示将计算结果限制在范围内。
50、进一步地,s5具体为:
51、
52、
53、其中,是退化方向加权正交投影矩阵,是非退化方向投影矩阵,是加权度量矩阵,是最终退化子空间的基矩阵,表示单位矩阵;
54、s6具体为:
55、
56、其中,表示imu预积分先验信息矩阵,表示边缘化先验信息矩阵,表示视觉辅助信息矩阵,表示二维激光雷达在退化方向上的保留权重,表示二维激光配准信息矩阵,表示视觉辅助置信度权重。
57、进一步地,s7中,所述根据方向选择性更新信息矩阵求解机器人位姿增量具体为:
58、构造方向选择性更新右端项:
59、
60、式中,是imu预积分先验信息矩阵对应的右端残差项,是边缘化先验信息矩阵对应的右端残差项,是二维激光配准信息矩阵对应的右端残差项,是视觉辅助信息矩阵对应的右端残差项;表示二维激光雷达在退化方向上的保留权重,表示视觉辅助置信度权重;是退化方向加权正交投影矩阵,是非退化方向投影矩阵;
61、并通过如下线性方程求解机器人位姿增量:
62、
63、式中,是方向选择性更新信息矩阵;
64、所述将位姿增量分解为退化方向分量和非退化方向分量具体为:
65、对于机器人位姿增量,分别得到其退化方向分量和非退化方向分量:
66、
67、
68、其中,表示位姿增量在最终退化子空间上的投影分量,表示位姿增量在非退化子空间上的投影分量。
69、进一步地,所述视觉辅助信息矩阵和对应右端残差项由同步图像序列中的视觉特征观测约束构造得到,具体包括:
70、对所述同步图像序列进行特征点提取与特征跟踪,获得相邻关键帧之间的有效匹配特征点;当所述同步图像序列由双目相机采集时,将第个空间特征点在第个关键帧中的双目观测表示为:
71、
72、其中,和分别表示所述空间特征点在左目图像中的像素横坐标和像素纵坐标,和分别表示所述空间特征点在右目图像中的像素横坐标和像素纵坐标;
73、根据双目相机外参、相机与机器人机体系之间的外参以及机器人当前位姿估计,将所述空间特征点投影至左目图像平面和右目图像平面,构造双目重投影残差:
74、
75、其中,表示第个空间特征点在第个关键帧中的双目重投影残差,表示双目视觉投影函数,表示第个关键帧对应的机器人位姿,表示第个空间特征点在世界坐标系下的三维坐标;
76、对所述双目重投影残差进行一阶线性化,得到视觉残差对机器人位姿增量的雅可比矩阵,并构造视觉辅助信息矩阵和对应右端残差项:
77、
78、
79、其中,表示所述双目重投影残差对应的权重矩阵。
80、本发明还提出一种基于几何先验的二维激光惯性退化检测与视觉辅助方向选择性更新系统,包括:
81、数据获取模块,用于获取二维激光雷达连续扫描数据、惯性测量单元imu测量数据以及同步图像序列;
82、imu传播与扫描去畸变模块,用于根据所述惯性测量单元imu测量数据对相邻激光帧之间的机器人运动状态进行传播,并对所述二维激光雷达连续扫描数据进行扫描去畸变处理,得到当前激光帧的位姿预测信息;
83、二维激光前端配准模块,用于基于所述位姿预测信息,将当前激光帧与局部子地图进行二维激光前端配准,获得激光配准残差,并由所述激光配准残差构造二维激光配准信息矩阵;
84、几何先验退化检测模块,用于根据所述局部子地图提取环境几何先验信息,根据所述环境几何先验信息构造候选退化子空间,并对所述二维激光配准信息矩阵进行弱约束方向分析,获得hessian弱约束子空间,通过所述候选退化子空间与所述hessian弱约束子空间之间的一致性核验确定最终退化子空间;
85、视觉辅助约束模块,用于根据所述同步图像序列进行视觉特征提取与特征跟踪,构造视觉辅助信息矩阵和对应右端残差项;
86、方向选择性后端更新模块,用于根据所述最终退化子空间构造退化方向加权正交投影矩阵和非退化方向投影矩阵,并根据所述退化方向加权正交投影矩阵和非退化方向投影矩阵,对二维激光配准信息、imu预积分先验信息、边缘化先验信息以及视觉辅助信息进行方向选择性融合,以在非退化方向上保留二维激光雷达配准约束和imu约束作为主定位约束,在退化方向上衰减二维激光雷达弱约束并引入视觉辅助约束,完成机器人当前位姿更新。
87、本发明的有益效果是:
88、本发明实现了环境几何先验方向与激光配准弱约束方向的联合核验,提高了退化检测的准确性。实现了对轴向平移、二维平移和航向角等退化方向的准确判定。根据激光退化程度和视觉观测质量动态调节融合权重,提高弱几何约束环境下机器人定位的精度和稳定性。
89、能够结合环境几何结构与激光配准信息准确检测退化方向,并仅在退化方向引入视觉辅助约束的定位方法,以充分保留二维激光雷达在非退化方向上的有效信息,提高移动机器人在弱几何约束环境下的定位精度和鲁棒性。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com