周云城 楊東曉 田 歡
(1. 華設(shè)設(shè)計集團(tuán)股份有限公司,江蘇 南京 210014;2. 江蘇大學(xué),江蘇 鎮(zhèn)江 212013;3. 清華大學(xué)蘇州汽車研究院(相城),江蘇 蘇州 215134)
隨著智能化設(shè)備的發(fā)展,出現(xiàn)了各式各樣的定位導(dǎo)航軟件,定位導(dǎo)航技術(shù)獲取方便,使用簡單,為人們的出行帶來了極大便利,智能化設(shè)備在大多數(shù)人的日常出行生活中已經(jīng)變得不可或缺了[1]。然而當(dāng)車輛駛?cè)氲叵峦\噲龅却竺娣e的室內(nèi)場所時,由于衛(wèi)星信號受到建筑物的嚴(yán)重干擾,車輛通常難以接收到GPS/北斗信號進(jìn)行定位,因此車輛已有的導(dǎo)航系統(tǒng)無法實現(xiàn)導(dǎo)航功能。尤其是當(dāng)車輛位于大型的地下停車場時,由于停車場的空間較大,各區(qū)域場景相仿,標(biāo)志物較少,不易分清方向,人們在尋找停車位以及尋車離場時往往會遇到一定困難。當(dāng)停車場中的車流較多時,會在停車場內(nèi)要安排一定的工作人員對車輛進(jìn)行引導(dǎo),但每個工作人員往往只負(fù)責(zé)某個固定區(qū)域,其視野有限且人員之間溝通不暢,停車場內(nèi)的車流容易出現(xiàn)混亂的狀況甚至導(dǎo)致?lián)矶拢囍鲗ぼ囯x場基本上只能依靠自己完成,用戶體驗較差[2]。這樣的車輛引導(dǎo)管理方式效率不高,停車位利用效率低,而且需要支出一定的人工費用,增加了停車場的運營成本。
該文對地下停車場的導(dǎo)航算法展開研究,與室內(nèi)高精度定位技術(shù)組合在一起,可以在司機(jī)尋找車位以及尋車離場時為司機(jī)導(dǎo)航,減少了司機(jī)在尋車上消耗的時間,提升了車輛流通的效率,進(jìn)而提高停車位的利用效率,在一定程度上可以幫助改善城市中“停車難”的狀況。在利用導(dǎo)航算法對停車場環(huán)境進(jìn)行路徑規(guī)劃時,首先要對停車場的平面圖進(jìn)行處理,對地圖的障礙物部分與道路部分進(jìn)行區(qū)分,可以用黑色對障礙物部分進(jìn)行標(biāo)識,用白色對道路進(jìn)行標(biāo)識,如圖1、圖2 所示。
圖1 地下停車場平面圖
圖2 標(biāo)識后的地下停車場地圖
在完成標(biāo)識后,便成功地區(qū)分了地下停車場環(huán)境中的障礙物與道路,但是由于導(dǎo)航算法程序并不能直接對處理后的圖片進(jìn)行運算處理,因此需要計算機(jī)對處理后的地圖進(jìn)行讀取,并將其轉(zhuǎn)換為可以直接進(jìn)行運算的數(shù)據(jù),由于停車場是以圖片的形式進(jìn)行環(huán)境輸入,并且用顏色對障礙物、道路進(jìn)行區(qū)分,因此可以用圖片中各點的像素點灰度值作為存儲形式進(jìn)行存儲。
程序載入地圖的圖片后,可以利用Python 常用的圖像處理庫Pillow 按照像素讀取地圖中該像素點的灰度值,并按順序?qū)⑵浯鎯υ? 個二維數(shù)組中,這樣就成功地將地圖數(shù)據(jù)以二維數(shù)組的形式存儲下來,并且二維數(shù)組中某點的序號即代表該像素點的像素坐標(biāo)。其中灰度值為0(黑色)的部分代表障礙物,灰度值為255(白色)的部分代表道路。
由于圖片中并非任意點均滿足白色部分灰度值為255、黑色部分灰度值為0 的條件,因此,灰度值可能會有細(xì)微的變動,為了明確地對障礙物、道路進(jìn)行區(qū)分,可以進(jìn)一步地對二維數(shù)組進(jìn)行簡化,對數(shù)組進(jìn)行二值化處理,將數(shù)組中灰度值小于127 的部分標(biāo)記為“#”,代表道路;將灰度值大于127 的部分標(biāo)記為“.”,代表障礙物。
PRM(Probabilistic Road Map)算法的基本思路是在地圖中利用隨機(jī)采樣節(jié)點構(gòu)造概率路徑圖[3],在構(gòu)造出的路徑圖中使用搜索算法進(jìn)行檢索,從而搜索出最短路徑,PRM 算法有2 個基本階段,即學(xué)習(xí)階段和查詢階段。
2.1.1 學(xué)習(xí)階段
學(xué)習(xí)階段的主要任務(wù)在于構(gòu)造出用于路徑檢索的概率路徑圖[4]。首先要對地圖進(jìn)行處理,用黑色對地圖中的障礙物部分進(jìn)行標(biāo)識,用白色對道路部分進(jìn)行標(biāo)識。完成對地圖的處理后,在地圖上隨機(jī)采樣N個節(jié)點。完成隨機(jī)采樣后,由于采樣的節(jié)點可能會位于障礙物內(nèi)部,因此需要對各采樣點進(jìn)行檢測,剔除其中與障礙物重疊的點。
當(dāng)完成節(jié)點的碰撞檢測后,用線段將剩余的節(jié)點連在一起,連線時需要保證各節(jié)點的連線不能與障礙物重疊,為了節(jié)省計算量,縮短路徑檢索時間,各節(jié)點只連接自己鄰域范圍內(nèi)的點,不太遠(yuǎn)的點進(jìn)行連接。當(dāng)所有節(jié)點均完成連線后即成功完成了概率路徑圖的構(gòu)造,如圖3 所示。
圖3 概率路徑圖
2.1.2 查詢階段
查詢階段的主要任務(wù)是在學(xué)習(xí)階段完成的概率路徑圖中檢索出從起始位置到達(dá)目標(biāo)位置的最優(yōu)路徑[5],其具體步驟如下:1)將起點與終點分別與其在概率路徑圖中與之距離最短的點進(jìn)行連接,從而將2 個點添加至學(xué)習(xí)階段構(gòu)造出的路徑圖中。2)利用Dijkstra 搜索算法在完成連接的路徑圖中進(jìn)行檢索,選用距離作為優(yōu)化指標(biāo),查詢從起始位置到達(dá)目標(biāo)位置的最短路徑,如圖4 所示。
圖4 隨機(jī)采樣點取200 個規(guī)劃出的路徑
由于在地下停車場環(huán)境中障礙物較多,因此在隨機(jī)采樣時大部分位于障礙物內(nèi)部的采樣節(jié)點都會被舍去,保留下的節(jié)點也會出現(xiàn)與障礙物距離過近的情況,這樣可能會導(dǎo)致規(guī)劃出的路徑與障礙物距離過短,即出現(xiàn)“貼邊”現(xiàn)象,并且由于各節(jié)點位置隨機(jī),各節(jié)點的連線通常與道路中心線不平行,路徑規(guī)劃效果較差,如果增加采樣節(jié)點,則會導(dǎo)致運算時間大幅增長,而路徑規(guī)劃效果并不會出現(xiàn)明顯提升,考慮到停車場中大部分道路結(jié)構(gòu)較為簡單,很少會有彎曲道路,因此可以將各路口中心點作為采樣節(jié)點集,路徑規(guī)劃效果會有大幅提升,同時解算時間也會有所縮短??梢詫?yōu)化前后的算法進(jìn)行簡單的對比實驗,實驗中將起點的像素坐標(biāo)設(shè)為(305,380),終點的像素坐標(biāo)設(shè)為(615,990),如圖5 所示。
圖5 優(yōu)化PRM 算法規(guī)劃出的路徑
由表1 可以看出原始的PRM 算法在增加采樣點數(shù)量及鄰域距離后,雖然路徑規(guī)劃效果有所提升,但是路徑解算耗費的時間大幅增長。這主要是因為增加采樣點及鄰域距離后,節(jié)點的連線數(shù)量會呈幾何倍數(shù)增長,其中大部分為無效連線,算法檢測連線是否穿越障礙物時耗費了大量資源。而采樣點數(shù)量較少時,路徑規(guī)劃效果很差。需要進(jìn)行大量實驗,讓采樣點數(shù)量、鄰域范圍處于一個適中的區(qū)間,使規(guī)劃效果與耗費時間之間達(dá)到一個合理的平衡。優(yōu)化的PRM 算法利用檢索出的節(jié)點,避免了對大量資源進(jìn)行搜索的工作,在大幅節(jié)省解算時間的同時,取得了最好的路徑規(guī)劃效果。
表1 優(yōu)化前后算法數(shù)據(jù)對比
同時考慮到地下停車場道路較為狹窄,車輛在道路上難以掉頭,在初始時,車輛只能沿著車頭方向向前行駛,因此需要對車輛的初始行駛方向進(jìn)行限制,可以在學(xué)習(xí)階段對路徑圖的生成過程進(jìn)行修改,在節(jié)點集中對節(jié)點進(jìn)行判斷,刪去與車輛位于同一道路上且處在車輛后方的節(jié)點,在構(gòu)造路徑圖時,不會生成與車頭方向相悖的路徑。這樣在后續(xù)的查詢階段中便不會搜索出與車頭朝向相悖的路徑,從而確定路徑初始方向。
當(dāng)駕駛者利用導(dǎo)航軟件進(jìn)行導(dǎo)航時,軟件在規(guī)劃出路徑的同時會結(jié)合車輛的定位對駕駛者進(jìn)行導(dǎo)航信息提示。在實際應(yīng)用中,導(dǎo)航信息中通常包括2 個信息:1)距離信息。車輛與下一路口間的距離。2) 方向信息。車輛在下一路口位置上應(yīng)進(jìn)行何種操作,例如直行、左轉(zhuǎn)或是右轉(zhuǎn)。
在進(jìn)行坐標(biāo)變換之后可以通過查詢車輛定位以及下一路口位置簡單算出導(dǎo)航信息所需要的距離,坐標(biāo)變換時為了保證精度,路口中心點的坐標(biāo)變換采用像素坐標(biāo)與世界坐標(biāo)一一對應(yīng)的方式,而起點、終點的坐標(biāo)則通過計算得出。方向信息的分析則略微復(fù)雜些,其本質(zhì)在于折線段的拐向判斷,判斷的基本方法是求向量積,其基本原理如圖6 所示。
在圖6 所示的簡單折線段中,要判斷A點處的拐向,可以分別將A點兩側(cè)的線段視作向量P、Q,設(shè)P向量為(x1,y1),Q向量為(x2,y2),將P、Q的向量積記作I。
圖6 簡單折線段拐向判斷
由向量積的性質(zhì)可知,當(dāng)I小于0 時,P在Q的逆時針方向,說明在A點處折線段向右拐,反之,當(dāng)I大于0時,P位于Q的順時針方向,A點處折線段拐向朝左,I等于0 時,P與Q同向,折線段變?yōu)橹本€段。
利用上述基本原理可對停車場地圖中規(guī)劃路徑的方向信息進(jìn)行分析,首先查詢規(guī)劃路徑中經(jīng)過的路口中心節(jié)點,將起點、路口節(jié)點以及終點按順序連接在一起,將每條連線視作向量,并用像素坐標(biāo)表示這些向量,求出任意相鄰2 個向量的向量積并與0 進(jìn)行比較,就可以得出路徑在各路口的方向信息。
在停車位供不應(yīng)求的問題短時間內(nèi)難以得到解決的大背景下,提高城市中已有停車位的利用效率是緩解“停車難”的一種有效手段,然而現(xiàn)有停車場的空間較大,且各區(qū)域場景相仿不易分清方向,從而在尋找停車位以及尋車離場時會存在一些困難。為此,該文在總結(jié)已有理論及技術(shù)的基礎(chǔ)上,對面向地下停車場的路徑規(guī)劃問題進(jìn)行研究,成功對地下停車場環(huán)境進(jìn)行路徑規(guī)劃并給出導(dǎo)航信息,在提高停車位利用效率的同時,也為駕駛者在停車場中的尋找車位、尋車離場帶來了便利。