一种基于Transformer的激光惯性SLAM方法及系统

摘要: 本发明涉及井下矿道内车辆自主定位,具体是一种基于transformer的激光惯性slam方法及系统。背景技术:1、煤矿井下巷道具有空间狭长、光照完全缺失、全球卫...
一种基于Transformer的激光惯性SLAM方法及系统

本发明涉及井下矿道内车辆自主定位,具体是一种基于transformer的激光惯性slam方法及系统。

背景技术:

1、煤矿井下巷道具有空间狭长、光照完全缺失、全球卫星导航系统信号彻底屏蔽等极端环境特征,移动机器人在此类环境下自主作业,必须依靠机载传感器在无外部绝对定位信号的前提下实现实时定位与建图。由于巷道侧壁、地面和顶板的几何结构高度重复且沿巷道走向缺乏显著几何变化,激光雷达点云在巷道走向方向上存在天然的退化性,纯激光里程计在长直巷道中极易因特征匹配模糊而迅速发散。

2、为克服上述退化问题,引入惯性测量单元(inertial measurement unit,imu)与激光雷达进行紧耦合融合是当前的主流技术路线。然而,煤矿井下实际工况中,移动载体在崎岖路面上面临持续的高频振动与冲击,单imu的角速度与加速度量测中混杂大量振动噪声,且imu自身零偏随温度与时间缓慢漂移,难以在长航时作业中保持稳定的惯性约束。更为关键的是,单imu无法直接测量刚体角加速度,而载体在加减速、转向等机动工况下的角加速度变化率直接影响姿态预测精度。常规激光惯性slam方法假设过程噪声协方差为固定常数,无法随载体运动状态和振动强度的变化而自适应调整,在井下高动态工况下易导致滤波预测置信度失真,位姿估计偏差持续累积。

3、在动态建图方面,煤矿巷道中移动矿车、巡检人员等动态物体会在激光点云中产生大量动态特征,若直接用于建图将导致地图中出现重影与拖尾。已有语义分割辅助的动态剔除方案(例如cn202210586859.x)采用二分类语义掩膜进行动态/静态区分,但井下点云因远距离稀疏、遮挡严重等原因导致语义分类置信度不足,二分类机制无法处理这类“不确定”区域,且缺乏对语义误判的几何纠偏手段,动态剔除的召回率与精度难以兼顾。

4、因此,针对以上现状,迫切需要开发一种基于transformer的激光惯性slam方法及系统,以克服当前实际应用中的不足。

技术实现思路

1、本发明的目的在于提供一种基于transformer的激光惯性slam方法及系统,以解决上述背景技术中提出的问题。

2、为实现上述目的,本发明提供如下技术方案:

3、一种基于transformer的激光惯性slam方法,包括以下步骤:

4、s1、获取呈空间金字塔布局的阵列惯性测量单元采集的角速度与线加速度数据,以及激光雷达采集的三维点云数据;

5、s2、基于阵列惯性测量单元的空间构型约束与杠杆臂运动学原理,利用主惯性测量单元与辅助惯性测量单元之间的测量冗余构建加速度一致性约束残差,通过残差递推最优估计实时解算载体刚体的三维角加速度矢量;

6、s3、将所述阵列惯性测量单元的角速度和线加速度数据与所述三维角加速度矢量进行时序联合编码,利用通道结构感知的多头自注意力编码器网络对预设滑动窗口内的联合特征序列进行时序关联建模,输出过程噪声协方差矩阵的自适应调节参数集;

7、s4、基于球面投影几何变换将所述三维点云映射至二维距离图像空间,利用轻量语义分割网络进行逐像素分类,生成包含静态背景类、潜在动态类和不确定类的三级语义分割掩膜;

8、s5、构建语义先验引导与局部几何一致性约束的两级过滤机制,利用所述三级语义分割掩膜执行第一级语义门控过滤,对经第一级过滤后的待校验点执行基于局部平面度响应值与自适应动态判别阈值的第二级几何验证,输出净化静态点云集合;

9、s6、将所述自适应调节参数集映射为过程噪声协方差矩阵并注入误差状态卡尔曼滤波的先验预测阶段,利用所述净化静态点云执行点云配准与量测更新;

10、s7、根据所述误差状态卡尔曼滤波的融合估计结果,输出载体的六自由度高频位姿,并构建剔除动态特征后的全局点云地图。

11、作为本发明进一步的方案:所述步骤s2中,辅助惯性测量单元的加速度与主惯性测量单元的加速度满足如下杠杆臂关系式:

12、;

13、其中,为第i个辅助imu的加速度 (i=1,2,3),为主imu加速度,为主imu角速度,ri为空间位置,表示通过构建加速度约束残差函数并递推最优估计获得的角加速度。

14、作为本发明进一步的方案:所述步骤s3具体包括:

15、s31、将四个惯性测量单元在采样时刻k的六维量测向量按主惯性测量单元至辅助惯性测量单元的固定空间拓扑顺序纵向堆叠,构建二十四维阵列惯性测量单元联合量测向量;将所述三维角加速度向量与所述二十四维联合量测向量沿纵向拼接,构建第k时刻的二十七维联合特征向量;

16、s32、以预设滑动窗口长度n对所述二十七维联合特征向量进行时序展开,构建n×27维的时序输入矩阵;

17、s33、对所述时序输入矩阵施加时序位置编码,得到携带时序位置信息的编码输入序列矩阵;

18、s34、将所述编码输入序列矩阵送入多层通道结构感知多头自注意力编码器网络;所述编码器网络中的自注意力计算引入通道结构感知偏置矩阵,所述偏置矩阵依据所述二十七维联合特征向量的分块组成规则划分为若干结构子块,各子块的初始值依据四个惯性测量单元的空间位置向量之间的欧氏距离进行物理引导初始化,并在训练阶段进行端到端的学习更新;经所述编码器网络逐层编码后,输出十二维过程噪声自适应参数向量,并以此构建所述过程噪声协方差对角矩阵。

19、作为本发明进一步的方案:所述步骤s4具体包括:

20、s41、将所述激光雷达在扫描帧中采集的三维点云通过球面投影变换逐点映射至二维距离图像空间,获取各激光点在距离图像中的像素坐标(u, v);

21、s42、将投影至每个有效像素坐标处的激光点属性构建为五通道输入特征张量ik,像素(u, v)处的五维特征向量为:

22、;

23、其中,xi、、yi、zi表示第i个激光点在其激光雷达坐标系下的三维空间坐标;intensittyi表示第i个激光点的反射强度;ri表示第i个激光点到激光雷达原点;rmax和imax分别为所述激光雷达的最大探测距离和最大反射强度标称值;

24、s43、将所述五通道输入特征张量送入以倒残差瓶颈模块为基本单元的轻量语义分割编码器进行多尺度层级化特征提取;所述编码器末端嵌入多分支异构上下文聚合模块,生成多尺度上下文增强的语义特征图;

25、s44、将所述语义特征图送入轻量解码器,经逐级上采样和浅层特征图与特征图的维度拼接并对其进行通道压缩与融合,输出恢复至原空间分辨率的逐像素语义特征图;

26、s45、对所述逐像素语义特征图通过分类头映射至三个语义类别,经归一化后获得每个像素属于各类别的后验概率估计值,生成所述三级语义分割掩膜,其中,h与w分别表示掩膜的高度与宽度;标签1为静态背景类、标签2为潜在动态类、标签3为不确定类。

27、作为本发明进一步的方案:所述两级过滤机制具体包括:

28、s51、基于球面投影索引映射关系,将所述三级语义分割掩膜反向追溯至三维点云空间,为原始点云数据集中各激光点赋予对应的类别标签;保留标签为1的静态背景点集pstatic,并将标签为2的潜在动态点集与标签为3的不确定点集合并构成待校验点集pverify;

29、s52、对各待校验点pj,利用所述误差状态卡尔曼滤波器的预测位姿将其变换至全局世界坐标系,并在局部子地图中获取最近邻点集qnear;

30、当最近邻点数满足预设阈值时,提取所述最近邻点集的局部几何结构特征,对协方差矩阵执行特征值分解,得到降序特征值,以对应的特征向量作为局部表面法向量估计;

31、定义局部平面度响应值:

32、;

33、当小于预设阈值时判定邻域未形成有效平面;当大于等于预设阈值时判定邻域形成有效平面,以待校验点至该局部拟合平面的距离作为几何一致性残差;

34、基于所述静态背景点集的几何残差分布,利用中值绝对偏差鲁棒统计量估计噪声尺度σmad,构建自适应动态判别阈值,为灵敏度系数,lj为第j个点的语义标签值;对待校验点pj执行几何一致性判别与标签修正:;

35、s53、提取经两级过滤后最终标记为静态标签的所有激光点,输出净化静态点云集合pclean。

36、作为本发明进一步的方案:所述步骤s6具体包括:

37、s61、构建三十维扩展误差状态向量,将阵列惯性测量单元各自独立的加速度计零偏以及由杠杆臂约束残差递推解算的三维角加速度估计误差纳入状态空间:

38、;

39、其中,δp为位置误差,δv为速度误差,δθ为姿态角误差,δba0至δba3分别为四个惯性测量单元的加速度计零偏误差,δbg为陀螺仪零偏误差,为角加速度估计误差;为重力向量误差;

40、s62、将所述十二维过程噪声自适应参数向量按分块对角结构映射注入三十维过程噪声协方差矩阵中,动态更新位置、速度、姿态及各加速度计零偏所对应的噪声子块;

41、s63、利用所述净化静态点云集合pclean执行误差状态卡尔曼滤波的观测更新,观测噪声协方差rk基于所述中值绝对偏差鲁棒估计获得的噪声尺度σmad在线设定,以与所述步骤s62中的过程噪声协方差自适应注入共同构成过程噪声与观测噪声的双端协同自适应机制。

42、作为本发明进一步的方案:所述步骤s7具体包括:

43、s71、利用所述误差状态后验更新量对经惯性量测传播的名义状态进行校正,输出当前时刻载体的六自由度位姿估计;

44、s72、利用校正后的所述六自由度位姿估计,将所述净化静态点云集合中的各激光点变换至全局世界坐标系,并插入全局地图中;同时,将该扫描帧对应的语义分割类别标签与全局地图点关联存储。

45、作为本发明进一步的方案:所述步骤s6中点云配准与所述步骤s5中几何校验共享局部几何结构:将所述步骤s52中判定的有效平面邻域及局部表面法向量复用为点云配准的几何基元;所述步骤s7中全局地图的插入频率、所述步骤s6中点云配准更新频率及所述步骤s3中编码器网络推理频率三者保持同步。

46、本发明还提出一种基于transformer的激光惯性slam系统,包括:

47、数据采集模块,由呈空间金字塔布局的阵列惯性测量单元及一个激光雷达构成,用于执行如权利要求1所述步骤s1的数据采集;

48、角加速度解算与自适应参数回归模块,用于执行如权利要求1所述步骤s2的杠杆臂残差递推解算三维角加速度,并由如权利要求1所述步骤s3的通道结构感知注意力编码器网络输出十二维过程噪声自适应参数向量;

49、语义动态过滤模块,用于执行如权利要求1所述步骤s4的球面投影与轻量语义分割网络生成三级语义分割掩膜,并执行如权利要求1所述步骤s5的局部平面度响应值与自适应动态判别阈值过滤,输出净化静态点云集合;

50、误差状态卡尔曼滤波紧耦合建图模块,以三十维扩展误差状态向量为滤波空间,将所述十二维过程噪声自适应参数向量按分块对角结构映射注入过程噪声协方差矩阵,融合所述净化静态点云集合执行递推更新,以双端协同自适应机制输出高频位姿并增量构建全局地图。

51、作为本发明进一步的方案:所述阵列惯性测量单元包括一个主惯性测量单元和三个辅助惯性测量单元;所述主惯性测量单元安装于激光雷达的正上方,三个辅助惯性测量单元分别安装于固定装置的前端两侧及后端,四个惯性测量单元在三维空间内共同构成金字塔型阵列排布;所述激光雷达安装于所述固定装置的中心点及载体中心的上方;所述激光雷达及四个惯性测量单元均与所述固定装置保持刚性连接,数据输出端口均与工控机的通信接口建立数据连接。

52、与现有技术相比,本发明的有益效果是:

53、本发明通过阵列惯性测量单元的空间金字塔布局构建了刚体角加速度的物理可观测条件。四个非共线imu之间的杠杆臂约束构成了冗余测量方程组,该方程组在数学上具有唯一解,从而突破了单imu无法直接感测角加速度的固有局限。所解算的三维角加速度矢量作为独立物理观测量被实时纳入滤波状态空间,使姿态预测模型能够显式响应载体角运动的变化率,从根本上抑制了高机动工况下的惯性递推发散趋势。

54、在阵列imu提供物理冗余观测的基础上,本发明构建了二十七维联合特征向量,将四个imu的原始量测与解算角加速度统一编码。通道结构感知注意力网络依据imu空间拓扑将偏置矩阵划分为对应子块,各子块的初始值由imu间的真实欧氏距离经径向基函数衰减确定,使得注意力权重的学习过程受到物理构型的约束与引导。该网络对滑动窗口内的时序特征序列进行长程依赖建模后,实时输出十二维参数动态调整滤波过程噪声协方差矩阵的对角元素,使滤波器对预测噪声的置信度评估能够随载体振动强度和运动状态的变化而自适应更新,克服了固定噪声参数在井下高振动工况下状态估计失真的缺陷。

55、在点云处理支路中,本发明生成包含静态背景、潜在动态和不确定三个类别。该三级分类机制将语义置信度不足的区域显式分离出来,避免二分类机制中低置信度像素被强制归入静态或动态类别所引入的误判。对于语义分支无法可靠判决的点,本发明进一步利用局部点云几何结构进行验证,通过特征值分解计算局部平面度响应值,并结合中值绝对偏差鲁棒统计量构建自适应判别阈值,使得不属于动态物体的点能够被纠偏回收。

56、上述各支路的处理结果在误差状态卡尔曼滤波框架内实现紧耦合融合:角加速度物理观测保障了预测模型的结构完整性,自适应噪声协方差赋予预测阶段动态调整能力,经语义与几何双重校验的纯净静态点云为观测更新提供了可靠的外部量测。三者协同作用,使系统在井下长距离高振动高动态环境中同时保持了位姿估计的长期稳定性与地图构建的几何纯净性。