潘 堯，吳 聰，張穎江，劉伶俐，辛 梓

智能車輛雙目視覺與地圖組合定位研究

潘 堯1，吳 聰1，張穎江1，劉伶俐2，辛 梓2

（1湖北工業(yè)大學(xué)計算機學(xué)院，湖北武漢430068；2武漢光庭信息有限公司，湖北武漢430073）

車身傳感器能實時感知車身位置信息，通過構(gòu)造部分道路信息的駕駛地圖進行映射完成初始定位，利用立體視覺實時感知環(huán)境進行精確定位，解決智能車輛“在哪里”的問題。在真實駕駛場景中測試了其可行性，初步實現(xiàn)了低成本高精度的定位效果。

視覺定位；高精度地圖；視覺里程計；駕駛地圖；無人駕駛

智能車輛常見定位技術(shù)主要有航位推算定位技術(shù)、慣性導(dǎo)航定位技術(shù)、全球?qū)Ш叫l(wèi)星定位技術(shù)、磁定位技術(shù)和視覺定位技術(shù)等。航位推算實施成本低，用當(dāng)前位置信息結(jié)合車身姿態(tài)信息推測出下一刻位置，容易產(chǎn)生累積誤差；慣導(dǎo)與航位推算原理類似，利用慣性元件來測量車輛的加速度，經(jīng)過積分和運算得到速度和位置［1］，高精度慣導(dǎo)也就意味著高成本的硬件投入，在某些道路場景中建筑物的遮擋、多徑效應(yīng)等［2］都會影響它的精度；GPS在智能車輛定位中屬于主流技術(shù)，但是容易受到“城市森林”等障礙物遮擋影響信號，民用GPS精度還達不到智能車輛厘米級定位需求［3］；磁定位技術(shù)不受環(huán)境干擾，擁有很穩(wěn)定的定位效果，當(dāng)應(yīng)用在智能車輛中，前期對道路環(huán)境投入成本過高［4］；視覺定位技術(shù)通過相機獲取豐富環(huán)境信息，對算法性能要求較高，具有較高的靈活性。Levinson和Thrun［5］用64線激光雷達記錄三維點云再通過激光束測出物體之間的距離達到定位目的，Moosmann等［6］將激光雷達定位與高精度的全球衛(wèi)星定位系統(tǒng)相結(jié)合，進一步提高了定位精度，也有很強的魯棒性，但太依賴于激光雷達。Badino［7－8］先用全局圖像中特征圖像繪制成地圖軌跡記錄并存儲下來，定位時用當(dāng)前圖像來匹配地圖信息，再用場景與之前地圖軌跡對應(yīng)得出位置，很明顯，稠密路標(biāo)的選取雖然使準(zhǔn)確性大大增加，但是算法效率不高。在同時將定位與地圖構(gòu)建［9－12］合在一起，該方法取決于車輛姿態(tài)和地標(biāo)的全球位置，主要有遞歸濾波和非線性最小二乘法來更新位置信息，Lategahn等［13－14］則將制圖和定位分開研究。Pink［15］提出基于道路特征定位，通過迭代圖像特征中最近點來估算單個姿態(tài)，用卡爾曼濾波對單一功能之間誤差進行平滑，更好地耦合了觀測和模型狀態(tài)［16］。國內(nèi)方面，張學(xué)習(xí)等［17］利用全向視覺獲取圖像，用顏色識別出目標(biāo)后，利用坐標(biāo)轉(zhuǎn)換實時構(gòu)建全局地圖，該方法具有一定的局限性；林成建等［18］運用改進粒子群算法與模糊神經(jīng)網(wǎng)絡(luò)相結(jié)合方法進行視覺定位，計算繁瑣，不適合工程應(yīng)用；仲躋煒等［19］設(shè)計了北斗高精度定位與高精度地圖組合平臺實現(xiàn)了車道級定位；北京交通大學(xué)的陳紅巖［20］只用視覺里程計在限制區(qū)域內(nèi)剔除運動估計中的誤匹配點，無法客觀評價其方法的好壞，姜雨菲［21］在雙目立體視覺下提出了CUFastSLAM算法，但是在室內(nèi)小尺度環(huán)境下進行等等。因此，利用適合智能駕駛的駕駛地圖進行初步定位，通過立體視覺實時感知道路環(huán)境進行進一步精確定位，實現(xiàn)一種不依賴高精度傳感器的低成本定位方式，并在室外真實駕駛環(huán)境中對該方法進行測試。

1 駕駛地圖

假設(shè)車輛始終行駛在道路上，將車輛當(dāng)前位置信息與導(dǎo)航系統(tǒng)地圖數(shù)據(jù)進行比較和匹配，獲得車輛所在道路的初步位置，再通過車輛點投射到道路上，進一步校正減少定位誤差［3］。利用地圖匹配技術(shù)還能將一些道路場景中難以識別或者識別實時運算量較大的場景（比如紅綠燈等）提前采集。厘米級高精度地圖在智能駕駛中具有重要作用，它能使車輛對所處環(huán)境進行精準(zhǔn)預(yù)判，提前選擇合適的行駛策略。然而厘米級高精度地圖里面數(shù)據(jù)采集信息太豐富，地圖匹配時造成時間消耗［16］，因此制作適合定位需求的地圖數(shù)據(jù)庫來減少搜索匹配時間代價。在視覺定位時只需要根據(jù)具有道路標(biāo)記信息（包括彎道、十字路口、紅綠燈等）的駕駛地圖來獲取車身當(dāng)前位置。道路軌跡抽象成黃色線段，交通標(biāo)志牌的位置與道路標(biāo)線的位置通過地圖采集車采集，用經(jīng)度、緯度、高度來表示。道路標(biāo)線屬性則用實線、虛線、限制線、停止線來表示。整個道路軌跡由若干路線組成，每個路線包含了該條路線的起點、終點和標(biāo)線屬性（圖1）。

圖1 駕駛地圖示意

2 雙目立體視覺

單目視覺算法可以得到相機姿態(tài)角的該變量和相對位置矢量方向，但難以得到場景中物體的實際尺寸，而雙目立體視覺在有共同視野時可重建出環(huán)境的三維信息，并精確地得到6個自由度的位姿增量。所以本文按照一定結(jié)構(gòu)［22］構(gòu)建雙目視覺系統(tǒng)。雙目相機就是模擬人眼，利用左右目相機成像視差計算出世界坐標(biāo)系中的觀測點與雙目相機模型之間的相對距離。如圖2所示，世界坐標(biāo)系中P（Xw，Yw，Zw）點與左右目相機光心OcL、OcR三點組成的平面為対極平面；PL（x，y）、PR（x’，y’）分別是P點在左右目相機圖像平面投影點，其橫向坐標(biāo)之差為視差d；OL（cx，cy）、OR（c＇x，c＇）分別是光心在左右目相機成像平面的投影點，也稱主點；主點O與光心OC所在的直線成為相機光軸，之間的距離為相機焦距f；雙目相機模型中以左目相機光心為參考原點。

根據(jù)三角形相似原理［23］易知，

圖2 世界坐標(biāo)系與圖像坐標(biāo)系之間轉(zhuǎn)換

因此，利用上式可以建立世界坐標(biāo)系點與圖像坐標(biāo)系點之間的對應(yīng)關(guān)系。采用Matlab camera calibration工具箱進行標(biāo)定，立體校正得出相機的內(nèi)外參數(shù)，如圖3所示部分標(biāo)定過程。

圖3 Matlab標(biāo)定工具箱部分過程

通過上述過程得出相機的內(nèi)外參數(shù)為：右目相對于左目旋轉(zhuǎn)矩陣

右目相對于左目平移矩陣

左目相機標(biāo)定參數(shù)

左目畸變矩陣

右目相機標(biāo)定參數(shù)

右目畸變矩陣

將這些參數(shù)寫入OpenCV中stereoRectify函數(shù)，利用極限約束得到圖4校正結(jié)果，（具體校正方法正在申請專利（受理號：201510756481.3）中，不再贄述），將應(yīng)用此立體相機參數(shù)開展下一步工作。

圖4 雙目校正后結(jié)果

3 視覺定位過程及實驗結(jié)果

在視覺定位時，在車輛中安裝IMU（慣性測量單元）來隨時感知車輛當(dāng)前姿態(tài)，在車前擋風(fēng)玻璃處安裝雙目立體相機進行實時圖像處理（圖5），利用上節(jié)標(biāo)定參數(shù)寫入圖像獲得矯正后的雙目圖像進行圖像分析。

圖5 車身傳感器安裝

車身IMU能隨時感知車輛的姿態(tài)，可抽象成非線性離散模型，其位置信息

其中，車身的初始位置（xveh，0，yveh，0）由高精度衛(wèi)星導(dǎo)航系統(tǒng)提供，xveh，0表示經(jīng)度信息，yveh，0表示緯度，、，信息車身當(dāng)前速度vφk由IMU獲取并采用WGS－84地球坐標(biāo)系進行單位轉(zhuǎn)換，Δt為兩個時刻時間差，其中i∈［0，1，2，…，N］，N為自然數(shù)。

圖6 定位過程

如圖6a所示，圓圈位置點代表地圖中車道線的位置采集點，用Mi表示。三角形位置點代表視覺識別出的車道線位置點，將圖像像素坐標(biāo)轉(zhuǎn)換為GPS坐標(biāo)，用Pci表示，這樣Mi作為系統(tǒng)模型，Pci作為觀測模型，卡爾曼濾波［2，14，16］生成實時糾正，為正方形所示，為了獲取連續(xù)穩(wěn)定的定位信息，利用立體圖像進行左右目特征點提取和前后幀特征點提取，通過提取圖像序列中一致信息來估計相機的位置Pc。由于車身IMU一般安裝在車后備箱位置，相機安裝在車前擋風(fēng)玻璃處，故估計相機位置Pc與車身位姿Pveh利用歐式距離算出P，如圖6b中所示，再將車身位姿與路面之間標(biāo)定得出R，將車身位姿映射到地面R·P與糾正P－i進行GPS坐標(biāo)換算即可得出定位信息。在實時場景中，道路標(biāo)線信息與駕駛地圖中位置進行比較，圖7a利用地圖映射技術(shù)將車輛在駕駛地圖中位置顯示完成初步定位，在圖7b中利用立體視覺識別出車道線與地圖中車道線信息做比較，利用卡爾曼濾波估計出當(dāng)前最佳點與車身當(dāng)前姿態(tài)進行換算，得出當(dāng)前最佳估計位置從而完成穩(wěn)定實時的定位過程。

如圖8所示，分別嘗試了ORB算法、SIFT算法和SURF算法進行特征點提取，參考表1三種算法處理時延可以看出，使用ORB算法在點特征提取與算法處理時間消耗上取得了較好的效果，因此，本文采用ORB算法進行點特征提取處理。

圖8 特征點匹配結(jié)果

表1 三種算法處理單幀分析

在圖7b中，藍線為視覺識別車道線位置，通過圖7a中車道線利用卡爾曼濾波進行估計，通過交通標(biāo)志位置信息進行縱向位置糾正，就達到了整個定位的目的。道路測試場景選擇光庭廠區(qū)某專用測試道路（箭頭示意路線，圖9）。該路段全長約1.4km，其中設(shè)置了6個道路箭頭，2個停止線，1條斑馬線，2個停車標(biāo)識牌，2個禁止標(biāo)識牌，餐廳和廠房位置為高層建筑，具備一定城市道路場景特點。

圖9 測試道路場景介紹

整個測試結(jié)果見圖10，橫軸代表經(jīng)度信息，縱軸代表緯度信息，其中實線軌跡代表使用高精度GPS采集，

圖10 測試結(jié)果對比

獲取了2000多個稠密采集點，將誤差精度控制在20cm以內(nèi)，具備很高的參考精度。虛線代表通過本文方法估計出位姿軌跡點，該軌跡點通過局部獲取的道路定位信息進行累加記錄，反映出了整個過程的定位結(jié)果。從整個過程來看在一些高樓場景和樹木林蔭場景也獲取到了定位結(jié)果，初步完成預(yù)期目標(biāo)。很明顯，在某些特殊路段中也出現(xiàn)異常值，初步調(diào)查結(jié)果認(rèn)為在彎道轉(zhuǎn)彎時，由于相機機身抖動造成獲取圖像不穩(wěn)定加上當(dāng)時所在環(huán)境樹木林蔭過于茂密影響了圖像獲取，造成了道路標(biāo)線識別不穩(wěn)定所致，這個也是使用視覺定位的條件限制。

4 結(jié)論

因此，本文給出了基于構(gòu)建駕駛地圖的視覺定位方法。將道路標(biāo)線、紅綠燈、交通標(biāo)志等信息融入到駕駛地圖中，在實時定位時，通過立體視覺相機獲取當(dāng)前道路信息，利用卡爾曼濾波與地圖道路標(biāo)線信息匹配進行位置初步映射，運用ORB算法進行特征點匹配左右目和前后幀提取圖像序列中的一致性信息，選取一致性信息在圖像序列中的位置變化進行相機運動估計，從而得出相機姿態(tài)，實現(xiàn)車輛的精確定位。該方法在實際駕駛環(huán)境中測試取得了較好的定位效果，初步實現(xiàn)了基于立體相機和地圖搭配的低成本定位。然而地圖與視覺信息深度融合在下一階段需要進行深入研究。

［1］ 秦永元.慣性導(dǎo)航［M］.第二版.北京，科學(xué)出版社，2014.

［2］ Julius Ziegler，Henning Lategahn，Markus Schreiber，et al.Video based localization for BERTHA［C］.Intelligent Vehicles Symposium.USA，IEEE，2014.

［3］ 劉興權(quán)，金美含.地圖匹配算法綜述［J］.科技信息，2014（4）：64－35.

［4］ 張營.智能車輛磁定位技術(shù)研究［D］.上海：上海交通大學(xué)，2008.

［5］ Levinson J，Thrun S.Robust vehicle localization in urban environments using probabilistic maps［C］／／IEEE International Conference on Robotics and Automation.IEEE，2010：4372－4378.

［6］ Moosmann F，Stiller C.Velodyne SLAM［C］／／Intelligent Vehicles Symposium.IEEE，2011：393－398.

［7］ Badino H，Huber D，Kanade T.Visual topometric localization［C］／／Intelligent Vehicles Symposium.IEEE，2011：794－799.

［8］ Badino H，Huber D，Kanade T.Real－time topometric localization［C］／／IEEE International Conference on Robotics and Automation.IEEE，2012：1635－1642.

［9］ Davison A J，Reid I D，Molton N D，et al.Mono－SLAM：real－time single camera SLAM.［J］.IEEE Transactions on Pattern Analysis ＆Machine Intelligence，2007，29（6）：1052.

［10］Bailey T，Durrant Whyte H.Simultaneous localization and mapping（SLAM）：Part ii［J］，Robotics ＆Automation Magazine，IEEE，2006，13（3）：108–117.

［11］Kummerle R，Grisetti G，Strasdat H，et al.G 2o：A general framework for graph optimization［C］／／IEEE International Conference on Robotics and Automation.IEEE，2011：3607－3613.

［12］Lu F，Milios E.Globally Consistent Range Scan Alignment for Environment Mapping［J］.Autonomous Robots，1997，4（4）：333－349.

［13］Lategahn H，Stiller C.Vision－Only Localization［J］.Intelligent Transportation Systems IEEE Transactions on，2014，15（3）：1246－1257.

［14］Lategahn H，Schreiber M，Ziegler J，et al.Urban localization with camera and inertial measurement unit［C］／／Intelligent Vehicles Symposium.IEEE，2013：719－724.

［15］Pink O.Visual map matching and localization using a global feature map［C］／／Computer Vision and Pattern Recognition Workshops，2008.CVPRW＇08.IEEE Computer Society Conference on.IEEE，2008：1－7.

［16］Schreiber M，Knoppel C，F(xiàn)ranke U.LaneLoc：Lane marking based localization using highly accurate maps［C］／／Intelligent Vehicles Symposium.IEEE，2013：449－454.

［17］張學(xué)習(xí)，楊宜民.基于全向是覺的移動機器人實時全局地圖構(gòu)建［J］.計算機測量與控制.2011，19（3）：643－646.

［18］Lin C J，Wang J G，Lee C Y.Pattern recognition using neural－fuzzy networks based on improved particle swam optimization［J］.Expert Systems with Applications，2009，36（3）：5402－5410.

［19］仲躋煒，陳大吾，朱峰.基于北斗高精度定位和車道級高精度地圖的車輛監(jiān)控平臺設(shè)計與實現(xiàn)［J］.全球定位系統(tǒng).2015，40（4）：50－54.

［20］陳紅巖.無人車立體視覺里程計研究［D］.北京：北京交通大學(xué)，2012.

［21］姜雪菲.雙目立體視覺SLAM研究［D］.北京：北京交通大學(xué)，2014.

［22］湖北工業(yè)大學(xué).基于機器視覺應(yīng)用的車載相機結(jié)構(gòu)［P］.中國：2015205648519，2015：7.

［23］馬頌德，張正友.計算機視覺［M］.北京：科學(xué)出版社，1998.

Research on Integrated Location with Stereo Vision and Map for Intelligent Vehicle

PAN Yao1，WU Cong1，ZHANG Yingjiang1，LIU Lingli2，XIN Zi2
（1 School of Computer Science，Hubei Univ.of Tech.，Wuhan 430068，China；2 Wuhan Kotei Informatics CO.，LTD，Wuhan 430073，China）

The sensors of vehicle could sense the body position information of vehicle in real－time，then map the initial location by the driving map which is built with some information of lane attributes，and use stereo vision which apperceives the road environment to accomplish the precise positioning.Those could resolve the intelligent vehicle’s situation of Where I am.The method is tested a good practicability in real driving scenarios.The position effect which consists of low－product sensors has been achieved high precision.

vision location；HD map；visual odometry；driving map；intelligent vehicle

TP391

A

［責(zé)任編校：張巖芳］

1003－4684（2017）04－0055－05

2016－05－10

湖北省自然科學(xué)基金（2012FFB00701）

潘 堯（1987－），男，湖北黃石人，湖北工業(yè)大學(xué)碩士研究生，研究方向為圖像處理

吳 聰（1982－），男，湖北武漢人，湖北工業(yè)大學(xué)副教授，研究方向為醫(yī)學(xué)圖像處理