基于Cardinal樣條的車道偏離預(yù)警測(cè)評(píng)關(guān)鍵參數(shù)估計(jì)

李宏海，陸紅偉，盧立陽(yáng)，徐啟敏

(1.交通運(yùn)輸部公路科學(xué)研究院，北京 100088;2.東南大學(xué) 儀器科學(xué)與工程學(xué)院，江蘇 南京 210096)

0 引言

智能車路系統(tǒng)(Intelligent Vehicle and Infra Structure System，IVIS)是目前交通領(lǐng)域的科技前沿，是為了實(shí)現(xiàn)車車和車路全方位多角度的協(xié)同互動(dòng)，從而提高行車和行人安全，減少交通事故的發(fā)生和提高道路通行效率。為了有效實(shí)現(xiàn)上述目標(biāo)，智能車路系統(tǒng)采用了先進(jìn)的通信技術(shù)和全時(shí)空高度統(tǒng)一的多傳感器融合采集技術(shù)等，能夠?qū)Φ缆泛蛙囕v信息進(jìn)行高精度實(shí)時(shí)可靠的信息采集和融合通信，對(duì)保障道路安全和提高出行效率具有十分重要的意義。當(dāng)前歐美日等發(fā)達(dá)國(guó)家均在實(shí)施和推進(jìn)IVIS發(fā)展的相關(guān)行動(dòng)計(jì)劃，我國(guó)智能汽車和智能路側(cè)設(shè)備等產(chǎn)業(yè)也發(fā)展迅速[1-3]。

先進(jìn)駕駛輔助系統(tǒng)(Advanced Driver Assistance System， ADAS)是智能車路系統(tǒng)的重要組成部分，對(duì)于智能車路系統(tǒng)的發(fā)展具有十分重要的意義，目前也被廣泛應(yīng)用于各種車輛上。ADAS是利用安裝于車輛端的各種傳感器進(jìn)行車輛行駛過(guò)程中環(huán)境的感知和數(shù)據(jù)采集分析，如利用衛(wèi)星導(dǎo)航進(jìn)行車輛實(shí)時(shí)高精度的定位，利用慣導(dǎo)進(jìn)行車輛姿態(tài)的識(shí)別和速度、加速度的采集，利用雷達(dá)(毫米波雷達(dá)、激光雷達(dá)等)和攝像頭進(jìn)行環(huán)境的感知和監(jiān)測(cè)等，然后將這些傳感器信息進(jìn)行融合處理后，進(jìn)行系統(tǒng)化分析處理，給出合理的駕駛建議，提高駕駛的安全性和舒適性。據(jù)維基百科的定義，ADAS包括自適應(yīng)巡航、車道偏離預(yù)警系統(tǒng)、車道保持系統(tǒng)等在內(nèi)的多個(gè)系統(tǒng)及功能。本研究主要研究車道偏離預(yù)警系統(tǒng)(Lane Departure Warning System,LDWS)，在車輛行駛過(guò)程中，LDWS會(huì)利用相關(guān)傳感器(如攝像頭等)實(shí)時(shí)監(jiān)測(cè)車輛與車道的相對(duì)位置情況，當(dāng)檢測(cè)到車輛偏離車道并且有一定的危險(xiǎn)時(shí)，會(huì)采取相應(yīng)的音視頻手段提醒駕駛員，為其提供充足的反應(yīng)時(shí)間。車道偏離預(yù)警系統(tǒng)能夠極大地提高行車安全，減少因駕駛員疲勞駕駛從而導(dǎo)致車道偏離乃至發(fā)生交通事故的風(fēng)險(xiǎn)，在智能車路系統(tǒng)中具有十分重要的地位和意義[4-6]。

目前國(guó)內(nèi)外針對(duì)LDWS均進(jìn)行了相關(guān)研究。在國(guó)外，許多發(fā)達(dá)國(guó)家都投入大量資金以及人才進(jìn)行LDWS的研究，比較具有代表性的LDWS有美國(guó)卡內(nèi)基梅隆大學(xué)研發(fā)的AURORA系統(tǒng)，該系統(tǒng)采用車載高性能工作站進(jìn)行圖片的實(shí)時(shí)處理，最大處理速率達(dá)60 Hz以上，能夠很好地滿足車輛行駛中對(duì)于預(yù)警系統(tǒng)實(shí)時(shí)性的需求[7-8]；美德兩國(guó)聯(lián)合研發(fā)了AutoVue系統(tǒng)[9]，該系統(tǒng)主要面向營(yíng)運(yùn)卡車并獲得實(shí)際應(yīng)用，獲得了較好的社會(huì)效益和經(jīng)濟(jì)效益；荷蘭研發(fā)了Mobileye_AWS系統(tǒng)[10]，該系統(tǒng)相對(duì)于其他系統(tǒng)優(yōu)點(diǎn)突出，既可以用于結(jié)構(gòu)化道路的車道線檢測(cè)和車道偏離預(yù)警，也可以用于沒(méi)有車道線的非結(jié)構(gòu)化路面，利用車道的路肩來(lái)判斷車輛的位置；日本三菱汽車公司設(shè)計(jì)和研發(fā)了DSS，并在模型車上進(jìn)行實(shí)際工況應(yīng)用[11]，在國(guó)際上獲得了諸多安全獎(jiǎng)項(xiàng)。在國(guó)內(nèi)，近些年來(lái)針對(duì)LDWS的研究也很多，如清華大學(xué)智能技術(shù)與系統(tǒng)國(guó)家重點(diǎn)實(shí)驗(yàn)室研制的“THMR-V”智能車[12]，浙江大學(xué)針對(duì)自主智能車輛引進(jìn)的分布式測(cè)試系統(tǒng)IVD (Intelligent Vehicle Detection-bed)[13-14]，國(guó)防科技大學(xué)研發(fā)的CITAVT系列智能車[15-16]，吉林大學(xué)開(kāi)發(fā)的JLUVA-1系統(tǒng)[17-18]和由東南大學(xué)基于DSP技術(shù)開(kāi)發(fā)的車道偏離報(bào)警系統(tǒng)[19-20]，除此之外國(guó)內(nèi)其他一些科研單位和高校在LDWS方面的研究也在如火如荼地進(jìn)行中。

當(dāng)前，雖然包括LDWS在內(nèi)的先進(jìn)駕駛輔助系統(tǒng)發(fā)展迅速，各個(gè)汽車廠家均推出了高智能度輔助駕駛的汽車產(chǎn)品，但近年來(lái)不管是特斯拉、谷歌還是Uber等都出現(xiàn)了很多交通事故，而且Uber還在2018年3月發(fā)生了第1起自動(dòng)駕駛汽車致死事故。這些都說(shuō)明當(dāng)前在大力發(fā)展高智能ADAS的同時(shí)，對(duì)系統(tǒng)的測(cè)試和評(píng)價(jià)也至關(guān)重要，如何科學(xué)地判別智能汽車的安全性和可靠性，也是當(dāng)前面臨的一個(gè)重要問(wèn)題。對(duì)于車道偏離預(yù)警系統(tǒng)(LDWS)的測(cè)評(píng)，目前我國(guó)已有標(biāo)準(zhǔn)《智能運(yùn)輸系統(tǒng) 車道偏離報(bào)警系統(tǒng) 性能要求與檢測(cè)方法》(GB/T 26773—2011)，但在該標(biāo)準(zhǔn)中僅列出測(cè)評(píng)場(chǎng)景及流程，未給出車道線距離等測(cè)評(píng)指標(biāo)的具體計(jì)算方法，在實(shí)際應(yīng)用中，依然會(huì)遇到計(jì)算方法不明、指標(biāo)不夠定量化等問(wèn)題。因此，對(duì)于LDWS的測(cè)評(píng)，亟需一種行之有效的測(cè)評(píng)方法和測(cè)評(píng)指標(biāo)。

針對(duì)上述問(wèn)題，本研究提出了一種基于Cardinal樣條的車道偏離預(yù)警測(cè)評(píng)關(guān)鍵參數(shù)估計(jì)方法。該方法首先利用高精度差分組合導(dǎo)航分別從車道線以及測(cè)試車輛上獲取測(cè)評(píng)所需基礎(chǔ)參數(shù)數(shù)據(jù)，然后對(duì)采集到的車道線坐標(biāo)進(jìn)行Cardinal樣條擬合，從而提高車道線的測(cè)量精度，減小測(cè)評(píng)誤差；在得到高精度車道線的基礎(chǔ)上，利用車道線信息和車載采集信息，結(jié)合牛頓迭代算法，進(jìn)行關(guān)鍵參數(shù)的解算。

1 基礎(chǔ)參數(shù)采集

精確的基礎(chǔ)參數(shù)采集是進(jìn)行LDWS測(cè)評(píng)的基礎(chǔ)，本研究采用基于GNSS/INS的組合導(dǎo)航系統(tǒng)來(lái)實(shí)現(xiàn)位置、速度等基礎(chǔ)參數(shù)信息的準(zhǔn)確采集。

1.1 全球?qū)Ш叫l(wèi)星系統(tǒng)

全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellites System，GNSS)是以接收和播發(fā)電磁波信號(hào)來(lái)達(dá)到導(dǎo)航、定位、授時(shí)(簡(jiǎn)稱PNT)的目的。其依賴于高軌道的衛(wèi)星播發(fā)的導(dǎo)航電文和載波信號(hào)，在大地測(cè)量、車聯(lián)網(wǎng)、自動(dòng)駕駛等領(lǐng)域發(fā)揮著重要作用。

長(zhǎng)期以來(lái)，一直以美國(guó)的GPS為代表的衛(wèi)星導(dǎo)航具有全天候、全天時(shí)的優(yōu)勢(shì)。隨著歐盟Gallieo、俄羅斯GLONASS以及中國(guó)BDS的發(fā)展，多頻率、多系統(tǒng)的衛(wèi)星導(dǎo)航定位已經(jīng)成為現(xiàn)實(shí)。就目前而言，衛(wèi)星導(dǎo)航的發(fā)展已久，前景無(wú)量，尤其是以北斗3組網(wǎng)完成為標(biāo)志，研究北斗與其他GNSS的組合，在我國(guó)具有非常重要的意義和價(jià)值。

除此之外，衛(wèi)星導(dǎo)航高精度定位已經(jīng)成為現(xiàn)實(shí)，如發(fā)展相對(duì)成熟的RTK技術(shù)、PPP技術(shù)等，相對(duì)于其他導(dǎo)航系統(tǒng)的優(yōu)勢(shì)在于誤差不會(huì)隨著時(shí)間累積。但是其研究也存在瓶頸，如PPP技術(shù)不能快速收斂、模糊度固定問(wèn)題、復(fù)雜環(huán)境下信號(hào)缺失問(wèn)題、模糊度重固定問(wèn)題、高精度的動(dòng)態(tài)定位問(wèn)題等，而這些問(wèn)題就目前而言僅僅依賴衛(wèi)星導(dǎo)航是基本不可能得到有效解決的，因此需要將衛(wèi)星導(dǎo)航同其他導(dǎo)航系統(tǒng)相結(jié)合，來(lái)彌補(bǔ)GNSS衛(wèi)星導(dǎo)航的先天性不足。

1.2 慣性導(dǎo)航

慣性導(dǎo)航系統(tǒng)(Inertial Navigation System，INS)是一種相對(duì)獨(dú)立自主的導(dǎo)航系統(tǒng)，不與外界發(fā)生任何信號(hào)交換便可完成導(dǎo)航和定位。隨著計(jì)算機(jī)技術(shù)的發(fā)展，基于捷聯(lián)技術(shù)的慣導(dǎo)因具有結(jié)構(gòu)簡(jiǎn)單等優(yōu)點(diǎn)，逐步取代了平臺(tái)式慣導(dǎo)，成為目前應(yīng)用最廣泛的INS，簡(jiǎn)稱SINS。

由于慣性導(dǎo)航自身的獨(dú)特性質(zhì)，使其在定位過(guò)程中的誤差會(huì)隨著時(shí)間逐漸累積，單靠INS完成長(zhǎng)時(shí)間導(dǎo)航定位的結(jié)果是非常不可靠的，因此其并不具備高精度的長(zhǎng)期導(dǎo)航定位的能力。

1.3 GNSS/INS組合導(dǎo)航

綜上所述，GNSS和INS各有優(yōu)缺點(diǎn)，但同時(shí)能夠優(yōu)勢(shì)互補(bǔ)，因此目前多將其進(jìn)行組合使用，這對(duì)于提高導(dǎo)航性能和精度有著重要的意義，其優(yōu)勢(shì)主要體現(xiàn)在：

(1)可以對(duì)慣性導(dǎo)航系統(tǒng)誤差進(jìn)行估計(jì)和校正，提高組合導(dǎo)航精度；

(2)可以彌補(bǔ)衛(wèi)星導(dǎo)航信號(hào)缺損問(wèn)題，提高組合導(dǎo)航能力；

(3)可以提高衛(wèi)星導(dǎo)航接收機(jī)對(duì)衛(wèi)星信號(hào)的捕獲和跟蹤能力，提高整體導(dǎo)航效率；

(4)可以增加觀測(cè)信息冗余度，提高系統(tǒng)容錯(cuò)能力；

(5)可以提高導(dǎo)航系統(tǒng)的抗干擾性，提高組合導(dǎo)航的完好性。

根據(jù)應(yīng)用要求及參與組合觀測(cè)量的深度，GNSS/INS組合的模式可分為松組合、緊組合和深組合。3種組合模式的對(duì)比示意圖如圖1所示，從圖中可以直觀地區(qū)分3種組合模式的層次。

圖1 GNSS/INS組合示意圖

在圖1中，分別用1，2，3代表深組合、緊組合和松組合。對(duì)于松組合來(lái)說(shuō)，是利用GNSS接收機(jī)輸出的位置、速度信息和INS輸出信息進(jìn)行組合，兩者共用1個(gè)濾波器進(jìn)行數(shù)據(jù)融合。由于GNSS接收機(jī)的導(dǎo)航解是作為觀測(cè)量輸入濾波器的，所以GNSS接收機(jī)必須能夠同時(shí)捕獲并跟蹤到4顆以上的衛(wèi)星才能獲得導(dǎo)航解。因此，在GNSS信號(hào)遮擋或受干擾時(shí)，系統(tǒng)在INS獨(dú)立導(dǎo)航的狀態(tài)下，長(zhǎng)時(shí)間工作會(huì)導(dǎo)致導(dǎo)航精度有所降低。與松組合不同，緊組合是利用GNSS接收機(jī)輸出的偽距、偽距率等觀測(cè)量，與INS結(jié)合星歷反算得到的偽距、偽距率進(jìn)行組合。這種參與組合的方式，使得GNSS接收機(jī)輸出了原始信息進(jìn)行參與組合，偽距和偽距率這些基本觀測(cè)量的相關(guān)性低，從而具有較大的優(yōu)勢(shì)，一方面可以在衛(wèi)星少于4顆時(shí)正常工作，另一方面定位的精度相比于松組合也得到了較大的提升。上述松組合和緊組合兩種組合模式本質(zhì)上都是利用GNSS接收機(jī)的輸出信息來(lái)輔助INS，在解算的過(guò)程中，GNSS接收機(jī)依然保持獨(dú)立，因而接收機(jī)本身的抗干擾能力并未得到本質(zhì)的提升。

針對(duì)上述兩種組合方式存在的不足，近年來(lái)又開(kāi)發(fā)了一種新的組合模式：深組合，是一種深層次的、涉及到硬件層面的組合模式。從圖1中可以看出，與前兩種組合模式相比，深組合取消了獨(dú)立不相關(guān)的碼和載波跟蹤環(huán)路，直接將相關(guān)器輸出的I/Q數(shù)據(jù)作為輸入信息，輸入到組合導(dǎo)航的卡爾曼濾波器中，然后將融合后的信息控制調(diào)整碼/載波數(shù)控振蕩器(Numerical Controlled Oscillator，NCO)。從信息處理流程分析可知，相關(guān)器輸出的I/Q信息在理論上滿足觀測(cè)噪聲相互獨(dú)立的特性，因此深組合模式具有理論上的“全局最優(yōu)”特性。深組合模式便于GNSS接收機(jī)與INS進(jìn)行一體化設(shè)計(jì)，實(shí)現(xiàn)了GNSS接收機(jī)和INS兩者之間的相互輔助，可以降低對(duì)INS的精度要求以及組合導(dǎo)航系統(tǒng)的成本和體積，這也為IMU參與高精度組合提供了可能。3種組合模式的性能比較如表1所示。

表1 不同組合模式性能比較

本研究基礎(chǔ)參數(shù)采集中使用的算法即為上述深組合算法，為車道偏離預(yù)警測(cè)評(píng)提供精準(zhǔn)的位置、速度、航向角等信息。

2 Cardinal樣條車道線擬合

利用上述組合導(dǎo)航系統(tǒng)進(jìn)行道路基礎(chǔ)位置坐標(biāo)的采集之后，由于采集到的點(diǎn)較為稀疏，無(wú)法準(zhǔn)確表征道路的實(shí)際情況，因此還需要利用擬合算法對(duì)車道離散點(diǎn)進(jìn)行插值擬合，從而更好地估計(jì)車道線的參數(shù)信息，為后續(xù)的測(cè)評(píng)參數(shù)解算打下良好的基礎(chǔ)。在此采用Cardinal樣條對(duì)離散車道坐標(biāo)點(diǎn)進(jìn)行擬合。

2.1 Cardinal樣條車道線擬合模型

采集得到的某段獨(dú)立道路軌跡數(shù)據(jù)為T={T0(c0,z0),T1(c1,z1),…,Tn(cn,zn)}，其中，cn，zn分別為道路軌跡點(diǎn)的北向位置坐標(biāo)和東向位置坐標(biāo);n為軌跡數(shù)據(jù)點(diǎn)的個(gè)數(shù)。

利用Cardinal樣條曲線對(duì)組合導(dǎo)航采集到的軌跡數(shù)據(jù)進(jìn)行插值，樣條曲線受坐標(biāo)集T中的控制點(diǎn)控制，獨(dú)立的單側(cè)車道數(shù)據(jù)塊中，除去兩端的控制點(diǎn)T0(c0,z0)，Tn-1(cn-1,zn-1)，Tn(cn,zn)后，對(duì)剩余的n-3個(gè)控制點(diǎn)，三次Cardinal樣條曲線可由任意4個(gè)連續(xù)控制點(diǎn)確定。設(shè)P(u)是兩控制點(diǎn)Tk和Tk+1間的擬合曲線的三次函數(shù)式，其中u為參變量，0≤u≤1；k=2,3,…,n-2；則取從Tk-1到Tk+2間的4個(gè)控制點(diǎn)用于建立擬合車道線的邊界條件：

P(0)=Tk，

(1)

P(1)=Tk+1，

(2)

(3)

(4)

將上述邊界條件轉(zhuǎn)換成矩陣形式為：

(5)

其中Cardinal矩陣為：

(6)

式中，s=(1-t)/2。

將上述矩陣方程展開(kāi)成多項(xiàng)式形式，則有：

(7)

進(jìn)一步將Tk-1，Tk，Tk+1，Tk+2分解成二維平面上x(chóng)，y方向上的分量，得到控制點(diǎn)Tk，Tk+1之間的曲線段的參數(shù)三項(xiàng)函數(shù)式在x，y方向上的表達(dá)式如下：

(8)

展開(kāi)后按u的升冪排列，得到控制點(diǎn)Tk，Tk+1之間的三次Cardinal樣條曲線參數(shù)形式如下：

(9)

式中參數(shù)如下：

(10)

定義參數(shù)矩陣：

(11)

經(jīng)由上述過(guò)程，得到了獨(dú)立的單側(cè)車道數(shù)據(jù)T1(c1,z1)到Tn-2(cn-2,zn-2)之間，經(jīng)過(guò)三次Cardinal樣條插值的車道線擬合方法，曲線參數(shù)集合Q=(t,Y2,…,Yk,…,Yn-2)，其中t為張量參數(shù)，k=2,3,…,n-2；由于Cardinal樣條法生成的曲線必定經(jīng)過(guò)原始控制點(diǎn)，因此利用高精度的車道線位置信息便可以精確地反映出車道線的位置與形狀；對(duì)于具體的原始車道位置數(shù)據(jù)，可由u在[0,1]上的取值步長(zhǎng)控制每段(兩個(gè)控制點(diǎn)之間)的插值個(gè)數(shù)，由t控制每段(兩個(gè)控制點(diǎn)之間)的曲線松緊程度，可以形成任意形狀的曲線，來(lái)滿足特定路段的控制要求與精度要求。

2.2 Cardinal樣條車道線擬合性能評(píng)價(jià)

為了充分驗(yàn)證本研究所提出的車道線擬合算法的有效性和通用性，選擇一段實(shí)際的直道彎道組合的道路進(jìn)行試驗(yàn)。首先利用第1節(jié)所述的組合導(dǎo)航系統(tǒng)進(jìn)行車道線位置點(diǎn)的采集，共采集40個(gè)離散的路徑點(diǎn)，如圖2所示。

圖2 離散路徑點(diǎn)采集

然后建立三次Cardinal樣條曲線，對(duì)上述離散車道線進(jìn)行擬合插值，為了充分驗(yàn)證算法的優(yōu)勢(shì)，將其與常用的Hermite樣條曲線的擬合結(jié)果進(jìn)行對(duì)比，得到的擬合結(jié)果如圖3所示。從平均位置誤差和算法時(shí)間兩個(gè)維度來(lái)評(píng)判算法的有效性，具體比較結(jié)果如表2所示。從圖3和表2中可以看出，在擬合的效果上Cardinal樣條要明顯優(yōu)于Hermite樣條，并且有著更快的擬合速度。

圖3 車道線擬合結(jié)果

表2 不同樣條函數(shù)擬合結(jié)果對(duì)比

3 關(guān)鍵參數(shù)估計(jì)方法

3.1 基礎(chǔ)參數(shù)預(yù)處理

對(duì)于車道偏離預(yù)警測(cè)評(píng)場(chǎng)景，車道偏離的界定標(biāo)準(zhǔn)為車輛前輪的外緣是否將要超出車道邊線，而組合導(dǎo)航系統(tǒng)記錄的車輛位置實(shí)際為系統(tǒng)主天線的位置，因此需要先將記錄的主天線運(yùn)動(dòng)軌跡轉(zhuǎn)換成主車(Host Vehicle,HV)前輪的運(yùn)動(dòng)軌跡，其過(guò)程如下：

假定該時(shí)刻為tc，tc時(shí)刻對(duì)應(yīng)的經(jīng)緯度坐標(biāo)為(xc,yc)，若組合導(dǎo)航信息的采集時(shí)刻沒(méi)有tc的對(duì)應(yīng)值，可采用線性插值計(jì)算得出經(jīng)緯度坐標(biāo)為(xc,yc)，xc為緯度，yc為經(jīng)度；若進(jìn)行插值操作，其余測(cè)量參數(shù)也應(yīng)進(jìn)行插值處理。然后將其轉(zhuǎn)換為高斯投影坐標(biāo)(xu,yu)(東向位置、北向位置確定的平面投影坐標(biāo)系)。

如圖4所示，假設(shè)主天線安裝在HV中軸線上，與兩前輪外緣處的縱向距離為d，橫向距離為L(zhǎng)，車輛航向角為H，H∈[0,360°]，定義北向?yàn)槠鹗挤较?，順時(shí)針?lè)较驗(yàn)檎?，由圖4可得：

圖4 車輛前輪位置解算

(12)

β=π-α，

(13)

(14)

d3=d1·cosβ，

(15)

d4=d-d3，

(16)

d2=d4·cosβ，

(17)

式中，α是車輛航向與正東的夾角；β是α的補(bǔ)角；d1，d2，d3，d4是車輛的幾何參數(shù)。

局部切平面直角坐標(biāo)系坐標(biāo)左前輪外緣處的位置坐標(biāo)Pl為：

(18)

類似的，可以根據(jù)幾何關(guān)系計(jì)算出右前輪外緣處的位置坐標(biāo)Pr：

(19)

對(duì)組合導(dǎo)航系統(tǒng)中記錄的HV位置均作以上處理，即可得到任意時(shí)刻HV前輪外緣的運(yùn)動(dòng)軌跡。

3.2 車輛與車道線的距離解算

車道偏離預(yù)警測(cè)評(píng)過(guò)程中，采用橫向距離來(lái)衡量車輛與車道線的距離，以上一節(jié)中計(jì)算所得的兩前輪外緣位置作為該特定部位，分別計(jì)算與兩側(cè)車道邊界的距離，以左側(cè)車輪外緣為例，計(jì)算過(guò)程如下：

假定該時(shí)刻仍為tc，局部切平面直角坐標(biāo)系坐標(biāo)左前輪外緣處的位置坐標(biāo)已知，為Pl；左側(cè)車道線擬合C1上，Pl位于控制點(diǎn)Tc和Tc+1之間確定路段的橫向區(qū)域內(nèi)，該路段對(duì)應(yīng)擬合曲線為：

(20)

式中，s=(1-t)/2；其本質(zhì)上對(duì)應(yīng)1個(gè)關(guān)于u∈[0,1]的多項(xiàng)式：

考慮到擬合實(shí)際車道線的情況下，兩控制點(diǎn)Tc和Tc+1之間擬合曲線斜率變化不大且較平緩，用點(diǎn)Pl到三次曲線P(u)的最短距離代替橫向距離，采用求解速度較快的牛頓迭代法求解：

設(shè)點(diǎn)Pl到曲線P(u)的距離為D(u)，

(D(u))2=|Pl-P(u)|2=

[xl-P(u)x]2+[yl-P(u)y]。

(22)

為求解min(D(u))2,u∈[0,1]，即求解(D(u))2在[0,1]上的極小值，問(wèn)題轉(zhuǎn)化為求極值問(wèn)題。(D(u))2的極值出現(xiàn)在其導(dǎo)數(shù)值為0的點(diǎn)，即：

2[xl-P(u)x]P(u)′x+2[yl-P(u)y]P(u)′y=0。

(23)

令：

f(u)=2[xl-P(u)x]P(u)′x+2[yl-P(u)y]P(u)′y，

(24)

(25)

用牛頓迭代公式求解：

(26)

選取曲線邊緣u=0或u=1為初值進(jìn)行迭代計(jì)算，得到的距離偏差D(uk+1)-D(uk)<0.02 m時(shí)停止迭代，取uc=uk+1，時(shí)刻tc對(duì)應(yīng)的距離為D(uc)，特別的，當(dāng)車輛輪胎外緣位于車道邊界線外側(cè)時(shí)，車輛與車道線距離D取負(fù)值。

3.3 偏離速度解算

車道偏離預(yù)警測(cè)評(píng)過(guò)程中，假如twarn時(shí)刻車道偏離報(bào)警被觸發(fā)，此時(shí)HV接近車道邊界的速度的垂直分量定義為偏離速度，以左偏情況為例，計(jì)算過(guò)程如下：

局部切平面直角坐標(biāo)系下，twarn時(shí)刻車輛左側(cè)附近車道線斜率為kwarn，kwarn由距離計(jì)算過(guò)程中迭代得出的插值點(diǎn)P(uk+1)處的切線斜率確定，車道線切線對(duì)應(yīng)角度為：

θ=arctan (kwarn)。

(27)

此時(shí)HV航向角為H，對(duì)應(yīng)角度為：

(28)

此時(shí)HV速度為v，對(duì)應(yīng)偏離速度為：

(29)

4 試驗(yàn)驗(yàn)證

針對(duì)上述提出的車道偏離預(yù)警測(cè)評(píng)關(guān)鍵基礎(chǔ)參數(shù)解算方法，利用實(shí)際測(cè)試數(shù)據(jù)進(jìn)行解算驗(yàn)證，以評(píng)價(jià)車道偏離預(yù)警有效性為例。

實(shí)際測(cè)試過(guò)程中，無(wú)論HV系統(tǒng)內(nèi)部如何設(shè)置報(bào)警觸發(fā)點(diǎn)，報(bào)警臨界線都應(yīng)位于最遲報(bào)警線內(nèi)側(cè)，因此選用最遲報(bào)警線作為偏離預(yù)警及時(shí)性的測(cè)評(píng)基準(zhǔn)。設(shè)最遲報(bào)警線位于車道邊界線外側(cè)距離為dlast處(乘用車dlast取0.3 m；商用車dlast取1 m)，當(dāng)HV有向車道邊界線偏離的趨勢(shì)時(shí)，該趨勢(shì)會(huì)反映在車輛與最遲報(bào)警線距離Dd、偏離速度vd和預(yù)計(jì)越界時(shí)間tcross的變化上，通過(guò)為其設(shè)定界限值分別可測(cè)評(píng)本次測(cè)試的有效性與預(yù)警情況的有效性。假定安全情況下車輛輪胎外緣與最遲報(bào)警線距離下限為D0，偏離速度上限為v0，預(yù)計(jì)越界時(shí)間下限為t0，界定條件如下：

Dd=D+dlast

(30)

(31)

(32)

當(dāng)預(yù)警時(shí)刻偏離速度vd、預(yù)計(jì)越界時(shí)間tcross滿足上述條件時(shí)，可認(rèn)定本次測(cè)試是有效的，即HV在測(cè)試中發(fā)生了偏離；在測(cè)試有效的前提下，HV偏離車道時(shí)，當(dāng)車輛位于臨界報(bào)警線之外時(shí)，系統(tǒng)應(yīng)持續(xù)報(bào)警，即當(dāng)HV與最遲報(bào)警線距離Dd

試驗(yàn)測(cè)試所得結(jié)果如圖5所示，圖中實(shí)線為利用采集的基礎(chǔ)數(shù)據(jù)擬合后的車道線，虛線為車輛行駛軌跡，五角星為標(biāo)注的預(yù)警點(diǎn)。根據(jù)上述提出的計(jì)算方法，分別利用Cardinal樣條和Hermite樣條進(jìn)行車道線的擬合，計(jì)算車道偏離預(yù)警時(shí)車輛位置與車道線的距離，并與實(shí)測(cè)真值進(jìn)行對(duì)比，結(jié)果如表3所示。表中，Cardinal樣條和Hermite樣條分別利用40個(gè)簡(jiǎn)化后的離散路徑點(diǎn)擬合曲線，再分別計(jì)算車道偏離預(yù)警發(fā)出時(shí)的車輛位置與擬合曲線的距離；真值為車道偏離預(yù)警發(fā)出時(shí)的車輛位置與4 000個(gè)原始路徑點(diǎn)的最短距離?？梢钥闯?，相對(duì)于Hermite樣條，Cardinal樣條擬合后的計(jì)算結(jié)果更加準(zhǔn)確。

圖5 實(shí)際測(cè)評(píng)結(jié)果

表3 關(guān)鍵參數(shù)解算結(jié)果對(duì)比

同時(shí)利用上述計(jì)算方法計(jì)算可得vd為1.28 m/s，tcross為0.21 s。這也驗(yàn)證了本研究所提出的測(cè)評(píng)方法可以準(zhǔn)確測(cè)量出測(cè)評(píng)所需關(guān)鍵參數(shù)，為開(kāi)展定量化測(cè)評(píng)和判斷測(cè)試結(jié)果提供準(zhǔn)確依據(jù)。

5 結(jié)論

本研究首先分析了當(dāng)前智能駕駛及車道偏離預(yù)警系統(tǒng)測(cè)評(píng)的必要性，指明了當(dāng)前缺乏有效的測(cè)評(píng)方法和指標(biāo)的現(xiàn)狀。然后，提出了一種基于Cardinal樣條的車道偏離預(yù)警測(cè)評(píng)關(guān)鍵參數(shù)估計(jì)方法，該方法首先利用高精度差分組合導(dǎo)航分別從車道線以及測(cè)試車輛上獲取測(cè)評(píng)所需基礎(chǔ)參數(shù)數(shù)據(jù)，然后對(duì)采集到的車道線坐標(biāo)進(jìn)行Cardinal樣條擬合，并且與Hermite樣條擬合方式進(jìn)行對(duì)比，擬合精度和速度均優(yōu)于后者。在得到高精度車道線的基礎(chǔ)上，利用車道線信息和車載采集信息，結(jié)合牛頓迭代算法，進(jìn)行關(guān)鍵參數(shù)的解算。