文剛 周仿榮 李濤 馬御棠 裴凌 劉亞東 錢國超 潘浩

摘要

為了能夠更加靈活地執(zhí)行變電站巡檢任務(wù)，非固定線路的機器人巡檢技術(shù)越來越受到關(guān)注.如何在復(fù)雜的變電站環(huán)境中實現(xiàn)高精度的定位是機器人在變電站執(zhí)行巡檢任務(wù)時需要解決的核心問題.單一傳感器難以滿足變電站可靠定位的要求，因此，本文設(shè)計了多傳感器融合的LINS-GNSS定位方法.其前端基于迭代誤差狀態(tài)卡爾曼濾波框架將激光雷達和慣性導(dǎo)航進行緊耦合，在每次迭代中生成新的特征對應(yīng)關(guān)系遞歸地校正估計狀態(tài).后端使用因子圖優(yōu)化的方法將衛(wèi)星導(dǎo)航的定位結(jié)果與LINS后端輸出的定位結(jié)果松耦合.優(yōu)化過程中先將局部坐標(biāo)系與全局坐標(biāo)系對齊，再將衛(wèi)星導(dǎo)航的位置約束作為先驗邊添加到后端的因子圖中，最后將定位結(jié)果在全局坐標(biāo)系下輸出.為了評估LINS-GNSS系統(tǒng)在變電站環(huán)境中的性能，本文在實際變電站中進行了測試.實驗結(jié)果表明，LINS-GNSS系統(tǒng)在變電站環(huán)境中可以達到優(yōu)于0.5 m的定位精度，且比現(xiàn)有最佳算法LIO-SAM定位精度更高.

關(guān)鍵詞多傳感器融合；因子圖優(yōu)化；卡爾曼濾波；衛(wèi)星導(dǎo)航；激光SLAM

中圖分類號

V249.3

文獻標(biāo)志碼

A

收稿日期

2022-01-05

資助項目

南方電網(wǎng)有限責(zé)任公司科技項目（YNKJXM20191246）；國家自然科學(xué)基金（61873163）；上海市科技創(chuàng)新行動計劃項目（20511103103）

作者簡介文剛，男，碩士，從事電網(wǎng)設(shè)備防災(zāi)減災(zāi)及無人機導(dǎo)航技術(shù)研究.1192381484@qq.com

0 引言

近年來，融合全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS） [1]和慣性導(dǎo)航系統(tǒng) （Inertial Navigation System，INS） [2]的組合定位系統(tǒng)已經(jīng)被廣泛地應(yīng)用在室外定位場景中.GNSS具有全局性，能夠在全球范圍內(nèi)提供導(dǎo)航定位服務(wù)，在室外場景的導(dǎo)航定位中發(fā)揮著十分重要的作用.慣性測量單元 （Inertial Measurement Unit，IMU） 是測量物體加速度和角速度的傳感器.由于IMU在導(dǎo)航推算時具有更新頻率高、受環(huán)境變化影響小的特點，所以常常被用來與各種傳感器進行融合.但是，在復(fù)雜的環(huán)境中，作為主要信息源的全球衛(wèi)星導(dǎo)航系統(tǒng)往往會由于遮擋或者電磁干擾而受到嚴(yán)重的多路徑和非視距 （Non-Light-Of-Sight，NLOS） 效應(yīng)[3]影響，導(dǎo)致定位精度降低.而IMU在進行慣導(dǎo)推算時，誤差會累積，最終導(dǎo)致定位結(jié)果發(fā)散.因此，如何實現(xiàn)變電站環(huán)境下的精確定位是一個重要且具有挑戰(zhàn)的問題.各種融合定位方法的出現(xiàn)為解決這個問題提供了理想的方案[4].GNSS可以修正INS誤差的積累，提高導(dǎo)航精度，而INS可以彌補GNSS的信號丟失或衰減導(dǎo)致的定位性能下降問題，提高導(dǎo)航的連續(xù)性.基于激光雷達的同步定位與建圖 （LiDAR Simultaneous Localization and Mapping，LiDAR SLAM） [5-6]算法受光照影響較小，因此在變電站環(huán)境中，3D LiDAR可以用于檢測由靜態(tài)環(huán)境以及動態(tài)物體引起的NLOS[7-8]，對GNSS定位起到輔助作用[9-10].在變電站環(huán)境下，一般巡檢機器人巡航時除了需要獲得定位結(jié)果外，還需要建立環(huán)境地圖以便之后導(dǎo)航使用[11].本文采用對衛(wèi)星、慣導(dǎo)、激光雷達進行組合的方法，對變電站中的機器人融合定位技術(shù)進行研究.整個融合定位算法LINS-GNSS的前端采用濾波框架，后端采用優(yōu)化框架.本文研究中的LINS （LiDAR-INS）[12] 是基于迭代誤差狀態(tài)卡爾曼濾波[13] （iterative Error-State Kalman Filter，iterative ESKF）的緊耦合LiDAR-IMU模型，但是LINS本身不具備與GNSS融合的能力，故在長期工作時會發(fā)散，且無法輸出全球定位坐標(biāo).此外， LIO-SAM [14] （Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping）是一種基于因子圖優(yōu)化的緊耦合LiDAR-IMU系統(tǒng)，并且該系統(tǒng)具備和GNSS進行松耦合的能力，但其耗費的資源比基于濾波的算法要多.本文所提算法既能保

留濾波算法的輕量級又能實現(xiàn)高精度的全球定位.

本文的創(chuàng)新點如下：

1）提出LINS-GNSS，其前端通過卡爾曼濾波將LiDAR和IMU緊耦合，后端通過因子圖優(yōu)化的方法將GNSS與LINS進行松耦合.為了融合GNSS數(shù)據(jù)，需要將LINS定位結(jié)果和GNSS定位結(jié)果進行坐標(biāo)系的對齊.

2）本文將GNSS因子加入后端的因子圖進行優(yōu)化后，將輸出的結(jié)果與真值進行比較來評估系統(tǒng)性能且與現(xiàn)有最優(yōu)算法LIO-SAM進行對比，在定位精度上LINS-GNSS超越了LIO-SAM.

1 LINS-GNSS算法總體架構(gòu)

本文提出的LINS-GNSS算法具體架構(gòu)如圖1所示，按照SLAM系統(tǒng)的慣例，它被分為前端與后端兩部分.LINS-GNSS的前端是基于迭代誤差狀態(tài)卡爾曼濾波的LiDAR/IMU緊組合，其狀態(tài)方程采用IMU在載體坐標(biāo)系下的誤差方程，觀測方程由激光雷達的點特征和面特征兩類特征共同約束構(gòu)建.圖 1中淺藍色框部分為LINS-GNSS的前端部分.LINS-GNSS的后端是基于因子圖優(yōu)化的，通過將LINS的定位結(jié)果和GNSS的定位結(jié)果匹配并進行坐標(biāo)轉(zhuǎn)換后，構(gòu)建位姿圖進行后端位姿優(yōu)化.

本文所涉及到的坐標(biāo)系均在圖 2中給出.主要坐標(biāo)系有地心地固坐標(biāo)系（Earth-Centered Earth-Fixed ，ECEF）系，這是GNSS定位輸出的系，一般采用WGS-84（World Geodetic System 1984）坐標(biāo)系.ECEF系的X軸指向赤道和本初子午線的交點；以地球的旋轉(zhuǎn)軸為Z軸，北極為正方向；Y軸垂直于X-Z平面，形成右手坐標(biāo)系.慣性測量單元IMU坐標(biāo)系為本文的載體系（Body系，b系）；IMU坐標(biāo)系初始時刻的坐標(biāo)系記作b 0系；IMU坐標(biāo)系初始時刻所在位置的東北天坐標(biāo)系被設(shè)定為本文的ENU系（N系）.激光雷達坐標(biāo)系為本文的LiDAR系（l系），l系的x軸沿激光雷達水平軸向右，z軸沿激光雷達縱軸向前，y軸垂直于X-Z平面，形成右手坐標(biāo)系.

圖2中涉及到如下幾個外參數(shù)：IMU和LiDAR之間的外參數(shù)Rb l與pb l；GNSS天線相位中心在IMU系的坐標(biāo)pG b；GNSS天線相位中心在LiDAR系的坐標(biāo)pG l.

2 LINS-GNSS前端算法

2.1 點云分割

點云分割首先將一幀激光點云投影到深度圖像中.此圖像的寬度為360°除以激光雷達水平分辨率，高度為激光雷達線數(shù).以使用Velodyne-16的激光雷達為例，寬度為1 800，高度為16.高度比較低的點會被判斷為地面點，地面點不進入到后續(xù)分割任務(wù)中，以降低計算量.具體分割時，采用圖像分割的方法將深度圖像進行聚類.聚類是通過深度優(yōu)先遍歷遞歸進行查找，從[0，0]點開始，遍歷它前、后、左、右的4個點，分別進行對比，如果相對角度大于60°，則認(rèn)為是同一個點云集群.最后分割出來的點云數(shù)量大于30個則認(rèn)為分割有效，點數(shù)過少的會被作為噪聲濾除，剩下的點根據(jù)深度圖像的分割結(jié)果分類，用于后續(xù)的特征提取.

2.2 特征提取

本文提取的激光點云特征點分為兩類，一類是邊緣點特征，一類是平面點特征.點云曲率是提取特征點的表征，曲率較大的為邊緣點特征，曲率較小的為平面點特征.某一點的曲率通過在深度圖像上找其左邊和右邊各5個點共11個點計算.為了能從360°方向提取特征點，深度圖像被分成了6個子圖，在每個子圖中都計算特征點.在每個子圖的每一行中，選取曲率最大且不屬于地面點的2個點作為邊緣點特征；選取曲率最小的4個點作為平面點特征.將6個深度子圖的特征點進行整理后便得到此幀激光點云的特征點.得到的特征點云通過點到直線和點到平面的迭代最近點算法進行點云匹配.

2.3 狀態(tài)傳遞

IMU在載體坐標(biāo)系下的狀態(tài)為t k時刻到t k+1時刻的位置pb t k b t k+1、速度vb t k b t k+1、姿態(tài)四元數(shù)qb t k b t k+1，t k+1時刻的加速度計零偏b a t k+1、陀螺儀零偏b g t k+1以及重力加速度在t k時刻的IMU系下的投影g t k.其中姿態(tài)四元數(shù)qb t k b t k+1對應(yīng)的旋轉(zhuǎn)矩陣形式為Rb t k b t k+1.

xb t k b t k+1=pb t k b t k+1，vb t k b t k+1，qb t k b t k+1，b a t k+1，b g t k+1，g t k.（1）

前端濾波框架所采用的狀態(tài)為IMU在載體坐標(biāo)系下的誤差狀態(tài)：

δxb t k b t k+1=δpb t k b t k+1，δvb t k b t k+1，δθb t k b t k+1，δb a t k+1，δb g t k+1，δg t k.（2）

連續(xù)時間的誤差狀態(tài)遞推方程：

δ（t）=F（t）δx（t）+G（t）w，（3）

F（t）=0I 30000

00-Rb t?? k b t t ×-Rb t?? k b t0I 3

00- t ×0-I 30

000000

000000000000，（4）

G（t）=0000

-Rb k t000

0-I 300

00I 30

000I 30000，（5）

w=nT a，nT g，nT b a，nT b gT，（6）

其中：I 3為3×3的單位矩陣， t ×為經(jīng)過零偏修正的加速度計測量對應(yīng)的反對稱矩陣， t ×為經(jīng)過零偏修正的陀螺儀測量對應(yīng)的反對稱矩陣，Rb t?? k b t表示t k時刻到t時刻姿態(tài)變化的旋轉(zhuǎn)矩陣形式，nT a表示加速度計的噪聲，nT g表示陀螺儀的噪聲，nT b a表示加速度計零偏隨機游走噪聲，nT b g表示陀螺儀零偏隨機游走噪聲.

將連續(xù)時間的誤差狀態(tài)遞推方程進行離散化得到離散的誤差狀態(tài)遞推方程：

δx t k+1=（I 18+F（t k+1）（t k+1-t k））δx t k，（7）

P t k+1=（I 18+F（t k+1）（t k+1-t k））P t k0（I 18+F（t k+1）·（t k+1-t k））T+（G（t k+1）Δt）Q（G（t k+1）Δt）T，（8）

其中Q矩陣為wTw，I 18為18×18的單位矩陣.

2.4 狀態(tài)更新

本文通過激光雷達在t k幀和t k+1幀的邊緣點特征和平面點特征對狀態(tài)δxb t k b t k+1進行更新.

邊緣點特征對應(yīng)的觀測方程由點到直線之間的距離來計算：

fe ixb t k b t k+1=l t k i-pl t k a×l t k i-pl t k bpl t k a-pl t k b，（9）

l t k i為t k時刻第i個點pl t k i在t k+1時刻對應(yīng)的點，計算方法為

l t k i=Rl bRb t k b t k+1Rb lpl t k+1 i+pb l+pb t k b t k+1-pb l，（10）

于是就可以得到l t k i關(guān)于誤差狀態(tài)δxb t k b t k+1各個分量導(dǎo)數(shù)的非零部分：

b t k iδpb t k b t k+1=Rl b，（11）

b t k iδθb t k b t k+1=-Rl bRb t k b t k+1Rb lpl t k+1 i+pb l ×，（12）

pl t k a和pl t k b是pl t k i最接近的兩個點.線性化后的雅克比矩陣為

He t k=fe it k i·l t k iδx=

l t k i-pl t k a×l t k i-pl t k bT（pl t k b-pl t k a） ×l t k i-pl t k a×l t k i-pl t k bpl t k a-pl t k b·l t k iδx.（13）

平面點特征對應(yīng)的觀測方程由點到平面之間的距離來計算：

fp ixb t k b t k+1=l t k i-pl t k aTpl t k a-pl t k b×pl t k a-pl t k cpl t k a-pl t k b×pl t k a-pl t k c.（14）

平面點特征觀測方程對狀態(tài)的雅克比矩陣：

Hp t k=fp it k i·l t k iδx=

l t k i-pl t k a×l t k i-pl t k bTl t k i-pl t k a×l t k i-pl t k b·l t k iδx，（15）

其中l(wèi) t k iδx與邊緣點特征對狀態(tài)的雅克比矩陣是一樣的，這里不再贅述.

點到直線的距離fe ixb t k b t k+1和點到平面的距離fp ixb t k b t k+1將被作為卡爾曼濾波中的觀測量，He t k和Hp t k描述了卡爾曼濾波的觀測量與需要估計的狀態(tài)之間的線性化關(guān)系，隨后進行標(biāo)準(zhǔn)卡爾曼濾波即可.此外，由于預(yù)測是通過IMU進行的，故在高動態(tài)的狀態(tài)下，激光雷達會產(chǎn)生較大的畸變，此時IMU可以提供一個大致的位姿估計，從而輔助激光雷達特征點之間的匹配.

3 LINS-GNSS后端優(yōu)化

后端優(yōu)化算法框架主要包括后端建圖、坐標(biāo)轉(zhuǎn)換、因子圖優(yōu)化3個部分.后端建圖是將特征點與周圍點云圖進行精確配準(zhǔn)，以獲得更精確的位姿.坐標(biāo)轉(zhuǎn)換是將后端建圖輸出的更精確的位置與GNSS定位結(jié)果進行坐標(biāo)轉(zhuǎn)換，都統(tǒng)一到同一坐標(biāo)系下.因子圖優(yōu)化是對一個由后端建圖輸出的位姿與GNSS定位結(jié)果構(gòu)建而成的位姿圖進行優(yōu)化.

3.1 后端建圖

選取在時序上較為相近的一些時刻特征點構(gòu)建對應(yīng)的全局點云地圖.通過優(yōu)化當(dāng)前時刻特征點與全局點云地圖的特征點之間的位姿約束，可以精細化后端建圖輸出的載體位姿.此優(yōu)化問題的初值為前端激光雷達與IMU迭代誤差卡爾曼濾波緊耦合輸出的載體位姿.

后端建圖的過程中，也可以通過回環(huán)檢測來進一步消除漂移，回環(huán)檢測是通過迭代最近點算法匹配當(dāng)前幀和之前的點云，添加新的空間約束，然后通過因子圖來優(yōu)化位姿圖.

3.2 坐標(biāo)轉(zhuǎn)換

通過后端建圖，可以獲得當(dāng)前時刻IMU坐標(biāo)系相對于IMU初始坐標(biāo)系（IMU 0系）的坐標(biāo).GNSS接收機得到的是GNSS天線相位中心相對于WGS-84系的坐標(biāo)值，給此坐標(biāo)值賦予導(dǎo)航計算機時間戳即可獲得有導(dǎo)航計算機時間戳的衛(wèi)星導(dǎo)航定位結(jié)果.通過時間戳將后端建圖輸出的定位結(jié)果和衛(wèi)星導(dǎo)航接收機輸出的定位結(jié)果進行內(nèi)插和外推，可以獲得一系列同時刻的IMU在IMU 0系下的位姿和GNSS在WGS-84系下的坐標(biāo)，分別記作：{pb 0 b 0，Rb 0 e，pb 1 b 0，Rb 1 e，…，pb n b 0，Rb n e}和{pG 0 e，pG 1 e，pG 2 e，…，pG n e}.

于是通過構(gòu)建優(yōu)化函數(shù)可以求出b 0系相對于WGS-84系的轉(zhuǎn)換參數(shù){pb 0 e，Rb 0 e}[10]：

argminRb 0 e∈SO（3），pb 0 e∑ni=1m2，（16）

式中，m=pb i b 0-Rb i b 0Rb 0 epG i e+pb 0 e-pb 0 b i.

上述問題可轉(zhuǎn)化成一個迭代最近點（Iterative Closest Point，ICP）問題：

argminRb i e∈SO（3），Rb i b 0pb 0 e∑ni=1n2，（17）

式中，n=pb i b 0+Rb i b 0pb 0 b i-Rb 0 epG i e+Rb i b 0pb 0 e.

通過解上述優(yōu)化問題就能獲得WGS84坐標(biāo)系和IMU 0系之間的坐標(biāo)轉(zhuǎn)換，此參數(shù)為后續(xù)因子圖優(yōu)化模型中的一個重要參數(shù).

3.3 因子圖優(yōu)化模型

LINS-GNSS后端通過構(gòu)建因子圖優(yōu)化模型對全體坐標(biāo)進行優(yōu)化.所構(gòu)建的因子圖模型如圖3所示.其中GNSS數(shù)據(jù)是直接通過GNSS接收機輸出獲得的，在本文實驗中，采用的是RTK（實時動態(tài)載波相位差分，Real-Time Kinematic）定位技術(shù).GNSS測量因子內(nèi)容為衛(wèi)星導(dǎo)航定位結(jié)果，LINS位姿測量因子包括LINS所估計的局部位置和姿態(tài)解算結(jié)果.

這將定位問題抽象成了一個由節(jié)點、邊組成的雙射圖，其中節(jié)點包含狀態(tài)節(jié)點和測量節(jié)點兩種.當(dāng)狀態(tài)與測量有關(guān)系時，它們之間就會有一條邊存在.

殘差方程包括兩部分，r 1為GNSS位置與LINS位置的差：

r 1=r GNSS_LINS=

RN MRM LPL G+tM L-tM N-PN G，（18）

r為LINS先驗位姿殘差：

r 2=r LINS_prior=RL M，i（tM L，j-tM L，i）

log（RL M，iRM L，j）∨，（19）

其中RN M和tM N為當(dāng)?shù)貣|北天坐標(biāo)系與LINS坐標(biāo)系之間的轉(zhuǎn)換參數(shù)，PN G為GNSS定位結(jié)果在當(dāng)?shù)貣|北天坐標(biāo)系下的結(jié)果，PL G為GNSS天線相位中心在激光雷達坐標(biāo)系下的位置，tM L為LINS輸出的位置結(jié)果，RL M，i為i時刻的LINS的姿態(tài)輸出，tM L，j為j時刻LINS的位置輸出，tM L，i為i時刻LINS的位置輸出.

4 實驗結(jié)果及分析

為驗證LINS-GNSS算法在變電站環(huán)境中的性能，本文將搭載了16線激光雷達、9軸IMU和GNSS接收機的巡檢機器人在變電站中進行導(dǎo)航定位實驗，并將LINS-GNSS的定位結(jié)果與RTK/INS結(jié)果（即真值）作對比.同時LIO-SAM在同一份數(shù)據(jù)上也進行了定位實驗，其定位所得軌跡作為對比實驗結(jié)果.實驗環(huán)境以及對應(yīng)數(shù)據(jù)采集如圖4所示.

實驗環(huán)境經(jīng)過三維激光建模以后獲得的結(jié)果如圖5所示.

從圖 5可見電線桿是會被激光雷達掃描出來的，并且整個環(huán)境也還算開闊，GNSS接收機可以提供一個較為準(zhǔn)確的值.而在運動激烈時，IMU的輸入可以提供一個位姿初值從而輔助LiDAR進行點云的匹配.綜上所述，在如圖 4所示的變電站環(huán)境中，GNSS/INS/LiDAR是一個很好的組合導(dǎo)航方案.但是GNSS會受到電磁干擾的影響，本文在電網(wǎng)環(huán)境中進行了靜態(tài)RTK測試，發(fā)現(xiàn)RTK定位結(jié)果和真實坐標(biāo)的水平定位中誤差為8 mm，高程定位中誤差15 mm.這一誤差水平對RTK來說是比較大的，其中部分誤差為電磁干擾所帶來的影響.為此在同一位置電網(wǎng)斷電情況下進行對比實驗，最終發(fā)現(xiàn)RTK定位結(jié)果和真實坐標(biāo)的水平定位中誤差為3 mm，高程定位中誤差6 mm，顯著低于通電情況.

4.1 實驗設(shè)置

實驗使用激光雷達為Velodyne16，IMU為Xsens Mti300，用于LINS-GNSS系統(tǒng)的衛(wèi)星導(dǎo)航接收機為u-blox ZED-F9P，用作定位真值的設(shè)備為Novatel的SPAN-CPT組合導(dǎo)航系統(tǒng)，在采集真值數(shù)據(jù)時， SPAN-CPT設(shè)備中配置了RTK.

4.2 標(biāo)定實驗

在LINS-GNSS算法中，IMU的噪聲參數(shù)以及IMU和LiDAR之間的外參是比較重要的參數(shù)，需要進行事先標(biāo)定.

本節(jié)將會給出本文所用設(shè)備的IMU噪聲參數(shù)標(biāo)定結(jié)果和LiDAR/IMU外參標(biāo)定結(jié)果.IMU的誤差分為確定性誤差與隨機誤差，確定性誤差比如零偏一般在SLAM系統(tǒng)會作為一個重要參數(shù)進行估計，因而在標(biāo)定實驗中只需對IMU的隨機誤差進行標(biāo)定，包括加速度與角速度的噪聲誤差與隨機游走，這兩個參數(shù)在本文中也被稱作IMU噪聲參數(shù).本文所標(biāo)定的IMU噪聲參數(shù)如表 1所示.IMU內(nèi)參通過Allan方差法進行標(biāo)定.

4.2.1 IMU噪聲參數(shù)標(biāo)定結(jié)果

通過將實驗設(shè)備靜止2 h，并使用Allan方差對實驗數(shù)據(jù)進行處理，得到Allan方差曲線如圖6—7所示.

圖6中的acc-x、acc-y、acc-z分別表示加速度計的x軸、y軸和z軸.圖7中的gyr-x、gyr-y、gyr-z分別表示陀螺儀的x軸、y軸和z軸.根據(jù)圖6和圖7可以得到如表2所示的IMU噪聲參數(shù)標(biāo)定結(jié)果.

4.2.2 LiDAR/IMU外參標(biāo)定

本文使用LI-calib工具箱[15]對LiDAR和IMU進行外參標(biāo)定，外參標(biāo)定的目的是用于校準(zhǔn)LiDAR

和IMU之間的6個自由度 （6 Degrees of Freedom，

6DoF） 的剛體變換參數(shù).LI-calib的流程如圖8所示，它在連續(xù)時間批量優(yōu)化框架中利用來自LiDAR和IMU傳感器的所有原始測量值進行標(biāo)定.流程主要分為4個步驟：外部旋轉(zhuǎn)初始化、數(shù)據(jù)關(guān)聯(lián)、批次優(yōu)化和迭代校準(zhǔn).

首先將來自LiDAR和IMU旋轉(zhuǎn)序列對齊來初始化外部旋轉(zhuǎn)，其中LiDAR的旋轉(zhuǎn)是從基于正態(tài)分布變換（NDT）配準(zhǔn)的LiDAR里程計獲得的.給定來自IMU傳感器的原始角速度測量值可以擬合旋轉(zhuǎn)B樣條曲線.

初始化后，能夠部分消除LiDAR掃描中的運動失真，并能夠從LiDAR測距中獲得更好的LiDAR姿態(tài)估計.使用LiDAR姿態(tài)初始化LiDAR面元地圖，再初始化點對面元的對應(yīng)關(guān)系.

批處理優(yōu)化是使用LiDAR和IMU量測將標(biāo)定問題轉(zhuǎn)化成基于圖的優(yōu)化問題，并假設(shè)所有測量結(jié)果均具有獨立的高斯噪聲.

最后利用優(yōu)化中當(dāng)前的最佳估計狀態(tài)更新面元圖，點對平面數(shù)據(jù)關(guān)聯(lián)，并迭代地優(yōu)化估計狀態(tài).通過連續(xù)時間批次優(yōu)化，狀態(tài)估計變得更加精確.

通過采集一小段運動充分的數(shù)據(jù)，即可獲得LiDAR/IMU外參標(biāo)定結(jié)果.為了使得結(jié)果具備可信度，總共采集5組數(shù)據(jù)并進行實驗，實驗的過程如圖9所示，其中彩色的為程序所提取的面特征，白色的線條為提取的線特征.

在外參估計實驗中，估計所得的外參穩(wěn)定性非常重要，而內(nèi)符合精度以估計的最似然估值為比對基準(zhǔn)，主要反映離散度，故可以很好地用來作為評判穩(wěn)定性的指標(biāo).對同一個LiDAR/IMU設(shè)備進行5次外參標(biāo)定實驗，然后統(tǒng)計5個結(jié)果的均值和標(biāo)準(zhǔn)差來反映內(nèi)符合精度.結(jié)果如表3所示，數(shù)據(jù)格式為：均值±標(biāo)準(zhǔn)差.

4.3 定位實驗

通過對比LINS-GNSS的結(jié)果和RTK/INS的結(jié)果，可以獲得如圖10和表4所示的結(jié)果.圖10左邊為LINS-GNSS估計軌跡與真值軌跡對比，軌跡為在ENU坐標(biāo)系下的軌跡，即圖中的X軸對應(yīng)東方向，Y軸對應(yīng)北方向，Z軸對應(yīng)高程方向，圖10右邊為LINS-GNSS三維誤差序列.表4為LINS-GNSS的3D誤差各項統(tǒng)計指標(biāo).

表4中：max誤差表示誤差的最大值，可以反映定位的魯棒性；mean表示誤差的均值，median表示誤差的中位數(shù)，rmse表示均方根誤差，這三類誤差一般用來反映定位的精度；std表示誤差的標(biāo)準(zhǔn)差，可以反映定位的穩(wěn)定性.從表 4可以看出，在變電站環(huán)境中，LINS-GNSS的算法能很好地獲得定位結(jié)果，其定位最大誤差在0.5 m以內(nèi)，定位誤差的均方差為0.211 m，滿足大部分變電站內(nèi)機器人的定位需求.

LIO-SAM在這個實驗中為對比實驗組，其定位結(jié)果的統(tǒng)計值如表5所示.

對比表 4和表 5可以看出，本文所提的LINS-GNSS算法，比最先進的LIO-SAM算法在變電站環(huán)境下定位性能更優(yōu).

LINS-GNSS算法和LIO-SAM算法的ENU三軸誤差和統(tǒng)計結(jié)果如圖11和表6所示.

從圖11和表6中可以看出，LINS-GNSS的三軸誤差均優(yōu)于LIO-SAM，驗證了本文所提算法的有效性.

5 結(jié)論與未來展望

本文提出在變電站環(huán)境中融合GNSS/INS/LiDAR的組合導(dǎo)航定位算法——LINS-GNSS，并在真實變電站環(huán)境中進行測試.LINS-GNSS算法架構(gòu)的創(chuàng)新點在于，其前端使用卡爾曼濾波緊耦合了LiDAR和IMU，后端用因子圖優(yōu)化松耦合了GNSS，具有松緊交替、濾波優(yōu)化交替的特性.

為了使LINS-GNSS性能更好，本文事先對IMU的噪聲參數(shù)以及LiDAR/IMU的外參進行了標(biāo)定.最終結(jié)果表明，LINS-GNSS在本文實驗所處變電站環(huán)境中可以達到最大誤差0.5 m以內(nèi)，均方根誤差0.211 m的定位精度，比現(xiàn)有的最優(yōu)GNSS/ INS/LiDAR組合導(dǎo)航算法LIO-SAM精度更高，充分證明了LINS-GNSS的有效性.

目前LiDAR和IMU是基于卡爾曼濾波的緊耦合，GNSS是基于優(yōu)化的松耦合，未來可以將GNSS原始信息融入緊耦合濾波框架.目前GNSS通過RTK獲取的定位結(jié)果，未來可以使用PPP或者PPP-RTK等GNSS定位技術(shù).

參考文獻

References

［1］ Groves P D.Principles of GNSS，inertial，and multisensor integrated navigation systems，2nd edition [J].IEEE Aerospace and Electronic Systems Magazine，2015，30（2）：26-27

[2] Barbour N M.Inertial navigation sensors[R].Cambridge，MA：Charles Stark Draper Laboratory，2011

[3] Wen W S，Zhang G H，Hsu L T.Exclusion of GNSS NLOS receptions caused by dynamic objects in heavy traffic urban scenarios using real-time 3D point cloud：an approach without 3D maps[C]//2018 IEEE/ION Position，Location and Navigation Symposium （PLANS）.April 23-26，2018，Monterey，CA，USA.IEEE，2018：158-165

[4] Qin T，Cao S Z，Pan J，et al.A general optimization-based framework for global pose estimation with multiple sensors[J].arXiv e-print，2019，arXiv：1901.03642

[5] 高翔，張濤，劉毅.視覺SLAM十四講：從理論到實踐[M].北京：電子工業(yè)出版社，2017：17-21

[6] Chen C X，Pei L，Xu C Q，et al.Trajectory optimization of LiDAR SLAM based on local pose graph[C]//China Satellite Navigation Conference （CSNC） 2019 Proceedings，2019

[7] Hsu L T.Analysis and modeling GPS NLOS effect in highly urbanized area[J].GPS Solutions，2017，22（1）：1-12

[8] Chen Y W，Zhu L L，Tang J，et al.Feasibility study of using mobile laser scanning point cloud data for GNSS line of sight analysis[J].Mobile Information Systems，2017：5407605

[9] Wen W S.3D LiDAR aided GNSS and its tightly coupled integration with INS via factor graph optimization[C]//The International Technical Meeting of the Satellite Division of the Institute of Navigation.September 22-25，2020.Institute of Navigation，2020：1649-1672

[10] Li T，Pei L，Xiang Y，et al.P3-LOAM：PPP/LiDARloosely coupled SLAM with accurate covariance estimation and robust RAIM in urban canyon environment[J].IEEE Sensors Journal，2021，21（5）：6660-6671

[11] 鄂盛龍，周剛，譚理慶，等.變電站環(huán)境下GNSS接收機性能及觀測數(shù)據(jù)質(zhì)量分析[J].全球定位系統(tǒng)，2020，45（4）：36-41，48

E Shenglong，ZHOU Gang，TAN Liqing，et al.Analysis on GNSS receive performance and observation data quality in substation environment[J].GNSS World of China，2020，45（4）：36-41，48

[12] Qin C，Ye H Y，Pranata C E，et al.LINS：a lidar-inertial state estimator for robust and efficient navigation[C]//2020 IEEE International Conference on Robotics and Automation.May 31-August 31，2020，Paris，F(xiàn)rance.IEEE，2020：8899-8906

[13] Sola J.Quaternion kinematics for the error-state Kalman filter[J].arXiv e-print，2017，arXiv：1711.02508

[14] Shan T X，Englot B，Meyers D，et al.LIO-SAM：tightly-coupled lidar inertial odometry via smoothing and mapping[J].2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.October 24-30，2020，Las Vegas，NV，USA.IEEE，2020：5135-5142

[15] Lü J J，Xu J H，Hu K W，et al.Targetless calibration of LiDAR-IMU system based on continuous-time batch estimation[C]//2020 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.October 24-30，2020，Las Vegas，NV，USA.IEEE，2020：9968-9975

LINS-GNSS：filter and optimization coupled GNSS/INS/LiDAR

positioning method for inspection robot localization

WEN Gang1 ZHOU Fangrong1 LI Tao2 MA Yutang1 PEI Ling2 LIU Yadong3 QIAN Guochao1 PAN Hao1

1Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology，

Yunnan Power Grid Co.，Ltd.，Kunming 650217

2Shanghai Key Laboratory of Navigation and Location-Based Services，Shanghai Jiao Tong University，Shanghai 200240

3School of Electronic Information and Electrical Engineering，Shanghai Jiao Tong University，Shanghai 200240

Abstract In the past few years，robots have become an important means of substation inspection，and robotic inspection technology for non-fixed lines has received increasing attention in order to perform inspection tasks more flexibly.How to achieve high-precision positioning in complex substation environment is one of the core problems to be solved.It is difficult for a single sensor to meet the requirements of reliable positioning in substations，therefore，this paper designs a multi-sensor fusion LINS-GNSS positioning method.Its front-end tightly couples LiDAR and inertial navigation based on an iterative error-state Kalman filter framework，which recursively corrects the estimated state by generating new feature correspondences in each iteration.The back-end uses a factor graph optimization approach to loosely couple the localization results from the satellite navigation with the localization results output from the LINS back-end.The optimization process first aligns the local coordinate system with the global coordinate system，then adds the position constraints of the GNSS as a priori edge to the factor graph in the back-end，and finally outputs the positioning results in the global coordinate system.In order to evaluate the performance of the LINS-GNSS system in the substation environment，this paper conducted field tests under real scenarios.The experimental results show that the LINS-GNSS system can achieve a positioning accuracy better than 0.5 m in the substation environment，better than LIO-SAM.

Key words multi-sensor fusion;factor graph optimization;Kalman filter;satellite navigation;LiDAR-SLAM