[发明专利]一种车辆路径跟随误差视觉测量优化方法

摘要

本发明涉及一种车辆路径跟随误差视觉测量优化方法。先通过车辆牵引点单目相机单目间接测量系统获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y；再通过车辆牵引点单目相机及跟随点单目相机的双目直接测量系统，获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC；最后进行基于无轨卡尔曼滤波的双系统融合算法，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正。本发明能够在更大的测量范围内获取更准确的车辆尾部侧向偏移距离测量值，此测量值可作为后轴主动转向系统控制器输入，以提高长轴距车辆的通过性。

主权项

1.一种车辆路径跟随误差视觉测量优化方法，其特征在于包括如下步骤：①通过车辆牵引点单目相机单目间接测量系统获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y；所述的步骤①包括如下步骤：(11)对牵引点单目相机所获取图像进行预处理，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对相邻两帧所提取的SURF特征描述向量进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；(12)对计算所得Homography矩阵进行奇异值分解，获得平动信息和转动信息，利用平动信息推算出牵引点测偏角βf以及绝对速度vf，利用转动信息推算出牵引点的横摆角ψf；(13)根据横摆平面车辆运动学模型，按如下公式计算出车辆行驶距离Sf以及牵引点位置(Xf，Yf)与跟随点位置(Xr，Yr)的全局位置信息：Sf＝∫vfdtγf＝ψf+βfXf＝∫vfcos(γf)dtYf＝∫vfsin(γf)dtXr＝Xf‑l cos(ψf)Yr＝Yf‑l sin(ψf)其中，γf为航向角，l为牵引点与跟随点之间的距离；(14)将牵引点单目相机提取的SURF特征、牵引点位置信息及车辆行驶距离Sf存入道路特征内存缓冲区，根据牵引点与跟随点之间的距离l，读取跟随点当前位置对应牵引点行驶过的全局坐标，进行坐标变换，转换到当前时刻车辆坐标系下，计算出车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y；②通过车辆牵引点单目相机及跟随点单目相机的双目直接测量系统，获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离yDC；所述的步骤②包括如下步骤：(21)对跟随点单目相机所获取图像进行预处理，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对生成的SURF特征向量与从道路特征内存缓冲区中读取的SURF特征进行匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；(22)对计算所得Homography矩阵进行奇异值分解，获得平动信息，将平动信息从相机坐标系转换到车辆坐标系下，跟随点的Y向分量即为车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离yDC，跟随点的X向分量用于修正牵引点与跟随点之间的距离l；③进行基于无轨卡尔曼滤波的双系统融合算法，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正；所述的步骤③为：当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离yDC小于设定值时，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，从而对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离zk；所使用的UKF无轨卡尔曼滤波更新以及测量方程如下：状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；状态量x_k：输入量u_k：状态更新：β(k+1|k)＝βSC，k+w3，kV(k+1|k)＝VSC，k+w4，k其中，Δt为采样时间间隔，wi，k为过程噪声；协方差矩阵Pk更新：其中，α，β，L为UKF参数，默认值分别为0.01，2，0，4；将步骤(13)和步骤(14)所描述的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离的计算过程定义为观测方程F，则修正后的侧向偏移距离由以下公式表示：zk＝F(xk)观测更新：卡尔曼增益K计算：状态与协方差修正：xk+1＝x(k+1|k)+K(yDC‑z(k+1|k))Pk+1＝P(k+1|k)‑K Pzz，(k+1|k)KT其中，yDC为双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离。