基于强度辅助与退化感知的SLAM隧道定位方法及系统与流程
本发明属于车辆定位与导航,具体为基于强度辅助与退化感知的slam隧道定位方法及系统。
背景技术:
1、本部分的陈述仅仅提到了与本发明相关的背景技术,并不必然构成现有技术。
2、公路隧道作为连接山地、跨越江河的重要交通基础设施,其线路长、空间封闭、光照变化剧烈。随着自动驾驶与高级驾驶辅助系统的快速发展,车辆在隧道内实现连续、高精度的自主定位成为保障行车安全的关键需求。
3、然而,隧道环境完全阻隔导航卫星系统信号,车辆无法依赖卫星定位获取绝对位置信息,因此通常采用激光雷达与惯性测量单元融合的同步定位与建图方案,通过持续感知隧道三维几何结构实现位姿估计。
4、由于隧道内部空间呈狭长结构,长直区段几何形状单一,两侧壁面近乎平行且沿纵深方向缺乏显著变化。基于几何特征的激光惯性slam(simultaneous localization andmapping,同步定位与地图构建)在解算位姿时,主要依赖壁面等几何约束估计车辆位置。在长直区段内,由于沿隧道纵深方向的几何约束严重不足,导致现有基于几何特征的定位方法在该方向上存在约束缺失的缺陷,引发车辆纵向位姿估计随行驶距离增加而持续漂移、误差不断累积的现象,最终使定位结果严重偏离真实位置,无法维持车道级定位精度。
技术实现思路
1、本发明提供基于强度辅助与退化感知的slam隧道定位方法及系统,通过引入隧道壁面强度信息构建纵向约束,有效抑制车辆前进方向的定位漂移,实现隧道场景下长时程、高精度的位姿估计。
2、为了实现上述目的,本发明采用如下技术方案:
3、本发明的第一个方面公开了基于强度辅助与退化感知的slam隧道定位方法,包括以下步骤:
4、获取隧道的三维点云数据,经投影生成二维强度图像并提取强度线特征;强度线特征为隧道壁面上激光反射强度发生突变的像素点,经反向投影至三维空间后形成的线状特征,用于反映隧道壁面具有反射强度差异的物理结构;
5、获取三轴加速度与三轴角速度,在相邻两个关键帧之间的时间段内,基于积分计算帧间位姿增量,并结合统计的协方差构建状态先验;以各关键帧的六自由度位姿、速度及零偏作为状态变量,以基于积分计算得到的预积分因子、基于激光点云得到的几何特征因子及强度线特征因子作为约束因子,构建因子图优化模型;
6、根据几何特征因子的雅可比矩阵计算退化因子,并基于设定的退化判定阈值,确定隧道纵向退化方向及退化程度;
7、根据得到的退化因子检测结果设计自适应加权函数,对几何特征因子与强度线特征因子的残差项进行权重动态调节,在退化方向上降低几何特征权重并增强强度线特征权重,抑制纵向位姿漂移;
8、基于因子图优化模型,对预积分因子、几何特征因子与强度线特征因子对应的残差项进行联合迭代求解,输出车辆六自由度位姿估计及隧道三维点云地图,完成隧道定位。
9、进一步的,经投影生成二维强度图像并提取强度线特征,具体为:
10、获取的隧道三维点云数据,根据激光雷达线束数与水平角度分辨率进行圆柱投影生成二维强度图像,像素值记录对应三维点的激光反射强度;
11、对二维强度图像经抑噪处理并提取强度发生突变的边缘像素点,边缘像素点至少对应隧道壁面混凝土接缝、管片拼缝和管线支架;
12、根据圆柱投影的几何映射关系,将边缘像素点反向投影至三维空间,获得三维强度边缘点集合;
13、对三维强度边缘点进行欧氏距离聚类,将空间相邻的边缘点聚合为独立的强度线特征段,计算每条强度线特征段的中心点坐标与方向向量,作为因子图优化中的强度线特征观测单元。
14、进一步的,几何特征因子包括基于激光点云获得的几何面特征因子与几何线特征因子,几何面特征因子的残差为点到平面的距离,几何线特征因子与强度线特征因子的残差均为点到直线的距离,通过迭代最小化加权残差平方和求解最优位姿。
15、进一步的,根据几何特征因子的雅可比矩阵计算退化因子,具体为:
16、获取当前优化窗口中所有几何特征观测对应的残差雅可比矩阵,构建信息矩阵的近似表达;
17、对信息矩阵进行奇异值分解,获得多个奇异值,分别对应位姿估计中平移与旋转六个自由度方向的约束强度;
18、根据奇异值计算总体退化因子和各自由度方向的退化因子,总体退化因子反映当前场景整体几何约束的充分程度;平移方向和旋转方向的退化因子为对应奇异值与相应方向最大奇异值的比值。
19、进一步的,基于设定的退化判定阈值确定隧道纵向退化方向及退化程度,具体为:设定退化判定阈值,当某自由度方向的退化因子小于退化判定阈值时,判定该方向存在几何退化并将其标记为退化状态;公路隧道场景中,车辆前进方向对应的奇异值越小,纵向约束退化程度越严重。
20、进一步的,设计自适应加权函数,具体为:
21、对于几何特征因子,定义权重衰减系数,当退化因子小于退化判定阈值时,权重衰减系数随退化程度加深而线性减小;
22、对于强度线特征因子,定义权重增强系数,并设定增益调节参数,当退化因子小于退化判定阈值时,权重增强系数随退化程度加深而线性增大;
23、将几何特征残差项乘以权重衰减系数,强度线特征残差项乘以权重增强系数,代入因子图优化的总体目标函数。
24、进一步的,对于未标记为退化的自由度方向,权重衰减系数与权重增强系数均保持为1,维持原始权重配置不变,确保正常方向上的优化不受影响。
25、进一步的,联合迭代求解期间,还包括构造强度边缘上下文描述子进行回环检测,具体为:
26、将关键帧点云投影至二维水平面,以激光雷达中心为原点建立极坐标系,沿半径方向均匀划分环带以覆盖不同距离范围,沿角度方向均匀划分扇区以覆盖360°视野,形成极坐标网格;
27、针对每个网格单元,分别统计落入其中的几何边缘点数量、强度边缘点数量以及强度边缘点的平均反射强度值;
28、将得到的三个统计量拼接为网格单元的特征向量,并对所有网格特征进行归一化处理;
29、将网格特征按行列顺序展开为一维描述子向量,同时保留每个元素对应的网格索引信息。
30、进一步的,采用两阶段分层匹配策略进行回环检测,具体为:
31、第一阶段利用描述子的全局统计特征进行初筛,排除明显不匹配的候选帧;
32、第二阶段对候选帧与当前帧的描述子进行逐网格精细比对,计算各网格特征的余弦相似度并求均值作为最终相似度得分;
33、当相似度得分超过预设阈值且当前帧与候选帧的时间间隔与空间距离均满足回环判定条件时,确认检测到回环,将回环约束作为闭环因子加入因子图,通过全局位姿图优化对历史关键帧位姿进行统一调整,生成全局一致的三维隧道点云地图。
34、本发明的第二个方面公开了基于强度辅助与退化感知的slam隧道定位系统,包括车载激光雷达、惯性测量单元、处理器和存储器,存储器存储有计算机程序,处理器执行计算机程序时实现上述基于强度辅助与退化感知的激光惯性slam隧道定位方法。
35、与现有技术相比,上述一个或多个技术方案存在以下有益效果:
36、隧道壁面固有的接缝、拼缝等物理结构对激光反射产生强度差异,利用该特性形成的强度线特征作为纵向方向的补充约束,结合惯性测量单元的预积分因子与几何特征因子共同作为约束,构建因子图优化模型,将隧道长直区段纵向几何约束不足的定位问题,转化为带有强度线特征纵向约束的因子图优化问题,从而利用隧道壁面的固有结构特点弥补长直隧道纵向方向的几何约束不足;进一步结合自适应加权函数,使强度线特征在几何约束最薄弱的方向上发挥主导作用,实现纵向位姿漂移的针对性抑制;最终基于因子图优化模型输出车辆六自由度位姿估计与隧道三维点云地图,实现隧道场景下车道级定位精度的维持。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com