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

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

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

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

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

1.1 Cartographer SLAM的原理

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

(1)

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

(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)化

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

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

(7)

q=(v,q0)，

(8)

(9)

(10)

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

圖1 未融合的三維效果圖

圖2 優(yōu)化的二維激光雷達3D點云數據

2 SLAM建圖仿真實驗

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

圖3 二維和三維地圖比較

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

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

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

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

圖4 機器人室內上下坡的仿真結果

圖5 算法流程框圖

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

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

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

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

圖7 變電站復雜環(huán)境地圖的點云數據

4 結論

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