CN106327433A - 一种基于单俯视相机与后轴转向的车辆路径跟随方法 - Google Patents

Abstract

本发明公开了一种基于单俯视相机与后轴转向的车辆路径跟随方法，其利用安装于车辆牵引点(最前端)的俯视单目相机，通过对相邻时序图片所采集的路面特征匹配，测量车辆牵引点的车速，侧偏角以及横摆角。基于上述测量状态量，利用车辆横摆平面运动学模型估算出车辆跟随点(最尾端)的侧向路径跟随偏移量。然后将此偏移量作为后轴自动转向系统的控制器输入量，计算车辆后轴的转向角。本方案可以提高车辆的通过性，适用于所有的长轴距车辆。

Description

一种基于单俯视相机与后轴转向的车辆路径跟随方法

技术领域

本发明涉及车辆控制领域，尤其是涉及一种基于单俯视相机与后轴转向的车辆路径跟随方法。

背景技术

长轴距车辆或列车，包括公交巴士，重型车以及长拖挂列车，此类车辆具有良好的运输效率。这类车辆质心高，车身长度长，因而其可操控性以及低速通过性较差。在低速转弯工况下，此类车辆尾部相对于前段会产生相对于转弯半径内侧的侧向偏移量。车身长度越长，转弯半径越小，该侧向偏移量越大，车辆相应的通过性也越差。

为了提高此类车辆的低速安全性能，一些后轴转向系统的应用可以使得整个列车更好的跟随驾驶员的期望行驶路径。此类后轴转向系统可分为两类：一类为“被动系统”，即后轴转向角成比例于前轴转向角(或多列车铰接角)；另一类为“主动系统”，即后轴转向角通过对车辆动力学状态的控制而得到。但现有系统均忽略了低速工况车辆的纵向以及侧向滑移，这类现象在光滑路面，纵向以及侧向坡道存在的工况下极其普遍。准确的测量此类车辆尾部相对于车辆前部的侧向偏移量对于后轴转向系统的应用具有十分重要的意义。

发明内容

本发明主要是解决现有技术所存在的缺乏对长轴距车辆低速转弯时的控制方法、通过性差的技术问题，提供一种可以准确测量车辆尾部相对于前部的侧向偏移量并进行校正控制，提高车辆通过性能的基于单俯视相机与后轴转向的车辆路径跟随方法。

本发明针对上述技术问题主要是通过下述技术方案得以解决的：一种基于单俯视相机与后轴转向的车辆路径跟随方法，包括以下步骤：

S1、单目相机获取原始图像；车辆最前端为牵引点，车辆最末端为跟随点，单目相机安装在牵引点；

S2、对原始图像进行预处理；

S3、对预处理后的原始图像进行FAST特征点提取，并生成SURF特征描述向量；

S4、利用FLANN特征匹配库对相邻两帧原始图像所得到的SURF特征描述向量进行特征匹配；

S5、利用RANSAC选取正确匹配样本，计算Homography矩阵；

S6、对计算所得Homography矩阵进行奇异值分解，获得平动与转动信息；

S7、利用平动信息推算出牵引点侧偏角以及绝对速度；利用转动信息推算出牵引点的横摆角速度；

S8、根据横摆平面车辆运动学模型计算出车辆行驶距离以及牵引点与跟随点的全局位置信息；

S9、将牵引点位置信息与行驶距离存入内存缓冲器，根据牵引点与跟随点之间的距离，读取跟随点当前位置对应牵引点行驶过的全局坐标；

S10、进行坐标变换，转换到当前时刻车辆坐标系下，计算出跟随点相对于牵引点行驶路径的侧向偏移量；

S11、将路径跟随的侧向误差输入到主动转向控制器，输出后轴对应转向角；

S12、重复步骤S1值步骤S11，持续输出后轴对应转向角。

作为优选，步骤S2中，预处理包括灰度化处理和除畸变处理。

作为优选，步骤S5具体为：

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

作为优选，所述Homography矩阵表示为：其中，R为相机平动信息，T为相机转动信息，d为图像平面对应的深度，N为图像平面对应的法向信息，K为相机内部参数矩阵，α为比例系数，α取决于相机安装高度，步骤S6具体为：对计算所得Homography矩阵进行奇异值分解，获得相机平动信息T与转动信息R；令：

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

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

$u_1=\frac{\sqrt{{\mathrm{\σ}}_1^2-1}{v}_3+\sqrt{1-{\mathrm{\σ}}_3^2{v}_1}}{\sqrt{{\mathrm{\σ}}_1^2-{\mathrm{\σ}}_3^2}},u_2=\frac{\sqrt{1-{\mathrm{\σ}}_3^2}{v}_1-\sqrt{{\mathrm{\σ}}_1^2-1}{v}_3}{\sqrt{{\mathrm{\σ}}_1^2-{\mathrm{\σ}}_3^2}}$

$U_1=\[v_2,u_1,\widehat{v_2}{u}_1\],U_2=\[v_2,u_2,\widehat{v_2}{u}_2\]$

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

解1：

$R_1=W_1{U}_1^T,N_1=\widehat{v_2}{u}_1,\frac{1}{d}{T}_1=(\stackrel{\‾}{H}-R_1)N_1$

解2：

$R_2=W_2{U}_2^T,N_2=\widehat{v_2}{u}_2,\frac{1}{d}{T}_2=(\stackrel{\‾}{H}-R_2)N_2$

解3：

R₃＝R₁,N₃＝-N₁,

解4：

R₄＝R₂,N₄＝-N₂,

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

作为优选，步骤S7具体为：

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

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

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

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

作为优选，步骤S8具体为：

按如下公式计算车辆行驶距离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为牵引点与跟随点之间的距离。

作为优选，步骤S11中，主动转向控制器为优化反馈控制，控制器首先确定车辆跟随点虚拟转向角度数，随后各轴转向角可由如下公式确定：

δ_r＝K_PIDY_r

${\mathrm{\δ}}_{axle}={\tan }^{-1}\left(\frac{l_r}{l}tan\right({\mathrm{\β}}_f)+\frac{l_f}{l}tan({\mathrm{\δ}}_r\left)\right)$

其中l为牵引点与跟随点距离，l_r为该轴到跟随点距离，l_f为该轴到牵引点距离，β_f为牵引点侧偏角，δ_r为跟随点虚拟转向角，K_PID为控制器比例系数，Y_r为跟随点在车辆坐标系下的侧向路径跟随偏移量。

本方案主要解决了以下几方面的问题：

1.单目图像预处理–通过测量单目相机参数，分别对两幅单目相机所采集图像进行除畸变。

2.挂车平面特征点提取–利用FAST特征点，提取挂车前表面或侧表面平面特征，并用SURF特征点进行描述。

3.道路特征点匹配–利用FLANN特征匹配算法库对牵引点相机所采集的相邻时序图像进行特征匹配，并计算Homography矩阵。

4.车辆动力学状态计算–通过对Homography矩阵进行奇异值分解，获取相机的平动信息与转动信息。其中，平动信息可转换为车辆的绝对速度与侧偏角信息；转动信息可转换为车辆的横摆角速度。

5.跟随点侧向路径跟随偏移量计算–通过横摆平面车辆运动学模型，计算出车辆牵引点与跟随点的实时全局位置信息并存入内存缓冲区。在车辆坐标系下，计算出车辆跟随点(最末端)侧向路径跟随偏移量计算。

6.后轴转向角计算–通过PID反馈控制确定车辆后轴转向角以使车辆尾部跟随车辆前部路径，从而提高车辆低速通过性。

本发明带来的实质性效果是，可以准确计算车辆跟随点的侧向路径跟随偏移量，进而得到车辆后轴的转向角，使跟随点与牵引点的路径重合，提高车辆的通过性。

附图说明

图1是本发明的一种流程图；

图2是本发明的一种低速路径跟随系统示意图。

具体实施方式

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

实施例：本实施例的一种基于单俯视相机与后轴转向的车辆路径跟随方法，流程总图如图1所示。一个单目相机的图像为本系统输入，车辆后轴转向角为本系统的输出。具体说明如下：

1.单目相机安装在车辆最前端，即为牵引点位置，如图2所示。该相机以垂直朝向路面方向安装，离地高度约为0.5m。本方法旨在在低速行驶中使跟随点(车辆最末端)重复牵引点所行驶路径，以提升车辆的通过性能。本方法适用于单一后轴以及多后轴车辆系统(图2未着色轮胎所示为二轴或三轴系统)。

2.从牵引点单目相机获取原始图像，对图像进行预处理，主要包括灰度化以及除畸变。

3.对牵引点相机所获取图像进行FAST特征点提取，并生成SURF特征描述向量。利用FLANN特征匹配库对相邻两帧所提取的SURF特征描述向量进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵。对计算所得Homography矩阵进行奇异值分解，获得平动与转动信息。其中，利用平动信息可推算出牵引点侧偏角(β_f)以及绝对速度(v_f)信息；利用转动信息可推算出牵引点的横摆角(ψ_f)。

4.根据横摆平面车辆运动学模型，按如下公式可以计算出车辆行驶距离(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为牵引点与跟随点之间的距离。

5.将牵引点位置信息与行驶距离存入内存缓冲器，根据牵引点与跟随点之间的距离，读取跟随点当前位置对应牵引点行驶过的全局坐标。进行坐标变换，转换到当前时刻车辆坐标系下，计算出跟随点相对于牵引点行驶路径的侧向偏移量。

6.将路径跟随的侧向误差输入到主动转向控制器，输出后轴对应转向角。该控制器为PID优化反馈控制。控制器首先确定车辆跟随点虚拟转向角度数，随后各轴转向角可由如下公式确定：

δ_r＝K_PID y_r

${\mathrm{\δ}}_{axle}={\tan }^{-1}\left(\frac{l_r}{l}tan\right({\mathrm{\β}}_f)+\frac{l_f}{l}tan({\mathrm{\δ}}_r\left)\right)$

其中l为牵引点与跟随点距离，l_r为该轴到跟随点距离，l_f为该轴到牵引点距离。β_f为牵引点侧偏角，δ_r为跟随点虚拟转向角。

本发明可实时测量单一车辆以及多列车尾部跟随点相对于前部牵引点的侧向偏移量，并产生相应的后轴转向操作以消除此侧向偏移量。该方法可以有效应对低速工况下车辆的纵向与侧向滑移运动，因此可以适用于光滑以及含坡度角道路工况下的路径跟随。该系统为单车辆单元独立系统，可适用于任何数量(1，2，3)后轴转向系统。本发明可应用于单一长轴距车辆也可用于多列车系统的每一个车辆单元。

本方案还可以使用SIFT或其他特征提取方法；也可以通过对周围环境的特征提取来取代路面特征。

本方案所涉及的部分技术名称解释如下：

FAST：此特征检测算法来源于corner的定义，采用机器学习的方法，按如下标准定义特征点：对于某像素点p，以其为中心的16个像素点，若其中有n个连续的像素点亮度值均大于p点亮度加上某阈值t(或小于p点亮度减去某阈值t)，则p为特征点；可设置参数为像素点数n，亮度阈值t以及是否使用非极大值抑制(Non-Maximum Suppression)。此特征点检测是公认的比较快速的特征点检测方法，只利用周围像素比较的信息就可以得到特征点，简单，有效。该方法多用于角点检测。

SURF：一种具有尺度与旋转特征不变性的特征描述算法，描述性强，速度快。过程包括基于上述特征圆的特征向量方向分配以及基于4*4子集的二维哈尔小波求和的特征值分配。

FLANN：一种快速近似最近邻搜索函数库，自动选择两个近似最近邻算法(K-d决策树以及优先搜索K-均值决策树)中最优的算法。

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

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

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

PID：比例-积分-微分控制器。

本文中所描述的具体实施例仅仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。

尽管本文较多地使用了牵引点、Homography矩阵、转向角等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。

Claims (7)

1.一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，包括以下步骤：

S1、单目相机获取原始图像；车辆最前端为牵引点，车辆最末端为跟随点，单目相机安装在牵引点；

S2、对原始图像进行预处理；

S3、对预处理后的原始图像进行FAST特征点提取，并生成SURF特征描述向量；

S4、利用FLANN特征匹配库对相邻两帧原始图像所得到的SURF特征描述向量进行特征匹配；

S5、利用RANSAC选取正确匹配样本，计算Homography矩阵；

S6、对计算所得Homography矩阵进行奇异值分解，获得平动与转动信息；

S7、利用平动信息推算出牵引点侧偏角以及绝对速度；利用转动信息推算出牵引点的横摆角速度；

S8、根据横摆平面车辆运动学模型计算出车辆行驶距离以及牵引点与跟随点的全局位置信息；

S9、将牵引点位置信息与行驶距离存入内存缓冲器，根据牵引点与跟随点之间的距离，读取跟随点当前位置对应牵引点行驶过的全局坐标；

S10、进行坐标变换，转换到当前时刻车辆坐标系下，计算出跟随点相对于牵引点行驶路径的侧向偏移量；

S11、将路径跟随的侧向误差输入到主动转向控制器，输出后轴对应转向角；

S12、重复步骤S1值步骤S11，持续输出后轴对应转向角。

2.根据权利要求1所述的一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，步骤S2中，预处理包括灰度化处理和除畸变处理。

3.根据权利要求1或2所述的一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，步骤S5具体为：

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

4.根据权利要求3所述的一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，所述Homography矩阵表示为：其中，R为相机平动信息，T为相机转动信息，d为图像平面对应的深度，N为图像平面对应的法向信息，K为相机内部参数矩阵，α为比例系数，步骤S6具体为：对计算所得Homography矩阵进行奇异值分解，获得相机平动信息T与转动信息R；令：

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

$u_1=\frac{\sqrt{{\mathrm{\σ}}_1^2-1}{v}_3+\sqrt{1-{\mathrm{\σ}}_3^2{v}_1}}{\sqrt{{\mathrm{\σ}}_1^2-{\mathrm{\σ}}_3^2}},u_2=\frac{\sqrt{1-{\mathrm{\σ}}_3^2}{v}_1-\sqrt{{\mathrm{\σ}}_1^2-1}{v}_3}{\sqrt{{\mathrm{\σ}}_1^2-{\mathrm{\σ}}_3^2}}$

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

解1：

解2：

解3：

R₃＝R₁,N₃＝-N₁,

解4：

R₄＝R₂,N₄＝-N₂,

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

5.根据权利要求4所述的一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，步骤S7具体为：

通过公式：计算得出实时车速V的绝对值v_f；

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

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

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

6.根据权利要求5所述的一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，步骤S8具体为：

按如下公式计算车辆行驶距离S_f以及牵引点(X_f,Y_f)与跟随点(X_r,Y_r)的全局位置信息：

S_f＝∫v_f dt

γ_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为牵引点与跟随点之间的距离。

7.根据权利要求6所述的一种基于单俯视相机与后轴转向的车辆路径跟随方法，其特征在于，步骤S11中，主动转向控制器为PID优化反馈控制，控制器首先确定车辆跟随点虚拟转向角度数，随后各轴转向角可由如下公式确定：

δ_r＝K_PID Y_r

${\mathrm{\δ}}_{axle}={\tan }^{-1}\left(\frac{l_r}{l}tan\right({\mathrm{\β}}_f)+\frac{l_f}{l}tan({\mathrm{\δ}}_r\left)\right)$

其中l为牵引点与跟随点距离，l_r为该轴到跟随点距离，l_f为该轴到牵引点距离，β_f为牵引点侧偏角，δ_r为跟随点虚拟转向角，K_PID为控制器比例系数，Y_r为跟随点在车辆坐标系下的侧向路径跟随偏移量。