基于稀疏地圖坐標的智能車導航路徑生成與優化

王任棟，章永進，徐友春，馬育林

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

電子地圖的精確導航是使智能車在復雜的道 路環境中快速準確地到達指定地點的重要條件［1］，是無人駕駛技術的難點之一，也是熱點之一。目前常用的方法有兩種:一種是基于高精度駕駛地圖的導航方法;另一種是基于現有常規電子地圖的導航方法。

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

與基于高精度駕駛地圖的導航方法相比，基于現有常規電子地圖的導航方法具有成本低、覆蓋范圍廣、路網完整、數據結構簡單、占用存儲空間少等優點，已經可以為有人駕駛汽車提供良好的導航服務。但由于其路網數據稀疏，難以直接應用于面向無人駕駛的智能車導航。針對該問題，本文提出一種基于現有地圖稀疏路網數據的導航路徑生成及優化方法。

1 算法基本理論

1.1 導航路徑生成算法

式中:xi、yi為數據點的經緯度坐標;θi為智能車在該點的行駛方向，且

1.2 導航路徑優化算法

其中

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

式中:di為路段的寬度;αi為路段li與li+1的夾角，由li與li+1的原始路網數據計算得到。

2 導航路徑的生成

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

從規劃路徑中獲取的路網數據集合為

式中:li為規劃路徑L中的路段;(xij，yij)為路段li上的節點坐標。

2.1 路網數據的插值

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

電子地圖的路徑規劃生成的是一條以起點和終點為端點、經過有限個節點的連續折線，用S表示起點到折線上任意一點的折線距離，S的取值與折線上的點存在一一對應的關系，因此，將S作為路網數據插值變量，并用S的大小表示路徑折線中任意一點的位置，即

對折射距離變量S的插值等價于對原始路網數據的插值。

通過分析地圖路網模型及各種插值方法的特點，分別采用線性插值、3次插值、樣條插值處理路網數據，并將插值結果與地圖規劃路線進行對比(如圖1所示)。

圖1 3種插值方法的結果與原始規劃路徑對比

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

2.2 擬合導航路徑曲線

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

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

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

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

向量T中的元素為單調不減數列。為了將樣條曲線的端點固定在控制點的起點和終點即b0和bn，將第一個和最后一個節點以一個定值重復p次，如下所示:

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

3 導航路徑的優化

3.1 導航路徑的約束模型

為使智能車在無外界環境干擾時能夠準確地沿引導路徑行駛，導航路徑需要滿足由車輛幾何特性與道路幾何特性決定的約束條件。

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

(2)道路幾何約束。導航路徑落在規劃路徑所在道路的可行駛區域內。

根據這兩個約束條件，構建圖2所示的車輛導航路徑約束模型。

圖2 車輛導航路徑約束模型

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

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

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

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

當P點落在l1、l2上其他位置時，Emin(s)=0。

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

則認為導航曲線滿足其約束條件。

3.2 擬合曲線的優化

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

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

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

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

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

若大于上限和小于下限的點同時存在，則將曲線分段，分別對每段曲線進行修正。

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

式中 i=0，1，…。

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

通過該算法對導航路徑進行修正，得到智能車導航路徑的數據集合

4 實驗驗證

使用百度地圖規劃導航路徑，規劃結果如圖3所示。實驗路段起點為圖上A點，終點為圖上B點。路徑總長5.3 km，包括了直道、彎道、路口轉彎、掉頭等路段。獲取規劃路徑上的節點坐標并進行糾偏，得到該路段的路網數據。使用第2章“導航路徑的生成”方法對該數據進行處理，生成了智能車導航路徑。原始路網數據及智能車導航數據如圖4所示。

圖3 百度地圖規劃結果

圖4 原始路網數據及智能車導航數據

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

圖5 不同ρ值對應的與S的關系曲線

圖6 不同ρ值對應的智能車導航路徑

5 結語

本文提出的一種基于常規電子地圖稀疏坐標的導航路徑校正方法，無需事先構建駕駛地圖，直接通過提取現有電子地圖的路網數據，利用插值、擬合處理與本文提出的基于模型約束的導航路徑自適應修正方法，實現對智能車行駛的精確導航。下一步將針對導航路徑約束模型，對車輛行駛速度等運動學約束以及約束范圍內的最優路徑進行研究。

［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，Farrell J A.High-precision lane-level road map building for vehicle navigation［C］//IEEE/ION PLANS，2010:1035-1042.