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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

其中，

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

其中，W为多个时刻的数据帧数，

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

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

S301、所述惯导旋转预积分与车辆旋转运动学预积分进行对齐，形成对惯导陀螺仪偏置的约束，根据相应多个时刻的对齐数据求解得到陀螺仪偏置的初值；

S302、所述惯导位移预积分与车辆位移运动学预积分进行对齐，形成对惯导加速度计偏置及重力矢量的约束，根据相应多个时刻的对齐数据求解得到加速度计偏置及重力矢量的初值，并根据重力矢量的模长限制对重力矢量进行再优化。

进一步地，所述车辆位移运动学预积分及车辆旋转运动学预积分的计算公式具体如下所示：

其中，

进一步地，所述车辆位移运动学预积分及车辆旋转运动学预积分的误差传播模型的公式如下所示：

其中，θ为方向盘转角，I为单位矩阵，

所述步骤S301具体过程为根据惯导旋转预积分计算的旋转量对惯导陀螺仪偏置值进行一阶泰勒近似，具体公式如下所示：

其中，

然后惯导旋转预积分计算得到的旋转量与车辆旋转运动学预积分计算得到的旋转量进行对齐，并根据相应多个时刻的对齐数据，通过最小二乘求解得到惯导陀螺仪偏置值，具体公式如下所示：

其中，W为多个时刻的数据帧数，

所述步骤S302具体过程为根据惯导位移预积分计算的位移量对惯导加速度计偏置值进行一阶泰勒近似，具体公式如下所示：

其中，

然后惯导位移预积分计算得到的位移量与车辆位移运动学预积分计算得到的位移量进行对齐，并根据相应多个时刻的对齐数据，通过最小二乘求解得到惯导加速度计偏置值及重力矢量，具体公式如下所示：

其中，W为多个时刻的数据帧数，

与现有技术相比，本发明具有以下有益效果：

本发明基于车辆运动学模型，充分利用自动驾驶汽车底盘精确已知的方向盘转角及车速等信息，生成车辆运动学预积分，融合基于视觉的单目视觉位姿估计与基于惯导信息的惯导预积分，完成视觉惯性SLAM系统初始化，提高视觉惯性SLAM系统初始化的准确性和稳定性，有效避免了现有技术中视觉惯性SLAM系统初始化仅依靠视觉与惯导的运动对齐而在某些工况下由于惯导激励不够导致参数估计结果发散甚至初始化失败的问题。

附图说明

图1为本发明的流程示意图；

图2为本发明实施例中系统坐标系的示意图；

图3为本发明实施例中阿克曼转向模型的示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例

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

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

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

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

如图2所示，本实施例中，(·)

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

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

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

其中，

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

其中，W为多个时刻的数据帧数，

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

如图3所示，基于车辆阿克曼转向模型，利用车辆底盘精确已知的方向盘转角θ及车速v，可求解车辆横摆角速度ω：

δ＝θ/i

ρ＝b/tan(δ)

ω＝v/ρ

其中，δ表示前轮转角，i

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

S301、惯导旋转预积分与车辆旋转运动学预积分进行对齐，形成对惯导陀螺仪偏置的约束，根据相应多个时刻的对齐数据求解得到陀螺仪偏置的初值；

S302、惯导位移预积分与车辆位移运动学预积分进行对齐，形成对惯导加速度计偏置及重力矢量的约束，根据相应多个时刻的对齐数据求解得到加速度计偏置及重力矢量的初值，并根据重力矢量的模长限制对重力矢量进行再优化。

本实施例中，车辆速度向量和横摆角速度向量基于平面假设计算得到，具体公式如下所示：

由此计算得到车辆运动的连续形式状态递推式如下所示：

车辆位移运动学预积分及车辆旋转运动学预积分的计算公式具体如下所示：

其中，

车辆位移运动学预积分及车辆旋转运动学预积分的误差传播模型的公式如下所示：

其中，θ为方向盘转角，I为单位矩阵，

步骤S301具体过程为根据惯导旋转预积分计算的旋转量对惯导陀螺仪偏置值进行一阶泰勒近似，具体公式如下所示：

其中，

然后惯导旋转预积分计算得到的旋转量与车辆旋转运动学预积分计算得到的旋转量进行对齐，并根据相应多个时刻的对齐数据，通过最小二乘求解得到惯导陀螺仪偏置值，具体公式如下所示：

其中，W为多个时刻的数据帧数，

步骤S302具体过程为根据惯导位移预积分计算的位移量对惯导加速度计偏置值进行一阶泰勒近似，具体公式如下所示：

其中，

然后惯导位移预积分计算得到的位移量与车辆位移运动学预积分计算得到的位移量进行对齐，并根据相应多个时刻的对齐数据，通过最小二乘求解得到惯导加速度计偏置值及重力矢量，具体公式如下所示：

其中，W为多个时刻的数据帧数，

此外，需要说明的是，本说明书中所描述的具体实施例，所取名称可以不同，本说明书中所描述的以上内容仅仅是对本发明结构所做的举例说明。凡依据本发明构思的构造、特征及原理所做的等效变化或者简单变化，均包括于本发明的保护范围内。本发明所属技术领域的技术人员可以对所描述的具体实例做各种各样的修改或补充或采用类似的方法，只要不偏离本发明的结构或者超越本权利要求书所定义的范围，均应属于本发明的保护范围。