基于SLAM的停車場定位與導(dǎo)航系統(tǒng)設(shè)計(jì)

王蘇昊

（浙江省溫州中學(xué)，浙江溫州，325014）

0 前言

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

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

1 相關(guān)技術(shù)

1.1 相機(jī)模型

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

1.2 視覺里程計(jì)

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

圖1 相機(jī)模型

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

1.3 IMU 技術(shù)

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

2 基于SLAM 的導(dǎo)航系統(tǒng)

基于SLAM 的導(dǎo)航系統(tǒng)結(jié)構(gòu)如圖2 所示。首先應(yīng)該有一個(gè)停車場的三維地圖，這一步可以由停車場的工作人員進(jìn)行掃描得到，拿一臺(tái)手持的深度相機(jī)或者雙目相機(jī)采集在運(yùn)動(dòng)的過程中采集到連續(xù)的圖片，通過這些圖片可以用SLAM技術(shù)恢復(fù)停車場的三維結(jié)構(gòu)。當(dāng)停車場出現(xiàn)比較大的修整時(shí)需要及時(shí)對(duì)地圖進(jìn)行相應(yīng)的調(diào)整。

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

圖2 基于SLAM 的導(dǎo)航系統(tǒng)

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

3 總結(jié)

本文中設(shè)計(jì)了一套可以用于在室內(nèi)停車場進(jìn)行導(dǎo)航的系統(tǒng)，這套系統(tǒng)應(yīng)用了多傳感器融合的SLAM 算法，在計(jì)算量，精度等各個(gè)方面都有比較好的表現(xiàn)。但是由于目前實(shí)驗(yàn)環(huán)境的限制，本文中只針對(duì)理論部分做了設(shè)計(jì)沒有進(jìn)行實(shí)驗(yàn)的操作，這是后續(xù)可以進(jìn)行工作的方向。此外，在停車場這種重復(fù)性場景比較多的環(huán)境中，單純的SLAM 可能會(huì)有比較大的挑戰(zhàn)，可以通過wifi 定位或者貼標(biāo)簽的方式完成重定位。