基于激光雷達巡檢機器人的變電站三維地圖構(gòu)建*

魯錦濤，王 慶，董元帥，張慶偉

(1.東南大學(xué) 儀器科學(xué)與工程學(xué)院，江蘇 南京 210096；2.南瑞集團(國網(wǎng)電力科學(xué)研究院)有限公司，江蘇 南京 211106；3.智能電網(wǎng)保護和運行控制國家重點實驗室，江蘇 南京 211106)

0 引 言

變電站的巡檢工作主要是日常對變電站設(shè)備、儀表進行巡查[1,2]，保證其正常的工作。為了實現(xiàn)變電站的自動巡檢，巡檢機器人[3]得到大力發(fā)展。文獻[4]對多種變電站巡檢機器人進行了介紹，其可使用多種傳感器實現(xiàn)變電站的巡檢工作。

國內(nèi)用于巡檢的機器人所用傳感器一般有差分全球定位系統(tǒng)(global positioning system，GPS)[5,6]，激光雷達(LiDAR)[7]，視覺傳感器等。其中基于GPS或者北斗等衛(wèi)星信號的定位方式容易受變電站環(huán)境的干擾，魯棒性差，一般的二維激光所建的柵格地圖在面對復(fù)雜且相似的環(huán)境時易出現(xiàn)錯定位，基于視覺傳感器的機器人易受光線的影響，夜間無法進行正常的巡檢工作?；谌S(3D)激光的三維地圖構(gòu)建可為機器人提供更準(zhǔn)確地圖，提高變電站巡檢機器人的穩(wěn)定性，且可供三維可視化，具有很高應(yīng)用前景。

本文主要介紹基于三維激光的巡檢機器人系統(tǒng)[8]，并且將三維激光的點云信息用于三維地圖構(gòu)建，使用改進的點云匹配算法解決相似環(huán)境誤匹配的問題，提高地圖的可靠性以及可視性。

1 場地與機器人系統(tǒng)設(shè)計

1.1 變電站場地

變電站是在電力系統(tǒng)中對電壓和電流進行交換以及電能分配的場所，場地中由很多支架柱構(gòu)成，規(guī)模大同小異，多為圓柱形外觀，導(dǎo)致變電站的場景多處極其相似，對建圖造成較大困難，場地實景如圖1所示。

圖1 變電站場地實景

1.2 機器人硬件系統(tǒng)

變電站巡檢機器人硬件組成部分包括傳感器模塊、主機、機動單元以及電源部分。傳感器部分包括三維激光雷達、慣性測量單元(inertial measurement unit,IMU)、可見光相機等度量器件，主機選用工控機用于對傳感器的數(shù)據(jù)進行處理，機器人采用輪式行進方式，保證數(shù)據(jù)采集的平穩(wěn)性。本次實驗所用硬件模塊具體框圖如圖2所示。

圖2 機器人硬件系統(tǒng)

1.3 三維點云顯示

此系統(tǒng)是基于機器人操作系統(tǒng)(robot operating system,ROS)的開源操作系統(tǒng)，采用ROS的Topic，以節(jié)點式對數(shù)據(jù)進行獲取、處理以及顯示。

選用的三維激光雷達為16線，雷達掃描一圈稱為一幀。在算法中將獲取的一幀一幀的點云數(shù)據(jù)使用點云庫(point cloud library,PCL)轉(zhuǎn)換成.pcl格式的點云數(shù)據(jù)。通過迭代最近點(iterative closest point，ICP)算法進行點云匹配以及位姿估計，將每一幀新的點云數(shù)據(jù)與上一幀進行聯(lián)結(jié)，不斷更新地圖，實現(xiàn)變電站的三維地圖構(gòu)建。

2 基于ICP的點云算法

基于三維物體的配準(zhǔn)問題，主要是根據(jù)兩組在不同三維坐標(biāo)系中的三維點云數(shù)據(jù)的匹配關(guān)系，求解出兩組點云數(shù)據(jù)的空間變換關(guān)系。這其中ICP算法[9,12]是在3D點對的位姿估計中使用最廣泛、最成熟的一種算法。經(jīng)典ICP算法中使用的點與點的對應(yīng)關(guān)系為

d=min‖B-(R·A+t)2‖

(1)

式中R和t分別為點云集A到點云集B的旋轉(zhuǎn)矩陣和平移向量，構(gòu)建的目標(biāo)函數(shù)為兩組點云集之間的距離。算法會對每一幀點云集中的點進行遍歷處理，通過尋找其最鄰近點，然后構(gòu)造相應(yīng)的目標(biāo)函數(shù)，進而求解出兩幀之間的位姿R和t。算法流程圖如圖3所示。

圖3 基本ICP算法流程圖

由于變電站場地中圓形立柱過多，導(dǎo)致場景中多處相似度過高，僅依靠搜尋最鄰近點的方式進行點云匹配，極易造成誤匹配，最終導(dǎo)致位姿求解出錯，地圖嚴(yán)重失真。如圖4所示，在機器人剛進入場地時，局部場地可以準(zhǔn)確重現(xiàn)，當(dāng)機器人行進至場地中部時，無法在相似場景中進行區(qū)分，地圖逐漸變形，圓形立柱被拉長，機器人的行進路徑跟蹤出錯，地標(biāo)發(fā)生嚴(yán)重偏移。

圖4 失真地圖

3 基于特征的改進算法

3.1 點云特征提取

傳統(tǒng)ICP算法在復(fù)雜場景中已經(jīng)無法解決相似度高的問題，本文對雷達點云數(shù)據(jù)進行特征提取[10，11]，以進行更魯棒的特征匹配。遍歷點云集，對所有的點分別求出其與前后5個點的偏差，以x坐標(biāo)為例

(2)

從式子中可以看出，由于無法提供足夠的數(shù)據(jù)，計算時會自動拋棄每一幀點云的前后5個點，同時dy和dz采用相同的式子進行計算求值，最終由式(3)計算出每個點的曲率值，作為對三維點進行特征區(qū)分的依據(jù)

Curvature=(dx)2+(dy)2+(dz)2

(3)

根據(jù)所求得的曲率值，將點云集中的點分為角點和平面點，評判根據(jù)實驗調(diào)節(jié)具體參數(shù)，如式(4)所示

(4)

通過建立上面的約束，對要處理的點云進行特征分類，如圖5所示。

圖5 特征點提取

根據(jù)曲率可將角點和平面點篩選出來，便于后續(xù)算法基于特征進行點云匹配，減少誤匹配的出現(xiàn)。

3.2 改進點云匹配算法

與傳統(tǒng)ICP算法不同，本文采用的改進算法不再只依賴尋找最鄰近點構(gòu)造三維點對關(guān)系。根據(jù)提取的角點和平面點，重新構(gòu)造兩類對應(yīng)關(guān)系：1)在前一幀中找兩個點，使之構(gòu)成的直線距當(dāng)前幀中的所求角點距離最近，從而構(gòu)成配對關(guān)系；2)在前一幀中找三個點，使之構(gòu)成的平面距當(dāng)前幀中的所求平面點距離最近，從而構(gòu)成配對關(guān)系，如圖6所示。

圖6 對應(yīng)關(guān)系示意

在圖6(a)中為點到線的距離，通過構(gòu)造三角形，可由三角形的面積公式得到點m到np的距離為

(5)

式中C為邊c所對角的角度大小，從該式可構(gòu)造第一種目標(biāo)函數(shù)

(6)

式中k-1為該點為第k幀的前一幀中的點。同理，圖6(b)中為當(dāng)前幀中的平面點到所匹配平面的距離的示意圖，根據(jù)所畫圖示，易寫出求解d的表達式

(7)

由式(7)可構(gòu)造出算法所需的第二種目標(biāo)函數(shù)式(8)

(8)

根據(jù)式(6)和式(8)可建立目標(biāo)函數(shù)，用Gauss-Newton非線性優(yōu)化迭代出最優(yōu)的位姿估計，用于糾正獲取的點云數(shù)據(jù)，并累計姿態(tài)變化，將新的點云數(shù)據(jù)加入到地圖中，構(gòu)建變電站的三維地圖。

4 實驗結(jié)果

根據(jù)改進點云匹配算法，對變電站的點云進行特征提取以及采用新的匹配方式，重新對變電站的三維結(jié)構(gòu)進行建圖，實驗結(jié)果如圖7所示。

圖7 變電站三維地圖

從圖7的全局地圖中可以看出，基于特征提取的改進點云匹配算法能夠有效解決誤匹配的情況發(fā)生，在機器人進入變電站中部以及在進行轉(zhuǎn)彎等易造成較大誤差的地方，依然能夠進行正確的點云匹配，構(gòu)建完整的變電站的三維地圖。圖7右上角是局部放大圖，為變電站局部支架柱的俯視圖，從圖中可見，該地標(biāo)物不僅沒有出現(xiàn)偏移，而且點云基本還原其圓形特征。

5 結(jié) 論

本文中基于三維激光的變電站機器人系統(tǒng)，通過改進的點云匹配算法，對點云數(shù)據(jù)進行特征提取從而進行更準(zhǔn)確的點云匹配，提出點到線以及點到面的約束方程求解位姿，有效解決相似場景中誤匹配的問題，完整地重現(xiàn)變電站的三維特征，建立的三維地圖可用于機器人的自主巡航以及可視化顯示，有極大的應(yīng)用前景。