基于Cardinal樣條的車道偏離預警測評關鍵參數估計

李宏海，陸紅偉，盧立陽，徐啟敏

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

0 引言

智能車路系統(Intelligent Vehicle and Infra Structure System，IVIS)是目前交通領域的科技前沿，是為了實現車車和車路全方位多角度的協同互動，從而提高行車和行人安全，減少交通事故的發生和提高道路通行效率。為了有效實現上述目標，智能車路系統采用了先進的通信技術和全時空高度統一的多傳感器融合采集技術等，能夠對道路和車輛信息進行高精度實時可靠的信息采集和融合通信，對保障道路安全和提高出行效率具有十分重要的意義。當前歐美日等發達國家均在實施和推進IVIS發展的相關行動計劃，我國智能汽車和智能路側設備等產業也發展迅速[1-3]。

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

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

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

針對上述問題，本研究提出了一種基于Cardinal樣條的車道偏離預警測評關鍵參數估計方法。該方法首先利用高精度差分組合導航分別從車道線以及測試車輛上獲取測評所需基礎參數數據，然后對采集到的車道線坐標進行Cardinal樣條擬合，從而提高車道線的測量精度，減小測評誤差；在得到高精度車道線的基礎上，利用車道線信息和車載采集信息，結合牛頓迭代算法，進行關鍵參數的解算。

1 基礎參數采集

精確的基礎參數采集是進行LDWS測評的基礎，本研究采用基于GNSS/INS的組合導航系統來實現位置、速度等基礎參數信息的準確采集。

1.1 全球導航衛星系統

全球導航衛星系統(Global Navigation Satellites System，GNSS)是以接收和播發電磁波信號來達到導航、定位、授時(簡稱PNT)的目的。其依賴于高軌道的衛星播發的導航電文和載波信號，在大地測量、車聯網、自動駕駛等領域發揮著重要作用。

長期以來，一直以美國的GPS為代表的衛星導航具有全天候、全天時的優勢。隨著歐盟Gallieo、俄羅斯GLONASS以及中國BDS的發展，多頻率、多系統的衛星導航定位已經成為現實。就目前而言，衛星導航的發展已久，前景無量，尤其是以北斗3組網完成為標志，研究北斗與其他GNSS的組合，在我國具有非常重要的意義和價值。

除此之外，衛星導航高精度定位已經成為現實，如發展相對成熟的RTK技術、PPP技術等，相對于其他導航系統的優勢在于誤差不會隨著時間累積。但是其研究也存在瓶頸，如PPP技術不能快速收斂、模糊度固定問題、復雜環境下信號缺失問題、模糊度重固定問題、高精度的動態定位問題等，而這些問題就目前而言僅僅依賴衛星導航是基本不可能得到有效解決的，因此需要將衛星導航同其他導航系統相結合，來彌補GNSS衛星導航的先天性不足。

1.2 慣性導航

慣性導航系統(Inertial Navigation System，INS)是一種相對獨立自主的導航系統，不與外界發生任何信號交換便可完成導航和定位。隨著計算機技術的發展，基于捷聯技術的慣導因具有結構簡單等優點，逐步取代了平臺式慣導，成為目前應用最廣泛的INS，簡稱SINS。

由于慣性導航自身的獨特性質，使其在定位過程中的誤差會隨著時間逐漸累積，單靠INS完成長時間導航定位的結果是非常不可靠的，因此其并不具備高精度的長期導航定位的能力。

1.3 GNSS/INS組合導航

綜上所述，GNSS和INS各有優缺點，但同時能夠優勢互補，因此目前多將其進行組合使用，這對于提高導航性能和精度有著重要的意義，其優勢主要體現在：

(1)可以對慣性導航系統誤差進行估計和校正，提高組合導航精度；

(2)可以彌補衛星導航信號缺損問題，提高組合導航能力；

(3)可以提高衛星導航接收機對衛星信號的捕獲和跟蹤能力，提高整體導航效率；

(4)可以增加觀測信息冗余度，提高系統容錯能力；

(5)可以提高導航系統的抗干擾性，提高組合導航的完好性。

根據應用要求及參與組合觀測量的深度，GNSS/INS組合的模式可分為松組合、緊組合和深組合。3種組合模式的對比示意圖如圖1所示，從圖中可以直觀地區分3種組合模式的層次。

圖1 GNSS/INS組合示意圖

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

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

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

本研究基礎參數采集中使用的算法即為上述深組合算法，為車道偏離預警測評提供精準的位置、速度、航向角等信息。

2 Cardinal樣條車道線擬合

利用上述組合導航系統進行道路基礎位置坐標的采集之后，由于采集到的點較為稀疏，無法準確表征道路的實際情況，因此還需要利用擬合算法對車道離散點進行插值擬合，從而更好地估計車道線的參數信息，為后續的測評參數解算打下良好的基礎。在此采用Cardinal樣條對離散車道坐標點進行擬合。

2.1 Cardinal樣條車道線擬合模型

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

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

P(0)=Tk，

(1)

P(1)=Tk+1，

(2)

(3)

(4)

將上述邊界條件轉換成矩陣形式為：

(5)

其中Cardinal矩陣為：

(6)

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

將上述矩陣方程展開成多項式形式，則有：

(7)

進一步將Tk-1，Tk，Tk+1，Tk+2分解成二維平面上x，y方向上的分量，得到控制點Tk，Tk+1之間的曲線段的參數三項函數式在x，y方向上的表達式如下：

(8)

展開后按u的升冪排列，得到控制點Tk，Tk+1之間的三次Cardinal樣條曲線參數形式如下：

(9)

式中參數如下：

(10)

定義參數矩陣：

(11)

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

2.2 Cardinal樣條車道線擬合性能評價

為了充分驗證本研究所提出的車道線擬合算法的有效性和通用性，選擇一段實際的直道彎道組合的道路進行試驗。首先利用第1節所述的組合導航系統進行車道線位置點的采集，共采集40個離散的路徑點，如圖2所示。

圖2 離散路徑點采集

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

圖3 車道線擬合結果

表2 不同樣條函數擬合結果對比

3 關鍵參數估計方法

3.1 基礎參數預處理

對于車道偏離預警測評場景，車道偏離的界定標準為車輛前輪的外緣是否將要超出車道邊線，而組合導航系統記錄的車輛位置實際為系統主天線的位置，因此需要先將記錄的主天線運動軌跡轉換成主車(Host Vehicle,HV)前輪的運動軌跡，其過程如下：

假定該時刻為tc，tc時刻對應的經緯度坐標為(xc,yc)，若組合導航信息的采集時刻沒有tc的對應值，可采用線性插值計算得出經緯度坐標為(xc,yc)，xc為緯度，yc為經度；若進行插值操作，其余測量參數也應進行插值處理。然后將其轉換為高斯投影坐標(xu,yu)(東向位置、北向位置確定的平面投影坐標系)。

如圖4所示，假設主天線安裝在HV中軸線上，與兩前輪外緣處的縱向距離為d，橫向距離為L，車輛航向角為H，H∈[0,360°]，定義北向為起始方向，順時針方向為正，由圖4可得：

圖4 車輛前輪位置解算

(12)

β=π-α，

(13)

(14)

d3=d1·cosβ，

(15)

d4=d-d3，

(16)

d2=d4·cosβ，

(17)

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

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

(18)

類似的，可以根據幾何關系計算出右前輪外緣處的位置坐標Pr：

(19)

對組合導航系統中記錄的HV位置均作以上處理，即可得到任意時刻HV前輪外緣的運動軌跡。

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

車道偏離預警測評過程中，采用橫向距離來衡量車輛與車道線的距離，以上一節中計算所得的兩前輪外緣位置作為該特定部位，分別計算與兩側車道邊界的距離，以左側車輪外緣為例，計算過程如下：

假定該時刻仍為tc，局部切平面直角坐標系坐標左前輪外緣處的位置坐標已知，為Pl；左側車道線擬合C1上，Pl位于控制點Tc和Tc+1之間確定路段的橫向區域內，該路段對應擬合曲線為：

(20)

式中，s=(1-t)/2；其本質上對應1個關于u∈[0,1]的多項式：

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

設點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]上的極小值，問題轉化為求極值問題。(D(u))2的極值出現在其導數值為0的點，即：

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為初值進行迭代計算，得到的距離偏差D(uk+1)-D(uk)<0.02 m時停止迭代，取uc=uk+1，時刻tc對應的距離為D(uc)，特別的，當車輛輪胎外緣位于車道邊界線外側時，車輛與車道線距離D取負值。

3.3 偏離速度解算

車道偏離預警測評過程中，假如twarn時刻車道偏離報警被觸發，此時HV接近車道邊界的速度的垂直分量定義為偏離速度，以左偏情況為例，計算過程如下：

局部切平面直角坐標系下，twarn時刻車輛左側附近車道線斜率為kwarn，kwarn由距離計算過程中迭代得出的插值點P(uk+1)處的切線斜率確定，車道線切線對應角度為：

θ=arctan (kwarn)。

(27)

此時HV航向角為H，對應角度為：

(28)

此時HV速度為v，對應偏離速度為：

(29)

4 試驗驗證

針對上述提出的車道偏離預警測評關鍵基礎參數解算方法，利用實際測試數據進行解算驗證，以評價車道偏離預警有效性為例。

實際測試過程中，無論HV系統內部如何設置報警觸發點，報警臨界線都應位于最遲報警線內側，因此選用最遲報警線作為偏離預警及時性的測評基準。設最遲報警線位于車道邊界線外側距離為dlast處(乘用車dlast取0.3 m；商用車dlast取1 m)，當HV有向車道邊界線偏離的趨勢時，該趨勢會反映在車輛與最遲報警線距離Dd、偏離速度vd和預計越界時間tcross的變化上，通過為其設定界限值分別可測評本次測試的有效性與預警情況的有效性。假定安全情況下車輛輪胎外緣與最遲報警線距離下限為D0，偏離速度上限為v0，預計越界時間下限為t0，界定條件如下：

Dd=D+dlast

(30)

(31)

(32)

當預警時刻偏離速度vd、預計越界時間tcross滿足上述條件時，可認定本次測試是有效的，即HV在測試中發生了偏離；在測試有效的前提下，HV偏離車道時，當車輛位于臨界報警線之外時，系統應持續報警，即當HV與最遲報警線距離Dd

試驗測試所得結果如圖5所示，圖中實線為利用采集的基礎數據擬合后的車道線，虛線為車輛行駛軌跡，五角星為標注的預警點。根據上述提出的計算方法，分別利用Cardinal樣條和Hermite樣條進行車道線的擬合，計算車道偏離預警時車輛位置與車道線的距離，并與實測真值進行對比，結果如表3所示。表中，Cardinal樣條和Hermite樣條分別利用40個簡化后的離散路徑點擬合曲線，再分別計算車道偏離預警發出時的車輛位置與擬合曲線的距離；真值為車道偏離預警發出時的車輛位置與4 000個原始路徑點的最短距離。可以看出，相對于Hermite樣條，Cardinal樣條擬合后的計算結果更加準確。

圖5 實際測評結果

表3 關鍵參數解算結果對比

同時利用上述計算方法計算可得vd為1.28 m/s，tcross為0.21 s。這也驗證了本研究所提出的測評方法可以準確測量出測評所需關鍵參數，為開展定量化測評和判斷測試結果提供準確依據。

5 結論

本研究首先分析了當前智能駕駛及車道偏離預警系統測評的必要性，指明了當前缺乏有效的測評方法和指標的現狀。然后，提出了一種基于Cardinal樣條的車道偏離預警測評關鍵參數估計方法，該方法首先利用高精度差分組合導航分別從車道線以及測試車輛上獲取測評所需基礎參數數據，然后對采集到的車道線坐標進行Cardinal樣條擬合，并且與Hermite樣條擬合方式進行對比，擬合精度和速度均優于后者。在得到高精度車道線的基礎上，利用車道線信息和車載采集信息，結合牛頓迭代算法，進行關鍵參數的解算。