CN103852047B - 利用车身信号侦测车道曲率的方法及其系统 - Google Patents

Abstract

本发明提供一种利用车身信号侦测车道曲率的方法及其系统，其利用速度感测器、方向盘角度计与航向侦测器等装置量测车辆的速度信息、方向盘角度及行进方向等车身信号，估测目前车辆的弯道曲率，并通过无线传输装置将弯道曲率讯息往后方车辆传送，且该弯道曲率可对车道曲率模型进行修正以增加估测的准确度。

Description

利用车身信号侦测车道曲率的方法及其系统

技术领域

本发明是有关一种侦测车道曲率的技术，特别是指一种利用车身信号以侦测车道曲率的方法及其系统。

背景技术

为了维护行车安全，特别是在行经弯道时，若驾驶人不注意减速慢行，极有可能因来不及转弯而冲出弯道或失速造成车辆翻覆，除了需要驾驶者的经验的外，亦可倚靠仪器对驾驶者提供警示，例如预先估算车道曲率半径、计算进入此弯道的极限速度等，再将该多个信息提供给驾驶者参考。

过去大多数的车道曲率半径侦测方式是通过影像撷取装置侦测车道线进而估测车道曲率半径，然而此方法易受限于环境影响而使曲率侦测失效，例如车道线标示不明显、车道过宽导致影像无法撷取车道线、陡坡致使影像无法捕捉车道线、道路行驶中大型车辆的车道线遮蔽干扰、光线过暗或过亮、树影或大楼阴影影响、无车道线的环境等，这些情况皆会使影像撷取装置所取得的影像清晰度不足，使估算难以进行。

有鉴于此，本发明遂针对上述现有技术的缺失，提出一种利用车身信号侦测车道曲率的方法及其系统，以有效克服上述的该等问题。

发明内容

本发明的主要目的在提供一种利用车身信号侦测车道曲率的方法及其系统，其是利用速度感测器、方向盘角度计与航向侦测器的信号撷取估测目前车辆的转弯曲率。

本发明的另一目的在提供一种利用车身信号侦测车道曲率的方法及其系统，其将估测出的弯道曲率通过无线传输装置往后车传送。

为达上述的目的，本发明提供一种利用车身信号侦测车道曲率的方法，是侦测一车辆的一行进方向、一速度信息及一方向盘角度；于一处理器中利用行进方向及速度信息建立一车道曲率模型并预测一车道曲率，及利用速度信息及方向盘角度另进行一次车道曲率估测；以及根据对该车道曲率估测的结果，利用记述统计(Descriptivestatistics)方法对车道曲率进行修正。

其中，该记述统计方法同时对该车道曲率模型进行修正，以供预测下一时间点的该车道曲率。

其中，修正后的车道曲率是通过一无线传输模块传送给至少一后方车辆参考，该无线传输模块为专用短程通讯。

其中，该车道曲率是利用一扩展型卡尔曼滤波器对该车道曲率模型计算所得到。

其中，该行进方向是利用一航向侦测器所测得，该速度信息是利用一速度感测器所测得；该方向盘角度是利用一方向盘角度计所量测。

其中，该航向侦测器为陀螺仪或GPS，该速度感测器为轮速计或加速规或GPS。

本发明另提供一种利用车身信号侦测车道曲率的系统，包括一车身信号侦测模块，侦测一车辆的一行进方向、一速度信息及一方向盘角度；一处理器，连接车身信号侦测模块，利用行进方向及速度信息建立一车道曲率模型并预测一车道曲率，及利用速度信息及方向盘角度另进行一次车道曲率估测，再根据对该车道曲率的估测结果，利用记述统计(Descriptivestatistics)方法对车道曲率进行修正；以及一无线传输模块，连接处理器，用以将修正的车道曲率传送给车辆的至少一后方车辆。

其中，该车道曲率是利用一扩展型卡尔曼滤波器对该车道曲率模型计算所得到。

其中，该车身信号侦测模块中更包括一航向侦测器、一速度感测器及一方向盘角度计。

其中，该航向侦测器为陀螺仪或GPS，以侦测该行进方向，而该速度感测器为轮速计或加速规或GPS，该方向盘角度计用以量测该方向盘角度。

本发明提供的利用车身信号侦测车道曲率的方法及其系统是利用速度感测器、方向盘角度计与航向侦测器所撷取的速度、方向盘角度及行进方向等车身信号来估测目前车辆的转弯曲率、预测转弯轨迹，并将计算出的结果传送给后方车辆参考，由于车身信号为真实的信号而不是计算所得，不存在误差，因此利用车身信号计算出的曲率较的以前方道路影像所计算的车道曲率更为精确，且不会受天色明暗影响，系统可全天候运行，提高行车安全性。

底下通过具体实施例详加说明，当更容易了解本发明的目的、技术内容、特点及其所达成的功效。

附图说明

图1为本发明利用车身信号侦测车道曲率的系统的方块图。

图2为本发明利用车身信号侦测车道曲率的方法的流程图。

图3为车辆进入弯道时入弯弧长及转向角度的示意图。

图4为车辆后轮的x轴及y轴方向的行进长度及转向角度示意图。

附图标记说明：

10车身信号侦测模块；102速度感测器；104航向侦测器；106方向盘角度计；12处理器；14无线传输模块；30前轮；32后轮。

具体实施方式

本发明是一种利用车身信号侦测车道曲率的方法及其系统，可广泛用于在全天候环境下车辆侦测车道曲率半径，适用于以阿克曼转向机构为转向结构的车辆的车身信号达到曲率估算及车辆转弯轨迹预测。

请参考图1，其为本发明利用车身信号侦测车道曲率的系统的方块图，包括一车身信号侦测模块10、一处理器12及一无线传输模块14，车身信号侦测模块10中包括一速度感测器102、一航向侦测器104及一方向盘角度计106，分别侦测一车辆的速度信息、行进方向及方向盘角度，其中速度感测器102可为轮速计、加速规、GPS或其他可感测车辆速度、加速度的感测器，航向侦测器104可为陀螺仪或GPS；处理器12连接车身信号侦测模块10，利用速度信息、行进方向及方向盘角度等进行车道曲率的估测，先以行进方向及速度信息建立一车道曲率模型并预测一车道曲率，同时利用速度信息及方向盘角度另进行一次车道曲率估测，之后再根据对该车道曲率的估测结果，利用记述统计(Descriptivestatistics)方法对车道曲率进行修正；无线传输模块14连接处理器12，用以将修正的车道曲率传送给车辆的至少一后方车辆。

图2为本发明利用车身信号侦测车道曲率的方法的流程图，步骤S10中利用航向侦测器、速度感测器及方向盘角度计分别侦测车辆的行进方向、速度信息及方向盘角度等车身信号，速度信息为车辆前轮的轮速；接着，步骤S12于一处理器中利用行进方向及速度信息建立一车道曲率模型，并在步骤S14利用扩展式卡尔曼滤波器(ExtendedKalmanfilter,EKF)预测一车道曲率，同时步骤S16利用速度信息及方向盘角度另进行一次车道曲率估测；步骤S18根据对该车道曲率估测的结果，利用记述统计方法对车道曲率进行修正，即完成车道曲率的估测，最后，步骤S20将估测出的车道曲率利用无线传输方式传送给后方车辆，该无线传输方式是可通过DSRC(专用短程通讯)进行传输。

步骤S16中关于一次车道曲率估测系详述如下：曲率的定义为在一单位弧长的情形下量得的半径长度，其比值为曲率。如图3所示，假设车辆为一钢体，运用速度感测器，经过积分计算之后估测车辆入弯的弧长S，再运用方向盘角度计量取方向盘角度λ(deg)，进而推求车辆转向角度θ(deg)，亦即前轮30偏转的角度。曲率定义如公式(1)：

${\mathrm{\κ}}_1=\frac{1}{R}=\frac{\mathrm{\θ}}{S}\≈\frac{\mathrm{\Δ}\mathrm{\θ}}{\mathrm{\Δ}S}---\left(1\right)$

其中，N为比例系数(每辆车的定值)，得到T为时间。

接着计算左、右后轮中心点的运动轨迹方程式，假设w为车宽，l为轴距，则后轮32中心点的运动轨迹方程式如下式(2)：

左轮： $\left\{\begin{array}{c}{x}_{rL}\left(t\right)=(l\·\cot \mathrm{\θ}-\frac{w}{2})\·\sin \left(\frac{v\·\sin \mathrm{\θ}}{l}t\right)\\ y_{rL}\left(t\right)=-(l\·\cot \mathrm{\θ}-\frac{w}{2})\·\cos \left(\frac{v\·\sin \mathrm{\θ}}{l}t\right)+l\·\cot \mathrm{\θ}\end{array}\right.$

右轮： $\left\{\begin{array}{c}{x}_{rR}\left(t\right)=(l\·\cot \mathrm{\θ}+\frac{w}{2})\·\sin \left(\frac{v\·\sin \mathrm{\θ}}{l}t\right)\\ y_{rR}\left(t\right)=-(l\·\cot \mathrm{\θ}+\frac{w}{2})\·\cos \left(\frac{v\·\sin \mathrm{\θ}}{l}t\right)+l\·\cot \mathrm{\θ}\end{array}\right.---\left(2\right)$

而后轴中心点的运动轨迹方程式则如下式(3)：

$\left\{\begin{array}{c}{x}_r\left(t\right)=l\·\cot \mathrm{\θ}\·\sin \left(\frac{v\·\sin \mathrm{\θ}}{l}t\right)\\ y_r\left(t\right)=-l\·\cot \mathrm{\θ}\·\cos \left(\frac{v\·\sin \mathrm{\θ}}{l}t\right)+l\cot \mathrm{\θ}\end{array}\right.---\left(3\right)$

得到弧长 $S={{\∫}_0^T(x_r^2+y_r^2)}^{\frac{1}{2}}dt\⇒\mathrm{\Δ}S={(x_r^2+y_r^2)}^{\frac{1}{2}}\×\mathrm{\Δ}T$

因此，一次车道曲率的估测如下式(4)：

${\mathrm{\κ}}_1=\frac{\mathrm{\ρ}(e_s+f_s)}{v^2}---\left(4\right)$

其中e_s为道路超高，f_s为轮胎横向摩擦系数，ρ为一参数并可表示为V为车速，低速时可为高速时可由速度感测装置量测获得。

而步骤S12计算车道曲率模型中，车道曲率近似模型的二次方程式如下式(5)：

$y_r\left(t\right)=\left(\frac{{\mathrm{\κ}}_2\left(t\right)}{2}\right)x_r^2\left(t\right)+\mathrm{\θ}\left(t\right)x_r\left(t\right)+\mathrm{\ϵ}+y_{r0}---\left(5\right)$

依据阿克曼转向原理，x_r为后轮位置，ε为车道偏移量(车辆中心点与车道的位移差)，y_r0为横向初始位置，κ₂为车道曲率，请同时参考图4因此可得到后轮32的x轴及y轴方向的行进长度及转向角度如下式(6)，在此将后轮32行进轨迹设为线性，但实际上弯道角度只是接近线性而并非完全线性，因此下式所估算者为近似值：

$\left\{\begin{array}{c}{x}_r\left(t\right)=\frac{\mathrm{\Δ}T}{2}\left(v_L\right(t)+v_R\left(t\right))\×cos\mathrm{\θ}+x_{r0}\\ y_r\left(t\right)=\frac{\mathrm{\Δ}T}{2}\left(v_L\right(t)+v_R\left(t\right))\×sin\mathrm{\θ}+y_{r0}\\ \frac{{\mathrm{\κ}}_2\left(t\right)}{2}{x}_r^2\left(t\right)=y_r\left(t\right)-\mathrm{\θ}\left(t\right)x_r\left(t\right)-\mathrm{\ϵ}-y_{r0}\end{array}\right.---\left(6\right)$

其中v_R及v_L分别代表右、左后轮32的速度，κ₂/2视为一个变数。

将公式(6)移项到矩阵中，建立如下式(7)的车道曲率模型，便可得知单位时间x轴、y轴及曲率的状态：

$\left[\begin{array}{c}{x}_r\left(t\right)\\ y_r\left(t\right)\\ {\mathrm{\κ}}_2\left(t\right)\end{array}\right]=\left[\begin{array}{c}\frac{\mathrm{\Δ}T}{2}\left(v_L\right(t)+v_R(t\left)\right)cos\mathrm{\θ}+x_{r0}\\ \frac{\mathrm{\Δ}T}{2}\left(v_L\right(t)+v_R\left(t\right))sin\mathrm{\θ}+y_{r0}\\ \frac{2}{x_r^2\left(t\right)}\left(y_r\right(t)-\mathrm{\θ}\left(t\right)x_r\left(t\right)-\mathrm{\ϵ})\end{array}\right]=F\left(x\right)+Gu+W$

其中，

$F\left(x\right)=\left[\begin{array}{c}0\\ 0\\ \frac{2\left(y_r\right(t)-\mathrm{\ϵ})}{x_r^2\left(t\right)}-\frac{-2\mathrm{\θ}}{x_r\left(t\right)}\end{array}\right],G=\left[\begin{array}{cc}\frac{\mathrm{\Δ}Tcos\mathrm{\θ}}{2}& \frac{\mathrm{\Δ}Tcos\mathrm{\θ}}{2}\\ \frac{\mathrm{\Δ}Tsin\mathrm{\θ}}{2}& \frac{\mathrm{\Δ}Tsin\mathrm{\θ}}{2}\\ 0& 0\end{array}\right]---\left(7\right)$

接着步骤S14的扩展式卡尔曼滤波器给定车道侦测系统为x＝F(x)+Gu+W,z＝H(x)，其中x＝[x_ry_rK₂]^T为系统状态，u＝[v_Lv_R]^T为可变控制输入，z为系统量测输出，F为系统方程式矩阵，G为控制输入矩阵，W为白色噪声。求取系统方程式矩阵的Jacobian矩阵如下式(8)：

$J_f=\frac{\∂F}{\∂x}=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ (\frac{2\mathrm{\θ}}{x_r}-\frac{4y_r-2\mathrm{\ϵ}}{x_r^3})& \frac{2}{x_r^2}& 0\end{array}\right]---\left(8\right)$

接着扩展式卡尔曼滤波器的计算如下：

状态预测： ${\hat{x}}_{k|k-1}=F({\hat{x}}_{k-1|k-1},u_{k-1})$

预测共变异数： $P_{k|k-1}=J_{f,k-1}{P}_{k-1|k-1}{J}_{f,k-1}^T+Q_{k-1}$

更新误差： $e_k=z_k-{\hat{x}}_{k|k-1}$

更新误差共变异数： $S_k=H_k{P}_{k|k-1}{H}_k^T+R_k$

卡曼增益： $K_k=P_{k|k-1}{H}_k^T{S}_k^{-1}$

更新状态： ${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k{e}_k$

更新共变异数：P_k|k＝(I-K_kH_k)P_k|k-1

如此便可得到一个更新的状态，此状态中含有x_r、y_r、κ₂，亦即得到更新的曲率κ₂。

而步骤S18的记述统计方法便是将步骤S14量测完的曲率及步骤S16估测出的曲率计算其平均值，如下式(9)：

$\stackrel{\‾}{\mathrm{\κ}}=\frac{\underset{k=1}{\overset{n}{\Σ}}{\mathrm{\κ}}_k}{n}---\left(9\right)$

再求偏差值，估算曲率平均值与实际真正曲率的误差，如下式(10)：

$\underset{k=1}{\overset{n}{\Σ}}({\mathrm{\κ}}_k-\stackrel{\‾}{\mathrm{\κ}})=\underset{k=1}{\overset{n}{\Σ}}{\mathrm{\κ}}_k-n\stackrel{\‾}{\mathrm{\κ}}=0---\left(10\right)$

求出偏差值后，接着计算变异数，如下式(11)：

$\begin{array}{c}{\mathrm{\σ}}^2=\frac{1}{n}\underset{k=1}{\overset{n}{\Σ}}{({\mathrm{\κ}}_k-\stackrel{\‾}{\mathrm{\κ}})}^2=\frac{1}{n}(\underset{k=1}{\overset{n}{\Σ}}{\mathrm{\κ}}_k^2-2\stackrel{\‾}{\mathrm{\κ}}\underset{k=1}{\overset{n}{\Σ}}{\mathrm{\κ}}_k+\underset{k=1}{\overset{n}{\Σ}}{\stackrel{\‾}{\mathrm{\κ}}}^2)\\ =\frac{1}{n}(\underset{k=1}{\overset{n}{\Σ}}{\mathrm{\κ}}_k^2-2n{\stackrel{\‾}{\mathrm{\κ}}}^2+n{\stackrel{\‾}{\mathrm{\κ}}}^2)=\frac{1}{n}\underset{k=1}{\overset{n}{\Σ}}{\mathrm{\κ}}_k^2-{\stackrel{\‾}{\mathrm{\κ}}}^2\end{array}---\left(11\right)$

再求出标准偏差值，估算曲率平均值与实际真正曲率的偏差值，如下式(12)：

${\mathrm{\σ}}_n=\sqrt{\frac{\underset{k=1}{\overset{n}{\Σ}}{({\mathrm{\κ}}_k-\stackrel{\‾}{\mathrm{\κ}})}^2}{n}}---\left(12\right)$

之后，计算标准差，只要在正确曲率的范围内能用即可，如下式(13)：

${\mathrm{\σ}}_{\stackrel{\‾}{\mathrm{\κ}}}=\frac{\mathrm{\σ}}{\sqrt{n}}=\sqrt{\frac{\underset{k=1}{\overset{n}{\Σ}}{({\mathrm{\κ}}_k-\stackrel{\‾}{\mathrm{\κ}})}^2}{n(n-1)}}---\left(13\right)$

接着利用曲率的平均值及标准差求出曲率输出量，如下式(14)：

${\mathrm{\κ}}_c=\stackrel{\‾}{\mathrm{\κ}}\±{\mathrm{\σ}}_{\stackrel{\‾}{\mathrm{\κ}}}---\left(14\right)$

其中κ_C为曲率输出量，同时也将此数值回代入步骤S14扩展式卡尔曼滤波器的z_k项，不但修正了车道曲率，将最后计算出的车道曲率κ_C传送到后方车辆，更将κ_C代入到公式(6)、(7)的κ₂，以供下一时间点建立车道曲率模型。

综上所述，本发明提供的利用车身信号侦测车道曲率的方法及其系统是利用速度感测器、方向盘角度计与航向侦测器所撷取的速度、方向盘角度及行进方向等车身信号来估测目前车辆的转弯曲率、预测转弯轨迹，并将计算出的结果传送给后方车辆参考，由于车身信号为真实的信号而不是计算所得，不存在误差，因此利用车身信号计算出的曲率较的以前方道路影像所计算的车道曲率更为精确，且不会受天色明暗影响，系统可全天候运行，提高行车安全性。

以上所述者，仅为本发明的较佳实施例而已，并非用来限定本发明实施的范围。故即凡依本发明申请范围所述的特征及精神所为的均等变化或修饰，均应包括于本发明的申请专利范围内。

Claims (10)

1.一种利用车身信号侦测车道曲率的方法，其特征在于，包括下列步骤：

侦测一车辆的一行进方向、一速度信息及一方向盘角度；

于一处理器中利用该行进方向及该速度信息建立一车道曲率模型并预测一车道曲率，及利用该速度信息及该方向盘角度另进行一次车道曲率估测；以及

根据对该车道曲率估测的结果，利用记述统计方法对该车道曲率进行修正。

2.根据权利要求1所述的利用车身信号侦测车道曲率的方法，其特征在于，该记述统计方法同时对该车道曲率模型进行修正，以供预测下一时间点的该车道曲率。

3.根据权利要求1所述的利用车身信号侦测车道曲率的方法，其特征在于，修正后的车道曲率是通过一无线传输模块传送给至少一后方车辆参考，该无线传输模块为专用短程通讯。

4.根据权利要求1所述的利用车身信号侦测车道曲率的方法，其特征在于，该车道曲率是利用一扩展型卡尔曼滤波器对该车道曲率模型计算所得到。

5.根据权利要求1所述的利用车身信号侦测车道曲率的方法，其特征在于，该行进方向是利用一航向侦测器所测得，该速度信息是利用一速度感测器所测得；该方向盘角度是利用一方向盘角度计所量测。

6.根据权利要求5所述的利用车身信号侦测车道曲率的方法，其特征在于，该航向侦测器为陀螺仪或GPS，该速度感测器为轮速计或加速规或GPS。

7.一种利用车身信号侦测车道曲率的系统，其特征在于，包括：

一车身信号侦测模块，侦测一车辆的一行进方向、一速度信息及一方向盘角度；

一处理器，连接该车身信号侦测模块，利用该行进方向及该速度信息建立一车道曲率模型并预测一车道曲率，及利用该速度信息及该方向盘角度另进行一次车道曲率估测，再利用该一次车道曲率估测的结果对该车道曲率进行修正；以及

一无线传输模块，连接该处理器，以将修正的该车道曲率传送给该车辆的至少一后方车辆。

8.根据权利要求7所述的利用车身信号侦测车道曲率的系统，其特征在于，该车道曲率是利用一扩展型卡尔曼滤波器对该车道曲率模型计算所得到。

9.根据权利要求7所述的利用车身信号侦测车道曲率的系统，其特征在于，该车身信号侦测模块中更包括一航向侦测器、一速度感测器及一方向盘角度计。

10.根据权利要求9所述的利用车身信号侦测车道曲率的系统，其特征在于，该航向侦测器为陀螺仪或GPS，以侦测该行进方向，而该速度感测器为轮速计或加速规或GPS，该方向盘角度计用以量测该方向盘角度。