基于A*算法的駕駛地圖路徑規劃實現

關泉珍，鮑 泓，史志堅

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

?

基于A*算法的駕駛地圖路徑規劃實現

關泉珍，鮑 泓，史志堅

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

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

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

引言

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

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

1　駕駛地圖柵格化

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

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

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

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

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

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

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

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

1.2坐標轉換算法

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

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

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

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

式中

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

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

1.3駕駛地圖的柵格化表示

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

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

2　基于A*算法的路徑規劃

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

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

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

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

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

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

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

5）當我們發現開始列表里出現了目標終點方塊的時候，說明路徑已經被找到。

3　實驗與分析

3.1實驗平臺

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

3.2實驗結果

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

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

3）A*算法的尋路實現（如圖12）。

3.3實驗分析

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

4　結束語

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

［參考文獻］

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

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

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

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

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

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

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

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

［9］ 王衛華.未知環境中移動機器人創建地圖的研究［D］.上海:上海交通大學，2000.

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

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

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

［13］ 陸州.移動機器人路徑規劃與路徑跟蹤研究［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］ 黃健生.移動機器人的路徑規劃研究［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-），女，河南省許昌市人，北京聯合大學北京市信息服務工程重點實驗室碩士，主要研究方向為路徑規劃。

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