基于A*算法的駕駛地圖路徑規(guī)劃實現(xiàn)

關泉珍，鮑 泓，史志堅

（北京聯(lián)合大學北京市信息服務工程重點實驗室，北京 100101）

?

基于A*算法的駕駛地圖路徑規(guī)劃實現(xiàn)

關泉珍，鮑 泓，史志堅

（北京聯(lián)合大學北京市信息服務工程重點實驗室，北京 100101）

［摘 要］采用高精度地圖構建技術還原路況信息，結合A*算法使智能車能夠在導航不起作用的情況下按照規(guī)劃路徑進行無障礙行駛。將高精度地圖用柵格數(shù)據(jù)模型表示，在標記為有障礙的柵格模型中，為機器人尋找一條恰當?shù)膹钠鹗键c到目標點的運動路徑，且可以使機器人在運動過程中安全、無碰撞地繞過障礙物。通過在無人駕駛智能車平臺上仿真實驗表明，這種方法具有形式簡單規(guī)范、一致性好并容易在計算機中實現(xiàn)的優(yōu)點。

［關鍵詞］路徑規(guī)劃；自主導航；高精度地圖；A*算法；柵格數(shù)據(jù)模型

引言

無人駕駛汽車是一種智能汽車，也稱之為輪式移動機器人［1］，是在沒有人類參與的情況下，依靠車內的計算機系統(tǒng)，通過智能駕駛系統(tǒng)來實現(xiàn)無人駕駛的功能［2］。它是一個集環(huán)境信息感知、智能規(guī)劃決策、輔助及自主駕駛等多種功能于一體的綜合車輛系統(tǒng)［3］，路徑規(guī)劃是無人駕駛汽車導航的基本環(huán)節(jié)之一。它是按照某一性能指標搜索一條從起始點到目標點的最優(yōu)或近似最優(yōu)的無碰路徑。對于路徑規(guī)劃來說，往往與無人駕駛汽車所處的空間位置和傳感器所感知的周圍環(huán)境信息密切相關。根據(jù)無人駕駛汽車傳感器對周圍環(huán)境信息知道的程度不同，可分為兩種類型:第一種是環(huán)境信息完全知道的宏觀路徑規(guī)劃也叫全局路徑規(guī)劃；第二種是環(huán)境信息完全未知或部分未知，通過雷達、攝像頭等傳感器對無人駕駛汽車的工作環(huán)境進行探測，獲取障礙物的位置、形狀和尺寸等信息的局部路徑規(guī)劃。全局路徑規(guī)劃可使所規(guī)劃的路徑達到最優(yōu)，局部路徑規(guī)劃則可使機器人完成實時避障。目前常用的移動機器人全局路徑規(guī)劃方法很多，主要有蟻群算法［4］、粒子群算法［5］、遺傳算法［6］等，此外GPS也是智能車輛進行全局規(guī)劃的重要手段之一，目前主要采用GPS/INS組合的導航技術。對蟻群算法來說，它存在收斂速度慢，容易陷入局部最優(yōu)等缺陷；粒子群優(yōu)化算法雖然具有通用性強、不依賴于問題、原理簡單、容易實現(xiàn)等優(yōu)點，但粒子群算法計算效率偏低，且抗噪能力差；遺傳算法存在編程實現(xiàn)比較復雜，且參數(shù)的選擇大部分是依靠經(jīng)驗等缺點。對GPS來說，雖然無人駕駛汽車通過GPS/INS等傳感器能全天候實時感知車輛在環(huán)境中的位置，提供準確的車輛行駛方向、速度、加速度等車輛自身狀態(tài)信息，但由于GPS的工作依賴于衛(wèi)星信號，因此，在衛(wèi)星信號不佳或信號無法獲取的情況下將會失效［7］，INS也會出現(xiàn)信號漂移等問題。

以上算法都存在編程實現(xiàn)較復雜等缺點。本文結合人類駕駛員駕駛經(jīng)驗，提出采用高精度地圖構建技術還原路況信息，結合A*算法使智能車能夠在導航不起作用的情況下按照規(guī)劃路徑進行無障礙行駛。將高精度地圖用柵格數(shù)據(jù)模型表示，在標記為有障礙的柵格模型中，為機器人尋找一條恰當?shù)膹钠鹗键c到目標點的運動路徑。高精度地圖的獲取可通過專業(yè)做地圖的公司采集處理或者由人類駕駛員駕駛智能汽車遍歷一塊區(qū)域內的所有車道，將傳感系統(tǒng)如64線激光雷達或攝像機等采集到的行駛軌跡、道路環(huán)境等信息經(jīng)過濾波、融合等算法處理，就得到了這一區(qū)域的道路網(wǎng)絡綜合信息。

1　駕駛地圖柵格化

在車輛導航系統(tǒng)中，電子地圖數(shù)據(jù)庫、路徑規(guī)劃和路徑引導是3個相輔相成的重要模塊。為了實現(xiàn)路徑規(guī)劃和路徑引導，首先需要一個電子地圖數(shù)據(jù)庫的支持。對于無人駕駛汽車，除了需要電子地圖上常見的道路形態(tài)和路邊環(huán)境信息，還需要抽象的道路網(wǎng)絡拓撲結構，用于宏觀層面上合理的路徑規(guī)劃和微觀層面上精確的路徑引導。比之目前一般的導航設備，無人駕駛將使用完全不同的地圖數(shù)據(jù)。精確到米的低端地圖（如圖1）只能給人類駕駛員領路，無人駕駛智能車要的是厘米級的精度（如圖2）所示。應用差分定位技術的全球衛(wèi)星定位系統(tǒng)和慣性導航組合可以為無人駕駛車的行駛提供厘米級的導航信息，但由于在發(fā)達城市越來越長的隧道興建和連續(xù)高層的遮擋，全球衛(wèi)星定位系統(tǒng)和慣性導航組合系統(tǒng)進入隧道不能滿足一定精度的要求以及在大型隧道內不能長期定位的缺點［7］，使得采用高精度地圖匹配技術［8］來克服以上缺點成為當務之急。

無人駕駛車輛的工作環(huán)境與一般機器人應用存在如下幾點不同:不具備已知的環(huán)境地圖，環(huán)境信息主要來自于車載傳感器系統(tǒng)的實時感知結果，而各個傳感器只能提供局部不完整的環(huán)境信息。因此，在車輛行駛過程中，即使環(huán)境是靜態(tài)的，規(guī)劃系統(tǒng)面對的局部環(huán)境信息也是在不斷更新的；此外，環(huán)境中可能存在較多的動態(tài)障礙物，易造成規(guī)劃結果不可行；這些特點對規(guī)劃系統(tǒng)的實時性提出了更高的要求，而高精度地圖無疑對智能車的行駛起到了很大的幫助作用。

對于高精度地圖來說，道路網(wǎng)絡的幾何坐標信息為智能汽車提供了定位和路徑引導的依據(jù)，利用高精度的道路網(wǎng)信息，將GPS/INS或其他傳感器的定位結果匹配到高精度地圖的道路網(wǎng)絡上，實現(xiàn)車輛位置糾正。當推算定位指示車輛在地圖上的某一位置時，車輛位置可以被調整到地圖上的絕對位置，這樣做會消除累積誤差，直到下一次地圖匹配步驟。在每一個連續(xù)的系統(tǒng)周期中完成這個過程，就能實時得到更加準確的車輛位置。地圖匹配技術的應用有兩個前提，即:用于匹配的數(shù)字地圖包含高精度的道路位置坐標以及被定位的車輛正在道路網(wǎng)中行駛。對于前者而言，高精度地圖無疑是滿足這一要求的。當滿足上述條件時，就可以把定位數(shù)據(jù)和車輛運行軌跡同高精度地圖所提供的道路位置信息進行比較，通過適當?shù)钠ヅ溥^程確定出車輛最可能的行駛路段以及車輛在該路段的最大可能位置。［9］

1.1駕駛地圖與柵格地圖的關系

柵格地圖對應投影坐標系統(tǒng)，高精度地圖對應地理空間坐標系。

投影坐標系統(tǒng)（Projection coordinate system）:使用基于X，Y值的坐標系統(tǒng)來描述地球上某個點所處的位置。這個坐標系是從地球的近似橢球體投影得到的，它對應于某個地理坐標系（地理空間坐標系:使用基于經(jīng)緯度坐標描述地球上某一點所處的位置。某一個地理坐標系是基于一個基準面來定義的?；鶞拭媸抢锰囟E球體對特定地區(qū)地球表面的逼近，因此每個國家或地區(qū)均有各自的基準面，也稱球面坐標。平面坐標系統(tǒng)地圖單位通常為米，也稱非地球投影坐標系統(tǒng)，或者是平面直角坐標。投影坐標系由以下兩項參數(shù)確定:地理空間坐標系以及投影方法。下面用公式（1）來表示地理空間坐標系和投影坐標系統(tǒng)的關系:

式中（B，L）是橢球面上某一點的大地經(jīng)緯度，而（X，Y）是該點投影到平面上的直角坐標。

把地球上的點位換算到平面上，稱為地圖投影。地圖投影的方法有很多，本文中采用的是高斯—克呂格投影（又稱高斯正形投影），簡稱高斯投影。它是由德國數(shù)學家高斯提出，由克呂格改進的一種分帶投影方法。它成功解決了將橢球面轉換為平面的問題。通過高斯投影，將中央子午線的投影作為縱坐標軸，用x表示，將赤道的投影作橫坐標軸，用y表示，兩軸的交點作為坐標原點，由此構成的平面直角坐標系稱為高斯平面直角坐標系，［10］如圖3所示。

1.2坐標轉換算法

將GPS-84坐標轉換成平面坐標的方法有兩種:一是先將WGS-84的大地坐標轉換為1954年北京大地坐標或1980年國家大地坐標，而后通過投影變換（如高斯投影變換）轉換成平面直角坐標（如圖4所示）；另一種方式是先將經(jīng)緯度坐標以WGS-84的參考橢球為基準進行高斯投影，前一種方法計算過程較復雜，計算機耗時相對要長一些，但其計算精度要高于后一種方法，后一方法簡化了公式［11-12］，提高了計算效率和可操作性。這種方法適用于實時性要求較高的情況，如實時車輛導航和監(jiān)控系統(tǒng)。本文著重討論后一種坐標轉換算法。

WGS-84的經(jīng)緯度（維度:B，經(jīng)度:L）坐標高斯投影到平面坐標（x，y）上，高斯—克呂格投影的正算公式（高斯—克呂格投影正算公式是把大地坐標換算成高斯—克呂格投影平面上的直角坐標）如公式（2）、公式（3）、公式（4）、公式（5）、公式（6）所示:

式中C0、C1、C2、C3是與點位無關而僅與橢球參數(shù)有關的常系數(shù)，L0為中央子午線經(jīng)度，X為自赤道量起的子午線弧長，N為卯酉圈曲率半徑。

高斯—克呂格投影的反算公式（高斯—克呂格反算公式是把高斯—克呂格投影平面直角坐標換算到橢球面上的大地坐標）如公式（7）、公式（8）、公式（9）、公式（10）所示:

式中

式中Bf為底點緯度，K0、K1、K2、K3、K4、K5是與點位無關而僅與橢球參數(shù)有關的常系數(shù)。

事實證明，高斯—克呂格投影正反算公式能夠進行任意帶之間的坐標變化，并且轉換精度高，迭代計算收斂速度快，效率高。由于地圖投影變形是球面轉化成平面的必然結果，沒有變形的投影是不存在的。對某一地圖投影來說，不存在這種變形，就必然存在另一種或兩種變形。在制圖時則可以做到:在有些投影圖上沒有角度或面積變形；在有些投影圖上沿某一方向無長度變形。高斯—克呂格投影具有等角性質，比較適合用于測制地形圖，且適用于多種比例尺，便于編制成套比例尺的地形圖，并且由于投影帶經(jīng)差不大，經(jīng)緯網(wǎng)同直角坐標網(wǎng)的偏差較小，閱讀和使用較方便。此外，坐標和子午線收斂角數(shù)值，只要計算一帶，其他帶都可以通用，可以大大減少計算工作量。

1.3駕駛地圖的柵格化表示

柵格數(shù)據(jù)結構又稱為網(wǎng)格數(shù)據(jù)或者柵格數(shù)據(jù)，是以規(guī)則的陣列來表示空間地物或現(xiàn)象分布的數(shù)據(jù)組織。每個網(wǎng)格作為一個像元或像素，由行、列號定義，并包含一個代碼，表示該像素的屬性類型或量值。每個柵格都存儲了它所覆蓋的表面部分的值。一個指定像元包含一個值，因此，表面的詳細程度取決于柵格像元的大小。同其他模型相比，柵格模型比較簡單、高效。一般柵格模型多用于區(qū)域性的、小比例尺的應用，本文將高精度地圖用柵格數(shù)據(jù)模型表達如圖5所示。

簡單說，柵格是單波段，任何柵格分析（處理）都是信息有損的處理。在定義柵格單元的大小時，需要平衡信息的精確性和數(shù)據(jù)量之間的矛盾。柵格單元代表的尺度越小，表達的信息就越精確。柵格單元代表的尺度越大，存儲數(shù)據(jù)所需要的空間就更少，同時，表達的信息也就不精確。柵格數(shù)據(jù)集像元的位深度容量（像素深度）決定著特定柵格文件可以存儲的值的范圍，該范圍可根據(jù)公式2n計算得出（其中，n表示位深度）。例如，一個8位的柵格可以具有256個不同的值（范圍從0至255）。柵格數(shù)據(jù)的表達如圖6所示。在本文中柵格數(shù)據(jù)只有兩類信息:分別是障礙物和非障礙物兩類。

2　基于A*算法的路徑規(guī)劃

A*算法是應用極為廣泛的啟發(fā)式搜索算法（Heuristic Search Algorithm），相對于其他盲目搜索算法如廣度優(yōu)先和深度優(yōu)先等，A*算法主要依靠啟發(fā)信息構造啟發(fā)函數(shù)，在規(guī)劃搜索路徑時對訪問節(jié)點進行代價評價，以滿足某些指標要求，例如時間最短、空間最小等，從而形成代價最優(yōu)的節(jié)點訪問路徑。它的基本思想是:把從起始點到節(jié)點的代價與從節(jié)點到目標點的代價結合起來對節(jié)點進行評價:f（n）=g（n）+h（n），在這個式子當中，h（n）是啟發(fā)函數(shù)，是從n到目標節(jié)點最佳路徑估價，f（n）是從初始點經(jīng)由節(jié)點n到終點的評價函數(shù)，g（n）是從初始節(jié)點到n節(jié)點的實際代價。［13-14］

借鑒A*算法的基本思想，無人駕駛車進行路徑規(guī)劃時，首先進行一次路徑規(guī)劃（也叫宏觀路徑規(guī)劃），規(guī)劃出一條從起始點到目標點的路徑，然后沿著此條路徑移動，當無人駕駛車傳感器探測到的環(huán)境信息與系統(tǒng)已知的環(huán)境信息不一致的時候，機器人重新規(guī)劃從當前位置到目標點的路徑，如此反復，直至到達目標點。A*算法的尋路步驟如下:

簡易地圖（如圖7所示），其中綠色方塊的是起點（用A表示），中間藍色的是障礙物，紅色的方塊（用B表示）是目的地。用一個二維數(shù)組來表示地圖，將地圖劃分成一個個的小方塊。尋路步驟如下:

1）從起點A開始，把它作為待處理的方格存入一個開啟列表（開啟列表就是一個等待檢查方格的列表），尋找起點A周圍可以到達的方格，將它們放入開啟列表，并設置它們的父方格為A。

2）從開啟列表中刪除起點A，并將起點A加入關閉列表（關閉列表中存放的都是不需要再次檢查的方格），從開啟列表中找出相對最靠譜的方塊，通過公式F=G+H來計算。G表示從起點A移動到網(wǎng)格上指定方格的移動耗費（可沿斜方向移動），H表示從指定的方格移動到終點B的預計耗費，從開啟列表中選擇F值最低的方格C。

3）把它從開啟列表中刪除，并放到關閉列表中，檢查它所有相鄰并且可以到達（障礙物和關閉列表的方格都不考慮）的方格。如果這些方格還不在開啟列表里的話，將它們加入開啟列表，計算這些方格的G，H和F值各是多少，并設置它們的父方格C如圖8。

4）如果某個相鄰方格D已經(jīng)在開啟列表里了，檢查如果用新的路徑（就是經(jīng)過C的路徑）到達它的話，G值是否會更低一些。如果新的G值更低，那就把它的“父方格”改為目前選中的方格C，然后重新計算它的F值和G值（H值不需要重新計算，因為對于每個方塊，H值是不變的）。如果新的G值比較高，就說明經(jīng)過C再到達D不是一個明智的選擇，因為它需要更遠的路，這時什么也不做。就這樣，從開啟列表找出F值最小的，將它從開啟列表中移掉，添加到關閉列表。再繼續(xù)找出它周圍可以到達的方塊，如此循環(huán)下去。

5）當我們發(fā)現(xiàn)開始列表里出現(xiàn)了目標終點方塊的時候，說明路徑已經(jīng)被找到。

3　實驗與分析

3.1實驗平臺

實驗是在北京聯(lián)合大學無人駕駛智能車仿真平臺上進行的，如圖9所示，硬件使用平臺為北汽C70智能車，軟件為MATLAB R2013b，操作系統(tǒng)Windows 7。

3.2實驗結果

1）選擇所用的柵格地圖（如圖10）。

2）設置地圖障礙物位置（如圖11）。

3）A*算法的尋路實現(xiàn)（如圖12）。

3.3實驗分析

本實驗將高精度地圖以柵格數(shù)據(jù)結構表示，通過在柵格地圖模型上設立障礙物，利用A*算法，為機器人尋找一條恰當?shù)膹钠鹗键c到目標點的運動路徑，且可以使機器人在運動過程中安全、無碰撞地繞過所有的障礙物。與其他方式相比，雖然用柵格法規(guī)劃地理空間，具有描述規(guī)范、形式簡單、一致性好、容易實現(xiàn)、在計算機中存儲方便等優(yōu)點，但是利用柵格數(shù)據(jù)模型表示地圖，對精度有很強的依賴性，且在復雜環(huán)境下，搜索空間變大，算法的效率可能降低。

4　結束語

在分析了智能汽車宏觀路徑導航和局部軌跡決策所需的道路網(wǎng)絡信息之后，本文提出了基于高精度地圖的道路網(wǎng)絡綜合信息表達方式，將高精度地圖利用柵格數(shù)據(jù)結構表示，結合A*算法實現(xiàn)了從環(huán)境初始點尋找最優(yōu)路徑，繞過障礙物到達終點。論文所提方法目前還是仿真驗證，環(huán)境假設是理想的靜態(tài)條件，作為一個在真實城市半結構化道路上無人駕駛車輛的完整路徑規(guī)劃系統(tǒng)，如何進行更高效的動態(tài)建圖和地圖定位也是目前工作中正研究的重點

［參考文獻］

［1］ 喬維高，徐學進.無人駕駛汽車的發(fā)展現(xiàn)狀及方向［J］.上海汽車，2007，40（1）:40-43.

［2］ 馮學強，張良旭，劉志宗.無人駕駛汽車的發(fā)展綜述［J］.山東工業(yè)技術，2015，51（1）:1-1.

［3］ 徐友春，王榮本.世界智能車輛近況綜述［J］.汽車工程，2001，23（5）:289-295.

［4］ 張銀玲，牛小梅.蟻群算法在移動機器人路徑規(guī)劃中的仿真研究［J］.計算機仿真，2011，28（6）:231-234.

［5］ 李擎，徐銀，梅張德政，等.基于粒子群算法的移動機器人全局路徑規(guī)劃策略［J］.北京科技大學學報，2010，32（3）:397 -402.

［6］ 石鐵峰.改進遺傳算法在移動機器人路徑規(guī)劃中的應用［J］.計算機仿真，2011，28（4）:193-195，303.

［7］ 方鵬.GPS/INS組合導航與定位系統(tǒng)研究［D］.上海:同濟大學，2008:1-11.

［8］ 潘吳，胡飛，楊立國.GPS車載導航系統(tǒng)的地圖匹配算法研究［J］.中國水運，2007，7（11）:174-176.

［9］ 王衛(wèi)華.未知環(huán)境中移動機器人創(chuàng)建地圖的研究［D］.上海:上海交通大學，2000.

［10］ 姜巖，王琦，龔建偉，等.無人駕駛車輛局部路徑規(guī)劃的時間一致性與魯棒性研究［J］.自動化學報，2015，41（3）:518-527.

［11］ 羅飛，楊澤超，伯魯江.大地坐標換帶計算在定向井軌跡設計中的應用［J］.內蒙古石油化工，2007，95（6）:1-3.

［12］ 劉飛，周琳琳，益建芳.GPS大地坐標向地方坐標轉換的實用方法研究［J］.華東師范大學學報，2005，75（1）:1-2.

［13］ 陸州.移動機器人路徑規(guī)劃與路徑跟蹤研究［D］.廣州:華南理工大學，2012:19-22.

［14］ Pamosoaji A K，Hong K.A path-planning algorithm using vector potential functions in triangular regions［J］.IEEE Trans Syst Man CybernSyst，2013，43（4）:832-842.

［15］ Jaillet L，Porta J M.Path planning under kinematic constraints by rapidly exploring manifolds［J］.IEEE Trans Robot，2013，29（1）:105-117.

［16］ 黃健生.移動機器人的路徑規(guī)劃研究［D］.杭州:浙江大學，2008:3-8.

（責任編輯 李亞青）

The Achievement of Path Planning Based on A*Algorithm

GUAN Quan-zhen，BAO Hong，SHI Zhi-jian
（Beijing Key Laboratory of Information Service Engineering，Beijing Union University，Beijing 100101，China）

Abstract:This study uses high-precision map to restore traffic information，combining with A*algorithm to make the driverless car travel according to the planned path without GPS.The high-precision map is presented in the form of raster model.A*algorithm can find a proper path from the starting point to end point for mobile robots in the environment which is presented in the form of raster and full of barriers.Through simulation experiments，it turns out that this method has the advantage of simplification，consistency and easy implementation on computer.

Key words:Path planning；Autonomous navigation；High-precision map；A*algorithm；Raster data model

［中圖分類號］U 491.2

［文獻標志碼］A

［文章編號］1005-0310（2016）02-0031-09

DOI：10.16255/j.cnki.ldxbz.2016.02.006

［收稿日期］2016-03-07

［基金項目］國家自然科學基金重大研究計劃項目（91420202）。

［作者簡介］關泉珍（1990-），女，河南省許昌市人，北京聯(lián)合大學北京市信息服務工程重點實驗室碩士，主要研究方向為路徑規(guī)劃。

［通訊作者］鮑泓（1958-），男，北京市人，博士，北京聯(lián)合大學副校長、教授，博士生導師，主要研究方向為圖像處理、智能控制。E-mail:baohong@buu.edu.cn