论文部分内容阅读
随着智能时代的到来,诸如自动驾驶、机器人、无人机等智能无人系统得到了广泛关注和快速发展,而高精度、高可靠的定位定姿信息是智能无人系统进行有效的环境感知、路径规划与运动控制的基础,因此,能够提供连续、稳健、高精度导航信息的多源融合导航技术也就成为了智能无人系统实现自主化的核心技术之一。经典的GNSS(Global Navigation Satellite System)/INS(Inertial Navigation System)组合导航系统可以连续地提供高精度的导航信息,但在GNSS拒止环境下,此组合导航系统将退化为纯惯导工作,若使用的是低成本IMU(Inertial Measurement Unit),则定位定姿误差将快速发散,无法满足要求。视觉惯性导航也是智能无人系统中常用的导航技术,作为一种递推导航系统,其定位和航向误差也是发散的,且易受外部环境视觉纹理条件的影响。虽然回环校正可在一定程度上消除累积漂移,但作为一种机遇性的修正信息,在实际的工作场景下可遇不可求。相比之下,同时使用视觉、惯性和GNSS定位信息的多源融合导航,可充分利用这三种信息源在导航能力方面的互补性,克服仅使用单一信息源或仅使用两种信息源导航的局限性。针对室外环境下智能无人系统对连续、稳健、高精度定位定姿信息的需求,本文开展了视觉/惯性/GNSS融合导航方面的相关研究。本文所提的视觉/惯性/GNSS融合导航方案以非线性优化器(图优化)作为多源融合状态估计的基础,整个方案分前端和后端两部分:前端主要负责ORB特征提取与立体匹配、局部地图匹配、运动状态估计、关键帧选取和GNSS异常定位探测;后端负责地图维护、动基座初始化和滑窗优化。其中,IMU量测以预积分的形式使用,在前端和后端都发挥作用。依托于此多源融合导航架构,本研究的主要工作和创新点包括以下几个方面:1.面向中高精度等级IMU改进了预积分算法。现有VIO(Visual-Inertial Odometry)方案中的IMU预积分算法主要是针对低端MEMS IMU而设计的,相对比较粗糙,忽略了地球自转角速度和重力变化(包括方向与大小)等细节影响因素。但中高精度等级IMU可有效感知地球自转,而目前在智能无人系统中被广泛使用的MEMS IMU模块已经达到或接近了中等精度等级;且在室外应用场景下载体相对于世界系的运动距离也可能足够大而导致较为明显的重力变化。针对这两点问题,本文对预积分算法进行了精化改进,以充分考虑地球自转和重力变化。同时,在改进预积分的解算算法中也考虑了圆锥效应和划船效应补偿。对于这种改进的预积分算法,在传播预积分误差不确定性时,IMU噪声相关参数可直接根据其自身实际的物理量测精度进行设置,而无需再顾及未建模误差的影响而做经验性的调参。在车载数据集上的测试结果表明,对于中高精度等级的IMU,使用改进预积分算法可有效地提高单/双目VIO的定位定姿精度。其中,对于基于高精度IMU(导航级)的单/双目VIO的定位定姿精度提升在50~97%之间;对于基于中等精度IMU(准战术级MEMS)的单/双目VIO的定位定姿精度提升在21~41%之间;但对于无法有效感知地球自转的消费级MEMS IMU,改进预积分无显著优势。2.结合三种现有主流滑动窗口估计器(即Conditioning-based、Prior-based sliding window optimizer和MSCKF)的优势,设计了一种混合滑动窗口优化器。具体来说,在滑动窗口外部定义了固定参考区,以保留跟踪长度超出滑窗范围的视觉观测;滑动窗口内部则被分为成熟区和成长区,成熟区内的状态节点被估计的次数相比于成长区更多,估计精度也就更高,为此,只有当地图点的全部视觉观测均滑出成长区后,才将其边缘化出去,以避免线性化误差过大。在边缘化过程中,考虑到地图点之间的独立性,在构建边缘化增量方程之前,先利用MSCKF的左零空间边缘化方法移除地图点位置,形成MSC残差函数,然后再与其它相关的线性化的残差函数一起构建边缘化增量方程,最后利用Schur补边缘化方法获取新的先验残差函数,此时Schur矩阵中待求逆矩阵的维度已大幅减小,为此计算效率将得以明显提升。例如,当要边缘化掉的地图点数量为150个时,相比于传统的不考虑地图点之间独立性的边缘化方法,本文所提边缘化策略的耗时可下降83.3%。另外,通过适当的参数调节,这种混合滑动窗口优化器可蜕变为这三种现有滑动窗口估计器中的任何一种。具体来说,若移除固定参考区,同时增加成熟区的尺寸,其表现将趋于基于先验的滑动窗口优化器;若同时减小优化算法的迭代步数,其表现将趋于MSCKF;若保留固定参考区,同时减小成熟区的尺寸,其表现将趋于基于固定参考的滑动窗口优化器。可见,此混合架构具有很强的灵活性和通用性,通过合理的参数设置,这种混合架构能在一定程度上克服各估计器的不足,在边缘化效率大幅提高的情况下,保持多源融合状态估计的精度不受影响。测试结果表明:在合理的参数设置下,相比于现有的6种主流单目VIO方案,基于混合滑动窗口优化器的单目VIO在开源的Eu Ro C数据集上可实现接近或更高的定位精度,且在自主采集的车载数据集上的平均定位误差漂移率(各里程段漂移率的均值)在0.3%~0.6%之间,达到了现有前沿VIO方案的精度水平。3.为视觉/惯性/GNSS融合导航专门设计了一种动基座初始化算法,可在载体行进状态下对运动状态初值以及传感器外参等多个关键参数进行现场估计。整个算法流程分三个步骤:1)基于手眼标定法(hand-eye calibration)计算相机与IMU之间的相对旋转,并在此基础上计算IMU的陀螺仪偏置;2)基于重力和载体位移这对矢量来计算视觉世界系和多源融合导航世界系之间的对准矩阵,具体计算过程分GNSS天线杆臂已知和未知两种情况,其中相机与IMU之间的相对平移与重力矢量在同一个方程组中计算;3)基于现有估计结果完成加速度计偏置估计和对准矩阵误差估计。待初始化过程收敛之后,即可利用上述这些估计值为非线性优化器设置一个较为准确的迭代初值。最后,利用典型的直道和弯道车载数据序列各5段对算法的有效性进行了测试,结果表明:相机与IMU之间的相对旋转、IMU的陀螺仪和加速度计偏置、两个世界系间的对准矩阵均可被有效估计。其中,两个世界系间的对准矩阵估计可在5~13秒内收敛,对于中等精度的准战术级MEMS IMU,在弯道和直道上的平均估计误差为1.11和1.44度;对于低精度消费级MEMS IMU,在弯道和直道上的平均估计误差也仅为1.22和2.21度,初始化的收敛时间与最终精度都完全满足本论文所研究的视觉/惯性/GNSS融合导航的需求。4.自主设计并搭建了一套视觉/惯性/GNSS车载多传感器数据采集与算法研究软硬件平台,完成了传感器间的精密时间同步和内外参数标定,实现了完整的视觉/惯性/GNSS融合导航算法,包括ORB特征提取、立体匹配、局部地图匹配、前端运动状态估计、关键帧选取、GNSS定位异常探测、动基座初始化和混合滑窗优化等核心算法模块,并基于此软硬件平台对视觉/惯性/GNSS融合导航的定位定姿精度进行了全面评估和分析。测试结果表明:1)在GNSS RTK定位可用的情况下,基于MEMS IMU的视觉/惯性/GNSS融合导航可以实现厘米级的定位精度,水平姿态精度优于0.04度,航向精度优于0.07度;2)在GNSS定位不可用的情况下,系统将退化为VIO,其定位定姿精度与IMU的精度等级以及单/双目相机配置有关:IMU的精度等级越高,VIO的定位和定姿精度就越高,且双目VIO的定位和水平姿态精度优于单目。与传统车载导航中的INS/NHC/轮式里程计组合相比,VIO在低速非空旷场景中精度更高、性能更稳定。除空旷测试数据序列外,基于两款不同精度等级MEMS IMU的单/双目VIO的平均定位误差漂移率都在0.5%及以下,达到了现有前沿VIO的精度水平。综上所述,本文对视觉/惯性/GNSS融合导航方法进行了较为全面的研究,完成了核心算法设计与实测验证,以及整套多传感器同步采集系统和算法研究软硬件平台搭建,并且通过多组车载数据序列对本文所提算法和系统的整体性能进行了全面、细致的测试与分析。本文研究成果为解决室外智能无人系统导航定位问题提供了一套完整可行的参考方案。