一种基于车辆运动学的自动驾驶车辆联合定位方法

摘要

本发明公开一种基于车辆运动学的自动驾驶车辆联合定位方法，该方法利用双目相机及IMU获取环境及车辆运动信息进行车辆状态估计，借助车辆总线获取车辆侧偏角构建运动约束，该方法通过引入车辆本身的运动学模型，将其作为车辆在利用双目视觉和IMU惯性测量单元进行定位过程中本身运动的约束，构建车辆位姿状态估计的优化模型，迭代求解获得符合车辆运动学模型的车辆定位结果，该方法在定位优化过程中借助于运动学约束，将车辆运动学与状态估计的优化模型相结合，得到适应车辆自身运动的定位轨迹信息，有效提高了视觉惯性联合定位优化模型本身的鲁棒性和精确性，适用于自动驾驶车辆在复杂环境下的定位问题。

说明书

技术领域

本发明涉及自动驾驶车辆定位领域，具体是一种基于车辆运动学的自动驾驶车辆联合定位方法。

背景技术

随着国家发改委《智能汽车创新发展战略》的出台，在政府力量和社会资本的不断推动下，自动驾驶领域得到如火如荼的发展。在高级辅助驾驶不断得到落地的今天，高等级自动驾驶实现在产业界和高校的努力下也逐渐看到了曙光。越来越多的企业如百度、阿里、文远知行等也在不断推出自己的自动驾驶产品。但是完全自动驾驶距离走进千家万户仍有诸多的障碍。为了实现完全自动驾驶，定位感知规划控制高精度地图五大模块缺一不可，在这其中，定位模块作为自动驾驶车辆的位置信息来源，对自动驾驶的实现起着基础性作用。

当前自动驾驶车辆定位的实现强依赖于GNSS组合导航技术，车辆通过获取经过RTK基站解算的定位信息实现高精度定位，但是在隧道桥梁等GNSS信号弱或消失环境下应用效果不佳。因此，基于各种传感器的车辆定位技术逐渐得到重视，如通过摄像头、IMU等设备结合高精度地图完成车辆定位。虽然视觉惯性联合定位技术已经在学术界有了较多的研究，但是由于其鲁棒性差和精确度不高的问题，其并没有在车辆定位中得到实际落地。并且目前已有的视觉惯性联合定位技术过于通用化，并没有有效利用车辆本身的运动特性对其进行优化，这也是限制视觉惯性联合定位在车辆应用的原因所在。因此，如何有效的将车辆本身运动特性应用于视觉惯性联合定位以增强车辆定位的鲁棒性和精确性成为亟待解决的技术问题。

发明内容

针对上述现有技术存在的问题，本发明提供一种基于车辆运动学的自动驾驶车辆联合定位方法，可以有效的解决上述问题。

为了实现上述目的，本发明通过以下技术方案实现：一种基于车辆运动学的自动驾驶车辆联合定位方法，所述自动驾驶车辆上搭载有双目相机和IMU惯性测量单元，所述自动驾驶车辆借助于车辆总线完成车辆内部通讯；

所述基于车辆运动学的自动驾驶车辆联合定位方法包括以下步骤：

（1）传感器采集获取自动驾驶车辆当前信息，包括通过双目相机获得车辆前方图像信息、通过IMU惯性测量单元获得车辆当前速度状态信息、通过车辆总线获取车辆侧偏角信息；

（2）车辆利用步骤（1）所述的车辆前方图像信息和所述车辆当前速度状态信息进行车辆的状态估计，获得当前车辆的位姿信息；

（3）利用步骤（2）所述的位姿信息，结合步骤（1）所述的车辆侧偏角信息建立运动学模型，构建运动学约束下的车辆位姿优化模型完成车辆定位；所述的优化模型为基于相机测量误差、IMU测量误差构建的误差函数，所述的运动学约束为自动驾驶车辆运动所需要满足的单车运动模型。

优选的，所述步骤（1）中，传感器采集获取自动驾驶车辆当前信息的具体过程如下：

传感器利用双目相机的左右相机同时采集获取车辆前方图像信息，并利用相机标定所述车辆前方图像信息的内外参数、利用单应矩阵对原始图像进行校正，使得图像处于同一平面且互相平行，再对像素点匹配从而获得每个点的深度，得到当前图像帧信息

车辆利用IMU 惯性测量单元采集车辆当前的速度状态信息

利用车辆总线获取车辆当前时刻的车轮侧偏角信息，其

优选的，所述步骤（2）中，获得当前车辆的位姿信息的具体过程如下：

（2.1）利用FAST算法对双目相机采集到的图像帧进行检测，提取图像帧的角点作为图像特征点，利用LK光流算法追踪所述图像特征点，利用ICP算法处理所述当前图像帧信息

（2.2）利用所述车辆当前速度状态信息

其中，

优选的，所述步骤（3）中，基于相机测量误差、IMU测量误差构建误差函数的具体过程如下：

相机测量误差定义为相机坐标系下在归一化平面上特征点坐标预测值与特征点坐标测量值之间的偏差，特征点

IMU测量误差，即IMU坐标系下时刻

其中，

优选的，所述步骤（3）中，所述运动学约束的具体过程如下：

将车辆运动学模型加入位姿优化的模型当中，将车辆简化为单车运动模型，即车辆满足的运动关系为：

其中，

因此，当前时刻

由上易得，车辆满足如下约束：

其中，

并且由于受到车辆自身结构限制，车辆前轮侧偏角满足约束

优选的，所述步骤（3）中，构建运动学约束下的车辆位姿优化模型的具体过程如下：

针对所述相机测量误差，IMU测量误差和车辆运动学约束，为了得到车辆最佳的位姿估计，利用所述相测量像误差和IMU测量误差建立位姿优化模型，在运动学约束下最小化位姿估计误差，将上述约束优化问题转换为无约束优化，问题形式化为：

其中，

本发明的有益效果为：本方法通过引入借助于车辆侧偏角建立的车辆自身运动学约束，将其与视觉惯性联合定位所获得的位姿优化模型相结合，所得到的位姿优化结果与车辆自身运动学相匹配，大幅度提高位姿优化结果在车辆执行方面的可行性；同时，通过考虑车辆自身运动约束，在位姿优化模型求解过程中也有效提高了视觉惯性联合定位的鲁棒性和精确度。

附图说明

图1为本发明的自动驾驶车辆联合定位方法的流程图；

图2为本发明的自动驾驶车辆联合定位方法的定位优化模型构建示意图；

图3为本发明的自动驾驶车辆联合定位方法的车辆运动学模型示意图。

具体实施方式

下面结合附图对本发明做进一步说明。

如图1至图3所示，一种基于车辆运动学的自动驾驶车辆联合定位方法，所述自动驾驶车辆上搭载有双目相机和IMU惯性测量单元，所述自动驾驶车辆借助于车辆总线完成车辆内部通讯；

所述基于车辆运动学的自动驾驶车辆联合定位方法包括以下步骤：

（1）传感器采集获取自动驾驶车辆当前信息，包括通过双目相机获得车辆前方图像信息、通过IMU惯性测量单元获得车辆当前速度状态信息、通过车辆总线获取车辆侧偏角信息；

（2）车辆利用所述的车辆前方图像信息和所述车辆当前速度状态信息进行车辆的状态估计，获得当前车辆的位姿信息；

（3）利用步骤（2）所述的位姿信息，结合步骤（1）所述的车辆侧偏角信息建立运动学模型，构建运动学约束下的车辆位姿优化模型完成车辆定位；所述的优化模型为基于相机测量误差、IMU测量误差构建的误差函数，所述的运动学约束为自动驾驶车辆运动所需要满足的单车运动模型。

所述步骤（1）中，传感器采集获取自动驾驶车辆当前信息的具体过程如下：

传感器利用双目相机的左右相机同时采集获取车辆前方图像信息，并利用相机标定所述车辆前方图像信息的内外参数、利用单应矩阵对原始图像进行校正，使得图像处于同一平面且互相平行，再对像素点进行匹配从而获得每个点的深度，得到当前图像帧信息

车辆利用IMU 惯性测量单元采集车辆当前的速度状态信息

所述步骤（2）中，获得当前车辆的位姿信息的具体过程如下：

（2.1）利用FAST算法对双目相机采集到的图像帧进行检测，提取图像帧的角点作为图像特征点。所采用的图像帧不限于左目或右目相机，若提取到的特征点数量不符合要求，则重新提取；

利用LK光流算法追踪所述图像特征点，基于灰度不变假设，同一窗口中像素具有相同的运动，确定帧间图像特征点

利用ICP算法处理所述当前图像帧信息

利用ICP算法对相邻两帧提取得到的匹配特征点

定义W矩阵为

对

（2.2）利用所述车辆当前速度状态信息

其中，

所述步骤（3）中，基于相机测量误差、IMU测量误差构建误差函数的具体过程如下：

相机测量误差定义为相机坐标系下在归一化平面上特征点坐标预测值与特征点坐标测量值之间的偏差，特征点

IMU测量误差，即IMU坐标系下时刻

其中，

所述步骤（3）中，所述运动学约束的具体过程如下：

将车辆运动学模型加入位姿优化的模型当中，将车辆简化为单车运动模型，即车辆满足的运动关系为：

其中，

因此，当前时刻

由上易得，车辆满足如下约束：

其中，

并且由于受到车辆自身结构限制，车辆前轮侧偏角满足约束

所述步骤（3）中，构建运动学约束下的车辆位姿优化模型的具体过程如下：

针对所述相机测量误差，IMU测量误差和车辆运动学约束，为了得到车辆最佳的位姿估计，需要利用所述相机测量误差和IMU测量误差建立位姿优化模型，在运动学约束下最小化位姿估计误差为：

将上述约束优化问题转换为无约束优化，问题形式化为：

其中，

通过引入借助于车辆侧偏角建立的车辆自身运动学约束，将其与视觉惯性联合定位所获得的位姿优化模型相结合，所得到的位姿优化结果与车辆自身运动学相匹配，大幅度提高位姿优化结果在车辆执行方面的可行性；同时，通过考虑车辆自身运动约束，在位姿优化模型求解过程中也有效提高了视觉惯性联合定位的鲁棒性和精确度。