CN106885523A - 一种车辆路径跟随误差视觉测量优化方法 - Google Patents

Abstract

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

Description

一种车辆路径跟随误差视觉测量优化方法

技术领域

本发明涉及车辆控制领域，尤其涉及一种车辆路径跟随误差视觉测量优化方法。

背景技术

长轴距车辆或列车，如公交巴士、商用车、重型车及拖挂车辆等，车辆质心较高，车身长度较长，因而其可操控性以及低速通过性较差。在低速转弯工况下，此类车辆尾部相对于车辆前部会产生相对于转弯半径内侧的侧向偏移距离。车身长度越长，转弯半径越小，该侧向偏移距离越大，车辆相应的通过性也越差。为了提高此类车辆的低速安全性能，一些后轴主动转向系统的应用可以使得整个车辆更好地跟随驾驶员的期望行驶路径。车辆尾部相对于车辆前部的侧向偏移距离是衡量此类系统表现的重要指标，此参数很难通过传感系统测量直接获得，通常需要依据车辆航向角以及横摆角速度等状态观测，通过车辆运动学模型估算而得。

部分视觉系统可以根据路面特征通过直接测量方法或间接测量方法得出车辆尾部侧向偏移距离。间接测量方法，可依据单目相机测量车辆牵引点的速度、侧偏角以及横摆角速度，利用牵引点位置移位寄存器以及车辆几何参数，推算出车辆跟随点的路径，从而获得车辆尾部侧向偏移距离。此方法测量范围不受侧向偏移距离限制，但精度易受横摆角速度积分累积误差的影响。直接测量方法，可依据牵引点相机与跟随点相机行驶路径的特征匹配，直接确定二者之间的侧向偏移距离。此方法精度较高，但测量范围易受两相机视野重合范围的影响。对于基于惯性传感器(陀螺仪)的计算方法，在光滑路面，纵向以及侧向坡道存在的工况下车辆尾部路径跟随测量误差较大。

准确地测量此类车辆尾部相对于车辆前部的侧向偏移距离对于后轴主动转向系统的应用具有十分重要的意义。

发明内容

本发明为了解决上述技术问题，提供一种车辆路径跟随误差视觉测量优化方法，其利用传感器融合技术，将两种基于视觉传感的车辆尾部侧向偏移距离的测量方法相结合，对车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离进行实时测量，能够在更大的测量范围内获取更准确的车辆尾部侧向偏移距离测量值，此测量值可作为后轴主动转向系统控制器输入，以提高长轴距车辆的通过性。

本发明的上述技术问题主要是通过下述技术方案得以解决的：本发明包括如下步骤：

①通过车辆牵引点单目相机单目间接测量系统获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y；

②通过车辆牵引点单目相机及跟随点单目相机的双目直接测量系统，获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC；

③进行基于无轨卡尔曼滤波的双系统融合算法，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正。

利用信息融合技术，将单目间接测量系统和双目直接测量系统的测量方法相结合，再通过基于无轨卡尔曼滤波的双系统融合算法对测量结果进行修正，能够在更大的测量范围内获取更准确的车辆尾部侧向偏移距离测量值，此测量值可作为后轴主动转向系统控制器输入，以提高长轴距车辆的通过性。

作为优选，所述的步骤①包括如下步骤：

(11)对牵引点单目相机所获取图像进行预处理，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对相邻两帧所提取的SURF特征描述向量进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；

(12)对计算所得Homography矩阵进行奇异值分解，获得平动信息和转动信息，利用平动信息推算出牵引点测偏角β_f以及绝对速度v_f，利用转动信息推算出牵引点的横摆角ψ_f；

(13)根据横摆平面车辆运动学模型，按如下公式计算出车辆行驶距离S_f以及牵引点位置(X_f，Y_f)与跟随点位置(X_r，Y_r)的全局位置信息：

S_f＝∫v_fdt

γ_f＝ψ_f+β_f

X_f＝∫v_f cos(γ_f)dt

Y_f＝∫v_f sin(γ_f)dt

X_r＝X_f-l cos(ψ_f)

Y_r＝Y_f-l sin(ψ_f)

其中，γ_f为航向角，l为牵引点与跟随点之间的距离；

(14)将牵引点单目相机提取的SURF特征、牵引点位置信息及车辆行驶距离S_f存入道路特征内存缓冲区，根据牵引点与跟随点之间的距离l，读取跟随点当前位置对应牵引点行驶过的全局坐标，进行坐标变换，转换到当前时刻车辆坐标系下，计算出车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y。

作为优选，所述的步骤②包括如下步骤：

(21)对跟随点单目相机所获取图像进行预处理，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对生成的SURF特征向量与从道路特征内存缓冲区中读取的SURF特征进行匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；

(22)对计算所得Homography矩阵进行奇异值分解，获得平动信息，将平动信息从相机坐标系转换到车辆坐标系下，跟随点的Y向分量即为车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC，跟随点的X向分量用于修正牵引点与跟随点之间的距离l。

Y向分量即为车辆坐标系下跟随点的侧向分量，X向分量即为车辆坐标系下跟随点的纵向分量。

作为优选，所述的步骤③为：当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC小于设定值时，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，从而对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离z_k；所使用的UKF无轨卡尔曼滤波更新以及测量方程如下：

状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；

状态量x_k：

输入量u_k：

状态量是指客观存在的车辆状态，比如车速等，并不是指由某个特定传感器所得的测量值，状态量是指车辆的状态，是由观测器根据输入量以及观测模型估算出来的。输入量是指从传感器获得的测量值，如由轮速传感器测得的车速值，输入量的下标SC，表示测量值源自单目视觉系统。

状态更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt为采样时间间隔，w_i，k为过程噪声，通过手动调节可得；

协方差矩阵P_k更新：

其中，

α，β，L为UKF参数，默认值分别为0.01，2，0，4；

将步骤(13)和步骤(14)所描述的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离的计算过程定义为观测方程F，则修正后的侧向偏移距离由以下公式表示：

z_k＝F(x_k)

观测更新：

卡尔曼增益K计算：

状态与协方差修正：

x_k+1＝x_(k+1|k)+K(y_DC-z_(k+1|k))

P_k+1＝P_(k+1|k)-K P_zz，(k+1|k)K^T

其中，y_DC为双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离。

作为优选，所述的步骤③包括：当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC大于设定值时，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离z_k，修正过程如下：

状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；

状态量x_k：

输入量u_k：

状态更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt为采样时间间隔，w_i，k为过程噪声；

将状态更新后的各参数代入观测方程F，则获得修正后的侧向偏移距离：

z_k＝F(x_k)。

本技术方案中用于判断的设定值，为双目直接测量系统测量范围上限，和相机的内外部参数有关，根据实验标定获得。如相机安装高度为0.8米，焦距为3mm时，设定值为0.3米。

本发明的有益效果是：利用信息融合的方法，能够有效消除单一视觉系统应用过程中存在的问题。相对于单目视觉系统，融合后的视觉系统有效消除了横摆角速度的积分误差，测量精度更高；相对于双目视觉系统，融合后的视觉系统可以利用滤波后的车辆状态有效估计较大测量值的侧向偏移距离，测量范围更大。融合后的视觉系统能够在光滑路面、纵向或侧向坡道存在的工况下有效地测量长轴距车辆尾部的侧向偏移距离，能够在更大的测量范围内获取更准确的车辆尾部侧向偏移距离测量值，从而精确描述长轴距车辆的低速通过性，此测量值可作为后轴主动转向系统控制器输入，以提高此类车辆的通过性。

附图说明

图1是本发明的算法流程总图。

图2是本发明中车辆在低速转弯工况下的一种俯视结构示意图。

图3是本发明中牵引点单目相机单目间接测量系统算法流程图。

图4是本发明中双目直接测量系统算法流程图。

图中1.车辆，2.车辆牵引点，3.车辆跟随点。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例：本实施例的一种车辆路径跟随误差视觉测量优化方法，算法流程总图如图1所示，包括如下步骤：

①通过车辆牵引点单目相机单目间接测量系统获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y；

②通过车辆牵引点单目相机及跟随点单目相机的双目直接测量系统，获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC；

③进行基于无轨卡尔曼滤波的双系统融合算法，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正。

两个单目相机获取的图像为本方法的输入，车辆跟随点3(车辆尾部)相对于车辆牵引点2(车辆前部)行驶路径的侧向偏移距离为本方法的输出。如图2所示，牵引点单目相机安装在车辆1最前端(通常为牵引点位置)，跟随点单目相机安装在车辆最尾端(通常为跟随点位置)，两单目相机均以垂直向下朝向路面方向安装，本实施例中单目相机的离地高度约为0.5m。图2中虚线为车辆牵引点行驶路径，l为牵引点和跟随点之间的距离，D为此刻实际发生的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离。

本优化方法的具体步骤如下：

一、单目间接测量系统，如图3所示：

(11)对牵引点单目相机所获取图像进行预处理(灰度化和除畸变)，接着进行FAST特征点提取，提取车辆前表面或侧表面平面特征，再生成SURF特征描述向量，然后利用FLANN特征匹配库对相邻两帧所提取的SURF特征描述向量进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；

通过m个循环，随机选取4个匹配特征，计算Homography矩阵，对剩余特征按该矩阵匹配结果进行打分，像素点匹配距离小于某阈值M，则视为正确匹配，选取打分最高的Homography矩阵，利用其对应的所有正确匹配特征对，重新计算得到最终Homography矩阵；中循环数m与距离阈值M均为预设值；

Homography矩阵表示为：其中，R为相机平动信息，T为相机转动信息，d为图像平面对应的深度，N为图像平面对应的法向信息，K’为相机内部参数矩阵，α’为比例系数，α’取决于相机安装高度；

(12)对计算所得Homography矩阵进行奇异值分解，获得平动信息和转动信息，利用平动信息推算出牵引点测偏角β_f以及绝对速度v_f，利用转动信息推算出牵引点的横摆角ψ_f；

对计算所得Homography矩阵进行奇异值分解，获得相机平动信息T与转动信息R；令：

∑＝diag(σ1，σ2，σ3)，V＝[v1，v2，v3]

这是对奇异值分解结果，∑为对角矩阵，V为向量σ1，σ2，σ3以及v1，v2，v3为对应数值；

上述奇异值分解理论上有四组解，如下所示：

解1：

解2：

解3：

R₃＝R₁，N₃＝-N₁，

解4：

R₄＝R₂，N₄＝-N₂，

选择方向最接近于[0，0，1]的法向量N对应的该组解；

通过公式：计算得出实时车速V的绝对值v_f，v_f即为平动信息；

通过公式：计算得出车辆的实时侧偏角β_f；

通过公式：计算车辆横摆角速度Ψ_f；

公式中：T_x为x轴方向牵引点单目相机的实时平动速度；T_y为y轴方向牵引点单目相机的实时平动速度；R_z为牵引点单目相机绕z轴的转动分量；t_s为单位时间步长；

(13)根据横摆平面车辆运动学模型，按如下公式计算出车辆行驶距离S_f以及牵引点位置(X_f，Y_f)与跟随点位置(X_r，Y_r)的全局位置信息：

S_f＝∫v_fdt

γ_f＝ψ_f+β_f

X_f＝∫v_f cos(γ_f)dt

Y_f＝∫v_f sin(γ_f)dt

X_r＝X_f-l cos(ψ_f)

Y_r＝Y_f-l sin(ψ_f)

其中，γ_f为航向角，l为牵引点与跟随点之间的距离；

(14)将牵引点单目相机提取的SURF特征、牵引点位置信息及车辆行驶距离S_f存入道路特征内存缓冲区，根据牵引点与跟随点之间的距离l，读取跟随点当前位置对应牵引点行驶过的全局坐标，进行坐标变换，转换到当前时刻车辆坐标系下，计算出车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y。

二、双目直接测量系统，如图4所示：

(21)对跟随点单目相机所获取图像进行预处理(灰度化，除畸变)，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对生成的SURF特征向量与从道路特征内存缓冲区中读取的SURF特征进行匹配，即利用FLANN特征匹配库对当前时刻车辆尾部的图像与内存中存储的相应位置处的车辆前端图像进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；

(22)对计算所得Homography矩阵进行奇异值分解，获得平动信息，将平动信息从相机坐标系转换到车辆坐标系下，跟随点的Y向分量即为车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC，跟随点的X向分量用于修正牵引点与跟随点之间的距离l。

三、基于无轨卡尔曼滤波的双系统融合算法：

当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC小于设定值时，为第一模式状态，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，从而对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离z_k；所使用的UKF无轨卡尔曼滤波更新以及测量方程如下：

状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；

状态量x_k：

输入量u_k：

状态更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt为采样时间间隔，w_i，k为过程噪声；

协方差矩阵P_k更新：

其中，

α，β，L为UKF参数，默认值分别为0.01，2，0，4；

将步骤(13)和步骤(14)所描述的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离的计算过程定义为观测方程F，则修正后的侧向偏移距离由以下公式表示：

z_k＝F(x_k)

观测更新：

卡尔曼增益K计算：

状态与协方差修正：

x_k+1＝x_(k+1|k)+K(y_DC-z_(k+1|k))

P_k+1＝P_(k+1|k)-K P_zz，(k+1|k)K^T

其中，y_DC为双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离。

当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC大于设定值时，为第二模式状态，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离z_k，修正过程如下：

状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；

状态量x_k：

输入量u_k：

状态更新：

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt为采样时间间隔，w_i，k为过程噪声；

将状态更新后的各参数代入观测方程F，则获得修正后的侧向偏移距离：

z_k＝F(x_k)。

即第二模式状态下，优化过程不进行卡尔曼增益计算与状态修正，直至车辆重新恢复到第一模式状态。

本方法的目的就是使测得的侧向偏移距离z_k更接近于实际发生的侧向偏移距离D。

相关技术术语的名词解释：

FAST：FAST特征检测算法来源于corner的定义，这个定义基于特征点周围的图像灰度值，检测候选特征点周围一圈的像素值，如果候选点周围邻域内有足够多的像素点与该候选点的灰度值差别够大，则认为该候选点为一个特征点。此特征点检测是公认的比较快速的特征点检测方法，只利用周围像素比较的信息就可以得到特征点，简单，有效。该方法多用于角点检测。

SURF：一种具有尺度与旋转特征不变性的特征描述算法，描述性强，速度快。

FLANN：一种快速近似最近邻搜索函数库，自动选择两个近似最近邻算法中最优的算法。

RANSAC：一种鲁棒的回归方法，用于排除不匹配特征信息。

Homography：两张图像中对应匹配特征点的投影变换矩阵

SIFT：尺度不变特征转换(SIFT)算法是一种特征提取的方法。它在尺度空间中寻找极值点，并提取出其位置、尺度、旋转不变量，并以此作为特征点并利用特征点的邻域产生特征向量。SIFT算法对于光线、噪声、和微小视角改变的容忍度相当高，且对于部分遮挡的物体也有较高的识别相率。

本发明利用传感器融合技术，将两种基于视觉传感的车辆尾部侧向偏移距离测量方法相结合，相比于单独应用其中某一视觉系统，本方法能够在更大的测量范围内获取更准确的车辆尾部侧向偏移距离。而且本发明运算量小，可移植性强，实时性好，不需要引入额外硬件投入。

Claims (5)

1.一种车辆路径跟随误差视觉测量优化方法，其特征在于包括如下步骤：

①通过车辆牵引点单目相机单目间接测量系统获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y；

②通过车辆牵引点单目相机及跟随点单目相机的双目直接测量系统，获取车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC；

③进行基于无轨卡尔曼滤波的双系统融合算法，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正。

2.根据权利要求1所述的一种车辆路径跟随误差视觉测量优化方法，其特征在于所述的步骤①包括如下步骤：

(11)对牵引点单目相机所获取图像进行预处理，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对相邻两帧所提取的SURF特征描述向量进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；

(12)对计算所得Homography矩阵进行奇异值分解，获得平动信息和转动信息，利用平动信息推算出牵引点测偏角β_f以及绝对速度v_f，利用转动信息推算出牵引点的横摆角ψ_f；

(13)根据横摆平面车辆运动学模型，按如下公式计算出车辆行驶距离S_f以及牵引点位置(X_f，Y_f)与跟随点位置(X_r，Y_r)的全局位置信息：

S_f＝∫v_fdt

γ_f＝ψ_f+β_f

X_f＝∫v_fcos(γ_f)dt

Y_f＝∫v_fsin(γ_f)dt

X_r＝X_f-l cos(ψ_f)

Y_r＝Y_f-l sin(ψ_f)

其中，γ_f为航向角，l为牵引点与跟随点之间的距离；

(14)将牵引点单目相机提取的SURF特征、牵引点位置信息及车辆行驶距离S_f存入道路特征内存缓冲区，根据牵引点与跟随点之间的距离l，读取跟随点当前位置对应牵引点行驶过的全局坐标，进行坐标变换，转换到当前时刻车辆坐标系下，计算出车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y。

3.根据权利要求2所述的一种车辆路径跟随误差视觉测量优化方法，其特征在于所述的步骤②包括如下步骤：

(21)对跟随点单目相机所获取图像进行预处理，接着进行FAST特征点提取，再生成SURF特征描述向量，然后利用FLANN特征匹配库对生成的SURF特征向量与从道路特征内存缓冲区中读取的SURF特征进行匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵；

(22)对计算所得Homography矩阵进行奇异值分解，获得平动信息，将平动信息从相机坐标系转换到车辆坐标系下，跟随点的Y向分量即为车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC，跟随点的X向分量用于修正牵引点与跟随点之间的距离l。

4.根据权利要求3所述的一种车辆路径跟随误差视觉测量优化方法，其特征在于所述的步骤③为：当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC小于设定值时，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，从而对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离z_k；所使用的UKF无轨卡尔曼滤波更新以及测量方程如下：

状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；

状态量x_k：

输入量u_k：

状态更新：

${\mathrm{\ψ}}_{(k+1|k)}={\mathrm{\ψ}}_k+({\stackrel{\·}{\mathrm{\ψ}}}_{SC,k}+{\stackrel{\·}{\mathrm{\ψ}}}_{bias,k})\mathrm{\Δ}t+w_{1,k}$

${\stackrel{\·}{\mathrm{\ψ}}}_{bias,(k+1|k)}={\stackrel{\·}{\mathrm{\ψ}}}_{bias,k}+w_{2,k}$

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt为采样时间间隔，w_i，k为过程噪声；

协方差矩阵P_k更新：

$x_k^a=\[x_k,x_k\±\sqrt{(L+\mathrm{\λ})P_k}\]$

$x_{(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^m{x}_{(k+1|k),i}^a$

$P_{(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^c(x_{(k+1|k),i}^a-x_{(k+1|k)}){(x_{(k+1|k),i}^a-x_{(k+1|k)})}^T$

其中，

$W_0^m=\frac{\mathrm{\λ}}{L+\mathrm{\λ}}$

$W_0^c=\frac{\mathrm{\λ}}{L+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β})$

$W_i^m=W_i^c=\frac{1}{2(L+\mathrm{\λ})},i=1,\mathrm{...},2L$

α，β，L为UKF参数，默认值分别为0.01，2，0，4；

将步骤(13)和步骤(14)所描述的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离的计算过程定义为观测方程F，则修正后的侧向偏移距离由以下公式表示：

z_k＝F(x_k)

观测更新：

$z_{(k+1|k)}^a=F\left(x_{(k+1|k)}^a\right)$

$z_{(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^m{z}_{(k+1|k),i}^a$

$P_{zz,(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_1^c(z_{(k+1|k),i}^a-z_{(k+1|k)}){(z_{(k+1|k),i}^a-z_{(k+1|k)})}^T$

$P_{xz,(k+1|k)}=\underset{i=0}{\overset{2L}{\Σ}}{W}_i^c(x_{(k+1|k),i}^a-x_{(k+1|k)}){(z_{(k+1|k),i}^a-z_{(k+1|k)})}^T$

卡尔曼增益K计算：

$K=P_{xz,(k+1|k)}{P}_{zz,(k+1|k)}^{-1}$

状态与协方差修正：

x_k+1＝x_(k+1|k)+K(y_DC-z_(k+1|k))

P_k+1＝P_(k+1|k)-KP_zz，(k+1|k)K^T

其中，y_DC为双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离。

5.根据权利要求4所述的一种车辆路径跟随误差视觉测量优化方法，其特征在于所述的步骤③包括：当双目直接测量系统测得的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y_DC大于设定值时，则利用双目直接测量系统的输出预测单目间接测量系统的横摆角测量误差，对单目间接测量系统输出的车辆跟随点相对于车辆牵引点行驶路径的侧向偏移距离y进行修正，获得修正后的侧向偏移距离z_k，修正过程如下：

状态量包括车辆横摆角ψ_k、横摆角速度误差牵引点侧偏角β_k以及速度V_k；输入量由牵引点单目间接测量系统提供，输入量包括车辆横摆角速度牵引点侧偏角β_SC，k以及牵引点车速V_SC，k；

状态量x_k：

输入量u_k：

状态更新：

${\mathrm{\ψ}}_{(k+1|k)}={\mathrm{\ψ}}_k+({\stackrel{\·}{\mathrm{\ψ}}}_{SC,k}+{\stackrel{\·}{\mathrm{\ψ}}}_{bias,k})\mathrm{\Δ}t+w_{1,k}$

${\stackrel{\·}{\mathrm{\ψ}}}_{bias,(k+1|k)}={\stackrel{\·}{\mathrm{\ψ}}}_{bias,k}+w_{2,k}$

β_(k+1|k)＝β_SC，k+w_3，k

V_(k+1|k)＝V_SC，k+w_4，k

其中，Δt为采样时间间隔，w_i，k为过程噪声；

将状态更新后的各参数代入观测方程F，则获得修正后的侧向偏移距离：

z_k＝F(x_k)。