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

技术特征：

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

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，持续输出后轴对应转向角。

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

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

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

4.根据权利要求3所述的一种基于双俯视相机与后轴转向的车辆路径跟随方法，其特征在于，所述Homography矩阵表示为：其中，R为相机平动信息、T为相机转动信息、d为图像平面对应的深度、N为图像平面对应的法向信息、K为相机内部参数矩阵，α为比例系数，步骤S6和步骤S12的具体算法为：对计算所得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所述的一种基于双俯视相机与后轴转向的车辆路径跟随方法，其特征在于，步骤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为跟随点在车辆坐标系下的侧向路径跟随偏移量。