楊 光，段富海，劉 強(qiáng)

（1.大連理工大學(xué)機(jī)械工程學(xué)院，遼寧 大連 116000；2.三峽大學(xué)機(jī)械與動(dòng)力學(xué)院，湖北 宜昌 443000）

1 引言

機(jī)器人為了在現(xiàn)實(shí)環(huán)境中獨(dú)立完成具體的導(dǎo)航任務(wù)，必須定位到在環(huán)境中的位置，這個(gè)問題可以看做是SLAM（Simultaneous Localization and Mapping）問題。移動(dòng)機(jī)器人中被廣泛使用的傳感器是2D激光雷達(dá)、視覺傳感器以及輪式編碼器。關(guān)于2D激光雷達(dá)SLAM的研究已經(jīng)趨于成熟，其中著名的是基于粒子濾波的Gmapping［1］，基于圖優(yōu)化的Cartographer［2］等。使用激光雷達(dá)傳感器估計(jì)機(jī)器人位置時(shí)，其準(zhǔn)確度取決于激光雷達(dá)的精度，但在大場景和相似結(jié)構(gòu)化環(huán)境中，由于環(huán)境結(jié)構(gòu)過于相似使得激光數(shù)據(jù)中的距離信息沒有明顯的變化，將導(dǎo)致機(jī)器人很難估計(jì)出自身位置。視覺傳感器包括單目相機(jī)、雙目相機(jī)和深度相機(jī)，基于視覺的SLAM 是近幾年的研究熱點(diǎn)。文獻(xiàn)［3］提出的LSD-SLAM 使用單目相機(jī)，通過使用直接法代替特征點(diǎn)法建立大尺度半稠密地圖。文獻(xiàn)［4］提出的基于特征點(diǎn)的半直接法ORB-SLAM2，對(duì)圖像提取特征點(diǎn)進(jìn)行匹配做幀間估計(jì)，并將系統(tǒng)設(shè)計(jì)為跟蹤，建圖和閉環(huán)檢測三個(gè)線程，提高了整個(gè)系統(tǒng)的實(shí)時(shí)性。但基于視覺的方法也存在對(duì)光照敏感，對(duì)環(huán)境紋理依賴性強(qiáng)的劣勢［5］。因此結(jié)合多種傳感器，優(yōu)勢互補(bǔ)，是解決機(jī)器人導(dǎo)航定位問題的重要研究方向。各類移動(dòng)機(jī)器人在導(dǎo)航時(shí)存在兩種共性難題：一個(gè)是任意位置啟動(dòng)時(shí)如何快速全局定位確定初始位置，另一個(gè)是運(yùn)動(dòng)過程中由于和地圖誤匹配導(dǎo)致的全局定位結(jié)果出現(xiàn)較大偏差［6］。這兩種情況可以統(tǒng)一看作是重定位問題，基于2D激光雷達(dá)的解決方法僅有二維平面點(diǎn)的距離信息，能夠代表環(huán)境的信息太少，在全局定位時(shí)容易出現(xiàn)誤匹配或者全局搜索時(shí)間過長，僅適合做幀間匹配［7］。而視覺傳感器具有豐富的環(huán)境信息，使用詞袋模型能夠更加快速準(zhǔn)確的檢測閉環(huán)，適合做全局的匹配定位［8］，若導(dǎo)航時(shí)使用激光雷達(dá)定位再結(jié)合視覺傳感器做全局重定位，優(yōu)勢互補(bǔ)，將有更好的魯棒性和適用性。國內(nèi)外學(xué)者對(duì)這方面進(jìn)行了相關(guān)研究［8-10］，但所提出的方法都是基于特征融合或者損失函數(shù)進(jìn)行融合，較為復(fù)雜且適用性低。這里提出了一種結(jié)合視覺和激光雷達(dá)的定位系統(tǒng)，設(shè)計(jì)了根據(jù)當(dāng)前定位狀態(tài)進(jìn)行模式切換的模式轉(zhuǎn)換模塊，當(dāng)機(jī)器人任意位置啟動(dòng)時(shí)使用信息豐富的視覺快速確定自己的位置，當(dāng)移動(dòng)過程中對(duì)定位不確定時(shí)切換視覺算法做重定位，當(dāng)正常移動(dòng)等其他情況使用計(jì)算量更小的激光雷達(dá)做定位。最后通過使用改進(jìn)的Cartographer和ORB-SLAM2算法在搭建的實(shí)驗(yàn)環(huán)境中驗(yàn)證了所提出方法的實(shí)用性和有效性。

2 系統(tǒng)框架

為提高機(jī)器人重定位能力所提出的方法主要有兩個(gè)階段：一個(gè)是前期建圖及地圖對(duì)齊階段，另一個(gè)是后期機(jī)器人導(dǎo)航時(shí)模式轉(zhuǎn)換階段，如圖1、圖2 所示。由于目前開源的視覺SLAM 方案中都是基于6-DoF計(jì)算的，而移動(dòng)機(jī)器人為平面移動(dòng)，因此為了減少計(jì)算量將6-DoF 簡化為3-DoF，減少視覺算法優(yōu)化時(shí)所用的時(shí)間。

圖1 建圖及地圖對(duì)齊過程Fig.1 Map Building and Map Alignment Process

圖2 導(dǎo)航及定位模式轉(zhuǎn)換過程Fig.2 Navigation and Localization Mode Conversion Process

建圖及地圖對(duì)齊階段，為了實(shí)現(xiàn)定位系統(tǒng)中兩種定位模式之間的切換，同時(shí)使用視覺和激光雷達(dá)建立地圖，并將兩種算法在時(shí)間上做同步處理，記錄運(yùn)動(dòng)軌跡，得到稀疏特征點(diǎn)地圖和二維占據(jù)柵格地圖。建圖完成后根據(jù)時(shí)間戳對(duì)齊相同時(shí)間節(jié)點(diǎn)的位姿，由于兩種算法計(jì)算頻率不同，遂對(duì)位置軌跡采用線性插值的方式確定對(duì)應(yīng)關(guān)系，然后構(gòu)建最小二乘問題求解兩條軌跡之間的變換矩陣，將特征點(diǎn)地圖經(jīng)變換矩陣轉(zhuǎn)換對(duì)齊到柵格地圖之上，最后將對(duì)齊好的復(fù)合地圖保存為二進(jìn)制文件。

導(dǎo)航階段，系統(tǒng)啟動(dòng)后加載已經(jīng)對(duì)齊好的復(fù)合地圖。初始化階段使用信息豐富的視覺確定初始位置，達(dá)到更快更魯棒的效果?？紤]到當(dāng)前啟動(dòng)視角可能提取視覺特征困難，遂采用啟動(dòng)機(jī)器人后隨機(jī)小范圍移動(dòng)的策略提高定位的成功率。確定初始位姿后將此結(jié)果作為初始值給到激光雷達(dá)定位算法，導(dǎo)航模塊將使用激光定位結(jié)果規(guī)劃路徑。當(dāng)里程計(jì)和定位算法的偏差或者激光定位算法優(yōu)化時(shí)的協(xié)方差矩陣大于預(yù)設(shè)的閾值后，系統(tǒng)將模式轉(zhuǎn)換為視覺重定位功能，尋找閉環(huán)，確定新的位姿再傳遞到激光雷達(dá)定位算法作為初始值，直到機(jī)器人移動(dòng)到指定的目標(biāo)位置。導(dǎo)航模塊使用的是DWA局部路徑規(guī)劃算法和A*全局路徑規(guī)劃算法。

3 ORB-SLAM2算法向3-DoF簡化

目前開源的主流V-SLAM 系統(tǒng)（MSCKF、ROVIO、SVO 和ORB-SLAM2等）都是針對(duì)三維空間6-DoF 下的優(yōu)化，沒有針對(duì)平面移動(dòng)機(jī)器人3-DoF下的優(yōu)化。在6-DoF進(jìn)行BA優(yōu)化時(shí)未被約束的自由度將導(dǎo)致優(yōu)化結(jié)果存在較大誤差，同時(shí)優(yōu)化復(fù)雜度也會(huì)更大。為減少計(jì)算量，將ORB-SLAM2 框架簡化為平面內(nèi)3-DoF進(jìn)行計(jì)算，提高計(jì)算頻率，平面3自由度系統(tǒng)圖優(yōu)化示意圖，如圖3所示。

圖3 平面內(nèi)3自由度圖優(yōu)化Fig.3 Planar 3-DoF Graph Optimization

在圖優(yōu)化框架的視覺SLAM算法中，可以列寫觀測誤差［15］：

式中：xk—k時(shí)刻相機(jī)的位姿；mj—yj對(duì)應(yīng)三維路標(biāo)點(diǎn)；zk，j—路標(biāo)點(diǎn)投影到圖像上的像素坐標(biāo)。

狀態(tài)估計(jì)其實(shí)是求觀測值與預(yù)測值值之差的最小值，將其他時(shí)刻的觀測量也考慮進(jìn)來，那么整體的代價(jià)函數(shù)為：

式（1）中的觀測方程為：

式（3）中K，R，t的形式分別為：

式中：K—相機(jī)的內(nèi)參矩陣；fx，fy—分別為相機(jī)像素平面X，Y方向的焦距；cx，cy—光心在像素平面上的坐標(biāo)；R—相機(jī)相對(duì)于全局參考坐標(biāo)系的旋轉(zhuǎn)矩陣；t—相機(jī)相對(duì)于全局參考坐標(biāo)系中的平移向量；θ—對(duì)應(yīng)的航向角。因此假設(shè)k時(shí)刻觀測到第mj個(gè)目標(biāo)定，并且mj的坐標(biāo)為（Xk，Yk，Zk），則觀測方程為：

將式（6）在X、Y、Z方向上展開得到：

則誤差項(xiàng)ek，j對(duì)路標(biāo)點(diǎn)位姿P（Xk，Yk，Zk）以及機(jī)器人位姿ξ（tx，ty，θ）的雅克比矩陣為：

在開源的6-DoF SLAM系統(tǒng)中，上述觀測方程中的旋轉(zhuǎn)矩陣一般為：

式中：θ，?，φ—三維坐標(biāo)系下的旋轉(zhuǎn)角；sθ—sinθ；c?—cos?。

對(duì)比兩個(gè)旋轉(zhuǎn)矩陣可以發(fā)現(xiàn)，簡化后3-DoF旋轉(zhuǎn)矩陣形式更加簡單，因此簡化后算法計(jì)算復(fù)雜度更低，將有更好地實(shí)時(shí)性。為了驗(yàn)證簡化后的算法，制作了機(jī)器人導(dǎo)航運(yùn)動(dòng)時(shí)的數(shù)據(jù)集，該數(shù)據(jù)集中共有586幀圖像。通過使用數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)得到兩種算法的運(yùn)行時(shí)間，如表1所示。由數(shù)據(jù)可知簡化后的3-DoF平均每幀計(jì)算所用的時(shí)間減少了3ms，幀率提升了3幀，計(jì)算效率提升高達(dá)10%。

表1 3-DoF與6-DoF運(yùn)行時(shí)間對(duì)比Tab.1 3-DoF and 6-DoF Running Time Comparison

4 特征點(diǎn)地圖與柵格地圖的對(duì)齊

基于激光雷達(dá)的SLAM的初始坐標(biāo)系是根據(jù)里程計(jì)信息確定的，而基于視覺的SLAM的初始坐標(biāo)系是根據(jù)第一個(gè)關(guān)鍵幀確定的，這兩個(gè)坐標(biāo)系之間存在一個(gè)變換關(guān)系，為了定位系統(tǒng)能夠在兩種定位模式中自由切換，需要找到這個(gè)變換關(guān)系然后將兩個(gè)地圖對(duì)齊。為了適用多種傳感器下的SLAM方法，采用對(duì)齊建圖運(yùn)動(dòng)軌跡的方式對(duì)齊兩個(gè)地圖，該方法不受傳感器類型限制，能夠適用各種不同的SLAM方法。通過定義兩條軌跡之間的距離損失函數(shù)，軌跡對(duì)齊問題也就變成了優(yōu)化問題，優(yōu)化求解使損失函數(shù)達(dá)到最小，就能得到對(duì)應(yīng)的變換關(guān)系。

建圖時(shí)兩種方法共同進(jìn)行，并進(jìn)行了時(shí)間同步處理，因此利用時(shí)間戳確定對(duì)應(yīng)位姿關(guān)系。但由于兩種算法計(jì)算頻率不同，遂對(duì)軌跡先采用線性插值的方式進(jìn)行處理再通過時(shí)間戳確定匹配關(guān)系。將確定好的匹配關(guān)系按式（11）構(gòu)建最小二乘問題并優(yōu)化求解，得到兩種地圖的變換關(guān)系，最后根據(jù)變換關(guān)系對(duì)齊兩種地圖，如圖4所示。

圖4 利用軌跡對(duì)齊地圖過程Fig.4 Use the Trajectory to Align the Map Process

式中：p1i，p2i—建立視覺地圖和柵格地圖時(shí)在第i時(shí)刻的位姿。

為了保存和加載地圖，設(shè)計(jì)了地圖保存和加載模塊，建圖完成后，使用地圖保存模塊將新建的復(fù)合地圖以二進(jìn)制形式保存為文件，當(dāng)重新啟動(dòng)系統(tǒng)時(shí)，地圖加載模塊將二進(jìn)制文件恢復(fù)為復(fù)合地圖。

5 模式轉(zhuǎn)換策略

前面部分描述了6-DoF向3-DoF簡化以及兩種地圖的對(duì)齊過程。這一部分將說明機(jī)器人在導(dǎo)航過程中定位系統(tǒng)如何進(jìn)行模式切換。在導(dǎo)航運(yùn)動(dòng)過程中，視覺會(huì)在已有的地圖中繼續(xù)補(bǔ)充周圍地圖的特征，使地圖更豐富，同時(shí)更多的是檢測閉環(huán)準(zhǔn)備隨時(shí)進(jìn)行模式切換，一旦系統(tǒng)觸發(fā)轉(zhuǎn)換條件切換到重定位模式，將使用視覺做重定位，并將定位結(jié)果作為初始位姿傳遞給激光定位算法。提出了兩種有效的模式轉(zhuǎn)換觸發(fā)條件，分別是里程計(jì)和激光定位結(jié)果的偏差以及激光定位結(jié)果的協(xié)方差。里程計(jì)估計(jì)的位姿雖然不夠精確但在短時(shí)間內(nèi)不會(huì)漂移太遠(yuǎn)，而激光定位算法雖然定位結(jié)果精確但若是激光數(shù)據(jù)和全局地圖發(fā)生誤匹配時(shí)將會(huì)出現(xiàn)嚴(yán)重的偏移，因此將兩者對(duì)比結(jié)果作為切換到視覺重定位的觸發(fā)條件，同時(shí)也根據(jù)激光定位算法輸出的協(xié)方差矩陣判斷當(dāng)前系統(tǒng)的不確定度作為另一個(gè)觸發(fā)條件，具體過程，如圖5所示。

圖5 定位模式轉(zhuǎn)換策略Fig.5 Localization Mode Conversion Strategy

（1）定位系統(tǒng)計(jì)算里程計(jì)位姿數(shù)據(jù)和激光定位算法數(shù)據(jù)；（2）計(jì)算步驟1 中兩個(gè)數(shù)據(jù)的偏差，以及激光定位算法輸出的協(xié)方差；（3）判斷偏差或協(xié)方差是否有一個(gè)大于對(duì)應(yīng)的閾值Td，Tc，若有則執(zhí)行步驟（4），否則返回步驟（1）并將定位結(jié)果傳遞給導(dǎo)航模塊，導(dǎo)航模塊規(guī)劃路徑移向目標(biāo)點(diǎn)；（4）當(dāng)前定位丟失，利用視覺信息做重定位，并將計(jì)算結(jié)果傳遞給激光定位算法作為初始值。

6 實(shí)驗(yàn)對(duì)比

6.1 搭建實(shí)驗(yàn)平臺(tái)

共使用了仿真實(shí)驗(yàn)平臺(tái)和實(shí)體機(jī)器人平臺(tái)，仿真平臺(tái)使用Gazebo仿真軟件搭建，大小為（9×8）m，如圖6中左圖所示，自主設(shè)計(jì)的實(shí)體機(jī)器人平臺(tái)如圖6中右圖所示。實(shí)驗(yàn)中使用的激光雷達(dá)為Rplidar A2，深度相機(jī)為Realsense D435i，使用激光SLAM開源方案Cartographer 以及經(jīng)過簡化的視覺ORB-SLAM2算法，地圖的建立和結(jié)合過程以及導(dǎo)航運(yùn)動(dòng)控制是運(yùn)行在配置為Inter?CoreTM i5-4210M CPU@2.60GHz×4 的硬件平臺(tái)上。軟件系統(tǒng)是運(yùn)行在ROS（The Robot Operating System）機(jī)器人操作系統(tǒng)中。

圖6 仿真實(shí)驗(yàn)環(huán)境與實(shí)體機(jī)器人平臺(tái)Fig.6 Simulation Experiment Environment and Robot Platform

6.2 圖優(yōu)化定位與AMCL定位效果對(duì)比

AMCL（Adaptive Monte Carlo Localization）是蒙特卡洛定位方法的一種改進(jìn)版，該方法利用粒子濾波對(duì)機(jī)器人在建立好的二維地圖中進(jìn)行定位，被應(yīng)用在不同的自主移動(dòng)機(jī)器人定位場景中，但是這種方法定位精度較低并且在大場景中需要撒大量粒子而導(dǎo)致計(jì)算量過大。因此采用圖優(yōu)化的方法對(duì)機(jī)器人進(jìn)行定位。我們對(duì)Cartographer進(jìn)行了改進(jìn)，使其能夠在已建立的地圖中做純定位的功能，并通過實(shí)驗(yàn)對(duì)比了兩種定位算法的效果。在Gazebo仿真軟件中進(jìn)行仿真實(shí)驗(yàn)，能夠得到移動(dòng)機(jī)器人實(shí)時(shí)位置，將這些實(shí)際位置記錄為Groundtruth，同時(shí)訂閱基于AMCL定位方法的定位結(jié)果和基于圖優(yōu)化定位的結(jié)果，將三者保存下來并按式（12）計(jì)算兩種定位算法和實(shí)際軌跡之間的均方根誤差，如表2所示。從計(jì)算結(jié)果可以看出，使用圖優(yōu)化定位方法定位精度更高，均方根誤差降低了70%。將兩者的軌跡繪制成圖，如圖7所示。從圖中可以看出基于圖優(yōu)化定位結(jié)果和機(jī)器人實(shí)際的運(yùn)動(dòng)軌跡更加接近。

表2 兩種定位方法定位均方差對(duì)比Tab.2 Comparison of Localization Mean Square Error of Two Localization Methods

圖7 定位精度對(duì)比Fig.7 Comparison of Localization Accuracy

式中：RMSE—軌跡tra1與軌跡tra2的均方根誤差。

6.3 修正錯(cuò)誤定位

為進(jìn)行測試，建立了復(fù)合地圖，如圖8所示。使用激光雷達(dá)定位時(shí)在具有幾何相似的區(qū)域?qū)?dǎo)致錯(cuò)誤的匹配結(jié)果，如圖中A區(qū)域和C區(qū)域周圍環(huán)境結(jié)構(gòu)相似，當(dāng)機(jī)器人在A區(qū)域僅使用激光雷達(dá)定位時(shí)會(huì)大概率定位到C區(qū)域。但兩處環(huán)境紋理不同，通過視覺傳感器的加入，地圖中的信息更加豐富，這個(gè)問題將被很容易的修正，所設(shè)計(jì)的定位系統(tǒng)提高了機(jī)器人的環(huán)境適應(yīng)性，使機(jī)器人在結(jié)構(gòu)化場景中也有出色的定位效果。

圖8 最終的復(fù)合地圖Fig.8 The Final Composite Map

6.4 不同算法重定位時(shí)間對(duì)比

為了驗(yàn)證所提出定位系統(tǒng)在定位時(shí)的有效性，設(shè)計(jì)實(shí)驗(yàn)對(duì)比使用單一傳感器定位方法的定位效果，成功定位與否通過檢測機(jī)器人定位算法輸出的位置probot和機(jī)器人在實(shí)際環(huán)境中的位置pgoal之間的差異是否小于給定的閾值Tl來確定。將機(jī)器人放在圖8中的A-G處做了若干組實(shí)驗(yàn)，計(jì)算定位時(shí)間的平均值，如表3所示。*表示在該位置定位失敗。

表3 同一地點(diǎn)不同定位方法對(duì)比Tab.3 Comparison of Different Localization Methods in the Same Place

從表3數(shù)據(jù)對(duì)比可見，文中所提方法在B和D-G中的定位時(shí)間遠(yuǎn)遠(yuǎn)小于其他兩種算法。在位置A、B、D 和E 中，環(huán)境紋理豐富，能夠提取到大量可區(qū)分性特征點(diǎn)，能夠快速定位，平均2.5s即可成功定位；在位置C 和F 中，走位環(huán)境白墻較多，提取特征困難，機(jī)器人需要隨機(jī)移動(dòng)調(diào)整，平均8.4s成功定位。A 和C兩個(gè)位置周圍環(huán)境結(jié)構(gòu)簡單且相似，激光雷達(dá)定位到錯(cuò)誤的位置，定位失效，但使用所提出的方法仍能夠有較好的定位效果，這是由于兩地環(huán)境雖然結(jié)構(gòu)相似，但是存在有利于視覺定位的紋理信息。整體上，所設(shè)計(jì)的具有模式轉(zhuǎn)換的定位系統(tǒng)在導(dǎo)航丟失重定位時(shí)比單一激光定位算法定位速度提高57.8%，正常導(dǎo)航時(shí)比AMCL方法定位速度提高24%，可以說明所設(shè)計(jì)定位系統(tǒng)的有效性和實(shí)用性。

7 結(jié)論

所提出的結(jié)合視覺和激光雷達(dá)傳感器的定位系統(tǒng)，能夠自動(dòng)根據(jù)當(dāng)前定位狀態(tài)進(jìn)行模式轉(zhuǎn)換，以解決機(jī)器人任意位置啟動(dòng)和導(dǎo)航位置丟失問題。利用簡化的ORB-SLAM2和Cartographer算法進(jìn)行了實(shí)驗(yàn)，通過實(shí)驗(yàn)表明該系統(tǒng)在局部和全局定位效果中，精度和效率都有很大提升。通過引入視覺傳感器也彌補(bǔ)了單一激光雷達(dá)的固有缺陷，所設(shè)計(jì)的系統(tǒng)能夠?yàn)闄C(jī)器人提供穩(wěn)定且精確的定位。后續(xù)工作會(huì)將視覺和激光傳感器在數(shù)據(jù)層面進(jìn)一步緊耦合，充分發(fā)揮兩種傳感器的優(yōu)勢，提高動(dòng)態(tài)環(huán)境下的定位導(dǎo)航能力。