王任棟，章永進(jìn)，徐友春，馬育林

(1.軍事交通學(xué)院研究生管理大隊(duì)，天津300161;2.軍事交通學(xué)院軍用車輛系，天津300161)

電子地圖的精確導(dǎo)航是使智能車在復(fù)雜的道 路環(huán)境中快速準(zhǔn)確地到達(dá)指定地點(diǎn)的重要條件［1］，是無(wú)人駕駛技術(shù)的難點(diǎn)之一，也是熱點(diǎn)之一。目前常用的方法有兩種:一種是基于高精度駕駛地圖的導(dǎo)航方法;另一種是基于現(xiàn)有常規(guī)電子地圖的導(dǎo)航方法。

Google、Benz等公司智能車研發(fā)團(tuán)隊(duì)采用的就是基于高精度駕駛地圖的導(dǎo)航方法。這種方法中高精度駕駛地圖的構(gòu)建是該方法實(shí)現(xiàn)的關(guān)鍵，分為數(shù)據(jù)采集、數(shù)據(jù)處理和道路建模3個(gè)步驟。首先使用激光雷達(dá)、攝像頭、GPS接收機(jī)、慣導(dǎo)等傳感器進(jìn)行道路環(huán)境數(shù)據(jù)的采集，然后對(duì)數(shù)據(jù)進(jìn)行提取、融合等處理，最后通過(guò)道路及環(huán)境建模的方式生成地圖。該地圖含有豐富的道路環(huán)境信息，不但可以為智能車進(jìn)行精確導(dǎo)航，還實(shí)現(xiàn)了智能車的高精度定位［2－3］。但由于需事先進(jìn)行大量的數(shù)據(jù)采集、處理并進(jìn)行地圖構(gòu)建，隨著地圖覆蓋范圍的擴(kuò)大，其工作量、耗時(shí)、成本以及所需的存儲(chǔ)空間都會(huì)大大增加。Kichun等［4］提出一種對(duì)高精度地圖導(dǎo)航數(shù)據(jù)進(jìn)行精簡(jiǎn)的方法，通過(guò)一種B樣條曲線的逐漸修正算法，將車輛行駛的歷史軌跡用很少的數(shù)據(jù)表示出來(lái)，降低了地圖存儲(chǔ)的數(shù)據(jù)量。但該方法依然需事先駕車進(jìn)行數(shù)據(jù)采集，且僅適用于與采集車行駛特性相近的智能車。

與基于高精度駕駛地圖的導(dǎo)航方法相比，基于現(xiàn)有常規(guī)電子地圖的導(dǎo)航方法具有成本低、覆蓋范圍廣、路網(wǎng)完整、數(shù)據(jù)結(jié)構(gòu)簡(jiǎn)單、占用存儲(chǔ)空間少等優(yōu)點(diǎn)，已經(jīng)可以為有人駕駛汽車提供良好的導(dǎo)航服務(wù)。但由于其路網(wǎng)數(shù)據(jù)稀疏，難以直接應(yīng)用于面向無(wú)人駕駛的智能車導(dǎo)航。針對(duì)該問(wèn)題，本文提出一種基于現(xiàn)有地圖稀疏路網(wǎng)數(shù)據(jù)的導(dǎo)航路徑生成及優(yōu)化方法。

1 算法基本理論

1.1 導(dǎo)航路徑生成算法

式中:xi、yi為數(shù)據(jù)點(diǎn)的經(jīng)緯度坐標(biāo);θi為智能車在該點(diǎn)的行駛方向，且

1.2 導(dǎo)航路徑優(yōu)化算法

其中

式中:vr為車輛的最小轉(zhuǎn)彎半徑;vd為車身寬度。

式中:di為路段的寬度;αi為路段li與li+1的夾角，由li與li+1的原始路網(wǎng)數(shù)據(jù)計(jì)算得到。

2 導(dǎo)航路徑的生成

本文使用規(guī)劃的路徑路網(wǎng)數(shù)據(jù)模型為基于離散點(diǎn)連線的折線模型，該模型的基本要素為道路節(jié)點(diǎn)和路段折線。節(jié)點(diǎn)是描述路網(wǎng)結(jié)構(gòu)的特征點(diǎn)，包括道路的起點(diǎn)、終點(diǎn)、曲率變化點(diǎn)、道路交叉點(diǎn)以及一些特殊位置點(diǎn)，位于由同向車道組成道路的中心線上。路段折線由節(jié)點(diǎn)依次連接而成，同一道路中同向行駛的多條車道用一條折線表示，不同道路或同一道路不同方向的車道分別用不同的折線表示。本文使用的路網(wǎng)數(shù)據(jù)精度較高，忽略其誤差。

從規(guī)劃路徑中獲取的路網(wǎng)數(shù)據(jù)集合為

式中:li為規(guī)劃路徑L中的路段;(xij，yij)為路段li上的節(jié)點(diǎn)坐標(biāo)。

2.1 路網(wǎng)數(shù)據(jù)的插值

由于原始路網(wǎng)數(shù)據(jù)稀疏且分布不均，直接根據(jù)地圖路網(wǎng)數(shù)據(jù)進(jìn)行曲線擬合很難取得良好效果。針對(duì)這一問(wèn)題，首先對(duì)路網(wǎng)數(shù)據(jù)進(jìn)行插值處理，在不改變道路幾何形狀的前提下，增加原始點(diǎn)數(shù)量，生成足夠多的擬合點(diǎn)，提高曲線擬合精度。

電子地圖的路徑規(guī)劃生成的是一條以起點(diǎn)和終點(diǎn)為端點(diǎn)、經(jīng)過(guò)有限個(gè)節(jié)點(diǎn)的連續(xù)折線，用S表示起點(diǎn)到折線上任意一點(diǎn)的折線距離，S的取值與折線上的點(diǎn)存在一一對(duì)應(yīng)的關(guān)系，因此，將S作為路網(wǎng)數(shù)據(jù)插值變量，并用S的大小表示路徑折線中任意一點(diǎn)的位置，即

對(duì)折射距離變量S的插值等價(jià)于對(duì)原始路網(wǎng)數(shù)據(jù)的插值。

通過(guò)分析地圖路網(wǎng)模型及各種插值方法的特點(diǎn)，分別采用線性插值、3次插值、樣條插值處理路網(wǎng)數(shù)據(jù)，并將插值結(jié)果與地圖規(guī)劃路線進(jìn)行對(duì)比(如圖1所示)。

圖1 3種插值方法的結(jié)果與原始規(guī)劃路徑對(duì)比

由于線性插值的特點(diǎn)與路網(wǎng)模型的結(jié)構(gòu)類似，插值得到的數(shù)據(jù)基本保持了原始路段的幾何形狀;而3次插值和樣條插值的結(jié)果與原始路段偏差較大。因此，采用線性插值的方式對(duì)變量S進(jìn)行處理，插值間距為ρ，得到一組擬合控制點(diǎn):|i=1，2，…，ma}={|i=1，2，…，ma}其中

2.2 擬合導(dǎo)航路徑曲線

路網(wǎng)數(shù)據(jù)的線性插值能夠表達(dá)規(guī)劃路徑所在道路的幾何形狀，但車輛的實(shí)際行駛路徑不可能完全與插值軌跡重合。原因是車輛行駛路徑為曲率平穩(wěn)變化的曲線，而線性插值得到的道路模型為連續(xù)的折線，二者在需要變更道路的路口或曲率較大的路段存在較大偏差。

由連續(xù)曲線段構(gòu)成的樣條曲線在道路模型的構(gòu)建中有著廣泛的應(yīng)用。樣條曲線是一個(gè)由多個(gè)分段曲線組成的線性方程組，用它進(jìn)行路徑幾何形狀的近似是非常穩(wěn)定和簡(jiǎn)單的［4］。其中插值樣條和逼近樣條是最常用的兩種。插值樣條生成的路徑嚴(yán)格穿過(guò)所有的曲線控制點(diǎn)，可以用控制點(diǎn)對(duì)路徑形狀進(jìn)行直觀的估計(jì)。其優(yōu)點(diǎn)是可以直接使用控制點(diǎn)對(duì)引導(dǎo)路徑模型進(jìn)行表示，但單個(gè)控制點(diǎn)的改變會(huì)對(duì)曲線的形狀產(chǎn)生較大影響。逼近樣條中，曲線的控制點(diǎn)不一定全部落在生成的曲線上，B樣條就是一種典型的逼近樣條方法［5］。雖然B樣條不如插值樣條表示得更加直觀，但其曲線的整體形狀受單個(gè)控制點(diǎn)變化的影響較小，更加適用于智能車導(dǎo)航路線的生成。此外，B樣條的很多性質(zhì)可以用來(lái)對(duì)車輛真實(shí)的行駛路徑進(jìn)行近似，如凸包性、幾何不變性、數(shù)值穩(wěn)定性等，通過(guò)B樣條擬合的曲線能夠保證車輛行駛的平順性。B樣條曲線的數(shù)學(xué)表達(dá)式為

式中:p為樣條的階數(shù);bj(j=0，1，…，n)=，為控制點(diǎn);t=S，為樣條參數(shù)。

控制點(diǎn)決定了樣條的凸包性。因此，樣條曲線開始于第一個(gè)控制點(diǎn)b0，終止于最后一個(gè)控制點(diǎn)bn，其間具有凸包性。節(jié)點(diǎn)向量T表示為

向量T中的元素為單調(diào)不減數(shù)列。為了將樣條曲線的端點(diǎn)固定在控制點(diǎn)的起點(diǎn)和終點(diǎn)即b0和bn，將第一個(gè)和最后一個(gè)節(jié)點(diǎn)以一個(gè)定值重復(fù)p次，如下所示:

式中 j=1，2，…，p。

3 導(dǎo)航路徑的優(yōu)化

3.1 導(dǎo)航路徑的約束模型

為使智能車在無(wú)外界環(huán)境干擾時(shí)能夠準(zhǔn)確地沿引導(dǎo)路徑行駛，導(dǎo)航路徑需要滿足由車輛幾何特性與道路幾何特性決定的約束條件。

(1)車輛幾何約束。轉(zhuǎn)彎處的導(dǎo)航路徑的曲率半徑大于車輛自身的最小轉(zhuǎn)彎半徑。

(2)道路幾何約束。導(dǎo)航路徑落在規(guī)劃路徑所在道路的可行駛區(qū)域內(nèi)。

根據(jù)這兩個(gè)約束條件，構(gòu)建圖2所示的車輛導(dǎo)航路徑約束模型。

圖2 車輛導(dǎo)航路徑約束模型

圖2中，P點(diǎn)為原始路徑規(guī)劃折線上的一點(diǎn)，P點(diǎn)的位置用從起點(diǎn)到該點(diǎn)的折線距離S表示。線段l1和l2表示規(guī)劃路徑中相鄰兩路段的中心線，將l1和l2沿各自法線方向向兩側(cè)平移，平移距離分別為d1和d2，得到位于中心線內(nèi)側(cè)和外側(cè)的兩組新的線段，其中內(nèi)側(cè)一組的兩條線段相交于一點(diǎn)C。過(guò)C作l1和l2的法線，與l1和l2分別相交于點(diǎn)Pc1和Pc2，它們的位置可根據(jù)路段寬度及路段間夾角求得。當(dāng)P點(diǎn)落在Pc1和Pc2之間的路段時(shí)，取|PC|－dv/2作為引導(dǎo)路徑允許的最大偏移量 Emax(s)，即

式中:dv為車身寬度;dl1、dl2為 l1、l2所在路段的路寬;SPPc1、SPPc2由 S唯一確定。

當(dāng)P點(diǎn)落在l1的其他位置時(shí)，Emax(s)=d1－dv/2;當(dāng)P點(diǎn)落在l2的其他位置時(shí)，Emax(s)=d2－dv/2。圓O是以r為半徑、與l1和l2同時(shí)相切的圓，切點(diǎn)為PO1、PO2。當(dāng)P點(diǎn)落在PO1和PO2之間的路段時(shí)，連接PO，與圓O相交于點(diǎn)Q，取|PQ|作為引導(dǎo)路徑的最小偏移量Emin(s)，即

式中:dm為車輛的最小轉(zhuǎn)彎直徑;SPPO1、SPPO2由S唯一確定。

當(dāng)P點(diǎn)落在l1、l2上其他位置時(shí)，Emin(s)=0。

取P點(diǎn)到擬合曲線的距離為eP，表示擬合曲線與原始規(guī)劃路徑之間的偏差。在給定路線上，對(duì)任意一點(diǎn)P，若eP均滿足

則認(rèn)為導(dǎo)航曲線滿足其約束條件。

3.2 擬合曲線的優(yōu)化

在生成智能車的導(dǎo)航曲線后，要對(duì)其是否滿足約束條件進(jìn)行檢驗(yàn)和調(diào)整。由于導(dǎo)航曲線是對(duì)控制點(diǎn)進(jìn)行B樣條擬合得到的，控制點(diǎn)的數(shù)量和位置決定了曲線擬合的質(zhì)量;而控制點(diǎn)是由原始數(shù)據(jù)通過(guò)線性插值產(chǎn)生的，其數(shù)量可由線性插值的密度進(jìn)行控制。因此，可通過(guò)調(diào)整控制點(diǎn)的插值間距ρ來(lái)改變導(dǎo)航曲線的生成效果，從而改變eP的大小，使其滿足式(10)的關(guān)系。對(duì)于給定的路線，eP和ρ之間滿足一一對(duì)應(yīng)且單調(diào)的函數(shù)關(guān)系，即插值間距ρ越小，同一位置點(diǎn)P對(duì)應(yīng)的eP越小。但由于無(wú)法求出eP和ρ之間的具體函數(shù)關(guān)系，故無(wú)法根據(jù)約束條件直接確定ρ的取值。對(duì)此，本文提出了基于迭代的自適應(yīng)修正算法，引入偏差修正系數(shù)k和修正偏差m，根據(jù)m的大小實(shí)現(xiàn)k的自適應(yīng)調(diào)整，從而對(duì)ρ值進(jìn)行修正，使eP快速收斂到約束條件范圍內(nèi)。算法流程如下:

(1)已知eP的約束條件為［Emin(s)，Emax(s)］，給定插值間距的初值ρ0，初始調(diào)整系數(shù)k0，初始調(diào)整偏差m0，并求ρ0對(duì)應(yīng)的eP。

(2)判斷eP是否滿足約束條件，若滿足，調(diào)整結(jié)束，得到ρout;否則，計(jì)算eP與約束條件上限和下限的差值。

若存在ep＞Emax(P)，則在大于上限的點(diǎn)中取出差值最大的一點(diǎn)P0，其位置為S0，令

若存在eP＜Emin(P)，則在小于下限的點(diǎn)中取出差值最大的一點(diǎn)P0，其位置為S0，令

若大于上限和小于下限的點(diǎn)同時(shí)存在，則將曲線分段，分別對(duì)每段曲線進(jìn)行修正。

(3)按照式(13)、(14)計(jì)算 ρi+1和 ki+1。

式中 i=0，1，…。

以ρi+1為插值間距更新擬合曲線，并計(jì)算相應(yīng)的偏差值eP，返回步驟(2)。

通過(guò)該算法對(duì)導(dǎo)航路徑進(jìn)行修正，得到智能車導(dǎo)航路徑的數(shù)據(jù)集合

4 實(shí)驗(yàn)驗(yàn)證

使用百度地圖規(guī)劃導(dǎo)航路徑，規(guī)劃結(jié)果如圖3所示。實(shí)驗(yàn)路段起點(diǎn)為圖上A點(diǎn)，終點(diǎn)為圖上B點(diǎn)。路徑總長(zhǎng)5.3 km，包括了直道、彎道、路口轉(zhuǎn)彎、掉頭等路段。獲取規(guī)劃路徑上的節(jié)點(diǎn)坐標(biāo)并進(jìn)行糾偏，得到該路段的路網(wǎng)數(shù)據(jù)。使用第2章“導(dǎo)航路徑的生成”方法對(duì)該數(shù)據(jù)進(jìn)行處理，生成了智能車導(dǎo)航路徑。原始路網(wǎng)數(shù)據(jù)及智能車導(dǎo)航數(shù)據(jù)如圖4所示。

圖3 百度地圖規(guī)劃結(jié)果

圖4 原始路網(wǎng)數(shù)據(jù)及智能車導(dǎo)航數(shù)據(jù)

選取路程 S∈［3.8，3.9］km 處的路段，取dm=11 m，dv=1.8 m，m0=1，k0=1，ρ0=8.5。將試驗(yàn)車的車寬dv、最小轉(zhuǎn)彎直徑dm以及該路段的路寬數(shù)據(jù)代入本文的約束模型，得到該路段下導(dǎo)航路徑偏移量eP的取值范圍;根據(jù)取值范圍對(duì)該路段的初始導(dǎo)航路徑進(jìn)行自適應(yīng)修正，得到了滿足約束條件的導(dǎo)航路徑。車輛在該路段需要進(jìn)行一次掉頭，由于道路較窄，導(dǎo)航路徑的約束條件比較苛刻。圖5為該路段中導(dǎo)航路徑與中心線的偏差與位置的函數(shù)關(guān)系。其中，實(shí)線為約束條件確定導(dǎo)航路徑偏移量的取值上限Emax(s)和取值下限Emin(s)，虛線為不同ρ值對(duì)應(yīng)的導(dǎo)航路徑的偏移量eP。圖6為不同ρ值對(duì)應(yīng)的智能車導(dǎo)航路徑，在導(dǎo)航路徑上每隔1.5 m取一個(gè)點(diǎn)，點(diǎn)的直徑為試驗(yàn)車車寬。當(dāng)控制點(diǎn)插值間距ρ=ρ0=8.5時(shí)，車輛沿導(dǎo)航路徑行駛會(huì)碰到道路邊沿，通過(guò)修正，得到一條較好的無(wú)碰路徑，如圖5(b)、圖6(b)所示。

圖5 不同ρ值對(duì)應(yīng)的與S的關(guān)系曲線

圖6 不同ρ值對(duì)應(yīng)的智能車導(dǎo)航路徑

5 結(jié)語(yǔ)

本文提出的一種基于常規(guī)電子地圖稀疏坐標(biāo)的導(dǎo)航路徑校正方法，無(wú)需事先構(gòu)建駕駛地圖，直接通過(guò)提取現(xiàn)有電子地圖的路網(wǎng)數(shù)據(jù)，利用插值、擬合處理與本文提出的基于模型約束的導(dǎo)航路徑自適應(yīng)修正方法，實(shí)現(xiàn)對(duì)智能車行駛的精確導(dǎo)航。下一步將針對(duì)導(dǎo)航路徑約束模型，對(duì)車輛行駛速度等運(yùn)動(dòng)學(xué)約束以及約束范圍內(nèi)的最優(yōu)路徑進(jìn)行研究。

［1］ Handel P.Handbook of Intelligent Vehicles［M］.London:Springer，2012:344-361.

［2］ Levinson J S.Automatic Laser Calibration，Mapping，and Localization for Autonomous Vehicles［D］.Palo Alto:Stanford University，2011:25-39.

［3］ Castro M，Iglesias L.Geometric modelling of highways using global positioning system(GPS)data and spline approximation［J］.Emerging Technol，2006，14(4):233-243.

［4］ Jo Kichun，Sunwoo Myoungho.Generation of a precise roadway map for autonomous cars［J］.IEEE Transaction on Intelligent Transportation System，2014，15(3):925-937.

［5］ Chen A，Ramanandan A，F(xiàn)arrell J A.High-precision lane-level road map building for vehicle navigation［C］//IEEE/ION PLANS，2010:1035-1042.