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

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

背景技术：

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

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

技术实现要素：

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

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

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

S2、分别对牵引点原始图像和跟随点原始图像进行预处理；

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

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

S5、利用RANSAC选取正确匹配样本，计算牵引点原始图像的Homography矩阵；

S6、对牵引点原始图像的Homography矩阵进行奇异值分解，获得牵引点平动信息；

S7、根据牵引点平动信息推算出牵引点侧偏角信息，并将牵引点平动信息对时间积分得到运动距离；将该距离作为指针与所提取的牵引点SURF特征描述向量存入内存缓冲区；

S8、从内存缓冲区中读取当前时刻牵引点后方D处的路面SURF特征描述向量，D为牵引点与跟随点之间的距离；

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

S10、利用FLANN特征匹配库对跟随点原始图像所得到的SURF特征描述向量和步骤S8中读取到的路面SURF特征描述向量进行特征匹配；

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

S12、对步骤S11中得到的偏差Homography矩阵进行奇异值分解，获得偏差平动信息；

S13、将偏差平动信息从相机坐标系转换到车辆坐标系下，侧向分量即为车辆尾部路径跟随的侧向偏移量，纵向分量用于修正距离D；

S14、将路径跟随的侧向偏移量输入到主动转向控制器，输出后轴对应转向角；

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

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

作为优选，步骤S5和步骤S11的具体算法为：

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

作为优选，所述Homography矩阵表示为：其中，R为相机平动信息，T为相机转动信息，d为图像平面对应的深度，N为图像平面对应的法向信息，K为相机内部参数矩阵，α为比例系数，α取决于相机安装高度；步骤S6和步骤S12的具体算法为：对计算所得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为单位时间步长。

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

δ_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为跟随点在车辆坐标系下的侧向路径跟随偏移量。

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

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

2.挂车平面特征点提取–利用FAST特征点，提取挂车前表面或侧表面平面特征，并用SURF特征点进行描述。将所提取的特征与该时刻运动距离对应存储于内存中。

3.道路特征点匹配–利用FLANN特征匹配算法库对当前时刻车辆尾部的图像与内存中存储的相应位置处的车辆前端图像进行特征匹配，并计算Homography矩阵。

4.侧向偏移量计算–通过对Homography矩阵进行奇异值分解，获取相机的平动信息，即为车辆尾部相机相对于车辆前端的侧向偏移量。

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

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

附图说明

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

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

具体实施方式

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

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

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

2.从前后两个单目相机分别获取原始图像，对图像进行预处理，主要包括灰度化以及除畸变。

3.对牵引点相机所获取图像进行FAST特征点提取，并生成SURF特征描述向量。利用FLANN特征匹配库对相邻两帧所提取的SURF特征描述向量进行特征匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵。对计算所得Homography矩阵进行奇异值分解，获得平动信息，可推算出牵引点测偏角信息，对时间积分可得运动距离。将该距离作为指针与所提取的SURF特征存入内存缓冲区。

4.从内存缓冲区中读取当前时刻牵引点后方D处的路面SURF特征信息(D为牵引点与跟随点距离)。对跟随点相机所获取的图像进行FAST特征点提取，并生成SURF特征描述向量。利用FLANN特征匹配库对所提取的SURF特征向量与缓冲区中读取的SURF特征进行匹配，利用RANSAC选取正确匹配样本，计算Homography矩阵。对计算所得Homography矩阵进行奇异值分解，获得平动信息。将该平动信息从相机坐标系转换到车辆坐标系下，侧向分量即为车辆尾部路径跟随的侧向偏移量，纵向分量用于修正距离D。

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

δ_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为牵引点与跟随点距离，lr为该轴到跟随点距离，lf为该轴到牵引点距离。β_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矩阵、转向角等术语，但并不排除使用其它术语的可能性。使用这些术语仅仅是为了更方便地描述和解释本发明的本质；把它们解释成任何一种附加的限制都是与本发明精神相违背的。