一种多传感器SLAM与VINS系统的两阶段初始化方法及系统

摘要: 本发明属于导航定位,更进一步涉及全球导航卫星系统gnss(globalnavigationsatellitesystem)定位中的一种多传感器同步定位与建图sl...
一种多传感器SLAM与VINS系统的两阶段初始化方法及系统

本发明属于导航定位,更进一步涉及全球导航卫星系统gnss(globalnavigation satellite system)定位中的一种多传感器同步定位与建图slam(simultaneous localization and mapping)及视觉惯性导航系统vins(visual-inertialnavigation system)的两阶段初始化方法。本发明可用于复杂环境下多传感器融合slam系统的可靠启动。

背景技术:

1、传统全球导航卫星系统gnss技术在复杂环境下仍面临诸多挑战。首先,在城市峡谷、密集森林或室内等场景中,卫星信号易受建筑物遮挡和多路径效应影响,导致定位精度显著下降。其次,gnss信号微弱,容易受到电磁干扰或人为欺骗信号的攻击,造成定位偏差甚至系统失效。此外,现有技术中将视觉、惯性与gnss数据耦合在单一非线性优化框架中进行初始化的方法,存在计算复杂度高、收敛慢、鲁棒性差等问题,尤其在gnss信号弱或不可用的初始阶段,难以完成有效对齐。同步定位与建图slam是实现移动机器人自主导航的核心技术。在大规模、复杂环境中,单一传感器(如相机、激光雷达)受限于感知范围与测量特性,难以提供鲁棒、无漂移的位姿估计。近年来,多传感器融合slam方法通过集成视觉、惯性测量单元imu(inertial measurement unit)、全球导航卫星系统gnss和磁力计等异构传感器,实现了优势互补,成为研究热点。初始化是多传感器融合slam系统的关键第一步,其目标是在系统启动时快速恢复尺度、速度、重力方向、传感器偏置以及局部坐标系与全局坐标系之间的变换关系。初始化的精度与效率直接决定后续状态估计的收敛性与稳定性。

2、基于单阶段联合优化的初始化方法典型代表为gvins(cao s, lu x, shen s.gvins: tightly

3、coupled gnss–visual–inertial fusion for smooth and consistent stateestimation[j]. ieee transactions on

4、robotics, 2022, 38(4): 2004-2021.)。gvins将gnss原始测量与视觉、惯性数据构建于同一因子图优化框架中,通过单阶段紧耦合方式进行初始化。该方法直接从多传感器观测中估计系统状态,包括尺度、位姿、速度和传感器偏置。主要局限:由于视觉、imu和gnss观测具有不同的噪声特性、采样频率和物理意义,单阶段联合初始化构建的优化问题可能呈现出病态,导致求解容易陷入局部极小值;同时,在gnss信号质量不佳时,该方法的初始化成功率和精度会显著下降,对传感器性能退化的场景缺乏足够的适应能力。

5、松耦合的视觉-惯性-卫星初始化方法典型代表为vins-fusion(qin t, cao s,pan j, et al. a

6、general optimisation‐based framework for global pose estimation withmultiple sensors[j]. iet

7、cyber‐systems and robotics, 2025, 7(1): e70023.)。该方法首先运行单目视觉-惯性里程计vio(visual-inertial odometry)进行局部初始化,恢复带有不确定尺度的轨迹;然后在后端利用gnss测量对位姿进行后验修正,实现局部到全局的对齐。主要局限:由于vio初始化阶段未引入任何全局尺度参考,其恢复的尺度因子可能存在漂移,后验的gnss修正通常只在全局位置层面对轨迹进行拉伸或平移,难以充分消除局部尺度累积的畸变;同时,该方法在gnss信号中断后再次恢复时,实现平滑的全局-局部重新对齐存在一定困难。

8、安徽机电职业技术学院在其申请的专利文献“面向智慧校园无人车的多传 感器数据融合定位导航系统”(申请日 2026-03-20 申请号 cn 202610346736.7申请公布号 cn122015830 a)中公开了一种面向智慧校园无人车的多传感器数据融合定位导航的技术方案。该技术方案包括采集车载多线激光雷达点云、imu 惯导数据及 gnss 卫星定位数据;在建图时,采用改进 slam 算法生成三维点云地图并转为二维栅格地图,在导航时,自适应选择定位模式,通过多传感器数据融合算法解算最优位姿;路径规划模块基于位姿、地图和目标点生成包含全局路径序列与局部避障轨迹的混合导航数据;该流程通过多源数据深度融合与时空同步,构建了从环境感知到运动执行的完整导航闭环。但是,该方法仍然存在的不足之处是,其采用的改进slam算法在建图与定位初始化阶段仍依赖于多传感器数据的联合优化求解,当系统启动时gnss信号受到遮蔽或视觉特征不足时,初始化过程面临计算复杂度高、收敛慢的问题;此外,其初始化过程中各传感器数据在同一框架下耦合求解,缺乏对传感器实时状态的检测与适应性调整机制,在传感器性能退化场景下初始化的鲁棒性不足。

9、综上所述,与本发明最相近的现有实现方案(如gvins、ic-gvins、vins-fusion等)在系统初始化阶段存在以下缺点:

10、(1)计算效率低,初始化耗时较长:现有单阶段联合初始化方法将所有异构传感器观测置于同一优化问题中,变量维度高、信息矩阵可能呈现病态,导致求解过程计算量大、收敛较慢,难以满足行进间快速启动的需求。部分以惯性为中心的方法还需较长的静止或特定运动激励采集时间,进一步延长了初始化过程。

11、(2)尺度恢复精度不足,对运动激励存在依赖:传统单目视觉-惯性初始化仅依靠视觉视差恢复尺度,在短时运动、匀速运动或低纹理场景下,因视差不足可能导致尺度估计出现畸变。虽然后续引入gnss进行全局对齐,但局部尺度误差已经产生,难以从根本上完全修正。

12、(3)局部-全局对齐耦合度高,影响初始化稳定性:现有方法将局部尺度恢复与全局坐标系变换估计紧密耦合,若局部视觉-惯性估计存在偏差(如速度、重力方向不准),全局对齐过程可能放大该偏差,导致系统启动时带有系统性漂移。

13、(4)对传感器性能变化缺乏适应能力,鲁棒性受限:现有初始化过程通常假设gnss信号良好、磁力计无干扰、视觉纹理丰富,未充分设计针对传感器状态异常的检测与容错机制。在gnss遮蔽、磁干扰、弱光照等性能退化环境下,初始化成功率可能显著下降,甚至难以完成初始化。

14、(5)坐标系变换参数难以在线修正:现有方法在初始化完成后,局部坐标系与全局坐标系之间的变换关系(旋转、平移)通常保持固定,不易根据后续观测进行在线调整。当传感器参数(如gnss天线安装位置)发生微小变化时,系统可能产生持续的全局偏差。

技术实现思路

1、本发明的目的是针对上述现有技术存在的不足,提出一种多传感器 slam 与vins系统的两阶段初始化方法及系统,旨在解决现有视觉惯性导航系统vins及多传感器融合slam技术在系统初始化阶段存在的效率偏低、尺度畸变、对齐耦合、退化适应性不足、适用场景受限的技术问题,实现复杂环境下多传感器融合slam系统的可靠启动。

2、实现本发明目的的技术思路是:

3、由于本发明使用将多传感器融合slam系统的初始化解耦为gnss辅助的视觉-惯性局部初始化与全局对齐校准两个序贯阶段的技术,该技术具有将高维联合优化问题分解为两个低维优化问题的突出的实质性特点。第一阶段利用视觉和imu数据,通过将图像特征匹配结果与imu预积分数据对齐、联合优化视觉重投影误差和imu预积分误差,恢复局部运动状态;第二阶段固定第一阶段结果,单独利用gnss和磁力计数据估计局部坐标系与全局坐标系之间的变换关系。各阶段仅处理可用传感器数据,失效约束不会对优化问题产生全局污染,降低了单次优化的问题规模与求解难度。本发明采用的两个序贯阶段的技术能够解决现有技术将视觉、imu、gnss和磁力计等传感器数据置于同一非线性优化问题中联合求解所导致的优化变量维度高、在初始化环境不佳时容易陷入局部极小值甚至失败的问题。

4、由于本发明使用在局部初始化阶段引入gnss位置差分测量作为尺度参考的技术,具体地,在滑动窗口内计算视觉-惯性子系统估计的帧间相对位移与对应gnss位置差分之间的差异,通过最小化尺度化后的视觉-惯性位移与gnss位移差分之间的差异来估计真实尺度因子。由于gnss位置差分反映的是真实的物理位移而非绝对位置,即使gnss存在数米的绝对定位误差,其差分精度仍可有效约束尺度估计,在保持局部一致性的同时降低了尺度不确定性。该技术具有通过构建最小化函数来估计真实尺度因子的突出的实质性特点,能够解决传统单目视觉-惯性初始化仅依靠相邻帧间的视觉视差恢复尺度,在短时运动、匀速运动或低纹理场景下因视差不足导致尺度估计出现畸变的问题。

5、由于本发明使用在全局坐标校准阶段仅以gnss定位与磁力计姿态作为观测锚点,固定局部视觉-惯性初始化位姿,以东北天enu坐标系(east-north-up coordinate frame)到世界w坐标系(world coordinate frame)的旋转变换与平移变换、以及横滚-俯仰-航向rpy坐标系(roll-pitch-yaw coordinate frame)到w系的旋转变换为待估计的优化变量,通过最小化gnss位置残差与磁力计姿态残差的联合目标函数,单独估计局部坐标系与全局坐标系之间变换参数的技术。该技术具有将局部尺度恢复与全局坐标系变换估计完全解耦的突出的实质性特点,能够解决现有方法因二者紧密耦合而导致局部估计偏差在全局对齐过程中被放大、系统启动时带有系统性漂移的问题。本发明在保证局部传感器系统精确稳定的前提下完成全局校准,在gnss信号遮蔽、磁干扰等传感器性能退化场景下仍能保持较高的初始化成功率与鲁棒性。

6、为实现上述目的,本发明方法的具体实现步骤包括如下:

7、步骤1,对获取到的图像数据、imu测量数据、gnss传感器数据和磁力计姿态测量进行预处理;

8、步骤2,在滑动窗口内,执行局部视觉-惯性里程计vio初始化,恢复带有尺度不确定性的运动轨迹、初始速度、重力矢量、imu零偏以及相机到imu的空间变换关系,作为视觉-惯性导航系统vins系统的局部初始运动状态;

9、步骤3,基于建立的局部初始运动状态,以窗口内gnss位置差分作为尺度参考,估计真实尺度因子,将带有尺度不确定性的局部轨迹恢复至真实物理尺度,得到真实尺度下的视觉-惯性初始化位姿;

10、步骤4,以gnss定位测量与磁力计姿态测量为全局观测锚点,将真实尺度下的视觉-惯性初始化位姿与全局坐标系进行对齐,估计局部坐标系与全局坐标系之间的变换关系,完成多传感器slam系统从局部vins系统到全局定位的初始化。

11、进一步地,所述预处理指的是,以视觉图像帧采集时刻为时间基准,对imu测量数据和gnss定位数据进行时间戳插值对齐;对视觉图像进行帧间特征点提取和匹配;对imu测量数据进行预积分,得到相邻关键帧之间的伪位移、速度增量和角度增量;gnss提供的位置测量经世界大地测量系统wgs84(world geodetic system 1984)标准由经纬高变换到enu系。

12、进一步地,所述执行局部视觉-惯性里程计初始化是指,对视觉与惯性传感器之间旋转外参进行初步对齐估计,确定相机与imu之间的相对旋转关系,校正imu陀螺仪的零偏误差,消除陀螺仪漂移对姿态估计的影响。

13、进一步地,所述恢复带有尺度不确定性的运动轨迹、初始速度、重力矢量、imu零偏以及相机到imu的空间变换关系的步骤如下:

14、第一步,在滑动窗口内,以视觉图像帧采集时刻为时间基准,将预处理后的图像特征匹配结果与imu预积分数据对齐,建立视觉观测与惯性观测之间的时空对应关系;

15、第二步,计算视觉特征匹配结果中相邻关键帧之间的相对位姿变换,并结合imu预积分得到的伪位移、速度增量和角度增量,通过联合优化视觉重投影误差和imu预积分误差,恢复带有尺度不确定性的运动轨迹、初始速度、重力矢量、加速度计零偏、陀螺仪零偏以及相机到imu的空间变换关系。

16、进一步地,所述真实尺度因子是由下式得到的:

17、 ;

18、其中,为待估计的尺度因子;表示滑动窗口内关键帧的数量,、分别表示滑动窗口内第帧和第帧关键帧的索引;表示由视觉-惯性里程计初始化得到的第帧关键帧到第帧关键帧的帧间相对位置变化量;表示视觉-惯性参数;表示对应的第帧关键帧到第帧关键帧的gnss位置差分;表示范数运算。

19、进一步地,所述局部-全局的对齐与校准是指,固定真实尺度下的视觉-惯性初始化位姿,以enu系到w系的旋转变换与平移变换、以及rpy系到w系的旋转变换为待估计的优化变量,以gnss定位测量与磁力计姿态测量为观测锚点,通过最小化gnss位置残差与磁力计姿态残差的联合目标函数,得到局部坐标系与全局坐标系之间的变换关系。

20、所述联合目标函数如下:

21、 ;

22、其中,表示取最小值操作,表示enu系到w系的旋转变换的待估计参数,表示enu系到w系的平移变换的待估计参数,表示rpy系到w系的旋转变换四元数的待估计参数,表示滑动窗口内关键帧的索引,表示第帧的gnss位置残差,表示第帧的磁力计姿态残差,表示系统待估计状态向量,表示第帧的gnss定位测量值,表示第帧的磁力计姿态测量值。

23、所述gnss位置残差由下式定义:

24、 ;

25、其中,表示第帧视觉-惯性初始化位姿的姿态分量对应的旋转矩阵,表示单位矩阵,上角标表示矩阵转置运算,表示第帧gnss定位测量数据在enu系下的位置坐标,表示相机到imu的平移外参,表示gnss物理天线相对于imu的杠杆臂向量,表示enu系到w系的平移变换;表示第帧视觉-惯性初始化位姿的位置分量。

26、所述磁力计姿态残差由下式定义:

27、 ;

28、其中,表示第帧的磁力计姿态测量在rpy系下的姿态四元数,⊗表示四元数乘法操作,表示第帧的视觉-惯性初始化位姿的姿态四元数;表示相机到imu的姿态外参四元数。

29、本发明的系统包括以下模块:

30、数据预处理模块,用于以视觉图像帧为时间基准,对imu测量数据和gnss定位数据进行时间戳插值对齐;对视觉图像进行帧间特征点提取和匹配;对imu测量数据进行预积分,得到相邻关键帧之间的伪位移、速度增量和角度增量;将gnss的经纬高坐标转换至enu坐标系;

31、视觉-惯性传感器旋转外参对齐模块,用于执行视觉与惯性传感器之间旋转外参的初步对齐估计,确定相机与imu之间的相对旋转关系,并对齐质量进行验证;

32、陀螺仪零漂校正模块,用于在视觉-惯性传感器旋转外参对齐成功后,校正imu陀螺仪的零偏误差;

33、系统状态初始化模块,用于在滑动窗口内,利用预处理后的图像特征匹配结果与imu预积分数据,执行视觉-惯性里程计初始化,恢复带有尺度不确定性的运动轨迹、初始速度、重力矢量、加速度计零偏、陀螺仪零偏以及相机到imu的空间变换关系;同时以窗口内gnss位置差分作为尺度参考,通过最小化尺度化后的视觉-惯性位移与gnss位移差分之间的差异,估计真实尺度因子,得到真实尺度下的视觉-惯性初始化位姿;

34、gnss-视觉-惯性-磁力计全局对齐模块,用于固定系统状态初始化模块得到的视觉-惯性初始化位姿,以gnss定位测量与磁力计姿态测量为观测锚点,通过最小化gnss位置残差与磁力计姿态残差的联合目标函数,联合估计enu系到w系的旋转变换与平移变换、以及rpy系到w系的旋转变换,实现局部坐标系与全局坐标系之间的对齐与校准。

35、本发明与现有技术相比较,具有如下优点:

36、第一,由于本发明使用将多传感器融合slam系统的初始化解耦为gnss辅助的视觉-惯性局部初始化与全局坐标校准两个序贯阶段的技术,克服了现有单阶段联合初始化方法将多传感器观测同时耦合于同一非线性优化框架中求解,导致变量维度高、信息矩阵易呈现病态、计算量大且收敛缓慢的缺陷,使得本发明降低了单次优化问题的规模与求解难度,局部视觉-惯性初始化阶段平均需约17.4ms,全局对齐校准阶段平均需约2.4ms,显著缩短了初始化时间,满足了实时系统在行进间快速启动的需求。

37、第二,由于本发明使用在局部初始化阶段引入gnss位置差分测量作为尺度参考的技术,通过最小化尺度化后的视觉-惯性位移与gnss位移差分之间的差异来估计真实尺度因子,克服了传统单目视觉-惯性初始化仅依靠视觉视差恢复尺度,在短时运动、匀速运动或低纹理场景下因视差不足导致尺度估计出现畸变的缺陷,使得本发明初始化位置误差可收敛至约0.129m,显著提高了初始化精度,有效降低了后续状态估计的系统性漂移。

38、第三,由于本发明使用在全局坐标校准阶段仅以gnss定位与磁力计姿态作为观测锚点,固定局部视觉-惯性初始化位姿、单独估计局部坐标系与全局坐标系之间变换参数的技术,克服了现有方法将局部尺度恢复与全局坐标系变换估计紧密耦合,若局部视觉-惯性估计存在偏差则全局对齐过程可能放大该偏差,导致系统启动时带有系统性漂移的缺陷,使得本发明在保证局部传感器系统精确稳定的前提下完成全局校准,在gnss信号遮蔽、磁干扰等传感器性能退化场景下仍能保持较高的初始化成功率与鲁棒性。