一種優(yōu)化的SLAM變電站復(fù)雜環(huán)境地圖構(gòu)建

(1.山東科技大學(xué)電氣與自動(dòng)化工程學(xué)院，山東 青島 266590；2.山東科技大學(xué)電氣信息系，山東 濟(jì)南 250031;3.中國(guó)移動(dòng)通信集團(tuán)江蘇有限公司淮安分公司，江蘇 淮安 223021)

近年來(lái)隨著電網(wǎng)的快速發(fā)展，變電站數(shù)量激增和變電站維護(hù)人員數(shù)量不足的矛盾日益凸顯，同時(shí)變電站工作是一項(xiàng)難度較大且危險(xiǎn)的工作，因此無(wú)人巡檢技術(shù)和設(shè)備研發(fā)十分必要。目前無(wú)人巡檢大多采用循跡或者激光即時(shí)定位與地圖構(gòu)建(simultaneous localization and mapping,SLAM)技術(shù)，循跡主要是利用計(jì)算機(jī)視覺(jué)技術(shù)，依靠路徑上的條帶標(biāo)記引導(dǎo)無(wú)人車進(jìn)行自主行駛，循跡的導(dǎo)航方式雖然可靠性、抗干擾性比較好，但是需要預(yù)先鋪設(shè)磁條，施工量大，成本較高，而且無(wú)法應(yīng)對(duì)突發(fā)事件進(jìn)行應(yīng)急處理，無(wú)法隨時(shí)更改路線。激光SLAM技術(shù)主要是利用激光器發(fā)射激光束通過(guò)三角測(cè)距原理或飛行時(shí)間測(cè)距法(time of flight，TOF)來(lái)測(cè)量周圍物體與自身的距離，從而獲得高精度的距離信息，結(jié)合SLAM技術(shù)實(shí)現(xiàn)同時(shí)定位與建圖。目前激光SLAM主要應(yīng)用在移動(dòng)機(jī)器人定位和導(dǎo)航、無(wú)人駕駛和測(cè)繪等領(lǐng)域[1]。國(guó)外的TU Munich公司的LSD-SLAM和Zaragoza大學(xué)的ORB-SLAM功能都十分完善和穩(wěn)定，國(guó)內(nèi)SLAM起步較晚，目前有思嵐科技、百度等多個(gè)公司研發(fā)。

現(xiàn)有的激光SLAM主要有基于濾波和基于圖優(yōu)化兩種技術(shù)。基于卡爾曼濾波的SLAM主要有EKF擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)、UKF無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)等。擴(kuò)展卡爾曼濾波算法結(jié)構(gòu)簡(jiǎn)單，具有一定的精度，因此得到了較好的應(yīng)用，但也存在一些不足:例如受環(huán)境、設(shè)備等因素影響，測(cè)量誤差較大,對(duì)于有突變的信號(hào)數(shù)據(jù)，收斂速度較慢[2-3]。其代表技術(shù)是Hector SLAM，使用一個(gè)更新頻率高、測(cè)量噪聲小的激光雷達(dá)，優(yōu)點(diǎn)是不需要里程計(jì)的信息就可以完成實(shí)時(shí)地圖創(chuàng)建和定位[4]，但是對(duì)于雷達(dá)的幀率要求很高。基于粒子濾波(particle filter，PF)的SLAM能夠比較精確地表達(dá)基于觀測(cè)量和控制量的后驗(yàn)概率分布，可以用于解決SLAM問(wèn)題。其中目前應(yīng)同最廣泛的是Gmapping, Gmapping是一種2D SLAM建圖方法，主要基于粒子濾波算法，在長(zhǎng)廊和特定的場(chǎng)合中應(yīng)用效果較好，缺點(diǎn)是嚴(yán)重依賴?yán)锍逃?jì)，沒(méi)有回環(huán)檢測(cè)[5]。這些方案中大多只能利用2D或者3D激光雷達(dá)，實(shí)現(xiàn)對(duì)應(yīng)的2D、3D地圖構(gòu)建，各自優(yōu)缺點(diǎn)無(wú)法互相補(bǔ)足[6]。

基于圖優(yōu)化框架SLAM代表技術(shù)是Google開(kāi)發(fā)的實(shí)時(shí)室內(nèi)項(xiàng)目Cartographer，采用基于Ceres非線性優(yōu)化的方法,其基本思想是將機(jī)器人不同時(shí)刻的位姿抽象為點(diǎn)，機(jī)器人在不同位置上觀測(cè)所產(chǎn)生的約束被抽象為點(diǎn)之間的邊[7]。采用了主流的SLAM框架，即特征提取、閉環(huán)檢測(cè)、后端優(yōu)化，由一定數(shù)量的LaserScan組成一個(gè)Submap子圖，一系列的Submap子圖構(gòu)成全局地圖，Cartographer SLAM最大優(yōu)點(diǎn)是具有回環(huán)檢測(cè)功能，可以同時(shí)滿足二維和三維的建圖需要[8]。通過(guò)一系列激光SLAM的對(duì)比，本研究選擇Cartographer SLAM方案，在Ubuntu環(huán)境下，基于ROS進(jìn)行仿真建圖，采用雙2D激光雷達(dá)互相垂直的方式實(shí)現(xiàn)三維建圖優(yōu)化，利用水平激光雷達(dá)采集數(shù)據(jù)，垂直激光雷達(dá)對(duì)水平數(shù)據(jù)進(jìn)行彌補(bǔ)，得到了良好的三維數(shù)據(jù)結(jié)果。

1 Cartographer的原理及其優(yōu)化

1.1 Cartographer SLAM的原理

SLAM也稱為并發(fā)建圖與定位[9](concurrent mapping and localization，CML)，即時(shí)定位與地圖構(gòu)建。利用局部連續(xù)掃面數(shù)據(jù)建立子圖，并不斷地與全局進(jìn)行掃面匹配。Cartographer的建圖理論本質(zhì)是基于圖優(yōu)化，將機(jī)器人在不同時(shí)刻的位姿抽象化成一個(gè)點(diǎn)，再將機(jī)器人在不同點(diǎn)觀測(cè)到的約束抽象成點(diǎn)與點(diǎn)之間的邊。隨著機(jī)器人的運(yùn)動(dòng)，這些點(diǎn)和邊就形成地圖，這個(gè)過(guò)程會(huì)隨著時(shí)間的累積產(chǎn)生誤差，后面需要再進(jìn)行全局優(yōu)化[10-11]。從二維建圖的角度，由每一個(gè)掃描到的Laser scan數(shù)據(jù)得到一個(gè)位姿關(guān)系ε=(εx,εy,εθ)，對(duì)于子圖的構(gòu)造，其實(shí)是反復(fù)掃描和子圖坐標(biāo)框架迭代的過(guò)程。隨著時(shí)間推進(jìn)，位姿關(guān)系會(huì)從激光掃描框架轉(zhuǎn)化成子圖框架Tε，Tε和ε的關(guān)系如下：

(1)

在子圖的構(gòu)建過(guò)程中引入給定分辨率的離散網(wǎng)格概念，這里對(duì)于所有網(wǎng)格的映射值會(huì)有被阻擋的概率。假設(shè)是一個(gè)r×r的網(wǎng)格，當(dāng)前進(jìn)行掃描時(shí)，對(duì)于新觀察到的命中點(diǎn)將賦予新的概率p。隨著迭代過(guò)程的進(jìn)行，誤差會(huì)不斷地累積。利用求解非線性最小二乘法(公式(2))，通過(guò)閉環(huán)的方式消除誤差，可以很容易地添加殘差E來(lái)考慮額外的數(shù)據(jù)，殘差的計(jì)算可以通過(guò)公式(3)和公式(4)得到，尋找每個(gè)位姿的最大可能性，并通過(guò)雙三插值法將整個(gè)子圖進(jìn)行平滑解算。

(2)

(3)

(4)

f(i+u,j+v)=(1-u)(1-v)f(i,j)+u(1-v)f(i+1,j)+(1-u)vf(i,j+1)+uvf(i+1,j+1),

(5)

(6)

1.2 SLAM建圖方案的優(yōu)化

在實(shí)際實(shí)驗(yàn)中，數(shù)據(jù)的精確性和完整性是最關(guān)鍵的，其次是通過(guò)方案優(yōu)化來(lái)降低成本，利用二維激光雷達(dá)滿足實(shí)驗(yàn)所需的三維建圖要求。但是傳統(tǒng)的單個(gè)2D采用舵機(jī)控制俯仰角，雖然能在一定程度上滿足三維建圖要求，但是實(shí)時(shí)性和完整性大大降低。因此在實(shí)驗(yàn)過(guò)程中，針對(duì)上述問(wèn)題，設(shè)計(jì)并實(shí)現(xiàn)了采用兩臺(tái)互相垂直的二維激光雷達(dá)實(shí)現(xiàn)三維建圖，其中水平激光雷達(dá)采集數(shù)據(jù)，垂直激光雷達(dá)對(duì)水平數(shù)據(jù)進(jìn)行彌補(bǔ)。

Cartographer的算法中三維建圖的要求是點(diǎn)云數(shù)據(jù)源，然而二維激光雷達(dá)本身發(fā)出的是Scan數(shù)據(jù)，因此首先要解決的是數(shù)據(jù)源的轉(zhuǎn)化問(wèn)題。這里利用Transform轉(zhuǎn)換的方法，利用四元數(shù)旋轉(zhuǎn)矩陣解決剛體三維向量的坐標(biāo)轉(zhuǎn)換時(shí)所帶來(lái)的冗余性和奇異性，假設(shè)四元數(shù)q=q0+q1i+q2j+q3k,四個(gè)參數(shù)分別代表三維坐標(biāo)和旋轉(zhuǎn)角度，則對(duì)應(yīng)的旋轉(zhuǎn)矩陣為公式(7)。其中i,j,k為四元數(shù)的虛數(shù)部分，如果簡(jiǎn)化虛數(shù)部分為向量v，則四元數(shù)可以簡(jiǎn)化為公式(8)，為了解決相對(duì)坐標(biāo)的旋轉(zhuǎn)問(wèn)題，假設(shè)坐標(biāo)為w=(wx,wy,wz)，以向量v旋轉(zhuǎn)θ角度下得到新的坐標(biāo)w，根據(jù)公式(7)的計(jì)算方式，則可以得到其解析式為公式(9)，那么在系統(tǒng)中的各個(gè)框架的相對(duì)旋轉(zhuǎn)便可以歐拉角的方式利用公式(10)獲得。發(fā)送變化信息，將每個(gè)單獨(dú)的激光線轉(zhuǎn)化成設(shè)定的框架，通過(guò)收集激光雷達(dá)的數(shù)據(jù)，能夠有效解決設(shè)備的移動(dòng)和出現(xiàn)俯仰角偏差時(shí)所帶來(lái)數(shù)據(jù)不匹配[14-15]。

(7)

q=(v,q0)，

(8)

(9)

(10)

將Laser Geometry數(shù)據(jù)包和統(tǒng)一的Sensor Msg數(shù)據(jù)格式通過(guò)算法將二維激光的Laserscan數(shù)據(jù)轉(zhuǎn)化成Pointcloud2數(shù)據(jù)，使得激光雷達(dá)在驅(qū)動(dòng)節(jié)點(diǎn)啟動(dòng)時(shí)，自動(dòng)完成數(shù)據(jù)類型的轉(zhuǎn)化，滿足3D建圖的需要。然而兩個(gè)二維激光雷達(dá)的初步建圖效果并不是十分理想，如圖1所示，對(duì)比系統(tǒng)的轉(zhuǎn)換關(guān)系時(shí)，發(fā)現(xiàn)垂直傳感器和水平傳感器的坐標(biāo)系并不能在數(shù)據(jù)上進(jìn)行融合，存在著完全分裂的現(xiàn)象。因此，為了得到更加實(shí)際可靠的點(diǎn)云數(shù)據(jù)和地圖數(shù)據(jù)，通過(guò)對(duì)傳感器進(jìn)行精確的標(biāo)定，確定各個(gè)傳感器和基坐標(biāo)的父子關(guān)系，并通過(guò)旋轉(zhuǎn)垂直傳感器的坐標(biāo)來(lái)配合水平坐標(biāo)系，實(shí)現(xiàn)對(duì)水平坐標(biāo)系的數(shù)據(jù)彌補(bǔ)。并通過(guò)QT Reader對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行了優(yōu)化，得到更加合理和符合實(shí)際的3D點(diǎn)云數(shù)據(jù)圖，如圖2所示。

圖1 未融合的三維效果圖

圖2 優(yōu)化的二維激光雷達(dá)3D點(diǎn)云數(shù)據(jù)

2 SLAM建圖仿真實(shí)驗(yàn)

仿真主要利用在Ubuntu 16.04的環(huán)境下的ROSKinetic和Rviz。實(shí)際的建圖過(guò)程中，選用了北陽(yáng)公司的二維激光雷達(dá)Hokuyo-30lx-ew和Velodyne的Vlp-16的三維激光雷達(dá)，二維激光雷達(dá)Hokuyo-30lx-ew具有高精度、高分辨率及寬視場(chǎng)設(shè)計(jì)的特點(diǎn)給自主導(dǎo)航機(jī)器人提供了良好的環(huán)境識(shí)別能力，且重量低、功耗低。Vlp-16的三維激光雷達(dá)也同樣具有高幀率、高精度的優(yōu)點(diǎn)[16]。

圖3 二維和三維地圖比較

在圖3(b)中可以發(fā)現(xiàn)三維激光雷達(dá)的建圖明顯要優(yōu)于二維激光雷達(dá)，不僅參考數(shù)據(jù)更真實(shí)，相對(duì)于二維激光雷達(dá)建圖(圖3(a))，在面對(duì)復(fù)雜地形和具有垂直高度的直立機(jī)器人尤其適用，在三維空間里能夠有效規(guī)避障礙物的同時(shí)進(jìn)行合理的路徑規(guī)劃，而且三維地圖顯然更加具有實(shí)際意義。

由圖4可以看出，在機(jī)器人上下坡時(shí)可以發(fā)現(xiàn)，在有室內(nèi)或者有多個(gè)高度較高的障礙物時(shí)，無(wú)論是障礙物的高度、方位，還是障礙物的大小，都可以利用柵格地圖進(jìn)行判斷。但是從數(shù)據(jù)的完整性上，單個(gè)三維激光雷達(dá)在近距離上出現(xiàn)了一定數(shù)據(jù)丟失，而且沒(méi)有頂層數(shù)據(jù)。在復(fù)雜的環(huán)境里，諸如室內(nèi)、變電站等，缺少了大量可以參考的數(shù)據(jù)，因而數(shù)據(jù)是不完整的。

另外，三維激光雷達(dá)的造價(jià)昂貴，制作工藝復(fù)雜，體積大，功耗也大，在圖像采集時(shí)工作頻率受到限制，掃描速度慢，難以用來(lái)處理變電站工作的無(wú)人巡檢導(dǎo)航的任務(wù)，因此提出了一種通過(guò)雙2D激光雷達(dá)實(shí)現(xiàn)三維建圖的方式解決上述缺陷。

3 基于ROS的建圖系統(tǒng)的原型機(jī)搭建

圖4 機(jī)器人室內(nèi)上下坡的仿真結(jié)果

圖5 算法流程框圖

機(jī)器人操作系統(tǒng)(robot operating system,ROS)是一套開(kāi)源操作系統(tǒng)，可用于機(jī)器人編程，把原來(lái)分散部分耦合在一起，并且為這些部分提供通信架構(gòu)。ROS提供了一系列程序庫(kù)和工具，其功能包含硬件抽象描述、底層驅(qū)動(dòng)程序管理、共用功能的執(zhí)行、程序間的消息傳遞、程序發(fā)行包管理等[17-18]。ROS采用分布式框架，通過(guò)點(diǎn)對(duì)點(diǎn)設(shè)計(jì)讓機(jī)器人的進(jìn)程可以分別運(yùn)行，便于模塊化的修改和定制，提高了系統(tǒng)的容錯(cuò)能力，可以有效地對(duì)機(jī)器人、無(wú)人機(jī)、無(wú)人駕駛等應(yīng)用進(jìn)行仿真和設(shè)計(jì)。對(duì)于不同的傳感器采用統(tǒng)一的消息傳遞模式和格式，非常適合算法的開(kāi)發(fā)[19-20]。

圖5為系統(tǒng)的建圖算法框圖，算法采用了雙循環(huán)的并行結(jié)構(gòu)，分別讀取數(shù)據(jù)節(jié)點(diǎn)和地圖算法節(jié)點(diǎn)。啟動(dòng)的角度互相獨(dú)立，條件判斷數(shù)據(jù)互相依賴，算法結(jié)構(gòu)在開(kāi)發(fā)和修改上更加友好，只要格式統(tǒng)一便可以將服務(wù)于相同主題的所有節(jié)點(diǎn)同時(shí)調(diào)用。當(dāng)某一節(jié)點(diǎn)出現(xiàn)問(wèn)題時(shí)可以通過(guò)邏輯判斷快速地查找到出錯(cuò)的語(yǔ)句，最終生成的地圖數(shù)據(jù)在算法端是完整的，可以保障地圖的正確性。在算法結(jié)構(gòu)的基礎(chǔ)上，搭建了復(fù)雜環(huán)境地圖構(gòu)建的原型系統(tǒng)如圖6所示。

從圖7中可以看出，利用原型系統(tǒng)得到的優(yōu)化后變電站點(diǎn)云數(shù)據(jù)相比較二維SLAM地圖更加全面，優(yōu)化后的點(diǎn)云數(shù)據(jù)補(bǔ)充了頂層數(shù)據(jù)和機(jī)器人上方障礙物數(shù)據(jù)，復(fù)雜環(huán)境下的點(diǎn)云數(shù)據(jù)補(bǔ)充了更多障礙物的尺寸和細(xì)節(jié)。對(duì)比前文圖3(b)的俯視圖可以清楚地發(fā)現(xiàn)，通過(guò)二維激光雷達(dá)進(jìn)行三維建圖的方案是可行的，并且補(bǔ)充了地圖的數(shù)據(jù)和細(xì)節(jié)，例如路牙邊線、樹(shù)木伸出的枝丫等障礙物。從經(jīng)濟(jì)的角度看，優(yōu)化后的方案也更加節(jié)約成本。從系統(tǒng)的冗余和負(fù)荷角度來(lái)說(shuō)，優(yōu)化后的方案降低了系統(tǒng)的冗余，兩臺(tái)二維激光雷達(dá)的負(fù)荷也小于一臺(tái)三維激光雷達(dá)。綜上，本系統(tǒng)的優(yōu)化方案具有可實(shí)施性。

圖6 復(fù)雜環(huán)境地圖構(gòu)建原型系統(tǒng)

圖7 變電站復(fù)雜環(huán)境地圖的點(diǎn)云數(shù)據(jù)

4 結(jié)論

對(duì)幾種主流激光SLAM算法進(jìn)行對(duì)比和分析，選用效果理想的Cartographer算法進(jìn)行激光雷達(dá)的建圖，基于TX2建立了一套完整的通訊結(jié)構(gòu)，并搭建了實(shí)驗(yàn)原型機(jī)。利用初步搭建的原型系統(tǒng)對(duì)真實(shí)環(huán)境進(jìn)行了模擬實(shí)驗(yàn)和實(shí)際建圖實(shí)驗(yàn)，分別得到了二維和三維的建圖實(shí)驗(yàn)結(jié)果以及區(qū)域內(nèi)復(fù)雜環(huán)境下的實(shí)際三維建圖的點(diǎn)云數(shù)據(jù)，為變電站巡檢機(jī)器人系統(tǒng)降低了成本并提高了建圖精度。系統(tǒng)實(shí)現(xiàn)了二維激光雷達(dá)的三維建圖，降低了系統(tǒng)的整體成本，提高了整體的精度，并展現(xiàn)了高度的可移植性和適應(yīng)性，后續(xù)將進(jìn)行底盤和導(dǎo)航功能優(yōu)化工作，繼續(xù)提升基于本系統(tǒng)的自主移動(dòng)機(jī)器人的實(shí)用性和功能性。