[发明专利]一种基于车辆运动学约束的视觉惯性SLAM系统初始化方法

说明书

本发明涉及一种基于车辆运动学约束的视觉惯性SLAM系统初始化方法，具体包括以下步骤：S1、获取基于视觉图像估计的单目视觉位姿估计、基于惯导数据的惯导预积分和基于车辆底盘数据的车辆运动学预积分；S2、单目视觉位姿估计与车辆运动学预积分对齐，恢复系统尺度；S3、惯导预积分与车辆运动学预积分对齐，计算得到惯导陀螺仪偏置的初值、惯导加速度计偏置及重力矢量的初值，完成视觉惯性SLAM系统的初始化。与现有技术相比，本发明具有提高视觉惯性SLAM系统初始化的准确性和稳定性、避免参数估计结果发散等优点。

技术领域

本发明涉及自动驾驶汽车定位技术领域，尤其是涉及一种基于车辆运动学约束的视觉惯性SLAM系统初始化方法。

背景技术

视觉惯性SLAM是应用于室内/地下等无GPS信号场景的主要定位手段，其中视觉惯性SLAM系统初始化是为后续优化提供系统参数的初始值，对定位精度有重要影响。目前视觉惯性SLAM系统初始化主要依靠视觉与惯导的运动对齐来形成对系统参数的约束，进而利用多帧数据进行求解。而当自动驾驶汽车运行在某些工况下，惯导的激励一旦不够，将导致参数的估计结果发散，甚至初始化失败。

考虑到自动驾驶汽车的方向盘转角及车速等信息精确已知，并可便捷的从CAN总线测量得到，因此可利用其无需激励且无零飘的特点，与视觉信息及惯导信息进行融合定位，提高系统定位的精度及稳定性，而如何基于车辆的运动学约束，联合视觉与惯导完成系统初始化，成为要解决的重要问题。

发明内容

本发明的目的就是为了克服上述现有技术存在的视觉惯性SLAM系统初始化过程中仅依靠视觉与惯导的运动对齐导致在某些工况下由于惯导激励不够使参数估计结果发散甚至初始化失败的缺陷而提供一种基于车辆运动学约束的视觉惯性SLAM系统初始化方法。

本发明的目的可以通过以下技术方案来实现：

一种基于车辆运动学约束的视觉惯性SLAM系统初始化方法，具体包括以下步骤：

S1、获取基于视觉图像估计的单目视觉位姿估计、基于惯导数据的惯导预积分和基于车辆底盘数据的车辆运动学预积分；

S2、所述单目视觉位姿估计与车辆运动学预积分对齐，恢复系统尺度；

S3、所述惯导预积分与车辆运动学预积分对齐，计算得到惯导陀螺仪偏置的初值、惯导加速度计偏置及重力矢量的初值，完成视觉惯性SLAM系统的初始化。

所述单目视觉位姿估计具体是通过相机的运动构建多视图几何约束，由PnP及BA的方法求解相机旋转和位移计算得到。

所述步骤S2具体为将单目视觉位姿估计计算得到的位移量与车辆运动学预积分计算得到的位移量及旋转量进行对齐，形成对系统尺度的约束，根据相应多个时刻的对齐数据恢复系统尺度。

所述单目视觉位姿估计与车辆运动学预积分对齐的公式具体如下所示：

其中，为根据单目视觉位姿估计计算的位移量，和分别为根据车辆运动学预积分计算的位移量和旋转量，s为系统尺度，为相机坐标系到车辆坐标系间的位移外参；

根据相应多个时刻的对齐数据，并对上式进行简化，最终通过最小二乘法求解来恢复系统尺度，具体公式如下所示：

其中，W为多个时刻的数据帧数，为根据单目视觉位姿估计在车辆坐标系下的位移量，为车辆运动学预积分。

所述惯导预积分根据图像关键帧间的惯导的运动信息进行积分计算得到，包括惯导旋转预积分与惯导位移预积分；所述车辆运动学预积分根据阿克曼转向模型，由车辆底盘数据中的方向盘转角及车速信息计算得到，包括车辆位移运动学预积分及车辆旋转运动学预积分。

所述步骤S3具体包括以下步骤：