面向智能汽車的地下停車場定位與路徑規劃

李祎承，楊東曉，高 翔，馬育林，許述財

（1.江蘇大學 汽車工程研究院，江蘇 鎮江 212013；2.清華大學 蘇州汽車研究院（相城），江蘇 蘇州 215133；3.安徽工程大學 機械工程學院，安徽 蕪湖 241000；4.清華大學 汽車安全與節能國家重點實驗室，北京 100084）

1 引言

近年來，計算機處理能力的提高以及人工智能的快速發展掀起了汽車行業自動駕駛研究的熱潮，關于智能車輛的研究在學術及工業領域中都受到了廣泛的關注。在典型框架實現中，智能車輛系統可被分為感知系統（Perception System）和決策系統（Decision Making System）［1］。其中，定位是智能車輛感知系統的核心功能之一，車輛定位效果的好壞將直接影響后續的規劃與控制。決策系統則負責在獲取定位及其它感知信息之后規劃出可行駛路線。

對于定位來說，在開闊的室外場景中，智能車輛借助全球導航衛星系統（Global Navigation Satellite System，GNSS）、慣性導航系統（Inertial Navigation System，INS）或者兩者結合可以取得較高的定位精度［2］，但在城市建筑物密集區域，由于多徑效應，全球定位系統（Global Positioning System，GPS）的定位精度將會下降，而在室內場景，特別是在地下停車場中，GPS 信號的缺失會使得此類系統快速失效。

目前，多種室內定位系統（Indoor Positioning System，IPS）被開發出來以解決室內定位問題，不同的IPS 可能會使用不同的傳感器和不同的算法，目前常用的室內定位方法主要包括航位推算（Dead Reckoning，DR）［3-6］、基于通訊技術的定位［7-9］和基于同時建圖與定位（Simultaneous Location and Mapping，SLAM）的方法［10-13］。DR 方法將自身加速度、角度等傳感器數據進行積分得到自身位置，其積分原理使得這類技術會產生較大的累積誤差。基于通訊技術的定位方法通過場景中的無線信號進行定位，但是這類方法通常需要在場景中預先架設設備，并且難以獲取車輛的精確姿態，而精確位姿是智能車輛后續自主決策與控制所必需的，這使得這類基于無線的定位技術很難應用于智能車輛室內定位。SLAM 方法是目前智能車輛在室內場景定位時應用較多的一種方法，SLAM 方法利用如相機、激光雷達之類的傳感器在不同時刻的數據通過拼接對周圍環境進行建圖并同時給出載體在地圖中的定位，但是其過高的復雜度使得計算資源會被大幅消耗。將SLAM 方法拆分成建圖、定位兩個步驟，用先建圖后定位的方式在保證精度的前提下減少復雜度是一種可行的方式。

在決策層面，智能車輛根據感知信息進行路徑規劃等工作。路徑規劃算法根據理論的不同大體上可以分為智能算法、基于圖搜索的算法、基于采樣的算法等［14］。智能算法對自然現象進行仿生，以較低的成本對具有不確定性的復雜問題進行解決，遺傳算法［15］、粒子群算法［16］、蟻群算法［17］等常用的智能優化方法都可以應用于路徑規劃，但這類方法需要大規模的訓練且計算量較大，難以應用于大規模環境中的精確求解。圖搜索算法利用可視圖方法對場景進行建模并在可視圖中進行圖搜索，是目前應用最為廣泛的算法，A*［18］、D*［19］等都是典型的圖搜索算法，這類方法需要對環境預先進行建模，并可與其他方法結合使用取得更好的效果。基于采樣的方法則可以在沒有構建好可視圖之前用隨機采樣的方式對環境進行探索，之后可以選擇結合圖搜索方法規劃出可行駛路線，典型的采樣方法包括概率路徑圖（Probabilistic Roadmap，PRM）［20］、快速擴展隨機樹（Rapidly-exploring Random Tree，RRT）［21］等。

通過上述分析可以得知，采用先建圖、后定位的策略在室內場景中進行定位是一種較優的方法，同時，基于采樣的路徑規劃方法較為適合道路構造簡單的地下停車場環境。因此，本文針對上述分析及地下停車場的特點，對地下停車場的定位及路徑規劃算法進行了設計。采用雙目離線建圖、單目在線定位的策略實現低成本高精度室內定位，并通過選取路口作為路徑圖生成節點，提出了一種基于路口路徑圖（Intersection Roadmap，IRM）的路徑規劃算法，提升了其在資源占用率、定位規劃效果及實時性方面的整體表現。

2 地下停車場定位方法

2.1 多層級地圖構建

地下停車場多層級地圖是實現智能汽車定位的基礎，多層級地圖的構建使得單次制圖，多次定位得到實現。多層級地圖建圖時采用一臺雙目攝像機，將地下停車場分成一系列的數據采集節點，每個節點間距為2 m。為了能夠在定位匹配時能夠同時保證高效性與準確性，多層級地圖將每個節點采集場景進行處理，得到不同層次的信息并分別存入3 個層級：（1）BoW（Bag of Words）［22］特征層，特征層對左右目圖像分別提取視覺局部特征，并將左目特征與BoW 模型結合，構成BoW 特征層；（2）場景結構層，將左右目局部特征進行匹配，并通過三角法計算三維信息構成場景結構層；（3）節點坐標層，即節點在地下停車場坐標系下的真實坐標，坐標信息的真實值通過節點間場景結構位姿計算綜合手工測量得出，同時借助如車道線一類的環境參照物可以標定出節點采集時鏡頭的朝向。在此基礎上得到這些節點的節點坐標系與地圖坐標系的旋轉矩陣，并作為推算剩余節點坐標系與地圖坐標系之間旋轉矩陣的基準定位結果的相對位姿，在結合坐標信息后可轉化為絕對位姿。文章構建的多層級視覺地圖的數據采集如圖1 所示。

圖1 多層級視覺地圖數據采集Fig.1 Data acquisition of multi-layer visual map

2.2 多層級地圖中的視覺定位

2.2.1 節點級定位

本文中車輛的節點級定位主要依靠詞袋法將車載相機拍攝畫面與視覺地圖庫中圖像進行匹配，得出與車載相機拍攝畫面最相似的地圖庫圖像，詞袋模型進行圖像匹配主要分為三個步驟：特征提取及描述，視覺詞典構造，節點匹配。

步驟1：特征提取及描述

對地圖庫中圖像進行預處理，將各圖像灰度化及直方圖均衡化并縮放至同一尺度，利用ORB（Oriented Fast and Rotated Brief）［23］算法提取預處理后各圖像的特征點與描述符并生成特征描述子。

步驟2：視覺詞典構造

采用K-means［24］聚類方法將提取到的特征構造成單詞表，對各地圖庫圖像的詞頻數進行統計得出詞頻直方圖并生成對應的特征頻描述子。

步驟3：節點匹配

采用同樣方法提取出待定位點圖片的特征頻描述子，并計算該描述子與視覺地圖庫中所有的特征頻描述子的海明距離，距離最小的特征頻描述子對應的節點即為與車輛最近的地圖節點。

2.2.2 位姿級定位

粗定位將車輛定位在視覺地圖中某節點附近，通過透視n點（Perspective n-points，PnP）［25］求解可以實現車輛的位姿級定位。首先對檢索得出節點是否為最近節點進行確認，其次通過標定矯正后雙目相機的拍攝畫面及定位節點對車輛的位姿進行確定。

位姿估計的主要求解目標是計算出車輛所在位置相對于視覺地圖中對應節點位置的位移向量t和旋轉矩陣R。當有n個特征點時，將第i個特征點記作Pi，將其世界坐標記作[XiYiZi]T，將其投影的像素坐標記作[uivi]T，對于透視n點問題，在齊次坐標下有：

其中：K代表車載相機的內參數矩陣，t為平移向量，R為車載相機坐標系與模型中的攝像機坐標系間的旋轉矩陣。

當有6 個及以上的特征點時，可以根據上式求出R和t，以此可以求出當前位置與節點的距離：

3 路口中心檢測與路徑規劃算法

PRM 是一種經典的采樣路徑規劃算法，其在場景中進行隨機采樣后，將各采樣點在其鄰域范圍內進行碰撞檢測并連線構建路徑圖，規劃時將起點、終點添加至路線圖中并進行尋路，這種單次構圖、多次規劃的規劃策略簡單而高效，但隨機采樣使得規劃路徑并不一定能達到最優且有可能規劃失敗，增加采樣點可以在一定程度上提高規劃效果，但規劃時間會大幅增加。針對此問題，本文提出了一種路口路徑圖（IRM）規劃方法，首先提取出道路的邊線，之后在提取到的邊線基礎上計算出路口中心，利用路口中心作為采樣節點來構建路徑圖，克服了PRM 算法的缺點。

3.1 道路邊線檢測

3.1.1 道路輪廓提取

Canny 邊緣檢測是目前應用非常廣泛的一種邊緣檢測算法，該算法使用基于梯度幅值的雙閾值方法對圖像輪廓、邊緣進行檢測，本文使用Canny 邊緣檢測算法提取道路輪廓。

Canny 算法對輸入圖像依次進行去噪、圖像梯度計算和非極大值抑制，最終使用雙閾值方法確定出邊緣。在梯度計算時，Canny 邊緣檢測利用一階微分算子計算圖像中各點處的梯度幅值和梯度方向，以圖像某點(i，j)為例，其兩方向偏導分別如式（3）、式（4）所示：

其中，I(·)代表某點處灰度值，此時點(i，j)處的梯度幅值G(i，j)和梯度方向θ(i，j)分別為：

將Canny 算法應用于地圖場景得到的道路輪廓如圖2 所示。

圖2 道路輪廓Fig.2 Road contour

3.1.2 道路邊線檢測

在提取的道路輪廓圖中，通過Hough 直線變換［26］可以得出道路的邊線。Hough 變換將笛卡爾空間或極坐標空間映射至霍夫空間，原空間中的點會映射為霍夫空間中的一條線，而原空間中的線會映射為霍夫空間中的一個點。根據變換性質，在霍夫坐標系下，多條直線的交點在原空間中很可能是條直線。因此，當霍夫坐標系內交于某點的曲線數達到某設定閾值時，就認為原空間中存在相應的一條直線，在道路輪廓圖中使用Hough 直線變換即可得出道路邊線的檢測結果。

由于Hough 變換的檢測原理，邊線檢測結果中，單條直線附近容易出現距離很近的冗余直線，因此需要在邊線檢測基礎上進行優化，通過形態學閉運算，對檢測圖像先膨脹后腐蝕，從而將距離很近的線條整合為單條直線，通過上述步驟即可得出道路邊線檢測的擬合結果，如圖3所示。

圖3 道路邊線檢測Fig.3 Detection of road edge

3.2 路徑圖生成與路徑規劃

3.2.1 道路邊線交點檢測

Shi-Tomasi 角點檢測［27］方法在經典的Harris角點檢測方法基礎上對角點響應函數進行了改進，Shi-Tomasi 方法以一個小窗口在圖像中進行滑動，并計算窗口內部的像素值變化量E(u，v)：

其中：[u，v]為窗口中心像素坐標；ω(x，y)為設定的窗口響應函數；Ix，Iy為相應的灰度偏導。

像素梯度矩陣的特征值可以計算出來并用于角點響應函數計算，記相應的特征值為λ1、λ2，根據特征值可對窗口區域進行非極大值抑制，Shi-Tomasi 角點檢測方法中設定的角點響應函數為：

R值可用于非極大值抑制及角點檢測，R值大于設定閾值時，即認為檢測出角點，圖4 中展示了邊線交點檢測的結果。

圖4 邊線交點檢測Fig.4 Edge line intersection detection

3.2.2 路口中心檢測

3.2.1 節中檢測出了各道路邊線交點，但各交點所歸屬的路口位置仍是未知的，因此無法根據交點直接計算出路口中心坐標，針對此問題，采用K-means 聚類算法來在多個交點中得出相應的路口中心，K-means 均值聚類隨機選取k個點作為分類中心點，并將每個數據點放入距離最近的中心點所在的類中，計算各個分類數據點中心點的平均值作為新的中心點并循環迭代至不發生變化，圖5 中展示了最終的路口檢測結果。

圖5 路口檢測結果Fig.5 Result of intersection detection

其中，Pip表示道路邊線交點，num（·）表示點數量。

路口節點坐標在初次計算完成后，可供PRM 算法解算路徑多次使用，無需重復計算。

3.2.3 基于IRM 的路徑規劃

在得到了各路口坐標之后，使用與PRM 算法類似的路徑圖生成策略可以構建出基于路口節點的路徑圖。

路徑圖構造時首先對檢測到的路口節點進行檢測，當節點位于障礙物內部時，將其作為誤檢測進行剔除。當節點檢測完成后，將剩余的節點進行連線，所有節點均連線完成后即成功完成了IRM 的構造，連線時需要保證各節點的連線不能與障礙物重疊，為了節省計算量，縮短路徑檢索時間，各節點只連接自己鄰域范圍內的點，太遠的點不進行連接，圖6 中展示了在地下停車場地圖中生成的IRM。

圖6 路口路徑圖Fig.6 Intersection Roadmap

當車輛在地下停車場環境中規劃全局可行駛路線時，在給定目標位置作為終點后，利用路徑圖構造方法將其添加至路徑圖中，同時使用本文提出的由粗到精視覺定位方法對車輛進行定位，并將定位結果作為規劃起點，起點隨著車輛行駛進行實時更新并以同樣的方式添加至路徑圖中。起點、終點均添加至路徑圖后，可利用A*、Dijkstra 等圖搜索算法在路徑圖中找到最短路徑。本文在實驗環節對于不同算法規劃出的路徑進行了對比，并將IRM 與PRM 算法的規劃時間和規劃路徑長度進行了測試。

4 實驗與結果

為驗證算法可靠性，選取江蘇大學汽車工程研究院一樓車庫和鎮江市吾悅廣場購物中心地下停車場作為實驗場景，選用ZED2 雙目相機作為實驗數據采集設備，圖7 中展示了使用的數據采集設備，圖8 中展示了實驗場景。

圖7 數據采集設備Fig.7 Data acquisition equipment

圖8 實驗場景Fig.8 Experimental scenario

在完成多層級地圖構建的停車場區域內進行多尺度定位的驗證。為方便圖像數據的處理，使用地圖構建時的測試車輛和ZED2 相機記錄車輛行駛時的相機錄像。設定ZED2 錄制視頻為15 幀每秒，并手動測量每一段行駛軌跡的長度作為真值，共采集30 條軌跡，長度在15 m到30 m。

將錄制的所有視頻按幀截取圖片，利用多尺度定位方法得到每一幀圖像在停車場地圖中的定位信息，連接所有待定位點，得到計算軌跡。

多尺度定位中首先驗證節點級定位的準確率。截取所有軌跡視頻總共得到2 657 幀待定位圖像。其中最近節點的匹配率為63%，最近兩個節點匹配率為82%，最近三個節點匹配率為98%。盡管有很多點沒有匹配到距離最近的節點，PnP 算法在5 m 內具有較高的計算精度，最近三個節點匹配率為98%，這意味著這些點距離匹配到的節點距離不超過3 m。在精確定位的過程中依然能夠順利計算相對節點的位姿信息。

通過算法的自檢過程，完成定位的待定位點為1 903 個，定位成功率為71.6%。比較所有計算得到軌跡與對應真值，其中最大誤差2.3 m。平均誤差為1.3 m，平均誤差率為7.4%。

同時，為了比較規劃算法的優劣，對本文使用的IRM 算法和典型的A*算法、PRM 算法、蟻群算法、RRT 算法進行了對比。圖9 中展示了不同算法規劃出的路徑，可以看出IRM 算法規劃的全局路徑最為合理，同時文章將IRM 算法和PRM算法在不同參數下的規劃時間和規劃路徑長度進行了對比，表1 中展示了不同參數下的規劃算法數據平均結果。

從圖9 的規劃結果對比中可看出，在均未經過進一步的路徑平滑或節點剪枝等處理之前，IRM 算法規劃出的路徑在各個規劃結果中最為合理且規劃路徑最短，實驗結果表明，在規劃路徑的合理性及可行性上，IRM 算法可以取得較優的效果。

圖9 不同算法規劃路徑Fig.9 Path of different algorithms

另外，由于PRM 算法與IRM 算法在路徑圖生成后的路徑搜索部分原理相同，因此對IRM算法和PRM 算法進行了對比分析。從表1 中可以看出，在增加了采樣點數量或者鄰域距離時，PRM 算法的規劃出的路徑長度會有所下降，但受限于其隨機采樣原理，其路徑長度無法達到最優，同時PRM 算法在路徑圖生成時會占用大量的計算資源，從表1 中可以看出采樣點數量及鄰域距離增加后，盡管規劃效果會有所提升，但規劃時間會快速增長；當采樣點數量由200 增長至500，鄰域距離由500 像素增長至1 000 像素時，規劃時間會由5 s 左右增長至接近1 min，這樣的規劃代價是難以接受的；并且PRM 的隨機性會使得在不同的節點采樣生成不同的規劃路徑，與原始的PRM 算法相比，IRM 算法的規劃時間由5 s 左右縮短至0.2 s 以內，路徑長度縮短了約9.56%。可以看出IRM 算法能夠穩定的輸出在極短時間內實時生成的最短路徑，IRM 算法的實時性和規劃可行性均得到了保證。

表1 規劃算法數據Tab.1 Data of the planning algorithm

5 結論

針對車輛在地下停車場中的定位導航問題，本文圍繞路徑規劃問題展開研究，針對地下停車場環境的特點，利用計算機視覺技術根據視覺地圖對車輛進行定位。導航采用的路徑規劃方法中使用路口中心坐標取代傳統PRM 算法的隨機采樣，使用單目相機的視覺定位平均誤差為1.3 m，平均誤差率為7.4%。基于路口節點的規劃算法相比原始的PRM 算法，規劃時間由5 s 左右縮短至0.2 s 以內，路徑長度縮短了約9.56%。定位導航系統能夠實時準確地實現車輛在停車場環境中的導航，具備良好的實用性，IRM 算法在規劃時間大幅縮短的情況下得到了最優的路徑，實現了在地下停車場環境中對司機尋路的引導。