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

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

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

0 引言

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

先進(jìn)駕駛輔助系統(tǒng)(Advanced Driver Assistance System， ADAS)是智能車(chē)路系統(tǒng)的重要組成部分，對(duì)于智能車(chē)路系統(tǒng)的發(fā)展具有十分重要的意義，目前也被廣泛應(yīng)用于各種車(chē)輛上。ADAS是利用安裝于車(chē)輛端的各種傳感器進(jìn)行車(chē)輛行駛過(guò)程中環(huán)境的感知和數(shù)據(jù)采集分析，如利用衛(wèi)星導(dǎo)航進(jìn)行車(chē)輛實(shí)時(shí)高精度的定位，利用慣導(dǎo)進(jìn)行車(chē)輛姿態(tài)的識(shí)別和速度、加速度的采集，利用雷達(dá)(毫米波雷達(dá)、激光雷達(dá)等)和攝像頭進(jìn)行環(huán)境的感知和監(jiān)測(cè)等，然后將這些傳感器信息進(jìn)行融合處理后，進(jìn)行系統(tǒng)化分析處理，給出合理的駕駛建議，提高駕駛的安全性和舒適性。據(jù)維基百科的定義，ADAS包括自適應(yīng)巡航、車(chē)道偏離預(yù)警系統(tǒng)、車(chē)道保持系統(tǒng)等在內(nèi)的多個(gè)系統(tǒng)及功能。本研究主要研究車(chē)道偏離預(yù)警系統(tǒng)(Lane Departure Warning System,LDWS)，在車(chē)輛行駛過(guò)程中，LDWS會(huì)利用相關(guān)傳感器(如攝像頭等)實(shí)時(shí)監(jiān)測(cè)車(chē)輛與車(chē)道的相對(duì)位置情況，當(dāng)檢測(cè)到車(chē)輛偏離車(chē)道并且有一定的危險(xiǎn)時(shí)，會(huì)采取相應(yīng)的音視頻手段提醒駕駛員，為其提供充足的反應(yīng)時(shí)間。車(chē)道偏離預(yù)警系統(tǒng)能夠極大地提高行車(chē)安全，減少因駕駛員疲勞駕駛從而導(dǎo)致車(chē)道偏離乃至發(fā)生交通事故的風(fēng)險(xiǎn)，在智能車(chē)路系統(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)采用車(chē)載高性能工作站進(jìn)行圖片的實(shí)時(shí)處理，最大處理速率達(dá)60 Hz以上，能夠很好地滿足車(chē)輛行駛中對(duì)于預(yù)警系統(tǒng)實(shí)時(shí)性的需求[7-8]；美德兩國(guó)聯(lián)合研發(fā)了AutoVue系統(tǒng)[9]，該系統(tǒng)主要面向營(yíng)運(yùn)卡車(chē)并獲得實(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)化道路的車(chē)道線檢測(cè)和車(chē)道偏離預(yù)警，也可以用于沒(méi)有車(chē)道線的非結(jié)構(gòu)化路面，利用車(chē)道的路肩來(lái)判斷車(chē)輛的位置；日本三菱汽車(chē)公司設(shè)計(jì)和研發(fā)了DSS，并在模型車(chē)上進(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”智能車(chē)[12]，浙江大學(xué)針對(duì)自主智能車(chē)輛引進(jìn)的分布式測(cè)試系統(tǒng)IVD (Intelligent Vehicle Detection-bed)[13-14]，國(guó)防科技大學(xué)研發(fā)的CITAVT系列智能車(chē)[15-16]，吉林大學(xué)開(kāi)發(fā)的JLUVA-1系統(tǒng)[17-18]和由東南大學(xué)基于DSP技術(shù)開(kāi)發(fā)的車(chē)道偏離報(bào)警系統(tǒng)[19-20]，除此之外國(guó)內(nèi)其他一些科研單位和高校在LDWS方面的研究也在如火如荼地進(jìn)行中。

當(dāng)前，雖然包括LDWS在內(nèi)的先進(jìn)駕駛輔助系統(tǒng)發(fā)展迅速，各個(gè)汽車(chē)廠家均推出了高智能度輔助駕駛的汽車(chē)產(chǎn)品，但近年來(lái)不管是特斯拉、谷歌還是Uber等都出現(xiàn)了很多交通事故，而且Uber還在2018年3月發(fā)生了第1起自動(dòng)駕駛汽車(chē)致死事故。這些都說(shuō)明當(dāng)前在大力發(fā)展高智能ADAS的同時(shí)，對(duì)系統(tǒng)的測(cè)試和評(píng)價(jià)也至關(guān)重要，如何科學(xué)地判別智能汽車(chē)的安全性和可靠性，也是當(dāng)前面臨的一個(gè)重要問(wèn)題。對(duì)于車(chē)道偏離預(yù)警系統(tǒng)(LDWS)的測(cè)評(píng)，目前我國(guó)已有標(biāo)準(zhǔn)《智能運(yùn)輸系統(tǒng) 車(chē)道偏離報(bào)警系統(tǒng) 性能要求與檢測(cè)方法》(GB/T 26773—2011)，但在該標(biāo)準(zhǔn)中僅列出測(cè)評(píng)場(chǎng)景及流程，未給出車(chē)道線距離等測(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樣條的車(chē)道偏離預(yù)警測(cè)評(píng)關(guān)鍵參數(shù)估計(jì)方法。該方法首先利用高精度差分組合導(dǎo)航分別從車(chē)道線以及測(cè)試車(chē)輛上獲取測(cè)評(píng)所需基礎(chǔ)參數(shù)數(shù)據(jù)，然后對(duì)采集到的車(chē)道線坐標(biāo)進(jìn)行Cardinal樣條擬合，從而提高車(chē)道線的測(cè)量精度，減小測(cè)評(píng)誤差；在得到高精度車(chē)道線的基礎(chǔ)上，利用車(chē)道線信息和車(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)稱(chēng)PNT)的目的。其依賴(lài)于高軌道的衛(wèi)星播發(fā)的導(dǎo)航電文和載波信號(hào)，在大地測(cè)量、車(chē)聯(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)題就目前而言?xún)H僅依賴(lài)衛(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)稱(chēng)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ù)采集中使用的算法即為上述深組合算法，為車(chē)道偏離預(yù)警測(cè)評(píng)提供精準(zhǔn)的位置、速度、航向角等信息。

2 Cardinal樣條車(chē)道線擬合

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

2.1 Cardinal樣條車(chē)道線擬合模型

采集得到的某段獨(dú)立道路軌跡數(shù)據(jù)為T(mén)={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è)車(chē)道數(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)用于建立擬合車(chē)道線的邊界條件：

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

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

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

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

然后建立三次Cardinal樣條曲線，對(duì)上述離散車(chē)道線進(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 車(chē)道線擬合結(jié)果

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

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

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

對(duì)于車(chē)道偏離預(yù)警測(cè)評(píng)場(chǎng)景，車(chē)道偏離的界定標(biāo)準(zhǔn)為車(chē)輛前輪的外緣是否將要超出車(chē)道邊線，而組合導(dǎo)航系統(tǒng)記錄的車(chē)輛位置實(shí)際為系統(tǒng)主天線的位置，因此需要先將記錄的主天線運(yùn)動(dòng)軌跡轉(zhuǎn)換成主車(chē)(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)，車(chē)輛航向角為H，H∈[0,360°]，定義北向?yàn)槠鹗挤较颍槙r(shí)針?lè)较驗(yàn)檎蓤D4可得：

圖4 車(chē)輛前輪位置解算

(12)

β=π-α，

(13)

(14)

d3=d1·cosβ，

(15)

d4=d-d3，

(16)

d2=d4·cosβ，

(17)

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

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

(18)

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

(19)

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

3.2 車(chē)輛與車(chē)道線的距離解算

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

假定該時(shí)刻仍為tc，局部切平面直角坐標(biāo)系坐標(biāo)左前輪外緣處的位置坐標(biāo)已知，為Pl；左側(cè)車(chē)道線擬合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í)際車(chē)道線的情況下，兩控制點(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)車(chē)輛輪胎外緣位于車(chē)道邊界線外側(cè)時(shí)，車(chē)輛與車(chē)道線距離D取負(fù)值。

3.3 偏離速度解算

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

局部切平面直角坐標(biāo)系下，twarn時(shí)刻車(chē)輛左側(cè)附近車(chē)道線斜率為kwarn，kwarn由距離計(jì)算過(guò)程中迭代得出的插值點(diǎn)P(uk+1)處的切線斜率確定，車(chē)道線切線對(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ì)上述提出的車(chē)道偏離預(yù)警測(cè)評(píng)關(guān)鍵基礎(chǔ)參數(shù)解算方法，利用實(shí)際測(cè)試數(shù)據(jù)進(jìn)行解算驗(yàn)證，以評(píng)價(jià)車(chē)道偏離預(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)警線位于車(chē)道邊界線外側(cè)距離為dlast處(乘用車(chē)dlast取0.3 m；商用車(chē)dlast取1 m)，當(dāng)HV有向車(chē)道邊界線偏離的趨勢(shì)時(shí)，該趨勢(shì)會(huì)反映在車(chē)輛與最遲報(bào)警線距離Dd、偏離速度vd和預(yù)計(jì)越界時(shí)間tcross的變化上，通過(guò)為其設(shè)定界限值分別可測(cè)評(píng)本次測(cè)試的有效性與預(yù)警情況的有效性。假定安全情況下車(chē)輛輪胎外緣與最遲報(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偏離車(chē)道時(shí)，當(dāng)車(chē)輛位于臨界報(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ù)擬合后的車(chē)道線，虛線為車(chē)輛行駛軌跡，五角星為標(biāo)注的預(yù)警點(diǎn)。根據(jù)上述提出的計(jì)算方法，分別利用Cardinal樣條和Hermite樣條進(jìn)行車(chē)道線的擬合，計(jì)算車(chē)道偏離預(yù)警時(shí)車(chē)輛位置與車(chē)道線的距離，并與實(shí)測(cè)真值進(jìn)行對(duì)比，結(jié)果如表3所示。表中，Cardinal樣條和Hermite樣條分別利用40個(gè)簡(jiǎn)化后的離散路徑點(diǎn)擬合曲線，再分別計(jì)算車(chē)道偏離預(yù)警發(fā)出時(shí)的車(chē)輛位置與擬合曲線的距離；真值為車(chē)道偏離預(yù)警發(fā)出時(shí)的車(chē)輛位置與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)前智能駕駛及車(chē)道偏離預(yù)警系統(tǒng)測(cè)評(píng)的必要性，指明了當(dāng)前缺乏有效的測(cè)評(píng)方法和指標(biāo)的現(xiàn)狀。然后，提出了一種基于Cardinal樣條的車(chē)道偏離預(yù)警測(cè)評(píng)關(guān)鍵參數(shù)估計(jì)方法，該方法首先利用高精度差分組合導(dǎo)航分別從車(chē)道線以及測(cè)試車(chē)輛上獲取測(cè)評(píng)所需基礎(chǔ)參數(shù)數(shù)據(jù)，然后對(duì)采集到的車(chē)道線坐標(biāo)進(jìn)行Cardinal樣條擬合，并且與Hermite樣條擬合方式進(jìn)行對(duì)比，擬合精度和速度均優(yōu)于后者。在得到高精度車(chē)道線的基礎(chǔ)上，利用車(chē)道線信息和車(chē)載采集信息，結(jié)合牛頓迭代算法，進(jìn)行關(guān)鍵參數(shù)的解算。