基于SLAM的停車場定位與導航系統設計

王蘇昊

（浙江省溫州中學，浙江溫州，325014）

0 前言

21 世紀以來，人民消費水平逐步提高，與此同時，對便捷交通的需求促使人們越來越多地購買私家車。為了滿足人們停車的需要，停車場的規模也日趨擴大。但是由于停車場面積大且擁有大量重復性場景，人們容易在停車場迷路。目前已經有的導航技術如全球定位系統（Global Positioning System，GPS）等一方面由于技術本身的缺陷，在室內環境中信號的強度不夠，所以導航難度大，另一方面，民用的GPS 難以達到用于要求的精度。而目前SLAM 作為一種可以在未知環境中進行定位和導航的技術已經大規模應用于虛擬現實，增強現實，自動駕駛等領域[1]。此外SLAM 可以作為在室內進行定位、導航的關鍵性技術。SLAM 技術是利用傳感器感知周圍環境進行工作的，它目前已大量被應用于自動駕駛、機器人領域，在定位與建圖過程中發揮著舉足輕重的作用[2]。SLAM 技術可以主要分為視覺SLAM 和激光SLAM 等，激光SLAM 是早期發展起來的SLAM 技術，其使用激光傳感器對周圍環境進行感知，因此可以獲得周圍環境比較準確的位置，但是激光傳感器的缺點在于成本較高，體積很大，在使用的過程中易受天氣變化影響[3]，同時存在有玻璃的環境中無法工作的問題。視覺SLAM 由于傳感器價格低廉、易獲取，且視覺傳感器獲取到的周圍環境信息更豐富，因而受到了研究者們的青睞。目前視覺SLAM 所使用的傳感器主要分為單目[4]，雙目，深度相機等。單目傳感器由于無法獲取到環境中物體的準確位置，所以算法上相對來說比較復雜。雙目傳感器相對來說可以獲取到比較準確的深度信息，但是計算起來相對較慢。深度相機可以通過物理的方法獲取到深度信息[5]。

本文將使用視覺SLAM 完成構建三維地圖、定位以及重定位的功能。視覺傳感器分為多種類型，單目相機雖然運算速度快，但是會損失深度信息；深度相機（RGB-D）在彌補了單目相機不足之處時[6]，也出現了距離過遠紅外線難以返回的問題，在實際操作中問題較多；而雙目相機則有運算復雜度大的缺陷。綜上考慮，本文將使用雙目相機。同時，單一傳感器無法滿足SLAM 在精度和速度上的要求，在使用過程中可能會出現圖像模糊等問題以致于出現偏差。因此多傳感器融合SLAM 系統成為目前主流發展方向[7]。由于單純的視覺技術在使用的過程中會存在穩定性不足，計算消耗大的情況，本文中將使用視覺加imu 技術進行定位。三維重構是將一些客觀的二維圖片在計算機中進行虛擬重建過程。作為一門新興的技術，三維重構在文物保護、醫療、娛樂等領域皆有廣泛的應用，本文中將使用三維重構技術構建停車場三維地圖[8]。

1 相關技術

1.1 相機模型

假設在空間中有一點P，它的世界坐標為Pw。運用外參數（即平移和旋轉）將它轉化為P 點的相機坐標Pc。點P 通過光心投影到相機的物理成像平面上，映射出點P'，點P’坐標可以通過Pc 坐標利用相似三角形知識計算。在這幅圖里就可以發現傳統相機模型的缺陷：點P'無法反推出空間中P 點的z 坐標，P 點可以在PP'連線上的任意一點。這時候，為了將無數P'點映射到一個平面內，就不得不損失“深度”信息，而采用歸一化平面（這時不再考慮物理成像平面，同時將P'點默認移至歸一化平面），人為規定z=1，從而得到P'點坐標，即P 點的“歸一化“相機坐標Pc。最后，由于圖片在計算機中以像素的形式存儲，所以要將歸一化平面上的點通過內參數K 轉化為像素平面上的坐標。同時為了方便觀測，還要將像素平面上的坐標原點平移至平面左上角，如圖1 所示。

1.2 視覺里程計

經典SLAM 模型中運用路標（特征點）來描述SLAM過程[9]。這里要求的特征點應是三維空間中固定不變的點，并且能在光照、視角發生少量變化時仍能保持一致，比較常見的算法有SIFT，SURF，ORB 等。這一類算法都首先在一張圖片中找到很多對于光照，視角，尺度等變化比較穩定的特征點如角點等，通常情況下一張圖片會有1000 個左右的特征點，在得到這些特征點之后還需要對這些特征點進行描述，這也就是生成描述符的步驟，這一步需要利用到特征點周圍區域的信息，以關鍵點為中心取一個區域，利用這個區域的特征比如直方圖等生成一個高維度的向量稱之為描述符。

圖1 相機模型

在得到描述符之后需要對兩幅圖片的描述符進行匹配，這一步的目的是知道那些特征點對應于真實世界的同一個點。尋找兩幅照片中的相同特征點進行匹配的過程中，計算兩幅圖片不同特征點之間的距離如歐式距離或海明距離等，距離最小的可以認為是最相似的點。在得到大量的匹配點之后，可以通過PnP 或者ICP 算法對機器人的運動進行計算。用相機移動拍攝中，總是以當前“機器人”（可以是手機）為原點構建坐標系，將像素坐標轉換為這個坐標系中的三維坐標，這時，可通過計算將當前坐標系中的點的坐標轉換至世界坐標或參考坐標系的坐標。

1.3 IMU 技術

機器人在運動過程中會有加速度與速度變化，內置的IMU 可以計算得到不同時刻機器人的朝向、速度以及移動距離[10]。由于視覺SLAM在運行的過程中需要很大的計算量，在資源受限的嵌入式設備中運行比較困難，加入IMU 之后，可以通過IMU 測量得到機器人的速度，加速度等通過積分的方式可以計算到比較準確的姿態，此時將不需要一直使用視覺跟蹤的技術，從而可以極大地減輕計算量。

2 基于SLAM 的導航系統

基于SLAM 的導航系統結構如圖2 所示。首先應該有一個停車場的三維地圖，這一步可以由停車場的工作人員進行掃描得到，拿一臺手持的深度相機或者雙目相機采集在運動的過程中采集到連續的圖片，通過這些圖片可以用SLAM技術恢復停車場的三維結構。當停車場出現比較大的修整時需要及時對地圖進行相應的調整。

首先，將整個停車場置于規定好的參照坐標系，確定坐標原點，然后通過SLAM 技術雙目相機傳感器拍攝采集圖像，重要標志物的無數P 點組成無數像素平面，并得到各個P 點坐標。運用三維空間剛體運動模型，將不同角度得到的同一特征點（P 點）的坐標結合計算分析，得到該點在參照坐標系中的坐標，以此類推，得到某幾樣標志物的坐標，構建主體模型。在SLAM 移動拍攝中，使用視覺里程計模型測量照片偏移距離及坐標，并以此反算出參照坐標系中偏移距離及坐標。在移動過程中，同時采用imu 技術計算機器人當前位置。由于imu 可能打滑出現誤判，這時可以通過回環檢測進行非線性優化，完善地圖。

圖2 基于SLAM 的導航系統

用戶在使用的過程中首先通過GPS 定位到當前所在的商場然后將對應數據庫中的地圖加載到客戶端。需要在停好車后將車停下后，客戶端記錄下車的坐標，記錄坐標采用重定位的方法即可，打開客戶端，客戶端將自動地訪問相機，通過相機采集到圖片，在地圖中進行重定位，并記錄下當前的位置。購物返程時，可以再次打開客戶端，同時，在起始位置，相機將被打開并拍攝周圍環境，系統從數據庫中尋找到所有相似照片。何為相似？即運用人工智能在不同照片標注收集特征點，描述，若一個吻合，則在照片中尋找下一個距離此點想通過的特征點，并用“1，0”標注此點原照片是否存在，以此類推，不斷逼近，最后，比對代碼得到大量相似圖片后，尋找在同一坐標附近的相似照片作為真實的“相似”，然后通過距離，偏角計算得此照片真實的坐標及人在坐標系中所處的位置及面朝的方向，相機關閉。接下來在跟蹤的過程中由于人的速度比較慢可以使用IMU 完成，這可以極大地減少計算量，但是由于IMU 跟蹤可能存在誤差，所以隔一段時間需要通過相機進行一次重定位。這時，客戶端能夠根據人的位置坐標與車位置坐標在坐標系中規劃得到最佳路徑。通過手機內置IMU 感受人的前進方向及偏轉角，實時在地圖顯示。同時，還是由于IMU 存在誤差，每隔10-30s 拍攝一次進行坐標偏角校正，當然，如偏離軌道，可重新進行軌跡規劃。

3 總結

本文中設計了一套可以用于在室內停車場進行導航的系統，這套系統應用了多傳感器融合的SLAM 算法，在計算量，精度等各個方面都有比較好的表現。但是由于目前實驗環境的限制，本文中只針對理論部分做了設計沒有進行實驗的操作，這是后續可以進行工作的方向。此外，在停車場這種重復性場景比較多的環境中，單純的SLAM 可能會有比較大的挑戰，可以通過wifi 定位或者貼標簽的方式完成重定位。