LINS-GNSS：濾波與優(yōu)化耦合的GNSS/INS/LiDAR巡檢機(jī)器人定位方法

文剛 周仿榮 李濤 馬御棠 裴凌 劉亞?wèn)| 錢(qián)國(guó)超 潘浩

摘要

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

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

中圖分類(lèi)號(hào)

V249.3

文獻(xiàn)標(biāo)志碼

A

收稿日期

2022-01-05

資助項(xiàng)目

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

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

0 引言

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

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

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

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

2）本文將GNSS因子加入后端的因子圖進(jìn)行優(yōu)化后，將輸出的結(jié)果與真值進(jìn)行比較來(lái)評(píng)估系統(tǒng)性能且與現(xiàn)有最優(yōu)算法LIO-SAM進(jìn)行對(duì)比，在定位精度上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)系下的誤差方程，觀(guān)測(cè)方程由激光雷達(dá)的點(diǎn)特征和面特征兩類(lèi)特征共同約束構(gòu)建.圖 1中淺藍(lán)色框部分為L(zhǎng)INS-GNSS的前端部分.LINS-GNSS的后端是基于因子圖優(yōu)化的，通過(guò)將LINS的定位結(jié)果和GNSS的定位結(jié)果匹配并進(jìn)行坐標(biāo)轉(zhuǎn)換后，構(gòu)建位姿圖進(jìn)行后端位姿優(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軸指向赤道和本初子午線(xiàn)的交點(diǎn)；以地球的旋轉(zhuǎn)軸為Z軸，北極為正方向；Y軸垂直于X-Z平面，形成右手坐標(biāo)系.慣性測(cè)量單元IMU坐標(biāo)系為本文的載體系（Body系，b系）；IMU坐標(biāo)系初始時(shí)刻的坐標(biāo)系記作b 0系；IMU坐標(biāo)系初始時(shí)刻所在位置的東北天坐標(biāo)系被設(shè)定為本文的ENU系（N系）.激光雷達(dá)坐標(biāo)系為本文的LiDAR系（l系），l系的x軸沿激光雷達(dá)水平軸向右，z軸沿激光雷達(dá)縱軸向前，y軸垂直于X-Z平面，形成右手坐標(biāo)系.

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

2 LINS-GNSS前端算法

2.1 點(diǎn)云分割

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

2.2 特征提取

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

2.3 狀態(tài)傳遞

IMU在載體坐標(biāo)系下的狀態(tài)為t k時(shí)刻到t k+1時(shí)刻的位置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時(shí)刻的加速度計(jì)零偏b a t k+1、陀螺儀零偏b g t k+1以及重力加速度在t k時(shí)刻的IMU系下的投影g t k.其中姿態(tài)四元數(shù)qb t k b t k+1對(duì)應(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ù)時(shí)間的誤差狀態(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)過(guò)零偏修正的加速度計(jì)測(cè)量對(duì)應(yīng)的反對(duì)稱(chēng)矩陣， t ×為經(jīng)過(guò)零偏修正的陀螺儀測(cè)量對(duì)應(yīng)的反對(duì)稱(chēng)矩陣，Rb t?? k b t表示t k時(shí)刻到t時(shí)刻姿態(tài)變化的旋轉(zhuǎn)矩陣形式，nT a表示加速度計(jì)的噪聲，nT g表示陀螺儀的噪聲，nT b a表示加速度計(jì)零偏隨機(jī)游走噪聲，nT b g表示陀螺儀零偏隨機(jī)游走噪聲.

將連續(xù)時(shí)間的誤差狀態(tài)遞推方程進(jìn)行離散化得到離散的誤差狀態(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)更新

本文通過(guò)激光雷達(dá)在t k幀和t k+1幀的邊緣點(diǎn)特征和平面點(diǎn)特征對(duì)狀態(tài)δxb t k b t k+1進(jìn)行更新.

邊緣點(diǎn)特征對(duì)應(yīng)的觀(guān)測(cè)方程由點(diǎn)到直線(xiàn)之間的距離來(lái)計(jì)算：

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時(shí)刻第i個(gè)點(diǎn)pl t k i在t k+1時(shí)刻對(duì)應(yīng)的點(diǎn)，計(jì)算方法為

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各個(gè)分量導(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最接近的兩個(gè)點(diǎn).線(xiàn)性化后的雅克比矩陣為

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）

平面點(diǎn)特征對(duì)應(yīng)的觀(guān)測(cè)方程由點(diǎn)到平面之間的距離來(lái)計(jì)算：

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）

平面點(diǎn)特征觀(guān)測(cè)方程對(duì)狀態(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與邊緣點(diǎn)特征對(duì)狀態(tài)的雅克比矩陣是一樣的，這里不再贅述.

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

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

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

3.1 后端建圖

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

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

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

通過(guò)后端建圖，可以獲得當(dāng)前時(shí)刻IMU坐標(biāo)系相對(duì)于IMU初始坐標(biāo)系（IMU 0系）的坐標(biāo).GNSS接收機(jī)得到的是GNSS天線(xiàn)相位中心相對(duì)于WGS-84系的坐標(biāo)值，給此坐標(biāo)值賦予導(dǎo)航計(jì)算機(jī)時(shí)間戳即可獲得有導(dǎo)航計(jì)算機(jī)時(shí)間戳的衛(wèi)星導(dǎo)航定位結(jié)果.通過(guò)時(shí)間戳將后端建圖輸出的定位結(jié)果和衛(wèi)星導(dǎo)航接收機(jī)輸出的定位結(jié)果進(jìn)行內(nèi)插和外推，可以獲得一系列同時(shí)刻的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}.

于是通過(guò)構(gòu)建優(yōu)化函數(shù)可以求出b 0系相對(duì)于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.

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

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.

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

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

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

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

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

r 1=r GNSS_LINS=

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

r為L(zhǎng)INS先驗(yàn)位姿殘差：

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天線(xiàn)相位中心在激光雷達(dá)坐標(biāo)系下的位置，tM L為L(zhǎng)INS輸出的位置結(jié)果，RL M，i為i時(shí)刻的LINS的姿態(tài)輸出，tM L，j為j時(shí)刻LINS的位置輸出，tM L，i為i時(shí)刻LINS的位置輸出.

4 實(shí)驗(yàn)結(jié)果及分析

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

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

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

4.1 實(shí)驗(yàn)設(shè)置

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

4.2 標(biāo)定實(shí)驗(yàn)

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

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

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

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

圖6中的acc-x、acc-y、acc-z分別表示加速度計(jì)的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]對(duì)LiDAR和IMU進(jìn)行外參標(biāo)定，外參標(biāo)定的目的是用于校準(zhǔn)LiDAR

和IMU之間的6個(gè)自由度 （6 Degrees of Freedom，

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

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

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

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

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

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

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

4.3 定位實(shí)驗(yàn)

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

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

LIO-SAM在這個(gè)實(shí)驗(yàn)中為對(duì)比實(shí)驗(yàn)組，其定位結(jié)果的統(tǒng)計(jì)值如表5所示.

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

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

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

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

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

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

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

參考文獻(xiàn)

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] 高翔，張濤，劉毅.視覺(jué)SLAM十四講：從理論到實(shí)踐[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接收機(jī)性能及觀(guān)測(cè)數(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