CN102509067B - 一种车道边界和主车方位检测方法 - Google Patents

Abstract

本发明公开了一种车道边界和主车方位检测方法，首先设置最佳车载摄像机外参数；然后根据车道图像像素梯度幅值自适应设置边缘阈值，并提取边缘点、计算边缘点方向；再根据车道边界投影模型、边缘点及其方向利用蚁群优化搜索定位车道边界；最后计算车道平面线形参数值和主车方位；本发明的检测方法能够快速有效搜索定位车道边界，能够适应各种车道线形以及天气和光照变化，并能够测算出车道平面线形参数值和主车在车道中的偏向角和位置。

Description

一种车道边界和主车方位检测方法

技术领域

本发明涉及一种能够识别复杂道路环境中的车辆所在车道边界和检测主车在车道中的位置和方向的方法，属于智能车辆技术领域。 

背景技术

在智能车辆系统中，车道边界和主车方位检测方法是智能车辆预警和控制系统的重要组成部分，主要应用于车道偏离报警、车辆智能巡航控制、车辆横向控制、车辆自主驾驶等。 

在现有的车道检测方法中，主要利用直线、二次多项式、样条曲线、圆弧等车道曲线模型检测车道边界，这些模型只能部分或近似地反映车道平面线形，而且无法测算车道平面线形参数值，如车道曲率、车道曲率变化率等，这些数据对车辆的自主驾驶、智能巡航的控制和决策是非常重要的。另一方面，车辆在行驶过程中道路的状况是复杂和多变的，车道检测需要能够适应各种车道标线、道路光照以及复杂环境，在复杂和恶劣条件下的车道检测是非常困难的，现有的车道检测方法还无法很好解决，需要进一步提高检测的可靠性和精度。另外，现有的车道检测方法在检测车道的同时还无法精确测算主车在车道中的位置和方向。 

经对现有技术文献的检索发现，专利号为200480023596.1的发明专利“路面行驶车道检测装置”包括边缘点检测装置、边缘直方图作成装置、方块标记线判定装置、车道边界边缘检测装置和车道边界线位置确定装置；该技术从图像中的轮廓线检测出边缘点，并相对于边缘点的水平成分计算垂直方向边缘直方图，然后根据垂直方向边缘直方图的分布周期性与正边缘以及负边缘的分布组合，判定是否存在方块状的标记线，再检测出存在于方块状标记线外侧的垂直方向边缘点，最后将与垂直方向边缘点相吻合的曲线位置确定为行驶车道的边界线。该专利的缺点在于：对摄像机的高度和姿态角未作最佳设置，也未给出具体的设置范围，在车道图像采集中，摄像机无法最大限度的采集车道信息，这会影响车道检测的准确度；对于存在阴影的道路，在轮廓线检测中会检测出阴影边缘点，这将严重影响车道边界检测的可靠性；另外，该技术还无法有效检测圆弧形和回旋曲线形车道的边界，并且无法检测或测算主车在车道中的位置和方向。 

发明内容

为了克服现有技术检测准确度和可靠性不高的不足，本发明提供一种车道边界和主车方位检测方法，该方法不仅能够提高车道信息采集的效率和车道检测的效率，而且能够在道路有阴影、车辆、光线变化等路况环境下检测主车所在车道边界和主车方位。 

本发明解决其技术问题所采用的技术方案包括以下步骤： 

步骤1：设置最佳车载摄像机外参数，将摄像机架设于车辆顶部中轴线前端位置，摄像机光轴与车辆中轴线位于同一垂直面，定义摄像机的俯仰角α向下为正、面向摄像机前方滚动角γ顺时针方向为正，设置摄像机的俯仰角α＝arctan(r_Od_y/f_c)、摄像机的滚动角γ＝0、摄像机距离路面的高度 

其中f_c为摄像机焦距，d_x、d_y分别为摄像机拍摄的图像像素点在水平和垂直方向上的物理尺寸，c_O和r_O分别为图像中心点的像素横坐标和纵坐标，W_lane为车道宽度； 

步骤2：根据摄像机拍摄的车道图像像素梯度幅值自适应设置边缘阈值，并提取边缘点、计算边缘点方向，包括以下步骤： 

步骤2.1：计算车道图像除第一行、最后一行、第一列、最后一列像素点外的其它各个像素点水平方向和垂直方向上的梯度幅值 

G_x(c，r)＝f(c+1，r-1)+2f(c+1，r)+f(c+1，r+1) 

         -f(c-1，r-1)-2f(c-1，r)-f(c-1，r+1) 

G_y(c，r)＝f(c-1，r+1)+2f(c，r+1)+f(c+1，r+1) 

         -f(c-1，r-1)-2f(c，r-1)-f(c+1，r-1) 

其中c、r分别表示像素点的横坐标和纵坐标，G_x(c，r)、G_y(c，r)分别表示像素点(c，r)水平方向和垂直方向的梯度幅值，f(c，r)表示像素点(c，r)的像素值，然后计算车道图像像素点的梯度幅值G_m(c，r)＝|G_x(c，r)|+|G_y(c，r)|； 

步骤2.2：计算提取边缘像素点的梯度阈值 

其中H、W分别表示车道图像的高和宽，w_G为系数，其取值范围为0.1≤w_G≤1； 

步骤2.3：将图像像素点(c，r)的梯度幅值G_m(c，r)与梯度阈值G_mth相比较，其中 c＝1，2，...，W-2，r＝1，2，...，H-2，若G_m(c，r)≥G_mth，则将该像素点标记为边缘像素点，若G_m(c，r)＜G_mth，则将该像素点标记为非边缘像素点，然后计算边缘像素点的边缘方向θ(c_edge，r_edge)＝arctan[G_y(c_edge，r_edge)/G_x(c_edge，r_edge)]，其中c_edge、r_edge分别表示边缘像素点的横坐标和纵坐标； 

步骤3：根据车道边界投影模型、边缘点及其方向利用蚁群优化搜索定位车道边界，包括以下步骤： 

步骤3.1：设置车道模型，将车道左、右边界投影模型，即图像中的车道边界曲线方程，也称为车道左、右边界线的投影曲线方程设置为 

$B_1{r}_{\mathrm{lane}}+B_3+B_4{r}_{\mathrm{lane}}^{-1}+B_5{r}_{\mathrm{lane}}^{-2}-c_{\mathrm{lane},L}=0$

$B_2{r}_{\mathrm{lane}}+B_3+B_4{r}_{\mathrm{lane}}^{-1}+B_5{r}_{\mathrm{lane}}^{-2}-c_{\mathrm{lane},R}=0$

其中r_lane表示车道边界曲线上像素点的纵坐标，c_lane，L和c_lane，R分别表示车道左、右边界曲线上像素点的横坐标，B₁、B₂、B₃、B₄、B₅为车道边界投影曲线的参数，并定义B＝(B₁，B₂，B₃，B₄，B₅)^T为车道边界投影模型参数向量，其取值范围为(-5，0，0，-2000，-3000)^T＜B＜(0，5，300，2000，3000)^T； 

步骤3.2：蚁群搜索初始化，初始化迭代计数器t＝0、最大迭代次数Iter_max，其取值范围为10≤Iter_max≤200，并将搜索空间B_min＜B＜B_max的每一维空间平均划分为M个子区间，分别标记为1，...，M，M的取值范围为5≤M≤200，其中 

B_min＝(B_1，min，B_2，min，B_3，min，B_4，min，B_5，min)＝(-5，0，0，-2000，-3000)^T

B_max＝(B_1，max，B_2，max，B_3，max，B_4，max，B_5，max)＝(0，5，300，2000，3000)^T

子区间的半径为 

$\mathrm{\Ω}=({\mathrm{\Ω}}_1,...,{\mathrm{\Ω}}_5)=\frac{1}{2}(\frac{R_{1,\max }-B_{1,\min }}{M},...,\frac{B_{5,\max }-B_{5,\min }}{M})$

子区间的边界值为2(m-1)Ω_n和2mΩ_n，其中m＝1，...，M，n＝1，...，5，对应于搜索空间的每一维子空间建立记录表[τ_nm(t)，Δτ_nm(t)，F_nm，max(t)，B_nm，max(t)]，其中t表示迭代次数，τ_nm(t)表示第n维第m子区间蚂蚁信息素的残留，Δτ_nm(t)表示当前第n维第m子区间的信息素增量即所有当前蚂蚁在第n维第m子区间留下的信息素，F_nm，max(t)表示 对应子区间的历史最大启发函数值即车道边界曲线置信度，B_nm，max(t)表示相应的路径即车道边界投影模型参数值，初始化τ_nm(t)，其取值范围为0.1≤τ_nm(t)≤30，初始化Δτ_nm(t)＝0，B_nm，max(t)＝(2m-1)Ω_n，F_nm，max(t)＝0，其中n＝1，...，5、m＝1，...，M，并初始化信息素挥发系数ρ，其取值范围为0＜ρ＜1；初始化蚁群规模即蚂蚁数量S，其取值范围为5≤S≤200且S≤M；定义 

分别表示第i只蚂蚁经过的子区间、路径即车道模型参数向量值，定义 

表示期望信息即第i只蚂蚁选择第n维第m子区间的期望值，初始化蚂蚁经过的路径Bⁱ(t)＝[B_1i，max(t)，...，B_5i，max(t)]、子区间Uⁱ(t)＝(i，i，i，i，i)、期望信息 

其中i＝1，...，S、n＝1，...，5、m＝1，...，M；然后计算启发函数值即车道边界曲线置信度F[Bⁱ(t)]，更新F_ni，max(t)＝F[Bⁱ(t)]，其中n＝1，...，5，并比较所有蚂蚁的启发函数值，进而初始化蚁群启发函数的历史最小值F^min(t)＝min{F[Bⁱ(t)]|i＝1，...，S}、历史最大值F^max(t)＝max{F[Bⁱ(t)]|i＝1，...，S}、对应历史最大值的最优路径B^max(t)及其子区间U^max(t)，并计算当前蚁群启发函数值的平均值 初始设定局域搜索的正态分布标准差σ＝(σ₁，...，σ₅)的值、蚂蚁选择概率的信息启发式因子α和期望启发式因子β的值，其取值范围分别为0.2Ω≤σ≤3Ω、0＜α≤5、0＜β≤5；进一步初始设置Uⁱ(t+1)＝Uⁱ(t)，其中i＝1，...，S；所述启发函数即车道边界曲线置信度F[Bⁱ(t)]的计算方法为 

其中Λ表示车道边界曲线的邻域，其半径的取值范围为[4，20]，μ_W、 

为常数，其取值范围分别为 $0.8\&times;\frac{d_y{W}_{\mathrm{lane}}\cos \mathrm{\&alpha;}}{d_x{h}_c}\&le;{\mathrm{\&mu;}}_W\&le;1.2\&times;\frac{d_y{W}_{\mathrm{lane}}\cos \mathrm{\&alpha;}}{d_x{h}_c},$ $0<{\mathrm{\&sigma;}}_W^2<1,$ $0<{\mathrm{\&sigma;}}_D^2\&le;100,$

D(c_edge，r_edge)表示边缘点(c_edge，r_edge)到车道边界曲线的距离，其计算方法为 

其中 

$D_L(c_{\mathrm{edge}},r_{\mathrm{edge}})=\frac{|B_1^i\left(t\right)r_{\mathrm{edge}}+B_3^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-1}+B_5^i\left(t\right)r_{\mathrm{edge}}^{-2}-c_{\mathrm{edge}}|}{\sqrt{{[-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+{2B}_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]}^2+1}}$

$D_R(c_{\mathrm{edge}},r_{\mathrm{edge}})=\frac{|B_2^i\left(t\right)r_{\mathrm{edge}}+B_3^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-1}+B_5^i\left(t\right)r_{\mathrm{edge}}^{-2}-c_{\mathrm{edge}}|}{\sqrt{{[-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+{2B}_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]}^2+1}}$

表示边缘点的边缘方向θ(c_edge，r_edge)与车道边界曲线的夹角，其计算方法为 

其中 

${\mathrm{\&psi;}}_L=\arctan [-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+2B_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]$

${\mathrm{\&psi;}}_R=\arctan [-B_2^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+2B_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]$

步骤3.3：将迭代计数器t加1； 

步骤3.4：蚁群局域搜索，根据蚂蚁所在区域Uⁱ(t)按高斯分布 

$\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_n}\exp \{-\frac{{[B_n^i\left(t\right)-B_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1)]}^2}{2{\mathrm{\&sigma;}}_n^2}\}$

随机产生新的路径 

其中i＝1，...，S、n＝1，...，5，将新路径Bⁱ(t)与Uⁱ(t)子区间的边界值相比较，若超出子区间边界则更新蚂蚁所在的子区间Uⁱ(t)，若超出搜索空间边界值则将Bⁱ(t)设为相应边界值，计算启发函数值F[Bⁱ(t)]，其中i＝1，...，S，再更新启发函数历史最大值F^max(t)＝max{F^max(t-1)，F[Bⁱ(t)]|i＝1，...，S}、F^max(t)对应的最优路径B^max(t)和子区间U^max(t)、当前蚁群的启发函数最小值F^min(t)＝min{F[Bⁱ(t)]|i＝1，...，S}，并计算更新当前蚁群的启发函数平均值 

步骤3.5：更新信息素表，初始化信息素增量Δτ_nm(t)＝0，其中n＝1，...，5、m＝1，...，M，计算当前蚂蚁留下的信息素 

${\mathrm{\&Delta;\&tau;}}_{{\mathrm{nU}}_n^i\left(t\right)}^i\left(t\right)=\exp \frac{F\left[B^i\left(t\right)\right]-\stackrel{\&OverBar;}{F}\left(t\right)}{F^{\max }\left(t\right)-F^{\min }\left(t\right)}$

其中i＝1，...，S、n＝1，...，5，更新信息素增量 

和信息素残留τ_nm(t)＝(1-ρ)τ_nm(t-1)+Δτ_nm(t)，其中n＝1，...，5、m＝1，...，M；然后将蚂蚁的启发函数值F[Bⁱ(t)]与 

相比较，若 $F\left[B^i\left(t\right)\right]>F_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1)$ 则更新  $F_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=F\left[B^i\left(t\right)\right],$ $B_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=B_n^i\left(t\right),$ 若 $F\left[B^i\left(t\right)\right]\&le;F_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1)$ 则更新  $F_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=F_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1),$ $B_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=B_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1),$ 其中i＝1，...，S、n＝1，...，5； 

步骤3.6：将迭代次数t和最大迭代次数Iter_max相比较，若t＞Iter_max，则跳转到步骤3.8，若t≤Iter_max，则执行下一步骤3.7； 

步骤3.7：蚁群全局搜索，计算蚂蚁的期望信息 ${\mathrm{\&eta;}}_{\mathrm{nm}}^i\left(t\right)=\exp \frac{f_{\mathrm{nm},\max }\left(t\right)-F\left[B^i\left(t\right)\right]}{F^{\max }\left(t\right)-F^{\min }\left(t\right)}$ 和蚂蚁选择路径的选择概率 $p_{\mathrm{nm}}^i\left(t\right)=\frac{{\left[{\mathrm{\&tau;}}_{\mathrm{nm}}\left(t\right)\right]}^{\mathrm{\&alpha;}}{\left[{\mathrm{\&eta;}}_{\mathrm{nm}}^i\left(t\right)\right]}^{\mathrm{\&beta;}}}{\underset{k=1}{\overset{M}{\mathrm{\&Sigma;}}}{\left[{\mathrm{\&tau;}}_{\mathrm{nk}}\left(t\right)\right]}^{\mathrm{\&alpha;}}{\left[{\mathrm{\&eta;}}_{\mathrm{nk}}^i\left(t\right)\right]}^{\mathrm{\&beta;}}},$ 其中i＝1，...，S、n＝1，...，5、m＝1，...，M，分别对每只蚂蚁产生(0，1)均值分布上的随机数Rand，更新蚂蚁的路径子区间 

其中i＝1，...，S、n＝1，...，5，然后跳转到步骤3.3； 

步骤3.8：输出车道边界曲线，根据最优路径B^max(t)以及车道边界投影模型，在图像上画出检测到的车道左、右边界曲线 

$B_1^{\max }\left(t\right)r+B_2^{\max }\left(t\right)+B_4^{\max }\left(t\right)r^{-1}+B_5^{\max }\left(t\right)r^{-2}-c=0$

$B_2^{\max }\left(t\right)r+B_3^{\max }\left(t\right)+B_4^{\max }\left(t\right)r^{-1}+B_5^{\max }\left(t\right)r^{-2}-c=0$

步骤4：计算车道平面线形参数值和主车方位，计算主车所在点处的车道曲率 

$C_0=[B_4^{\max }\left(t\right)f_c+\frac{3}{2}{B}_5^{\max }\left(t\right)Q_1]Q_3$

和主车前方车道曲率的变化率 

$C_1=3B_5^{\max }\left(t\right)Q_2{Q}_3\cos \mathrm{\&alpha;}$

其中Q₁＝d_y sin 2α、 $Q_2=\frac{d_y\cos \mathrm{\&alpha;}}{h_c},$ $Q_3=\frac{{2d}_x{\cos }^2\mathrm{\&alpha;}}{f_c^3}{Q}_2;$ 计算主车在车道中的偏向角 

$\mathrm{\&beta;}=\frac{-d_x\cos \mathrm{\&alpha;}}{f_c^3}[(B_3^{\max }\left(t\right)-c_o)f_c^2+B_4^{\max }\left(t\right)f_c{Q}_1+\frac{3}{4}{B}_5^{\max }\left(t\right)Q_1^2]$

和车辆偏离车道中心线的距离 

本发明的有益效果是：本发明利用了改进的连续域蚁群优化并简化了像素点梯度幅值的计算和边缘点到车道边界曲线距离的计算，能够快速有效搜索定位车道边界和测算主车在车道中的方位，并且能够适应车道有阴影、车辆、光线变化等路况环境，抗干扰能力强，具有较强的鲁棒性；本发明中摄像机的高度和俯仰角的设置能够使摄像机最大限度的采集到车道标线信息，有效提高了车道图像中车道标线信息的含量，减少了车道检测过程中对无关信息的存储和处理；本发明简化的车道边界投影模型符合结构化车道设计规范规定的车道平面线形在图像中的投影，能够计算出车道平面线形的参数值以及主车在车道中的位置和方向。 

下面结合附图和实施例对本发明进一步说明。 

附图说明

图1是本发明一种车道边界和主车方位检测方法实施例的系统结构示意图； 

图2是本发明实施例所述的一种车道边界和主车方位检测方法的总流程图； 

图3A是本发明实施例所述的摄像机外参数最佳设置时的侧视图，以及图3B是本发明实施例所述的摄像机外参数最佳设置时的顶视图； 

图4是本发明实施例的车载摄像机外参数最佳设置时采集的车道图像示意图。 

图5是本发明实施例所述的根据车道边界投影模型、边缘点及其方向利用蚁群优化搜索定位车道边界的方法流程图； 

具体实施方式

本发明将一种车道边界和主车方位检测方法应用在车道边界和主车方位检测装置中，该装置的系统结构示意图如图1所示，其具体实施方式如下： 

摄像机：采用CCD(Charged Coupled Device电荷耦合器件)图像传感器摄像机，与模数转换器的数据输入接口连接，拍摄车辆前方道路，并将拍摄的模拟图像信号发送到模数转换器； 

模数转换器：其数据输入接口与摄像机的数据输出接口连接，数据输出接口与输入接口电路的数据输入接口连接，控制接口与系统控制总线连接，该组件将摄像机的模拟图像信号转换成数字图像信号并传输给输入接口电路； 

输入接口电路，连接模数转换器和系统数据总线的适配电路，其数据输入接口与模数转换器的数据输出接口连接，数据输出接口与系统数据总线连接，控制接口与系统控制总线连接，该组件对道路图像数据进行输入和缓冲； 

存储器，其数据接口与系统数据总线连接，控制接口与系统控制总线连接，该组件存储系统控制指令和车道图像； 

车道数字图像处理器，其数据接口与系统数据总线连接，控制接口与系统控制总线连接，该组件从存储器中读出车道图像并对其进行车道检测，然后将检测结果通过系统数据总线传输给输出接口电路7； 

系统控制器，其数据输入接口与系统数据总线连接，控制接口与系统控制总线连接，该组件从存储器中将控制指令取出，经分析后根据指令分析结果发送相应的操作控制信号，控制系统各部件协调工作； 

输出接口电路，连接系统数据总线与输出设备的适配电路，其数据输入接口与系统数据总线连接，数据输出接口与输出设备连接，控制接口与系统控制总线连接，该组件输出和缓冲车道检测结果； 

系统数据总线，由一组导线和相关的控制、驱动电路组成，与各部件的数据接口连接，传输数据信息； 

系统控制总线，由一组导线和相关的控制、驱动电路组成，与各部件的控制接口连接，传送控制信号、时序信号和状态信息； 

电源电路，将外部电源提供的电能转换成符合各部件电压和频率要求的电能，为各部件提供电能； 

利用本发明实施例，其总流程图如图2所示，车辆沿车道行驶，检测装置实时采集 车辆前方道路图像，并对道路图像进行车道边界和主车方位检测，其具体实施方式如下： 

步骤1：设置最佳车载摄像机外参数，如图3A和图3B所示，X_cY_cZ_c为摄像机直角坐标系，其中Z_c为摄像机光轴，摄像机架设于车辆项部中轴线前端位置，摄像机光轴Z_c与车辆中轴线位于同一垂直面，定义摄像机的俯仰角α向下为正、面向摄像机前方滚动角γ顺时针方向为正，设置摄像机的俯仰角α＝arctan(r_Od_y/f_c)、摄像机的滚动角γ＝0、摄像机距离路面高度 

其中f_c为摄像机焦距，d_x、d_y分别为摄像机拍摄的图像像素点在水平和垂直方向上的物理尺寸，c_O和r_O分别为图像中心点的像素横坐标和纵坐标，W_lane为车道宽度；摄像机的俯仰角、滚动角、高度设置的具体原理为：在道路图像采集过程中，摄像机的视角、成像面积的大小是不变的，车道宽度的变化也很小，为了最大限度的获取车道边界信息，摄像机的高度、俯仰角和滚动角需要设置在最佳位置，如图4所示，当图像中的地平线为r＝0，并且车道左、右边界线穿过图像左下角和右下角时，图像中的车道边界标线最长，根据像素坐标系中的车道边界线方程，此时摄像机的最佳俯仰角、最佳滚动角和最佳高度分别为α_opt＝arctan(r_Od_y/f_c)、γ＝0和 

因此，本发明实施例在车道图像的采集中可提高图像中车道信息的含量，进而提高了车道和主车方位检测的精确性； 

步骤2：根据摄像机拍摄的车道图像像素梯度幅值自适应设置边缘阈值，并提取边缘点、计算边缘点方向；具体地，所述步骤2包括以下步骤： 

步骤2.1：计算车道图像除第一行、最后一行、第一列、最后一列像素点外的其它各个像素点水平方向和垂直方向上的梯度幅值 

G_x(c，r)＝f(c+1，r-1)+2f(c+1，r)+f(c+1，r+1) 

         -f(c-1，r-1)-2f(c-1，r)-f(c-1，r+1) 

G_y(c，r)＝f(c-1，r+1)+2f(c，r+1)+f(c+1，r+1) 

         -f(c-1，r-1)-2f(c，r-1)-f(c+1，r-1) 

其中c、r分别表示像素点的横坐标和纵坐标，G_x(c，r)、G_y(c，r)分别表示像素点(c，r)水平方向和垂直方向的梯度幅值，f(c，r)表示像素点(c，r)的像素值，然后计算车道 图像像素点的梯度幅值G_m(c，r)＝|G_x(c，r)|+|G_y(c，r)|，本发明中用|G_x(c，r)|+|G_y(c，r)|替代 

近似计算像素点的梯度幅值可以减少计算量并且保持梯度的相对变化，提高计算速度； 

步骤2.2：计算提取边缘像素点的梯度阈值 

其中H、W分别表示图像的高和宽，具体地H＝240、W＝320，w_G为系数，其取值范围为0.1≤w_G≤1，具体地w_G＝0.6，本发明实施例的边缘阈值可根据道路图像的明暗自适应调整，能适应光线变化，进而提高检测的鲁棒性； 

步骤2.3：将图像像素点(c，r)的梯度幅值G_m(c，r)与梯度阈值G_mth相比较，其中c＝1，2，...，W-2，r＝1，2，...，H-2，若G_m(c，r)≥G_mth，则将该像素点标记为边缘像素点，若G_m(c，r)＜G_mth，则将该像素点标记为非边缘像素点，然后计算边缘点的边缘方向θ(c_edge，r_edge)＝arctan[G_y(c_edge，r_edge)/G_x(c_edge，r_edge)]，其中c_edge、r_edge分别表示边缘像素点的横坐标和纵坐标； 

步骤3：根据车道边界投影模型、边缘点及其方向利用蚁群优化搜索定位车道边界；具体地，所述步骤3的方法流程图如图5所示，包括以下步骤： 

步骤3.1：设置车道模型，将车道左、右边界投影模型即图像中的车道边界曲线方程，也称为车道左、右边界线的投影曲线方程设置为 

$B_1{r}_{\mathrm{lane}}+B_3+B_4{r}_{\mathrm{lane}}^{-1}+B_5{r}_{\mathrm{lane}}^{-2}-c_{\mathrm{lane},L}=0$

$B_2{r}_{\mathrm{lane}}+B_3+B_4{r}_{\mathrm{lane}}^{-1}+B_5{r}_{\mathrm{lane}}^{-2}-c_{\mathrm{lane},R}=0$

其中r_lane表示车道边界曲线上像素点的纵坐标，c_lane，L和c_lane，R分别表示车道左、右边界曲线上像素点的横坐标，B₁、B₂、B₃、B₄、B₅为车道边界投影曲线的参数，并定义B＝(B₁，B₂，B₃，B₄，B₅)^T为车道边界投影模型参数向量，其取值范围为(-5，0，0，-2000，-3000)^T＜B＜(0，5，300，2000，3000)^T；根据公路平面路线设计规范，公路平面线形由直线、圆曲线、回旋线三种线形组成，车道曲率C随车道长度l的变化关系可表示为C(l)＝C₀+C₁l，其中C₀为主车所在处的车道曲率，C₁为车道曲率的变化率，将上述变化关系式通过坐标系变换并考虑摄像机的俯仰角α＝arctan(r_Od_y/f_c)， 可得所述的车道左、右边界投影模型即图像中的车道边界曲线方程，由于本发明实施例摄像机外参数的最佳设置，车道左、右边界投影模型得到了简化； 

步骤3.2：蚁群搜索初始化，初始化迭代计数器t＝0、最大迭代次数Iter_max，其取值范围为10≤Iter_max≤200，具体地Iter_max＝50，并将搜索空间B_min＜B＜B_max的每一维空间平均划分为M个子区间，分别标记为1，...，M，M的取值范围为5≤M≤200，具体地M＝50，其中 

B_min＝(B_1，min，B_2，min，B_3，min，B_4，min，B_5，min)＝(-5，0，0，-2000，-3000)^T

B_max＝(B_1，max，B_2，max，B_3，max，B_4，max，B_5，max)＝(0，5，300，2000，3000)^T

子区间的半径为 

$\mathrm{\&Omega;}=({\mathrm{\&Omega;}}_1,...,{\mathrm{\&Omega;}}_5)=\frac{1}{2}(\frac{R_{1,\max }-B_{1,\min }}{M},...,\frac{B_{5,\max }-B_{5,\min }}{M})$

子区间的边界值为2(m-1)Ω_n和2mΩ_n，其中m＝1，...，M，n＝1，...，5，对应于搜索空间的每一维子空间建立记录表[τ_nm(t)，Δτ_nm(t)，F_nm，max(t)，B_nm，max(t)]，其中t表示迭代次数，τ_nm(t)表示第n维第m子区间蚂蚁信息素的残留，Δτ_nm(t)表示当前第n维第m子区间的信息素增量即所有当前蚂蚁在第n维第m子区间留下的信息素，F_nm，max(t)表示对应子区间的历史最大启发函数值即车道边界曲线置信度，B_nm，max(t)表示相应的路径即车道边界投影模型参数值，初始化τ_nm(t)，其取值范围为0.1≤τ_nm(t)≤30，具体地τ_nm(t)＝1，初始化Δτ_nm(t)＝0，B_nm，max(t)＝(2m-1)Ω_n，F_nm，max(t)＝0，其中n＝1，...，5、m＝1，...，M，并初始化信息素挥发系数ρ，其取值范围为0＜ρ＜1，具体地ρ＝0.6；初始化蚁群规模即蚂蚁数量S，其取值范围为5≤S≤200且S≤M，具体地S＝30；定义  $U^i\left(t\right)=[U_1^i\left(t\right),...,U_5^i\left(t\right)],$ $B^i\left(t\right)=[B_1^i\left(t\right),...,B_5^i\left(t\right)]$ 分别表示第i只蚂蚁经过的子区间、路径即车道模型参数向量值，定义 

表示期望信息即第i只蚂蚁选择第n维第m子区间的期望值，初始化蚂蚁经过的路径Bⁱ(0)＝[B_1i，max(0)，...，B_5i，max(0)]、子区间Uⁱ(0)＝(i，i，i，i，i)、期望信息 

其中i＝1，...，S、n＝1，...，5、m＝1，...，M；然后计算启发函数值即车道边界曲线置信度F[Bⁱ(t)]，更新F_ni，max(t)＝F[Bⁱ(t)]，其中n＝1，...，5，并比较所有蚂蚁的启发函数值，进而初始化蚁群启发函数的历史最小值 F^min(t)＝min{F[Bⁱ(t)]|i＝1，...，S}、历史最大值F^max(t)＝max{F[Bⁱ(t)]|i＝1，...，S}、对应历史最大值的最优路径B^max(t)及其子区间U^max(t)，并计算当前蚁群启发函数值的平均值 

初始设定局域搜索的正态分布标准差σ＝(σ₁，...，σ₅)的值、蚂蚁选择概率的信息启发式因子α和期望启发式因子β的值，其取值范围分别为0.2Ω≤σ≤3Ω、0＜α≤5、0＜β≤5，具体地σ＝2Ω、α＝1、β＝2；进一步初始设置Uⁱ(t+1)＝Uⁱ(t)，其中i＝1，...，S；所述启发函数即车道边界曲线置信度F[Bⁱ(t)]的计算方法为 

其中Λ表示车道边界曲线的邻域，其半径的取值范围为[4，20]，具体地邻域半径为10，μ_W、 为常数，其取值范围分别为 $0.8\&times;\frac{d_y{W}_{\mathrm{lane}}\cos \mathrm{\&alpha;}}{d_x{h}_c}\&le;{\mathrm{\&mu;}}_W\&le;1.2\&times;\frac{d_y{W}_{\mathrm{lane}}\cos \mathrm{\&alpha;}}{d_x{h}_c},$ $0<{\mathrm{\&sigma;}}_W^2<1,$ $0<{\mathrm{\&sigma;}}_D^2\&le;100,$

具体地μ_W＝1.3、 ${\mathrm{\&sigma;}}_W^2=0.6,$ ${\mathrm{\&sigma;}}_D^2=25,$

D(c_edge，r_edge)表示边缘点(c_edge，r_edge)到车道边界曲线的距离，其计算方法为 

其中 

$D_L(c_{\mathrm{edge}},r_{\mathrm{edge}})=\frac{|B_1^i\left(t\right)r_{\mathrm{edge}}+B_3^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-1}+B_5^i\left(t\right)r_{\mathrm{edge}}^{-2}-c_{\mathrm{edge}}|}{\sqrt{{[-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+{2B}_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]}^2+1}}$

$D_R(c_{\mathrm{edge}},r_{\mathrm{edge}})=\frac{|B_2^i\left(t\right)r_{\mathrm{edge}}+B_3^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-1}+B_5^i\left(t\right)r_{\mathrm{edge}}^{-2}-c_{\mathrm{edge}}|}{\sqrt{{[-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+{2B}_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]}^2+1}}$

上面二式是对距离计算的简化，可减少计算量，提高计算速度， 

表示边缘点的边缘方向θ(c_edge，r_edge)与车道边界曲线的夹角，其计算方法为 

其中 

${\mathrm{\&psi;}}_L=\arctan [-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+2B_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]$

${\mathrm{\&psi;}}_R=\arctan [-B_2^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+2B_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]$

步骤3.3：将迭代计数器t加1； 

步骤3.4：蚁群局域搜索，根据蚂蚁所在区域Uⁱ(t)按高斯分布 

$\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_n}\exp \{-\frac{{[B_n^i\left(t\right)-B_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1)]}^2}{2{\mathrm{\&sigma;}}_n^2}\}$

随机产生新的路径 

其中i＝0，...，S、n＝1，...，5，将新路径Bⁱ(t)与Uⁱ(t)子区间的边界值相比较，若超出子区间边界则更新蚂蚁所在的子区间Uⁱ(t)，若超出搜索空间边界值则将Bⁱ(t)设为相应边界值，计算启发函数值F[Bⁱ(t)]，其中i＝1，...，S，再更新启发函数历史最大值F^max(t)＝max{F^max(t-1)，F[Bⁱ(t)]|i＝1，...，S}、F^max(t)对应的最优路径B^max(t)和子区间U^max(t)、当前蚁群的启发函数最小值F^min(t)＝min{F[Bⁱ(t)]|i＝1，...，S}，并计算更新当前蚁群的启发函数平均值 

步骤3.5：更新信息素表，初始化信息素增量Δτ_nm(t)＝0，其中n＝1，...，5、m＝1，...，M，计算当前蚂蚁留下的信息素 

${\mathrm{\&Delta;\&tau;}}_{{\mathrm{nU}}_n^i\left(t\right)}^i\left(t\right)=\exp \frac{F\left[B^i\left(t\right)\right]-\stackrel{\&OverBar;}{F}\left(t\right)}{F^{\max }\left(t\right)-F^{\min }\left(t\right)}$

其中i＝1，...，S、n＝1，...，5，更新信息素增量 

和信息素残留τ_nm(t)＝(1-ρ)τ_nm(t-1)+Δτ_nm(t)，其中n＝1，...，5、m＝1，...，M；然后将蚂蚁的启发函数值F[Bⁱ(t)]与 

相比较，若 $F\left[B^i\left(t\right)\right]>F_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1)$ 则更新  $F_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=F\left[B^i\left(t\right)\right],$ $B_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=B_n^i\left(t\right),$ 若 $F\left[B^i\left(t\right)\right]\&le;F_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1)$ 则更新  $F_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=F_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1),$ $B_{{\mathrm{nU}}_n^i\left(t\right),\max }\left(t\right)=B_{{\mathrm{nU}}_n^i\left(t\right),\max }(t-1),$ 其中i＝1，...，S、n＝1，...，5； 

步骤3.6：将迭代次数t和最大迭代次数Iter_max相比较，若t＞Iter_max，则跳转到步 骤3.8，若t≤Iter_max，则执行下一步骤3.7； 

步骤3.7：蚁群全局搜索，计算蚂蚁的期望信息 ${\mathrm{\&eta;}}_{\mathrm{nm}}^i\left(t\right)=\exp \frac{f_{\mathrm{nm},\max }\left(t\right)-F\left[B^i\left(t\right)\right]}{F^{\max }\left(t\right)-F^{\min }\left(t\right)}$ 和蚂蚁选择路径的选择概率 $p_{\mathrm{nm}}^i\left(t\right)=\frac{{\left[{\mathrm{\&tau;}}_{\mathrm{nm}}\left(t\right)\right]}^{\mathrm{\&alpha;}}{\left[{\mathrm{\&eta;}}_{\mathrm{nm}}^i\left(t\right)\right]}^{\mathrm{\&beta;}}}{\underset{k=1}{\overset{M}{\mathrm{\&Sigma;}}}{\left[{\mathrm{\&tau;}}_{\mathrm{nk}}\left(t\right)\right]}^{\mathrm{\&alpha;}}{\left[{\mathrm{\&eta;}}_{\mathrm{nk}}^i\left(t\right)\right]}^{\mathrm{\&beta;}}},$ 其中i＝1，...，S、n＝1，...，5、m＝1，...，M，分别对每只蚂蚁产生(0，1)均值分布上的随机数Rand，更新蚂蚁的路径子区间 

其中i＝1，...，S、n＝1，...，5，然后跳转到步骤3.3； 

步骤3.8：输出车道边界曲线，根据最优路径B^max(t)以及车道边界投影模型，在图像上画出检测到的车道左、右边界曲线 

$B_1^{\max }\left(t\right)r+B_2^{\max }\left(t\right)+B_4^{\max }\left(t\right)r^{-1}+B_5^{\max }\left(t\right)r^{-2}-c=0$

$B_2^{\max }\left(t\right)r+B_3^{\max }\left(t\right)+B_4^{\max }\left(t\right)r^{-1}+B_5^{\max }\left(t\right)r^{-2}-c=0$

步骤4：计算车道平面线形参数值和主车方位，计算主车所在处的车道曲率 

$C_0=[B_4^{\max }\left(t\right)f_c+\frac{3}{2}{B}_5^{\max }\left(t\right)Q_1]Q_3$

和主车前方车道曲率的变化率 

$C_1=3B_5^{\max }\left(t\right)Q_2{Q}_3\cos \mathrm{\&alpha;}$

其中Q₁＝d_y sin 2α、 $Q_2=\frac{d_y\cos \mathrm{\&alpha;}}{h_c},$ $Q_3=\frac{{2d}_x{\cos }^2\mathrm{\&alpha;}}{f_c^3}{Q}_2;$ 计算主车在车道中的偏向角 

$\mathrm{\&beta;}=\frac{-d_x\cos \mathrm{\&alpha;}}{f_c^3}[(B_3^{\max }\left(t\right)-c_o)f_c^2+B_4^{\max }\left(t\right)f_c{Q}_1+\frac{3}{4}{B}_5^{\max }\left(t\right)Q_1^2]$

和车辆偏离车道中心线的距离 

车辆安装本发明方法的实施例装置后，启动本发明实施例装置即进入工作状态。 如前所述，本发明的一种车道边界和主车方位检测方法设置了摄像机的最佳俯仰角、滚动角和高度，简化了车道边界投影模型、像素点梯度幅值计算以及边缘点到车道边界曲线距离的计算，利用改进的连续域蚁群优化，可以快速有效搜索定位车道边界，有效减小阴影、车辆、光线对车道边界检测的影响，并能够测算出主车在车道中的偏向角和位置，可应用于智能车辆的预警和控制系统。 

Claims (1)

1.一种车道边界和主车方位检测方法，其特征在于包括下述步骤：

步骤1：设置最佳车载摄像机外参数，将摄像机架设于车辆顶部中轴线前端位置，摄像机光轴与车辆中轴线位于同一垂直面，定义摄像机的俯仰角α向下为正、面向摄像机前方滚动角γ顺时针方向为正，设置摄像机的俯仰角α＝arctan(r_Od_y/f_c)、摄像机的滚动角γ＝0、摄像机距离路面的高度其中f_c为摄像机焦距，d_x、d_y分别为摄像机拍摄的图像像素点在水平和垂直方向上的物理尺寸，c_O和r_O分别为图像中心点的像素横坐标和纵坐标，W_lane为车道宽度；

步骤2：根据摄像机拍摄的车道图像像素梯度幅值自适应设置边缘阈值，并提取边缘点、计算边缘点方向，包括以下步骤：

步骤2.1：计算车道图像除第一行、最后一行、第一列、最后一列像素点外的其它各个像素点水平方向和垂直方向上的梯度幅值

G_x(c,r)＝f(c+1,r-1)+2f(c+1,r)+f(c+1,r+1)

-f(c-1,r-1)-2f(c-1,r)-f(c-1,r+1)

G_y(c,r)＝f(c-1,r+1)+2f(c,r+1)+f(c+1,r+1)

-f(c-1,r-1)-2f(c,r-1)-f(c+1,r-1)

其中c、r分别表示像素点的横坐标和纵坐标，G_x(c,r)、G_y(c,r)分别表示像素点(c,r)水平方向和垂直方向的梯度幅值，f(c,r)表示像素点(c,r)的像素值，然后计算车道图像像素点的梯度幅值G_m(c,r)＝G_x(c,r)+G_y(c,r)；

步骤2.2：计算提取边缘像素点的梯度阈值

其中H、W分别表示车道图像的高和宽，w_G为系数，其取值范围为0.1≤w_G≤1；

步骤2.3：将图像像素点(c,r)的梯度幅值G_m(c,r)与梯度阈值G_mth相比较，其中c＝1,2，...，W-,，r＝1,2，...，H-2，若G_m(c,r)≥G_mth，则将该像素点标记为边缘像素点，若G_m(c,r)＜G_mth，则将该像素点标记为非边缘像素点，然后计算边缘像素点的边缘方向θ(c_edge,r_edge)＝arctan[G_y(c_edge,r_edge)/G_x(c_edge,r_edge)]，其中c_edge、r_edge分别表示边缘像素点的横坐标和纵坐标；

步骤3：根据车道边界投影模型、边缘点及其方向利用蚁群优化搜索定位车道边界，包括以下步骤：

步骤3.1：设置车道模型，将车道左、右边界线的投影曲线方程设置为

$B_1{r}_{\mathrm{lane}}+B_3+B_4{r}_{\mathrm{lane}}^{-1}+B_{5}B{r}_{\mathrm{lane}}^{-2}-c_{\mathrm{lane},L}=0$

$B_2{r}_{\mathrm{lane}}+B_3+B_4{r}_{\mathrm{lane}}^{-1}+B_5{r}_{\mathrm{lane}}^{-2}-c_{\mathrm{lane},R}=0$

其中r_lane表示车道边界曲线上像素点的纵坐标，c_lane,L和c_lane,R分别表示车道左、右边界曲线上像素点的横坐标，B₁、B₂、B₃、B₄、B₅为车道边界投影曲线的参数，并定义B＝(B₁,B₂,B₃,B₄,B₅)^T为车道边界投影模型参数向量，其取值范围为(-5,0,0,-2000,-3000)^T＜B＜(0,5,300,2000,3000)^T；

步骤3.2：蚁群搜索初始化，初始化迭代计数器t＝0、最大迭代次数Iter_max，其取值范围为10≤Iter_max≤200，并将搜索空间B_min＜B＜B_max的每一维空间平均划分为M个子区间，分别标记为1，...，M，,的取值范围为5≤M≤200，其中

B_min＝(B_1,min,B_2,min,B_3,min,B_4,min,B_5,min)＝(-5,0,0,-2000,-3000)^T

B_max＝(B_1,max,B_2,max,B_3,max,B_4,max,B_5,max)＝(0,5,300,2000,3000)^T

子区间的半径为

$\mathrm{\&Omega;}=({\mathrm{\&Omega;}}_1,...,{\mathrm{\&Omega;}}_5)=\frac{1}{2}(\frac{B_{1,\max }-B_{1,\min }}{M},...,\frac{B_{5,\max }-B_{5,\min }}{M})$

子区间的边界值为2(m-1)Ω_n和2mΩ_n，其中m＝1,...，M，,＝1,...，5，对应于搜索空间的每一维子空间建立记录表[τ_nm(t),Δτ_nm(t),F_nm,max(t),B_nm,max(t)]，其中t表示迭代次数，τ_nm(t)表示第n维第m子区间蚂蚁信息素的残留，Δτ_nm(t)表示当前第n维第m子区间的信息素增量即所有当前蚂蚁在第n维第m子区间留下的信息素，F_nm,max(t)表示对应子区间的历史最大启发函数值即车道边界曲线置信度，B_nm,max(t)表示相应的路径即车道边界投影模型参数值，初始化τ_nm(t)，其取值范围为0.1≤τ_nm(t)≤30，初始化Δτ_nm(t)＝0，B_nm,max(t)＝(2m-1)Ω_n，F_nm,max(t)＝0，其中n＝1,...，5、,＝1,...，M，并初始化信息素挥发系数ρ，其取值范围为0＜ρ＜1；初始化蚁群规模即蚂蚁数量S，其取值范围为5≤S≤200且S≤M；定义

分别表示第i只蚂蚁经过的子区间、路径即车道模型参数向量值，定义

表示期望信息即第i只蚂蚁选择第n维第m子区间的期望值，初始化蚂蚁经过的路径Bⁱ(t)＝[B_1i,max(t),,...，B_5i，max(t)]、子区间Uⁱ(t)＝(i,i,i,i,i)、期望信息

其中i＝1,...，S、n＝1,...，5、m＝1,...，M；然后计算启发函数值即车道边界曲线置信度F[Bⁱ(t)]，更新F_ni,max(t)＝F[Bⁱ(t)]，其中n＝1,...，5，并比较所有蚂蚁的启发函数值，进而初始化蚁群启发函数的历史最小值F^min(t)＝min{F[Bⁱ(t)]i＝1,...，S}、历史最大值F^max(t)＝max{F[Bⁱ(t)]i＝1,...，S}、对应历史最大值的最优路径B^max(t)及其子区间U^max(t)，并计算当前蚁群启发函数值的平均值

初始设定局域搜索的正态分布标准差σ＝(σ₁,...，σ₅)的值、蚂蚁选择概率的信息启发式因子α和期望启发式因子β的值，其取值范围分别为0.2Ω≤σ≤3Ω、0＜α≤5、0＜β≤5；进一步初始设置Uⁱ(t+1)＝Uⁱ(t)，其中i＝1,...，S；所述启发函数即车道边界曲线置信度F[Bⁱ(t)]的计算方法为

其中Λ表示车道边界曲线的邻域，其半径的取值范围为[4,20]，μ_W、

为常数，其取值范围分别为 $0.8\&times;\frac{d_y{W}_{\mathrm{lane}}\cos \mathrm{\&alpha;}}{d_x{h}_c}\&le;{\mathrm{\&mu;}}_W\&le;1.2\&times;\frac{d_y{W}_{\mathrm{lane}}\cos \mathrm{\&alpha;}}{d_x{h}_c},0<{\mathrm{\&sigma;}}_W^2<1,$

D(c_edge,r_edge)表示边缘点(c_edge,r_edge)到车道边界曲线的距离，其计算方法为

其中

$D_L(c_{\mathrm{edge}},r_{\mathrm{edge}})=\frac{|B_1^i\left(t\right)r_{\mathrm{edge}}+B_3^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-1}+B_5^i\left(t\right)r_{\mathrm{edge}}^{-2}-c_{\mathrm{edge}}|}{\sqrt{{[-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+2B_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]}^2+1}}$

$D_R(c_{\mathrm{edge}},r_{\mathrm{edge}})=\frac{|B_2^i\left(t\right)r_{\mathrm{edge}}+B_3^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-1}+B_5^i\left(t\right)r_{\mathrm{edge}}^{-2}-c_{\mathrm{edge}}|}{\sqrt{{[-B_2^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+2B_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]}^2+1}}$

表示边缘点的边缘方向θ(c_edge,r_edge)与车道边界曲线的夹角，其计算方法为

其中

${\mathrm{\&Psi;}}_L=\arctan [-B_1^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+{2B}_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]$

${\mathrm{\&Psi;}}_R=\arctan [-B_2^i\left(t\right)+B_4^i\left(t\right)r_{\mathrm{edge}}^{-2}+{2B}_5^i\left(t\right)r_{\mathrm{edge}}^{-3}]$

步骤3.3：将迭代计数器t加1；

步骤3.4：蚁群局域搜索，根据蚂蚁所在区域Uⁱ(t)按高斯分布

$\frac{1}{\sqrt{2\mathrm{\&pi;}}{\mathrm{\&sigma;}}_n}\exp \{-\frac{{[B_n^i\left(t\right)-B_{n{U}_n^i\left(t\right),\max }(t-1)]}^2}{2{\mathrm{\&sigma;}}_n^2}\}$

随机产生新的路径

其中i＝1,...，S、n＝1,...，5，将新路径Bⁱ(t)与Uⁱ(t)子区间的边界值相比较，若超出子区间边界则更新蚂蚁所在的子区间Uⁱ(t)，若超出搜索空间边界值则将Bⁱ(t)设为相应边界值，计算启发函数值F[Bⁱ(t)]，其中i＝1,...，S，再更新启发函数历史最大值F^max(t)＝max{F^max(t-1),F[Bⁱ(t)]i＝1,...，S}、F^max(t)对应的最优路径B^max(t)和子区间U^max(t)、当前蚁群的启发函数最小值F^min(t)＝min{F[Bⁱ(t)]i＝1,...，S}，并计算更新当前蚁群的启发函数平均值

步骤3.5：更新信息素表，初始化信息素增量Δτ_nm(t)＝0，其中n＝1,...，5、m＝1,...，M，计算当前蚂蚁留下的信息素

$\mathrm{\&Delta;}{\mathrm{\&tau;}}_{n{U}_n^i\left(t\right)}^i\left(t\right)=\exp \frac{F\left[B^i\left(t\right)\right]-\stackrel{\&OverBar;}{F}\left(t\right)}{F^{\max }\left(t\right)-F^{\min }\left(t\right)}$

其中i＝1,...，S、,＝1,...，5，更新信息素增量

和信息素残留τ_nm(t)＝(1-ρ)τ_nm(t-1)+Δτ_nm(t)，其中n＝1,...，5、m＝1,...，M；然后将蚂蚁的启发函数值F[Bⁱ(t)]与 $F_{n{U}_n^i\left(t\right),\max }(t-1)$ 相比较，若 $F\left[B^i\left(t\right)\right]>F_{n{U}_n^i\left(t\right),\max }(t-1)$ 则更新 $F_{n{U}_n^i\left(t\right),\max }\left(t\right)=F\left[B^i\left(t\right)\right],B_{n{U}_n^i\left(t\right),\max }\left(t\right)=B_n^i\left(t\right),$ 若 $F\left[B^i\left(t\right)\right]\&le;F_{n{U}_n^i\left(t\right),\max }(t-1)$ 则更新 $F_{n{U}_n^i\left(t\right),\max }\left(t\right)=F_{n{U}_m^i\left(t\right),\max }(t-1),B_{n{U}_n^i\left(t\right),\max }\left(t\right)=B_{n{U}_n^i\left(t\right),\max }(t-1),$ 其中i＝1,...，S、,＝1,...，5；

步骤3.6：将迭代次数t和最大迭代次数Iter_max相比较，若t＞Iter_max，则跳转到步骤3.8，若t≤Iter_max，则执行下一步骤3.7；

步骤3.7：蚁群全局搜索，计算蚂蚁的期望信息

和蚂蚁选择路径的选择概率其中i＝1,...，S、,＝1,...，5、m＝1,...，M，分别对每只蚂蚁产生(0,1)均值分布上的随机数Rand，更新蚂蚁的路径子区间

其中i＝1,...，S、,＝1,...，5，然后跳转到步骤3.3；

步骤3.8：输出车道边界曲线，根据最优路径B^max(t)以及车道边界投影模型，在图像上画出检测到的车道左、右边界曲线

$B_1^{\max }\left(t\right)r+B_2^{\max }\left(t\right)+B_4^{\max }\left(t\right)r^{-1}+B_5^{\max }\left(t\right)r^{-2}-c=0$

$B_2^{\max }\left(t\right)r+B_3^{\max }\left(t\right)+B_4^{\max }\left(t\right)r^{-1}+B_5^{\max }\left(t\right)r^{-2}-c=0;$

步骤4：计算车道平面线形参数值和主车方位，计算主车所在点处的车道曲率

$C_0=[B_4^{\max }\left(t\right)f_c+\frac{3}{2}{B}_5^{\max }\left(t\right)Q_1]Q_3$

和主车前方车道曲率的变化率

$C_1=3B_5^{\max }\left(t\right)Q_2{Q}_3\cos \mathrm{\&alpha;}$

其中 $Q_1=d_y\sin 2\mathrm{\&alpha;},Q_2=\frac{d_y\cos \mathrm{\&alpha;}}{h_c},Q_3=\frac{2d_x{\cos }^2\mathrm{\&alpha;}}{f_c^3}{Q}_2;$ 计算主车在车道中的偏向角

$\mathrm{\&beta;}=\frac{-d_x\cos \mathrm{\&alpha;}}{f_c^3}[(B_3^{\max }\left(t\right)-c_o)f_c^2+B_4^{\max }\left(t\right)f_c{Q}_1+\frac{3}{4}{B}_5^{\max }\left(t\right)Q_1^2]$

和车辆偏离车道中心线的距离

Images