CN103411536B - 基于双目立体视觉的驾驶辅助障碍物检测方法 - Google Patents

Abstract

本发明公开了一种基于双目立体视觉的驾驶辅助障碍物检测方法，属于环境感知技术领域。其主要步骤包括：同步采集安装在车辆正前方的两个CCD摄像机各自输出的一帧图像；对所采集的图像分别进行畸变校正；调用立体匹配子流程对校正后的两幅图像进行立体匹配，得到所有像素点的对应关系；根据摄像机参数分别计算两个CCD摄像机的投影矩阵；寻找两幅校正后的图像中所有对应像素点对；根据投影矩阵对该时刻采集的图像进行场景的三维重建，得到障碍物场景的空间坐标，并进行障碍物区域筛选，输出障碍物位置信息图像。本发明实现了车辆在静止或行进过程中对路面障碍物的自动检测功能，其突出优点是，可无人值守、检测结果精度高。

Description

基于双目立体视觉的驾驶辅助障碍物检测方法

技术领域

本发明属于环境感知技术领域，主要涉及一种智能车辆的障碍物检测方法，尤其涉及一种基于双目立体视觉的驾驶辅助障碍物检测方法。

背景技术

障碍物检测技术是智能车辆辅助驾驶信息感知系统中的关键技术之一。利用障碍物检测技术可以感知到车辆前方可能存在的障碍物，包括车辆前方障碍物的位置、尺寸、与本车的距离等信息，进而判断自身车辆是否需要采取障碍物信息预警或障碍物躲避等。实现对车辆前方障碍物的探测及对危险的预测是智能车辆辅助驾驶系统中不可缺少的一个功能，也是智能车辆实现自主驾驶的前提条件。

目前，绝大部分智能车辆采用距离传感器进行障碍物检测，虽然距离传感器可以提供关于障碍物距离-朝向信息，但是只能检测位于扫描平面上的障碍物，而机器视觉系统则可以为智能车辆辅助驾驶提供丰富和实时的环境信息，因此得到了广泛的应用。

公开号为CN201010189865.9的中国专利申请公开了一种基于机器人双目立体视觉的地面障碍物检测方法，该方法使用双目视觉系统俯视地面并采集图像，根据双目基线长度、焦距和系统距地面的高度，解算图像中各行的地面视差值，并在此基础上，通过反投影模型计算像素对应的场景点坐标，从而判断该像素属于障碍物或是地面点，建立栅格地图。该方法在室内较为复杂的环境中可精确识别障碍物，并具有很高的实时性。但由于该方法在视差解算时，视差精度与双目视觉系统距地面的高度成反比关系，因此该方法的检测误差随着双目视觉系统距地面高度的减小而增大，对系统的安装位置有一定的要求，此外，该方法使用俯视方式采集地面图像，导致对障碍物高度信息采集不完整，因此对被检测的障碍物高度也有一定的限制。

发明内容

本发明要解决的技术问题是，为智能车辆辅助驾驶系统提供一种障碍物检测方法，具体地说，是提供一种基于双目立体视觉的驾驶辅助障碍物检测方法。

为解决上述技术问题，本发明提供的检测方法是由装有驾驶辅助障碍物检测软件包的计算机实现的，当计算机上电后，该软件包执行以下操作步骤：

第一步，打开CCD摄像机Cam_L和CCD摄像机Cam_R的电源，将存储器中存有的已知参数加载至计算机内存中，且计数参数circle_num清零；

第二步，查询检测指令，若未接到检测指令，等待；若接到检测指令，通过图像采集卡同步采集CCD摄像机Cam_L和CCD摄像机Cam_R各自输出的一帧分辨率均为M×N的图像C_L和图像C_R，并存储于存储器中；

第三步，调用存储器中的图像C_L和图像C_R，根据下组公式对图像C_L和图像C_R进行畸变校正，分别得到两幅分辨率均为M×N的图像C_L′和图像C_R′：

$\left\{\begin{array}{c}{x_{\mathrm{cl}}}^{\′}=x_{\mathrm{cl}}+x_{\mathrm{cl}}(k_{l1}(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+k_{12}{(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)}^2)+p_{l1}({3x}_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+2p_{12}{x}_{\mathrm{cl}}{y}_{\mathrm{cl}}\\ {y_{\mathrm{cl}}}^{\′}=y_{\mathrm{cl}}+y_{\mathrm{cl}}(k_{l1}(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+k_{l2}{(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)}^2)+p_{12}(3y_{\mathrm{cl}}^2+x_{\mathrm{cl}}^2)+2p_{l1}{x}_{\mathrm{cl}}{y}_{\mathrm{cl}}\end{array}\right.$

$\left\{\begin{array}{c}{x_{\mathrm{cr}}}^{\′}=x_{\mathrm{cr}}+x_{\mathrm{cr}}\left(k_{r1}(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2){+k}_{r2}{(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)}^2\right)+p_{r1}({3x}_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+2p_{r2}{x}_{\mathrm{cr}}{y}_{\mathrm{cr}}\\ {y_{\mathrm{cr}}}^{\′}=y_{\mathrm{cr}}+y_{\mathrm{cr}}(k_{r1}(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+k_{r2}{(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)}^2)+p_{r2}({3y}_{\mathrm{cr}}^2+x_{\mathrm{cr}}^2)+2p_{r1}{x}_{\mathrm{cr}}{y}_{\mathrm{cr}}\end{array}\right.$

式中，(x_cl,y_cl)和(x_cr,y_cr)分别是图像C_L和图像C_R中任意像素点的图像坐标，(x_cl′,y_cl′)和(x_cr′,y_cr′)分别是图像C_L′和图像C_R′中任意像素点的图像坐标，且x_cl=1,2,...M，x_cr=1,2,...M，x_cl′=1,2,...M，x_cr′=1,2,...M，y_cl=1,2,...N，y_cr=1,2,...N，y_cl′=1,2,...N，y_cr′=1,2,...N，k_l1、k_l2均为CCD摄像机Cam_L的径向畸变参数，p_l1、p_l2均为CCD摄像机Cam_L切向畸变参数，k_r1、k_r2均为CCD摄像机Cam_R的径向畸变参数，p_r1、p_r2均为CCD摄像机Cam_R的切向畸变参数，k_l1、k_l2、p_l1、p_l2、k_r1、k_r2、p_r1、p_r2均为对摄像机标定得到的已知参数；

第四步，对图像C_L′和图像C_R′进行立体匹配，具体步骤如下：

（4.1）计算图像C_L′和图像C_R′的极线约束基本矩阵F；

（4.2）将图像C_L′作为基准图像，图像C_R′作为待配准图像，进行如下匹配操作：

（4.2.1）在基准图像C_L′中选取像素点A_L(x_A,y_A)，其中，x_A=n+1，y_A=n+1，且n满足n≤(min(M,N)-1)/2；

（4.2.2）计算像素点A_L(x_A,y_A)在图像C_R′中满足下式的对应待配准像素点集S_A={P_q(x_q,y_q)}，q=1,2,...,k，其中，k为像素点集S_A中像素点的个数；

[x_qy_q1]F[x_Ay_A1]^T=0

（4.2.3）在图像C_L′中建立一个以像素点A_L(x_A,y_A)为中心，大小为(2n+1)×(2n+1)的配准窗口，在图像C_R′中建立一个以像素点集S_A={P_q(x_q,y_q)}中第q个像素点为中心，大小为(2n+1)×(2n+1)的配准窗口，并按照下式计算像素点A_L(x_A,y_A)和像素点集S_A={P_q(x_q,y_q)}中所有像素点的相关参数C_q，共计得到k个C_q值；

$\left\{\begin{array}{c}{C}_q=\underset{i=-n}{\overset{n}{\mathrm{\Σ}}}\underset{j=-n}{\overset{n}{\mathrm{\Σ}}}|I_l(x_A+i,y_A+j)-I_r(x_q+d_q+i,y_q+j)|\\ d_q=x_A-x_q\end{array}\right.$

式中，I_l(x_A+i,y_A+j)为图像C_L′中对应像素点(x_A+i,y_A+j)的灰度值，I_r(x_q+d_q+i,y_q+j)为图像C_R′中对应像素点(x_q+d_q+i,y_q+j)的灰度值，其中，i=-n,-n+1,...,n，j=-n,-n+1,...,n，d_q为图像C_L′和图像C_R′中对应待配准像素点对A(x_A,y_A)和P_q(x_q,y_q)的视差；

（4.2.4）将k个相关参数C_q与阈值T_th逐一比较，若有t个C_q值满足C_q<T_th，则计数参数circle_num=circle_num+t，其中1≤t≤k；

（4.2.5）判断计数参数circle_num的值：

若circle_num=0，直接跳至（4.2.6）；

若circle_num=1，选择满足C_q<T_th时对应的像素点对A_L(x_A,y_A)和P_q(x_q,y_q)为一个配准点对，并存储，跳至步骤（4.2.6）；

若circle_num≥2，选择满足C_q<T_th且满足C_q=min{C_q}时对应的像素点对A_L(x_A,y_A)和P_q(x_q,y_q)为一个配准点对，并存储；

（4.2.6）令y_A=y_A+1，且计数参数circle_num清零，反复重复步骤（4.2.2）-（4.2.5），直至计算出图像C_L′中y_A=N-n的像素点在图像C_R′中的对应配准点为止；

（4.2.7）令x_A=x_A+1，反复重复步骤（4.2.2）-（4.2.6），直至计算出图像C_L′中坐标值为(M-n,N-n)的像素点在图像C_R′中的对应配准点为止；

（4.3）根据基准图像C_L′和待配准图像C_R′中的所有配准点对，并利用最小二乘法计算基准图像C_L′和待配准图像C_R′的像素点对应关系f_lr，其中，f_ij为矩阵f_lr中的元素，i=1,2,3，j=1,2,3；

${[x_A,y_A,1]}^T=f_{\mathrm{lr}}{[x_q,y_q,1]}^T=\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]{[x_q,y_q,1]}^T$

第五步，根据CCD标定的已知参数，计算CCD摄像机Cam_L的投影矩阵M_l和CCD摄像机Cam_R的投影矩阵M_r；

第六步，根据下式计算图像C_L′中所有像素点(x_cl′,y_cl′)在图像C_R′中一一对应的配准点的坐标值(x_r,y_r)；

${[x_r,y_r,1]}^T={\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]}^{-1}{[{x_{\mathrm{cl}}}^{\&prime;},{y_{\mathrm{cl}}}^{\&prime;},1]}^T$

第七步，根据下式对图像C_L′中和图像C_R′中所有的对应配准点对(x_cl′,y_cl′)和(x_r,y_r)进行三维场景重建，得到所有的对应配准点对(x_cl′,y_cl′)和(x_r,y_r)在世界坐标系对应的实际坐标(X_W,Y_W,Z_W)，并将其存入存储器中；

A=(P^TP)^-1P^Tb

其中，A=[X_W,Y_W,Z_W]^T为图像C_L′中和图像C_R′中对应配准点对(x_cl′,y_cl′)和(x_r,y_r)在世界坐标系中对应的实际三维坐标(X_W,Y_W,Z_W)的列向量，其中，x_cl′∈{1,2,3,...,M}，x_r∈{1,2,3,...,M}，y_cl′∈{1,2,3,...,N}，y_r∈{1,2,3,...,N}，矩阵P为一个4×3的矩阵，表示如下：

$P=\left[\begin{array}{ccc}{m}_{l31}{x_{\mathrm{cl}}}^{\&prime;}-m_{l11}& m_{l32}{x_{\mathrm{cl}}}^{\&prime;}-m_{l12}& m_{l33}{x_{\mathrm{cl}}}^{\&prime;}-m_{l13}\\ m_{l31}{y_{\mathrm{cl}}}^{\&prime;}-m_{l21}& m_{l32}{y_{\mathrm{cl}}}^{\&prime;}{-m}_{l22}& m_{l33}{y_{\mathrm{cl}}}^{\&prime;}-m_{l23}\\ m_{r31}{x}_r-m_{r11}& m_{r32}{x}_r-m_{r12}& m_{r33}{x}_r-m_{r13}\\ m_{r31}{y}_r-m_{r21}& m_{r32}{y}_r-m_{r22}& m_{r33}{y}_r-m_{r23}\end{array}\right]$

b为一个四维列向量，表示如下：

$b=\left[\begin{array}{c}{m}_{l14}-m_{l34}{x_{\mathrm{cl}}}^{\&prime;}\\ m_{l24}-m_{l34}{y_{\mathrm{cl}}}^{\&prime;}\\ m_{r14}-m_{r34}{x}_r\\ m_{r24}-m_{r34}{y}_r\end{array}\right]$

其中，m_lij为CCD摄像机Cam_L投影矩阵M_l中的元素，m_rij为CCD摄像机Cam_R投影矩阵M_r中的元素，i=1,2,3，j=1,2,3,4；

第八步，根据世界坐标系中每一个场景点的实际坐标用下式进行障碍物区域筛选，得到筛选后的所有场景点的实际坐标(X_W′,Y_W′,Z_W′)；

其中，Des_th为探测距离阈值；

第九步，根据下式建立障碍物信息图像IMG，存储并在计算机屏幕上显示；

$\left\{\begin{array}{c}{x}_{\mathrm{img}}={X_W}^{\&prime;}\\ y_{\mathrm{img}}={Y_W}^{\&prime;}\\ I(x_{\mathrm{img}},y_{\mathrm{img}})=255({Z_W}^{\&prime;}/{\mathrm{Des}}_{\mathrm{th}})\end{array}\right.$

其中，(x_img,y_img)为障碍物信息图像IMG中的像素点坐标，I(x_img,y_img)为障碍物信息图像IMG中对应像素点(x_img,y_img)的灰度值；

第十步，计算障碍物信息图像IMG中满足I(x_img,y_img)≠0的区域面积S，若满足S≥S₀,则进入第十一步，若满足S<S₀，则进入第十二步，其中，S₀为障碍物正向投影面积阈值；

第十一步，计算障碍物信息图像IMG中满足I(x_img,y_img)≠0区域中所有像素点坐标(x_img,y_img)所对应的Z_W′坐标值的平均值Z_average，将Z_average与障碍物报警距离阈值Z_th进行比较：若满足Z_average<Z_th时，向蜂鸣器发送障碍物报警指令，若满足Z_average≥Z_th时，则直接进入第十二步，其中，Z_th=V_MOV·time_th，V_MOV为车辆当前行驶速度，time_th为车辆当前位置至与障碍物发生碰撞所需的时间阈值；

第十二步，查询关机指令，若未接到关机指令，返回第二步；若接到关机指令，结束。

本发明的有益效果体现在以下几个方面。

（一）本发明是基于双目立体视觉系统，提出了一种不需人为干涉的障碍物检测方法，它使用两个经过标定的CCD摄像机，同步采集车辆在静止或行驶过程中正前方路面的环境图像数据，并自主通过同一时刻采集的两幅图像进行基于三维重构的一系列图像处理和相应的计算后，精确获取车辆前方场景中障碍物的位置、尺寸、与本车的距离等信息，使智能车辆可以实时的进行车辆前方场景中障碍物的检测，从而实现障碍物告警。

（二）本发明在两个CCD摄像机采集图像的畸变校正过程中加入了切向畸变校正，进一步有效的提高图像坐标获取的准确性，同时在立体匹配的过程中加入了极线约束条件，缩小了特征点匹配范围，减少了立体匹配过程的计算量，提高了立体特征匹配的匹配精度，从而实现更精确的获取障碍物场景的三维信息，为车辆障碍物告警与规避提供了很好的准备条件。

附图说明

图1是双目立体视觉障碍物检测设备组成示意图。

图2是本发明驾驶辅助障碍物检测方法的主流程图。

图3是图2中立体匹配子程序的流程图。

具体实施方式

下面结合附图及实施例对本发明作进一步的详述。

本发明的主要任务是提供一种基于双目立体视觉的驾驶辅助障碍物检测方法，实现在车辆处于静止或行驶状态下对车辆正前方的障碍物进行实时检测。

根据图1所示，实现本发明障碍物检测方法所需的检测设备包括两个带有光学镜头的CCD摄像机、一台计算机和一个用于报警的蜂鸣器。两个CCD摄像机的参数完全相同，两个光学镜头的参数完全相同。两个CCD摄像机安装在车辆正前方，两个CCD摄像机成像中心点的连线O_LO_R称为双目基线，其长度范围可任意取值，但应与车辆尺寸和摄像机的径向外形尺寸相匹配；两个CCD摄像机光轴的夹角为α，且满足0°≤α<180°；双目基线O_LO_R和两个CCD摄像机的光轴OO_L、OO_R分别与地面平行。本优选实施例中，双目基线O_LO_R=180mm，两个CCD摄像机的光轴夹角α=0°。计算机装有图像采集卡、存储器和驾驶辅助障碍物检测软件包。计算机和两个CCD摄像机通过电缆相连。计算机和蜂鸣器通过电缆相连。图像采集卡的采集速率应不小于30fps，存储器中存有实现驾驶辅助障碍物检测方法所需要的已知参数，这些参数包括经过立体标定得到的CCD摄像机Cam_L的图像坐标系X轴上尺度因子a_xl，图像坐标系Y轴上尺度因子a_yl，图像中心点坐标(u_l0,v_l0)，摄像机坐标系与世界坐标系之间的旋转矩阵R_l和平移向量T_l，摄像机的径向畸变参数k_l1、k_l2和切向畸变参数p_l1、p_l2；CCD摄像机Cam_R的图像坐标系X轴上尺度因子a_xr、图像坐标系Y轴上尺度因子a_yr，图像中心点坐标(u_r0,v_r0)，摄像机坐标系与世界坐标系之间的旋转矩阵R_r和平移向量T_r，摄像机的径向畸变参数k_r1、k_r2和切向畸变参数p_r1、p_r2；CCD摄像机Cam_L和CCD摄像机Cam_R的相对角度因子γ，相对旋转矩阵R和相对平移向量T，其中，R_l、R_r、R均为3×3的正单位矩阵，T_l、T_r、T均为三维列向量。

不难看出，实现驾驶辅助障碍物检测的核心是驾驶辅助障碍物检测软件包，当计算机上电后，该软件包按照图2所示的流程执行以下操作步骤。

第一步，初始化。打开CCD摄像机Cam_L和CCD摄像机Cam_R的电源，将存储器中存有的已知参数加载至计算机内存中，且计数参数circle_num清零；

第二步，查询检测指令，若未接到检测指令，等待；若接到检测指令，通过图像采集卡同步采集CCD摄像机Cam_L和CCD摄像机Cam_R各自输出的一帧分辨率均为M×N的图像C_L和图像C_R，并存储于存储器中，在本优选实施例中，M=1920，N=1080；

第三步，调用存储器中的图像C_L和图像C_R，根据式（1）和式（2）所示模型对图像C_L和图像C_R进行畸变校正，分别得到校正后的两幅分辨率均为M×N的图像C_L′和图像C_R′。

$\left\{\begin{array}{c}{x_{\mathrm{cl}}}^{\&prime;}=x_{\mathrm{cl}}+x_{\mathrm{cl}}(k_{l1}(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+k_{12}{(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)}^2)+p_{l1}({3x}_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+2p_{12}{x}_{\mathrm{cl}}{y}_{\mathrm{cl}}\\ {y_{\mathrm{cl}}}^{\&prime;}=y_{\mathrm{cl}}+y_{\mathrm{cl}}(k_{l1}(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+k_{l2}{(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)}^2)+p_{12}(3y_{\mathrm{cl}}^2+x_{\mathrm{cl}}^2)+2p_{l1}{x}_{\mathrm{cl}}{y}_{\mathrm{cl}}\end{array}\right.---\left(1\right)$

$\left\{\begin{array}{c}{x_{\mathrm{cr}}}^{\&prime;}=x_{\mathrm{cr}}+x_{\mathrm{cr}}\left(k_{r1}(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2){+k}_{r2}{(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)}^2\right)+p_{r1}({3x}_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+2p_{r2}{x}_{\mathrm{cr}}{y}_{\mathrm{cr}}\\ {y_{\mathrm{cr}}}^{\&prime;}=y_{\mathrm{cr}}+y_{\mathrm{cr}}(k_{r1}(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+k_{r2}{(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)}^2)+p_{r2}({3y}_{\mathrm{cr}}^2+x_{\mathrm{cr}}^2)+2p_{r1}{x}_{\mathrm{cr}}{y}_{\mathrm{cr}}\end{array}\right.---\left(2\right)$

式中，(x_cl,y_cl)和(x_cr,y_cr)分别是图像C_L和图像C_R中任意像素点的图像坐标，(x_cl′,y_cl′)和(x_cr′,y_cr′)分别是图像C_L′和图像C_R′中任意像素点的图像坐标，且x_cl=1,2,...M，x_cr=1,2,...M，x_cl′=1,2,...M，x_cr′=1,2,...M，y_cl=1,2,...N，y_cr=1,2,...N，y_cl′=1,2,...N，y_cr′=1,2,...N；

第四步，调用如图3所示的立体匹配子流程对第三步中经畸变校正后的图像C_L′和图像C_R′进行立体匹配，具体步骤如下：

（4.1）根据式（3）计算图像C_L′和图像C_R′的极线约束基本矩阵F，即

$F={\left[\begin{array}{ccc}{a}_{\mathrm{xr}}& \mathrm{\&gamma;}& u_{r0}\\ 0& a_{\mathrm{yr}}& v_{r0}\\ 0& 0& 1\end{array}\right]}^{-T}{\left[T\right]}_{\&times;}R{\left[\begin{array}{ccc}{a}_{\mathrm{xl}}& \mathrm{\&gamma;}& u_{l0}\\ 0& a_{\mathrm{yl}}& v_{l0}\\ 0& 0& 1\end{array}\right]}^{-1}---\left(3\right)$

其中，[T]_×为两个CCD摄像机Cam_L和Cam_R的相对平移向量T的反对称矩阵，计算得到的极线约束基本矩阵F为3×3的矩阵。

（4.2）将图像C_L′作为基准图像，图像C_R′作为待配准图像，进行如下匹配操作：

（4.2.1）在基准图像C_L′中选取像素点A_L(x_A,y_A)，其中，x_A=n+1，y_A=n+1，且n满足n≤(min(M,N)-1)/2；

（4.2.2）计算像素点A_L(x_A,y_A)在图像C_R′中满足式（4）的对应待配准像素点集S_A={P_q(x_q,y_q)}，q=1,2,...,k，其中，k为像素点集S_A中像素点的个数，即k=size(S)；

[x_qy_q1]F[x_Ay_A1]^T=0（4）

（4.2.3）在图像C_L′中建立一个以像素点A_L(x_A,y_A)为中心，大小为(2n+1)×(2n+1)的配准窗口，在图像C_R′中建立一个以像素点集S_A={P_q(x_q,y_q)}中第q个像素点为中心，大小为(2n+1)×(2n+1)的配准窗口，并按照式（5）计算像素点A_L(x_A,y_A)和像素点P_q(x_q,y_q)的相关系数C_q。

$\left\{\begin{array}{c}{C}_q=\underset{i=-n}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=-n}{\overset{n}{\mathrm{\&Sigma;}}}|I_l(x_A+i,y_A+j)-I_r(x_q+d_q+i,y_q+j)|\\ d_q=x_A-x_q\end{array}\right.---\left(5\right)$

式（5）中，I_l(x_A+i,y_A+j)为图像C_L′中对应像素点(x_A+i,y_A+j)的灰度值，I_r(x_q+d_q+i,y_q+j)为图像C_R′中对应像素点(x_q+d_q+i,y_q+j)的灰度值，其中，i=-n,-n+1,...,n，j=-n,-n+1,...,n，d_q为图像C_L′和图像C_R′中对应待配准像素点对A(x_A,y_A)和P_q(x_q,y_q)的视差。

本步骤中，反复利用式（5）计算像素点A_L(x_A,y_A)和像素点集S_A={P_q(x_q,y_q)}中所有像素点的相关参数C_q，共计得到k个C_q值；

（4.2.4）将k个相关参数C_q与阈值T_th逐一比较，若有t个C_q值满足C_q<T_th，则计数参数circle_num=circle_num+t，其中1≤t≤k；

（4.2.5）判断计数参数circle_num的值：

若circle_num=0，直接跳至（4.2.6）；

若circle_num=1，选择满足C_q<T_th时对应的像素点对A_L(x_A,y_A)和P_q(x_q,y_q)为一个配准点对，并存储，跳至步骤（4.2.6）；

若circle_num≥2，选择满足C_q<T_th且满足C_q=min{C_q}时对应的像素点对A_L(x_A,y_A)和P_q(x_q,y_q)为一个配准点对，并存储，跳至步骤（4.2.6）；

（4.2.6）令y_A=y_A+1，且计数参数circle_num清零，反复重复步骤（4.2.2）-（4.2.5），直至计算出图像C_L′中y_A=N-n的像素点在图像C_R′中的对应配准点为止；

（4.2.7）令x_A=x_A+1，反复重复步骤（4.2.2）-（4.2.6），直至计算出图像C_L′中坐标值为(M-n,N-n)的像素点在图像C_R′中的对应配准点为止；

（4.3）调用经过步骤（4.1）-（4.2）计算得到的基准图像C_L′和待配准图像C_R′中的所有配准点对，分别代入（6）式，利用最小二乘法计算基准图像C_L′和待配准图像C_R′的像素点对应关系f_lr，其中，f_lr为一个3×3的矩阵；

${[x_A,y_A,1]}^T=f_{\mathrm{lr}}{[x_q,y_q,1]}^T=\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]{[x_q,y_q,1]}^T---\left(6\right)$

第五步，根据式（7）-（8）分别计算CCD摄像机Cam_L的投影矩阵M_l和CCD摄像机Cam_R的投影矩阵M_r：

$M_l=\left[\begin{array}{cccc}{m}_{l11}& m_{l12}& m_{l13}& m_{l14}\\ m_{l21}& m_{l22}& m_{l23}& m_{l24}\\ m_{l31}& m_{l32}& m_{l33}& m_{l34}\end{array}\right]=\left[\begin{array}{ccc}{a}_{\mathrm{xl}}& \mathrm{\&gamma;}& u_{l0}\\ 0& a_{\mathrm{yl}}& v_{l0}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_l& T_l\end{array}\right]---\left(7\right)$

$M_r=\left[\begin{array}{cccc}{m}_{r11}& m_{r12}& m_{r13}& m_{r14}\\ m_{r21}& m_{r22}& m_{r23}& m_{r24}\\ m_{r31}& m_{r32}& m_{r33}& m_{r34}\end{array}\right]=\left[\begin{array}{ccc}{a}_{\mathrm{xr}}& \mathrm{\&gamma;}& u_{r0}\\ 0& a_{\mathrm{yr}}& v_{r0}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}{R}_r& T_r\end{array}\right]---\left(8\right)$

其中，CCD摄像机Cam_L的投影矩阵M_l和CCD摄像机Cam_R的投影矩阵M_r均为3×4的矩阵；

第六步，根据式（9）计算图像C_L′中所有像素点(x_cl′,y_cl′)在图像C_R′中一一对应的配准点的坐标值(x_r,y_r)。

${[x_r,y_r,1]}^T={\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]}^{-1}{[{x_{\mathrm{cl}}}^{\&prime;},{y_{\mathrm{cl}}}^{\&prime;},1]}^T---\left(9\right)$

第七步，根据式（10）对图像C_L′中和图像C_R′中所有的对应配准点对(x_cl′,y_cl′)和(x_r,y_r)进行三维场景重建，得到所有的对应配准点对(x_cl′,y_cl′)和(x_r,y_r)在世界坐标系对应的实际坐标(X_W,Y_W,Z_W)，并将其存入存储器中。

A=(P^TP)^-1P^Tb（10）

其中，A=[X_W,Y_W,Z_W]^T为图像C_L′中和图像C_R′中对应配准点对(x_cl′,y_cl′)和(x_r,y_r)在世界坐标系中对应的实际三维坐标(X_W,Y_W,Z_W)的列向量，其中，x_cl′∈{1,2,3,...,M}，x_r∈{1,2,3,...,M}，y_cl′∈{1,2,3,...,N}，y_r∈{1,2,3,...,N}，矩阵P为一个4×3的矩阵，表示如下：

$P=\left[\begin{array}{ccc}{m}_{l31}{x_{\mathrm{cl}}}^{\&prime;}-m_{l11}& m_{l32}{x_{\mathrm{cl}}}^{\&prime;}-m_{l12}& m_{l33}{x_{\mathrm{cl}}}^{\&prime;}-m_{l13}\\ m_{l31}{y_{\mathrm{cl}}}^{\&prime;}-m_{l21}& m_{l32}{y_{\mathrm{cl}}}^{\&prime;}{-m}_{l22}& m_{l33}{y_{\mathrm{cl}}}^{\&prime;}-m_{l23}\\ m_{r31}{x}_r-m_{r11}& m_{r32}{x}_r-m_{r12}& m_{r33}{x}_r-m_{r13}\\ m_{r31}{y}_r-m_{r21}& m_{r32}{y}_r-m_{r22}& m_{r33}{y}_r-m_{r23}\end{array}\right]$

b为一个四维列向量，表示如下：

$b=\left[\begin{array}{c}{m}_{l14}-m_{l34}{x_{\mathrm{cl}}}^{\&prime;}\\ m_{l24}-m_{l34}{y_{\mathrm{cl}}}^{\&prime;}\\ m_{r14}-m_{r34}{x}_r\\ m_{r24}-m_{r34}{y}_r\end{array}\right]$

第八步，调用第七步中计算得到的世界坐标系中所有场景点的实际坐标(X_W,Y_W,Z_W)，并对世界坐标系中每一个场景点的实际坐标使用式（11）进行障碍物区域筛选：

得到筛选后的所有场景点的实际坐标(X_W′,Y_W′,Z_W′)，其中，Des_th为探测距离阈值，其值可根据实际情况选取，并应小于两个CCD摄像机的实际最远探测距离。

第九步，针对第八步计算得到的所有场景点的实际坐标(X_W′,Y_W′,Z_W′)，根据式（12）建立障碍物信息图像IMG，存储并在计算机屏幕上显示。

$\left\{\begin{array}{c}{x}_{\mathrm{img}}={X_W}^{\&prime;}\\ y_{\mathrm{img}}={Y_W}^{\&prime;}\\ I(x_{\mathrm{img}},y_{\mathrm{img}})=255({Z_W}^{\&prime;}/{\mathrm{Des}}_{\mathrm{th}})\end{array}\right.---\left(12\right)$

其中，(x_img,y_img)为障碍物信息图像IMG中的像素点坐标，I(x_img,y_img)为障碍物信息图像IMG中对应像素点(x_img,y_img)的灰度值。

第十步，调用第九步得到的障碍物信息图像IMG，计算障碍物信息图像IMG中满足I(x_img,y_img)≠0的区域面积S，若满足S≥S₀,则进入第十一步，若满足S<S₀，则进入第十二步，其中，S₀为障碍物正向投影面积阈值，可根据经验选取。

第十一步，当S≥S₀时，计算障碍物信息图像IMG中满足I(x_img,y_img)≠0区域中所有像素点坐标(x_img,y_img)所对应的Z_W′坐标值的平均值Z_average，将Z_average与障碍物报警距离阈值Z_th进行比较：若满足Z_average<Z_th时，向蜂鸣器发送障碍物报警指令，若满足Z_average≥Z_th时，则直接进入第十二步，其中，Z_th=V_MOV·time_th，V_MOV为车辆当前行驶速度，time_th为车辆当前位置至与障碍物发生碰撞所需的时间阈值，根据经验，time_th的值取10秒。

第十二步，查询关机指令，若未接到关机指令，返回第二步；若接到关机指令，结束。

Claims (1)

1.一种基于双目立体视觉的驾驶辅助障碍物检测方法，其特征在于，该方法是由装有驾驶辅助障碍物检测软件包的计算机实现的，当计算机上电后，该软件包执行以下操作步骤：

第一步，打开CCD摄像机Cam_L和CCD摄像机Cam_R的电源，将存储器中存有的已知参数加载至计算机内存中，且计数参数circle_num清零；

第二步，查询检测指令，若未接到检测指令，等待；若接到检测指令，通过图像采集卡同步采集CCD摄像机Cam_L和CCD摄像机Cam_R各自输出的一帧分辨率均为M×N的图像C_L和图像C_R，并存储于存储器中；

第三步，调用存储器中的图像C_L和图像C_R，根据下组公式对图像C_L和图像C_R进行畸变校正，分别得到两幅分辨率均为M×N的图像C_L′和图像C_R′：

$\left\{\begin{array}{c}{x_{\mathrm{cl}}}^{\&prime;}=x_{\mathrm{cl}}+x_{\mathrm{cl}}(k_{l1}(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+k_{l2}{(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)}^2)+p_{l1}({3x}_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+2p_{l2}{x}_{\mathrm{cl}}{y}_{\mathrm{cl}}\\ {y_{\mathrm{cl}}}^{\&prime;}=y_{\mathrm{cl}}+y_{\mathrm{cl}}(k_{l1}(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)+k_{l2}{(x_{\mathrm{cl}}^2+y_{\mathrm{cl}}^2)}^2)+p_{l2}(3y_{\mathrm{cl}}^2+x_{\mathrm{cl}}^2)+{2p}_{l1}{x}_{\mathrm{cl}}{y}_{\mathrm{cl}}\end{array}\right.$

$\left\{\begin{array}{c}{x_{\mathrm{cr}}}^{\&prime;}=x_{\mathrm{cr}}+x_{\mathrm{cr}}(k_{r1}(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+k_{r2}{(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)}^2)+p_{r1}({3x}_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+2p_{r2}{x}_{\mathrm{cr}}{y}_{\mathrm{cr}}\\ {y_{\mathrm{cr}}}^{\&prime;}=y_{\mathrm{cr}}+y_{\mathrm{cr}}(k_{r1}(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)+k_{r2}{(x_{\mathrm{cr}}^2+y_{\mathrm{cr}}^2)}^2)+p_{r2}({3y}_{\mathrm{cr}}^2+x_{\mathrm{cr}}^2)+2p_{r1}{x}_{\mathrm{cr}}{y}_{\mathrm{cr}}\end{array}\right.$

式中，(x_cl,y_cl)和(x_cr,y_cr)分别是图像C_L和图像C_R中任意像素点的图像坐标，(x_cl′,y_cl′)和(x_cr′,y_cr′)分别是图像C_L′和图像C_R′中任意像素点的图像坐标，且x_cl＝1,2,...M，x_cr＝1,2,...M，x_cl′＝1,2,...M，x_cr′＝1,2,...M，y_cl＝1,2,...N，y_cr＝1,2,...N，y_cl′＝1,2,...N，y_cr′＝1,2,...N，k_l1、k_l2均为CCD摄像机Cam_L的径向畸变参数，p_l1、p_l2均为CCD摄像机Cam_L切向畸变参数，k_r1、k_r2均为CCD摄像机Cam_R的径向畸变参数，p_r1、p_r2均为CCD摄像机Cam_R的切向畸变参数，k_l1、k_l2、p_l1、p_l2、k_r1、k_r2、p_r1、p_r2均为对摄像机标定得到的已知参数；

第四步，对图像C_L′和图像C_R′进行立体匹配，具体步骤如下：

(4.1)计算图像C_L′和图像C_R′的极线约束基本矩阵F；

(4.2)将图像C_L′作为基准图像，图像C_R′作为待配准图像，进行如下匹配操作：

(4.2.1)在基准图像C_L′中选取像素点A_L(x_A,y_A)，其中，x_A＝n+1，y_A＝n+1，且n满足n≤(min(M,N)-1)/2；

(4.2.2)计算像素点A_L(x_A,y_A)在图像C_R′中满足下式的对应待配准像素点集S_A＝{P_q(x_q,y_q)}，q＝1,2,...,k，其中，k为像素点集S_A中像素点的个数；

[x_qy_q1]F[x_Ay_A1]^T＝0

(4.2.3)在图像C_L′中建立一个以像素点A_L(x_A,y_A)为中心，大小为(2n+1)×(2n+1)的配准窗口，在图像C_R′中建立一个以像素点集S_A＝{P_q(x_q,y_q)}中第q个像素点为中心，大小为(2n+1)×(2n+1)的配准窗口，并按照下式计算像素点A_L(x_A,y_A)和像素点集S_A＝{P_q(x_q,y_q)}中所有像素点的相关参数C_q，共计得到k个C_q值；

$\left\{\begin{array}{c}{C}_q=\underset{i=-n}{\overset{n}{\mathrm{\&Sigma;}}}\underset{j=-n}{\overset{n}{\mathrm{\&Sigma;}}}|I_l(x_A+i,y_A+j)-I_r(x_q+d_q+i,y_q+j)|\\ d_q=x_A-x_q\end{array}\right.$

式中，I_l(x_A+i,y_A+j)为图像C_L′中对应像素点(x_A+i,y_A+j)的灰度值，I_r(x_q+d_q+i,y_q+j)为图像C_R′中对应像素点(x_q+d_q+i,y_q+j)的灰度值，其中，i＝-n,-n+1,...,n，j＝-n,-n+1,...,n，d_q为图像C_L′和图像C_R′中对应待配准像素点对A(x_A,y_A)和P_q(x_q,y_q)的视差；

(4.2.4)将k个相关参数C_q与阈值T_th逐一比较，若有t个C_q值满足C_q＜T_th，则计数参数circle_num＝circle_num+t，其中1≤t≤k；

(4.2.5)判断计数参数circle_num的值：

若circle_num＝0，直接跳至(4.2.6)；

若circle_num＝1，选择满足C_q＜T_th时对应的像素点对A_L(x_A,y_A)和P_q(x_q,y_q)为一个配准点对，并存储，跳至步骤(4.2.6)；

若circle_num≥2，选择满足C_q＜T_th且满足C_q＝min{C_q}时对应的像素点对A_L(x_A,y_A)和P_q(x_q,y_q)为一个配准点对，并存储后跳至步骤(4.2.6)；

(4.2.6)令y_A＝y_A+1，且计数参数circle_num清零，反复重复步骤(4.2.2)-(4.2.5)，直至计算出图像C_L′中y_A＝N-n的像素点在图像C_R′中的对应配准点为止；

(4.2.7)令x_A＝x_A+1，反复重复步骤(4.2.2)-(4.2.6)，直至计算出图像C_L′中坐标值为(M-n,N-n)的像素点在图像C_R′中的对应配准点为止；

(4.3)根据基准图像C_L′和待配准图像C_R′中的所有配准点对，并利用最小二乘法计算基准图像C_L′和待配准图像C_R′的像素点对应关系f_lr，其中，f_ij为矩阵f_lr中的元素，i＝1,2,3，j＝1,2,3；

${[x_A,y_A,1]}^T=f_{\mathrm{lr}}{[x_q,y_q,1]}^T=\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]{[x_q,y_q,1]}^T$

第五步，根据CCD标定的已知参数，计算CCD摄像机Cam_L的投影矩阵M_l和CCD摄像机Cam_R的投影矩阵M_r；

第六步，根据下式计算图像C_L′中所有像素点(x_cl′,y_cl′)在图像C_R′中一一对应的配准点的坐标值(x_r,y_r)；

${[x_r,y_r,1]}^T={\left[\begin{array}{ccc}{f}_{11}& f_{12}& f_{13}\\ f_{21}& f_{22}& f_{23}\\ f_{31}& f_{32}& f_{33}\end{array}\right]}^{-1}{[{x_{\mathrm{cl}}}^{\&prime;},{y_{\mathrm{cl}}}^{\&prime;},1]}^T$

第七步，根据下式对图像C_L′中和图像C_R′中所有的对应配准点对(x_cl′,y_cl′)和(x_r,y_r)进行三维场景重建，得到所有的对应配准点对(x_cl′,y_cl′)和(x_r,y_r)在世界坐标系对应的实际坐标(X_W,Y_W,Z_W)，并将其存入存储器中；

A＝(P^TP)^-1P^Tb

其中，A＝[X_W,Y_W,Z_W]^T为图像C_L′中和图像C_R′中对应配准点对(x_cl′,y_cl′)和(x_r,y_r)在世界坐标系中对应的实际三维坐标(X_W,Y_W,Z_W)的列向量，其中，x_cl′∈{1,2,3,...,M}，x_r∈{1,2,3,...,M}，y_cl′∈{1,2,3,...,N}，y_r∈{1,2,3,...,N}，矩阵P为一个4×3的矩阵，表示如下：

$P=\left[\begin{array}{ccc}{m}_{l31}{x_{\mathrm{cl}}}^{\&prime;}-m_{l11}& m_{l32}{x_{\mathrm{cl}}}^{\&prime;}-m_{l12}& m_{l33}{x_{\mathrm{cl}}}^{\&prime;}-m_{l13}\\ m_{l31}{y_{\mathrm{cl}}}^{\&prime;}-m_{l21}& m_{l32}{y_{\mathrm{cl}}}^{\&prime;}-m_{l22}& m_{l33}{y_{\mathrm{cl}}}^{\&prime;}-m_{l23}\\ m_{r31}{x}_r-m_{r11}& m_{r32}{x}_r-m_{r12}& m_{r33}{x}_r-m_{r13}\\ m_{r31}{y}_r-m_{r21}& m_{r32}{y}_r-m_{r22}& m_{r33}{y}_r-m_{r23}\end{array}\right]$

b为一个四维列向量，表示如下：

$b=\left[\begin{array}{c}{m}_{l14}-m_{l34}{x_{\mathrm{cl}}}^{\&prime;}\\ m_{l24}-m_{l34}{y_{\mathrm{cl}}}^{\&prime;}\\ m_{r14}-m_{r34}{x}_r\\ m_{r24}-m_{r34}{y}_r\end{array}\right]$

其中，m_lij为CCD摄像机Cam_L投影矩阵M_l中的元素，m_rij为CCD摄像机Cam_R投影矩阵M_r中的元素，i＝1,2,3，j＝1,2,3,4；

第八步，根据世界坐标系中每一个场景点的实际坐标用下式进行障碍物区域筛选，得到筛选后的所有场景点的实际坐标(X_W′,Y_W′,Z_W′)；

其中，Des_th为探测距离阈值；

第九步，根据下式建立障碍物信息图像IMG，存储并在计算机屏幕上显示；

$\left\{\begin{array}{c}{x}_{\mathrm{img}}={X_W}^{\&prime;}\\ y_{\mathrm{img}}={Y_W}^{\&prime;}\\ I(x_{\mathrm{img}},y_{\mathrm{img}})=255\&times;({Z_W}^{\&prime;}/D{\mathrm{es}}_{\mathrm{th}})\end{array}\right.$

其中，(x_img,y_img)为障碍物信息图像IMG中的像素点坐标，I(x_img,y_img)为障碍物信息图像IMG中对应像素点(x_img,y_img)的灰度值；

第十步，计算障碍物信息图像IMG中满足I(x_img,y_img)≠0的区域面积S，若满足S≥S₀,则进入第十一步，若满足S＜S₀，则进入第十二步，其中，S₀为障碍物正向投影面积阈值；

第十一步，计算障碍物信息图像IMG中满足I(x_img,y_img)≠0区域中所有像素点坐标(x_img,y_img)所对应的Z_W′坐标值的平均值Z_average，将Z_average与障碍物报警距离阈值Z_th进行比较：若满足Z_average＜Z_th时，向蜂鸣器发送障碍物报警指令，若满足Z_average≥Z_th时，则直接进入第十二步，其中，Z_th＝V_MOV·time_th，V_MOV为车辆当前行驶速度，time_th为车辆当前位置至与障碍物发生碰撞所需的时间阈值；

第十二步，查询关机指令，若未接到关机指令，返回第二步；若接到关机指令，结束。