[发明专利]一种基于车辆运动学的自动驾驶车辆联合定位方法

权利要求书

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

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

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

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

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

2.根据权利要求1所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（1）中，传感器采集获取自动驾驶车辆当前信息的具体过程如下：

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

车辆利用IMU 惯性测量单元采集车辆当前的速度状态信息，即车辆线加速度和角速度，其中表示i时刻车辆的加速度信息，表示i时刻车辆角速度信息；

利用车辆总线获取车辆当前时刻的车轮侧偏角信息，其中和分别表示车辆前轮和后轮的侧偏角。

3.根据权利要求2所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（2）中，获得当前车辆的位姿信息的具体过程如下：

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

（2.2）利用所述车辆当前速度状态信息，采用IMU预积分模型对车辆的运动状态进行估计，得到车辆预积分模型如下：

其中，表示IMU坐标系下车辆从i时刻到j时刻的位移距离；表示IMU坐标系下车辆从i时刻到j时刻的速度变化；表示IMU坐标系下从i时刻到j时刻的旋转量；表示IMU坐标系下从i时刻到t时刻的旋转量；表示t时刻下的IMU坐标系，通过IMU惯性测量单元得到旋转矩阵 和平移矩阵 。

4.根据权利要求3所述的基于车辆运动学的自动驾驶车辆联合定位方法，其特征在于，所述步骤（3）中，基于相机测量误差、IMU测量误差构建误差函数的具体过程如下：

相机测量误差定义为相机坐标系下在归一化平面上特征点坐标预测值与特征点坐标测量值之间的偏差，特征点m在第i帧的相机测量误差具体定义如下：

IMU测量误差，即IMU坐标系下时刻i与时刻j之间的预积分误差定义为：

其中，为利用IMU测量值进行两次积分后所获得的车辆位移，其中表示世界坐标系到IMU坐标系的转换；为通过对IMU进行积分获得的车辆速度；表示利用IMU获得的车辆旋转信息；表示IMU坐标系下从j时刻到i时刻的旋转量。