LINS-GNSS:滤波与优化耦合的GNSS/INS/LiDAR巡检机器人定位方法

文刚，周仿荣，李涛，马御棠，裴凌，刘亚东，钱国超，潘浩; WEN Gang; ZHOU Fangrong; LI Tao; MA Yutang; PEI Ling; LIU Yadong; QIAN Guochao; PAN Hao

引 en

LINS-GNSS:滤波与优化耦合的GNSS/INS/LiDAR巡检机器人定位方法

文刚¹
机构：
1. 云南电网有限责任公司电力科学研究院/电力遥感技术联合实验室,昆明, 650217
×
，周仿荣¹
机构：
1. 云南电网有限责任公司电力科学研究院/电力遥感技术联合实验室,昆明, 650217
×
，李涛²
机构：
2. 上海交通大学上海市北斗导航与位置服务重点实验室,上海, 200240
×
，马御棠¹
机构：
1. 云南电网有限责任公司电力科学研究院/电力遥感技术联合实验室,昆明, 650217
×
，裴凌²
机构：
2. 上海交通大学上海市北斗导航与位置服务重点实验室,上海, 200240
×
，刘亚东³
机构：
3. 上海交通大学电子信息与电气工程学院,上海, 200240
×
，钱国超¹
机构：
1. 云南电网有限责任公司电力科学研究院/电力遥感技术联合实验室,昆明, 650217
×
，潘浩¹
机构：
1. 云南电网有限责任公司电力科学研究院/电力遥感技术联合实验室,昆明, 650217
×

1. 云南电网有限责任公司电力科学研究院/电力遥感技术联合实验室,昆明, 650217；
2. 上海交通大学上海市北斗导航与位置服务重点实验室,上海, 200240；
3. 上海交通大学电子信息与电气工程学院,上海, 200240；

LINS-GNSS:filter and optimization coupled GNSS/INS/LiDAR positioning method for inspection robot localization

WEN Gang¹
Affiliation：
1. Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology, Yunnan Power Grid Co.,Ltd.,Kunming 650217
×
，ZHOU Fangrong¹
Affiliation：
1. Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology, Yunnan Power Grid Co.,Ltd.,Kunming 650217
×
，LI Tao²
Affiliation：
2. Shanghai Key Laboratory of Navigation and Location-Based Services,Shanghai Jiao Tong University,Shanghai 200240
×
，MA Yutang¹
Affiliation：
1. Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology, Yunnan Power Grid Co.,Ltd.,Kunming 650217
×
，PEI Ling²
Affiliation：
2. Shanghai Key Laboratory of Navigation and Location-Based Services,Shanghai Jiao Tong University,Shanghai 200240
×
，LIU Yadong³
Affiliation：
3. School of Electronic Information and Electrical Engineering,Shanghai Jiao Tong University,Shanghai 200240
×
，QIAN Guochao¹
Affiliation：
1. Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology, Yunnan Power Grid Co.,Ltd.,Kunming 650217
×
，PAN Hao¹
Affiliation：
1. Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology, Yunnan Power Grid Co.,Ltd.,Kunming 650217
×

1. Electric Power Research Institute/ Joint Laboratory of Power Remote Sensing Technology, Yunnan Power Grid Co.,Ltd.,Kunming 650217；
2. Shanghai Key Laboratory of Navigation and Location-Based Services,Shanghai Jiao Tong University,Shanghai 200240；
3. School of Electronic Information and Electrical Engineering,Shanghai Jiao Tong University,Shanghai 200240；

作者简介:

文刚,男,硕士,从事电网设备防灾减灾及无人机导航技术研究.1192381484@qq.com;

李涛(通信作者),男,博士生,从事多源融合导航相关技术的研究.tao_li@sjtu.edu.cn

中图分类号:V249.3

文献标识码:A

DOI:10.13878/j.cnki.jnuist.2023.01.009

全文
Comment
参考文献
出版信息

参考文献 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].北京:电子工业出版社,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

鄂盛龙,周刚,谭理庆,等.变电站环境下GNSS接收机性能及观测数据质量分析[J].全球定位系统,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,France.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

查找原文

目录contents

摘要 Abstract
关键词 Keywords
0 引言
1 LINS-GNSS算法总体架构
2 LINS-GNSS前端算法
2.1 点云分割
2.2 特征提取
2.3 状态传递
2.4 状态更新
3 LINS-GNSS后端优化
3.1 后端建图
3.2 坐标转换
3.3 因子图优化模型
4 实验结果及分析
4.1 实验设置
4.2 标定实验
4.2.1 IMU噪声参数标定结果
4.2.2 LiDAR/IMU外参标定
4.3 定位实验
5 结论与未来展望
参考文献

摘要

为了能够更加灵活地执行变电站巡检任务,非固定线路的机器人巡检技术越来越受到关注.如何在复杂的变电站环境中实现高精度的定位是机器人在变电站执行巡检任务时需要解决的核心问题.单一传感器难以满足变电站可靠定位的要求,因此,本文设计了多传感器融合的LINS-GNSS定位方法.其前端基于迭代误差状态卡尔曼滤波框架将激光雷达和惯性导航进行紧耦合,在每次迭代中生成新的特征对应关系递归地校正估计状态.后端使用因子图优化的方法将卫星导航的定位结果与LINS后端输出的定位结果松耦合.优化过程中先将局部坐标系与全局坐标系对齐,再将卫星导航的位置约束作为先验边添加到后端的因子图中,最后将定位结果在全局坐标系下输出.为了评估LINS-GNSS系统在变电站环境中的性能,本文在实际变电站中进行了测试.实验结果表明,LINS-GNSS系统在变电站环境中可以达到优于0.5 m的定位精度,且比现有最佳算法LIO-SAM定位精度更高.

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.

关键词

多传感器融合；因子图优化；卡尔曼滤波；卫星导航；激光SLAM

Keywords

multi-sensor fusion ； factor graph optimization ； Kalman filter ； satellite navigation ； LiDAR-SLAM

0 引言
近年来，融合全球导航卫星系统（Global Navigation Satellite System，GNSS）^[1]和惯性导航系统（Inertial Navigation System，INS）^[2]的组合定位系统已经被广泛地应用在室外定位场景中.GNSS具有全局性，能够在全球范围内提供导航定位服务，在室外场景的导航定位中发挥着十分重要的作用.惯性测量单元（Inertial Measurement Unit，IMU）是测量物体加速度和角速度的传感器.由于IMU在导航推算时具有更新频率高、受环境变化影响小的特点，所以常常被用来与各种传感器进行融合.但是，在复杂的环境中，作为主要信息源的全球卫星导航系统往往会由于遮挡或者电磁干扰而受到严重的多路径和非视距（Non-Light-Of-Sight，NLOS）效应^[3]影响，导致定位精度降低.而IMU在进行惯导推算时，误差会累积，最终导致定位结果发散.因此，如何实现变电站环境下的精确定位是一个重要且具有挑战的问题.各种融合定位方法的出现为解决这个问题提供了理想的方案^[4].GNSS可以修正INS误差的积累，提高导航精度，而INS可以弥补GNSS的信号丢失或衰减导致的定位性能下降问题，提高导航的连续性.基于激光雷达的同步定位与建图（LiDAR Simultaneous Localization and Mapping，LiDAR SLAM）^[5-6]算法受光照影响较小，因此在变电站环境中，3D LiDAR可以用于检测由静态环境以及动态物体引起的NLOS^[7-8]，对GNSS定位起到辅助作用^[9-10].在变电站环境下，一般巡检机器人巡航时除了需要获得定位结果外，还需要建立环境地图以便之后导航使用^[11].本文采用对卫星、惯导、激光雷达进行组合的方法，对变电站中的机器人融合定位技术进行研究.整个融合定位算法LINS-GNSS的前端采用滤波框架，后端采用优化框架.本文研究中的LINS（LiDAR-INS）^[12] 是基于迭代误差状态卡尔曼滤波^[13]（iterative Error-State Kalman Filter，iterative ESKF）的紧耦合LiDAR-IMU模型，但是LINS本身不具备与GNSS融合的能力，故在长期工作时会发散，且无法输出全球定位坐标.此外，LIO-SAM ^[14]（Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping）是一种基于因子图优化的紧耦合LiDAR-IMU系统，并且该系统具备和GNSS进行松耦合的能力，但其耗费的资源比基于滤波的算法要多.本文所提算法既能保留滤波算法的轻量级又能实现高精度的全球定位.
本文的创新点如下:
1）提出LINS-GNSS，其前端通过卡尔曼滤波将LiDAR和IMU紧耦合，后端通过因子图优化的方法将GNSS与LINS进行松耦合.为了融合GNSS数据，需要将LINS定位结果和GNSS定位结果进行坐标系的对齐.
2）本文将GNSS因子加入后端的因子图进行优化后，将输出的结果与真值进行比较来评估系统性能且与现有最优算法LIO-SAM进行对比，在定位精度上LINS-GNSS超越了LIO-SAM.
1 LINS-GNSS算法总体架构
本文提出的LINS-GNSS算法具体架构如图1所示，按照SLAM系统的惯例，它被分为前端与后端两部分.LINS-GNSS的前端是基于迭代误差状态卡尔曼滤波的LiDAR/IMU紧组合，其状态方程采用IMU在载体坐标系下的误差方程，观测方程由激光雷达的点特征和面特征两类特征共同约束构建.图1中浅蓝色框部分为LINS-GNSS的前端部分.LINS-GNSS的后端是基于因子图优化的，通过将LINS的定位结果和GNSS的定位结果匹配并进行坐标转换后，构建位姿图进行后端位姿优化.
本文所涉及到的坐标系均在图2中给出.主要坐标系有地心地固坐标系（Earth-Centered Earth-Fixed，ECEF）系，这是GNSS定位输出的系，一般采用WGS-84（World Geodetic System 1984）坐标系.ECEF系的X轴指向赤道和本初子午线的交点; 以地球的旋转轴为Z轴，北极为正方向; Y轴垂直于X-Z平面，形成右手坐标系.惯性测量单元IMU坐标系为本文的载体系（Body系，b系）; IMU坐标系初始时刻的坐标系记作b₀系; IMU坐标系初始时刻所在位置的东北天坐标系被设定为本文的ENU系（N系）.激光雷达坐标系为本文的LiDAR系（l系），l系的x轴沿激光雷达水平轴向右，z轴沿激光雷达纵轴向前，y轴垂直于X-Z平面，形成右手坐标系.
图2中涉及到如下几个外参数:IMU和LiDAR之间的外参数 $R_{l}^{b}$ 与 $p_{l}^{b}$ ; GNSS天线相位中心在IMU系的坐标 $p_{b}^{G}$ ; GNSS天线相位中心在LiDAR系的坐标 $p_{l}^{G}$ .
2 LINS-GNSS前端算法
2.1 点云分割
点云分割首先将一帧激光点云投影到深度图像中.此图像的宽度为360°除以激光雷达水平分辨率，高度为激光雷达线数.以使用Velodyne-16的激光雷达为例，宽度为1 800，高度为16.高度比较低的点会被判断为地面点，地面点不进入到后续分割任务中，以降低计算量.具体分割时，采用图像分割的方法将深度图像进行聚类.聚类是通过深度优先遍历递归进行查找，从[0，0]点开始，遍历它前、后、左、右的4个点，分别进行对比，如果相对角度大于60°，则认为是同一个点云集群.最后分割出来的点云数量大于30个则认为分割有效，点数过少的会被作为噪声滤除，剩下的点根据深度图像的分割结果分类，用于后续的特征提取.
2.2 特征提取
本文提取的激光点云特征点分为两类，一类是边缘点特征，一类是平面点特征.点云曲率是提取特征点的表征，曲率较大的为边缘点特征，曲率较小的为平面点特征.某一点的曲率通过在深度图像上找其左边和右边各5个点共11个点计算.为了能从360°方向提取特征点，深度图像被分成了6个子图，在每个子图中都计算特征点.在每个子图的每一行中，选取曲率最大且不属于地面点的2个点作为边缘点特征; 选取曲率最小的4个点作为平面点特征.将6个深度子图的特征点进行整理后便得到此帧激光点云的特征点.得到的特征点云通过点到直线和点到平面的迭代最近点算法进行点云匹配.
图1 LINS-GNSS算法架构
Fig.1 LINS-GNSS algorithm architecture
图2 LINS-GNSS算法所涉及到的坐标系
Fig.2 The coordinate system involved in LINS-GNSS algorithm
2.3 状态传递
IMU在载体坐标系下的状态为t_k时刻到t_k₊₁时刻的位置 $p_{t_{k} b_{t_{k + 1}}}^{b}$ 、速度 $v_{b_{t_{k + 1}}}^{b_{t_{k}}}$ 、姿态四元数 $q_{b_{t_{k + 1}}}^{b_{t_{k}}}$ ，t_k₊₁时刻的加速度计零偏 $b_{a_{t_{k + 1}}}$ 、陀螺仪零偏 $b_{g_{t_{k + 1}}}$ 以及重力加速度在t_k时刻的IMU系下的投影 $g_{t_{k}}$ .其中姿态四元数 $q_{b_{t_{k + 1}}}^{b_{t_{k}}}$ 对应的旋转矩阵形式为 $R_{b_{t_{k + 1}}}^{b_{t_{k}}}$ .

x_{b_{t_{k + 1}}}^{b_{t_{k}}} = [p_{b_{t_{k + 1}}}^{b_{t_{k}}}, v_{b_{t_{k + 1}}}^{b_{t_{k}}}, q_{b_{t_{k + 1}}}^{b_{t_{k}}}, b_{a_{t_{k + 1}}}, b_{g_{t_{k + 1}}}, g_{t_{k}}] .

(1)

前端滤波框架所采用的状态为IMU在载体坐标系下的误差状态:

δ x_{b_{t_{k + 1}}}^{b_{t_{k}}} = [δ p_{b_{t_{k + 1}}}^{b_{t_{k}}}, δ v_{b_{t_{k + 1}}}^{b_{t_{k}}}, δ θ_{b_{t_{k + 1}}}^{b_{t_{k}}}, δ b_{a_{t_{k + 1}}}, δ b_{g_{t_{k + 1}}}, δ g_{t_{k}}] .

(2)

连续时间的误差状态递推方程:

δ \dot{x} (t) = F (t) δ x (t) + G (t) w,

(3)

F (t) = [\begin{matrix} 0 & I_{3} & 0 & 0 & 0 & 0 \\ 0 & 0 & - R_{b_{t}}^{b_{t_{k}}} {[{\hat{a}}_{t}]}_{\times} & - R_{b_{t}}^{b_{t_{k}}} & 0 & I_{3} \\ 0 & 0 & - {[{\hat{ω}}_{t}]}_{\times} & 0 & - I_{3} & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 \end{matrix}],

(4)

G (t) = [\begin{matrix} 0 & 0 & 0 & 0 \\ - R_{t}^{b_{k}} & 0 & 0 & 0 \\ 0 & - I_{3} & 0 & 0 \\ 0 & 0 & I_{3} & 0 \\ 0 & 0 & 0 & I_{3} \\ 0 & 0 & 0 & 0 \end{matrix}],

(5)

w = {[n_{a}^{T}, n_{g}^{T}, n_{b_{a}}^{T}, n_{b_{g}}^{T}]}^{T},

(6)

其中:I₃为3×3的单位矩阵， ${[{\hat{a}}_{t}]}_{\times}$ 为经过零偏修正的加速度计测量对应的反对称矩阵， $[{\hat{ω}}_{t}] x$ 为经过零偏修正的陀螺仪测量对应的反对称矩阵， $R_{b_{t}}^{b_{t_{k}}}$ 表示t_k时刻到t时刻姿态变化的旋转矩阵形式， $n_{a}^{T}$ 表示加速度计的噪声， $n_{g}^{T}$ 表示陀螺仪的噪声， $n_{b_{a}}^{T}$ 表示加速度计零偏随机游走噪声， $n_{b_{g}}^{T}$ 表示陀螺仪零偏随机游走噪声.
将连续时间的误差状态递推方程进行离散化得到离散的误差状态递推方程:

δ x_{t_{k + 1}} = (I_{18} + F (t_{k + 1}) (t_{k + 1} - t_{k})) δ x_{t_{k}},

(7)

\begin{matrix} P_{t_{k + 1}} = (I_{18} + F (t_{k + 1}) (t_{k + 1} - t_{k})) P_{t_{k 0}} (I_{18} + F (t_{k + 1}) \cdot \\ {(t_{k + 1} - t_{k}))}^{T} + (G (t_{k + 1}) Δ t) Q {(G (t_{k + 1}) Δ t)}^{T}, \end{matrix}

(8)

其中Q矩阵为 $w^{T} w$ ，I₁₈为18×18的单位矩阵.
2.4 状态更新
本文通过激光雷达在t_k帧和t_k₊₁帧的边缘点特征和平面点特征对状态 $δ x_{b_{t_{k + 1}}}^{b_{t_{k}}}$ 进行更新.
边缘点特征对应的观测方程由点到直线之间的距离来计算:

f_{i}^{e} (x_{b_{t_{k + 1}}}^{b_{t_{k}}}) = \frac{|({\hat{p}}_{i}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}}) \times ({\hat{p}}_{i}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}})|}{|p_{a}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}}|}

(9)

${\hat{p}}_{i}^{l_{t_{k}}}$ 为t_k时刻第i个点 $p_{i}^{l_{t_{k}}}$ 在t_k₊₁时刻对应的点，计算方法为

{\hat{p}}_{i}^{l_{t_{k}}} = R_{b}^{l} (R_{b_{t_{k + 1}}}^{b_{t_{k}}} (R_{l}^{b} p_{i}^{l_{t_{k + 1}}} + p_{l}^{b}) + p_{b_{t_{k + 1}}}^{b_{t_{k}}} - p_{l}^{b}),

(10)

于是就可以得到 ${\hat{p}}_{i}^{l_{t_{k}}}$ 关于误差状态 ${δ x}_{b_{t_{k + 1}}}^{b_{t_{k}}}$ 各个分量导数的非零部分:

\frac{\partial {\hat{p}}_{i}^{b_{t_{k}}}}{\partial δ p_{b_{t_{k + 1}}}^{b_{t_{k}}}} = R_{b}^{l},

(11)

\frac{\partial {\hat{p}}_{i}^{b_{t_{k}}}}{\partial δ θ_{b_{t_{k + 1}}}^{b_{t_{k}}}} = - R_{b}^{l} {(R_{b_{t_{k + 1}}}^{b_{t_{k}}} (R_{l}^{b} p_{i}^{l_{t_{k + 1}}} + p_{l}^{b}))}_{\times},

(12)

$p_{a}^{l_{t_{k}}}$ 和 $p_{b}^{l_{t_{k}}}$ 是 $p_{i}^{l_{t_{k}}}$ 最接近的两个点.线性化后的雅克比矩阵为

\frac{{[({\hat{p}}_{i}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}}) \times ({\hat{p}}_{i}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}})]}^{T} {(p_{b}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}})}_{\times}}{|({\hat{p}}_{i}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}}) \times ({\hat{p}}_{i}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}})| |p_{a}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}}|} \cdot \frac{\partial {\hat{p}}_{i}^{l_{t_{k}}}}{\partial δ x} .

(13)

平面点特征对应的观测方程由点到平面之间的距离来计算:

f_{i}^{p} (x_{b_{t_{k + 1}}}^{b_{t_{k}}}) = \frac{|{({\hat{p}}_{i}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}})}^{T} ((p_{a}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}}) \times (p_{a}^{l_{t_{k}}} - p_{c}^{l_{t_{k}}}))|}{|(p_{a}^{l_{t_{k}}} - p_{b^{}}^{l_{t_{k}}}) \times (p_{a_{}}^{l_{t_{k}}} - p_{c}^{l_{t_{k}}})|} .

(14)

平面点特征观测方程对状态的雅克比矩阵:

H_{t_{k}}^{p} = \frac{\partial f_{i}^{p}}{\partial {\hat{p}}_{i}^{t_{k}}} \cdot \frac{\partial {\hat{p}}_{i}^{l_{t_{k}}}}{\partial δ x} = \frac{{[({\hat{p}}_{i}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}}) \times ({\hat{p}}_{i}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}})]}^{T}}{|({\hat{p}}_{i}^{l_{t_{k}}} - p_{a}^{l_{t_{k}}}) \times ({\hat{p}}_{i}^{l_{t_{k}}} - p_{b}^{l_{t_{k}}})|} \cdot \frac{\partial {\hat{p}}_{i}^{l_{t_{k}}}}{\partial δ x},

(15)

其中 $\frac{\partial {\hat{p}}_{i}^{l_{t_{k}}}}{\partial δ x}$ 与边缘点特征对状态的雅克比矩阵是一样的，这里不再赘述.
点到直线的距离 $f_{i}^{e} (x_{b_{t_{k + 1}}}^{b_{t_{k}}})$ 和点到平面的距离 $f_{i}^{p} (x_{b_{t_{k + 1}}}^{b_{t_{k}}})$ 将被作为卡尔曼滤波中的观测量， $H_{t_{k}}^{e}$ 和 $H_{t_{k}}^{p}$ 描述了卡尔曼滤波的观测量与需要估计的状态之间的线性化关系，随后进行标准卡尔曼滤波即可.此外，由于预测是通过IMU进行的，故在高动态的状态下，激光雷达会产生较大的畸变，此时IMU可以提供一个大致的位姿估计，从而辅助激光雷达特征点之间的匹配.
3 LINS-GNSS后端优化
后端优化算法框架主要包括后端建图、坐标转换、因子图优化3个部分.后端建图是将特征点与周围点云图进行精确配准，以获得更精确的位姿.坐标转换是将后端建图输出的更精确的位置与GNSS定位结果进行坐标转换，都统一到同一坐标系下.因子图优化是对一个由后端建图输出的位姿与GNSS定位结果构建而成的位姿图进行优化.
3.1 后端建图
选取在时序上较为相近的一些时刻特征点构建对应的全局点云地图.通过优化当前时刻特征点与全局点云地图的特征点之间的位姿约束，可以精细化后端建图输出的载体位姿.此优化问题的初值为前端激光雷达与IMU迭代误差卡尔曼滤波紧耦合输出的载体位姿.
后端建图的过程中，也可以通过回环检测来进一步消除漂移，回环检测是通过迭代最近点算法匹配当前帧和之前的点云，添加新的空间约束，然后通过因子图来优化位姿图.
3.2 坐标转换
通过后端建图，可以获得当前时刻IMU坐标系相对于IMU初始坐标系（IMU0系）的坐标.GNSS接收机得到的是GNSS天线相位中心相对于WGS-84系的坐标值，给此坐标值赋予导航计算机时间戳即可获得有导航计算机时间戳的卫星导航定位结果.通过时间戳将后端建图输出的定位结果和卫星导航接收机输出的定位结果进行内插和外推，可以获得一系列同时刻的IMU在IMU0系下的位姿和GNSS在WGS-84系下的坐标，分别记作: $\{p_{b_{0}}^{b_{0}} ， R_{e}^{b_{0}} ， p_{b_{0}}^{b_{1}} ， R_{e}^{b_{1}} ， \dots ， p_{b_{0}}^{b_{n}} ， R_{e}^{b_{n}}\}$ 和 $\{p_{e}^{G_{0}} ， p_{e}^{G_{1}} ， p_{e}^{G_{2}} ， \dots ， p_{e}^{G_{n}}\}$ .
于是通过构建优化函数可以求出b₀系相对于WGS-84系的转换参数 $\{p_{e}^{b_{0}} ， R_{e}^{b_{0}}\}$ ^[10]:

(16)

式中， $m = p_{b_{0}}^{b_{i}} - R_{b_{0}}^{b_{i}} (R_{e}^{b_{0}} p_{e}^{G_{i}} + p_{e}^{b_{0}} - p_{b_{i}}^{b_{0}}) .$
上述问题可转化成一个迭代最近点（Iterative Closest Point，ICP）问题:

(17)

式中， $n = p_{b_{0}}^{b_{i}} + R_{b_{0}}^{b_{i}} p_{b_{i}}^{b_{0}} - (R_{e}^{b_{0}} p_{e}^{G_{i}} + R_{b_{0}}^{b_{i}} p_{e}^{b_{0}}) .$
通过解上述优化问题就能获得WGS84坐标系和IMU0系之间的坐标转换，此参数为后续因子图优化模型中的一个重要参数.
3.3 因子图优化模型
LINS-GNSS后端通过构建因子图优化模型对全体坐标进行优化.所构建的因子图模型如图3所示.其中GNSS数据是直接通过GNSS接收机输出获得的，在本文实验中，采用的是RTK（实时动态载波相位差分，Real-Time Kinematic）定位技术.GNSS测量因子内容为卫星导航定位结果，LINS位姿测量因子包括LINS所估计的局部位置和姿态解算结果.
这将定位问题抽象成了一个由节点、边组成的双射图，其中节点包含状态节点和测量节点两种.当状态与测量有关系时，它们之间就会有一条边存在.
残差方程包括两部分，r₁为GNSS位置与LINS位置的差:

\begin{matrix} r_{1} = r_{G N S S_LINS} = \\ R_{M}^{N} (R_{L}^{M} P_{G}^{L} + t_{L}^{M} - t_{N}^{M}) - P_{G}^{N}, \end{matrix}

(18)

r为LINS先验位姿残差:

r_{2} = r_{LINS_prior} = [\begin{matrix} R_{M, i}^{L} (t_{L, j}^{M} - t_{L, i}^{M}) \\ l o g {(R_{M, i}^{L} R_{L, j}^{M})}^{\lor} \end{matrix}],

(19)

图3 LINS-GNSS后端位姿图模型
Fig.3 Pose graph model in the back-end of LINS-GNSS
其中 $R_{M}^{N}$ 和 $t_{N}^{M}$ 为当地东北天坐标系与LINS坐标系之间的转换参数， $P_{G}^{N}$ 为GNSS定位结果在当地东北天坐标系下的结果， $P_{G}^{L}$ 为GNSS天线相位中心在激光雷达坐标系下的位置， $t_{L}^{M}$ 为LINS输出的位置结果， $R_{M ， i}^{L}$ 为i时刻的LINS的姿态输出， $t_{L ， j}^{M}$ 为j时刻LINS的位置输出， $t_{L ， i}^{M}$ 为i时刻LINS的位置输出.
4 实验结果及分析
为验证LINS-GNSS算法在变电站环境中的性能，本文将搭载了16线激光雷达、9轴IMU和GNSS接收机的巡检机器人在变电站中进行导航定位实验，并将LINS-GNSS的定位结果与RTK/INS结果（即真值）作对比.同时LIO-SAM在同一份数据上也进行了定位实验，其定位所得轨迹作为对比实验结果.实验环境以及对应数据采集如图4所示.
图4 变电站环境以及数据采集载体
Fig.4 Substation environment and data collection carrier
实验环境经过三维激光建模以后获得的结果如图5所示.
从图5可见电线杆是会被激光雷达扫描出来的，并且整个环境也还算开阔，GNSS接收机可以提供一个较为准确的值.而在运动激烈时，IMU的输入可以提供一个位姿初值从而辅助LiDAR进行点云的匹配.综上所述，在如图4所示的变电站环境中，GNSS/INS/LiDAR是一个很好的组合导航方案.但是GNSS会受到电磁干扰的影响，本文在电网环境中进行了静态RTK测试，发现RTK定位结果和真实坐标的水平定位中误差为8 mm，高程定位中误差15 mm.这一误差水平对RTK来说是比较大的，其中部分误差为电磁干扰所带来的影响.为此在同一位置电网断电情况下进行对比实验，最终发现RTK定位结果和真实坐标的水平定位中误差为3 mm，高程定位中误差6 mm，显著低于通电情况.
图5 变电站环境三维点云地图
Fig.5 3D point cloud map of substation environment
4.1 实验设置
实验使用激光雷达为Velodyne16，IMU为Xsens Mti300，用于LINS-GNSS系统的卫星导航接收机为u-blox ZED-F9P，用作定位真值的设备为Novatel的SPAN-CPT组合导航系统，在采集真值数据时，SPAN-CPT设备中配置了RTK.
4.2 标定实验
在LINS-GNSS算法中，IMU的噪声参数以及IMU和LiDAR之间的外参是比较重要的参数，需要进行事先标定.
本节将会给出本文所用设备的IMU噪声参数标定结果和LiDAR/IMU外参标定结果.IMU的误差分为确定性误差与随机误差，确定性误差比如零偏一般在SLAM系统会作为一个重要参数进行估计，因而在标定实验中只需对IMU的随机误差进行标定，包括加速度与角速度的噪声误差与随机游走，这两个参数在本文中也被称作IMU噪声参数.本文所标定的IMU噪声参数如表1所示.IMU内参通过Allan方差法进行标定.
表1 IMU噪声参数
Table1 IMU noise parameters
4.2.1 IMU噪声参数标定结果
通过将实验设备静止2 h，并使用Allan方差对实验数据进行处理，得到Allan方差曲线如图6—7所示.
图6 实验所用加速度计的Allan方差曲线
Fig.6 Allan variance curves of the accelerometer used in the experiment
图6中的acc-x、acc-y、acc-z分别表示加速度计的x轴、y轴和z轴.图7中的gyr-x、gyr-y、gyr-z分别表示陀螺仪的x轴、y轴和z轴.根据图6和图7可以得到如表2所示的IMU噪声参数标定结果.
4.2.2 LiDAR/IMU外参标定
本文使用LI-calib工具箱^[15]对LiDAR和IMU进行外参标定，外参标定的目的是用于校准LiDAR和IMU之间的6个自由度（6 Degrees of Freedom，6DoF）的刚体变换参数.LI-calib的流程如图8所示，它在连续时间批量优化框架中利用来自LiDAR和IMU传感器的所有原始测量值进行标定.流程主要分为4个步骤:外部旋转初始化、数据关联、批次优化和迭代校准.
图7 实验所用陀螺仪的Allan方差曲线
Fig.7 Allan variance curves of the gyroscope used in the experiment
表2 IMU噪声参数标定结果
Table2 IMU noise parameters calibration results
首先将来自LiDAR和IMU旋转序列对齐来初始化外部旋转，其中LiDAR的旋转是从基于正态分布变换（NDT）配准的LiDAR里程计获得的.给定来自IMU传感器的原始角速度测量值可以拟合旋转B样条曲线.
初始化后，能够部分消除LiDAR扫描中的运动失真，并能够从LiDAR测距中获得更好的LiDAR姿态估计.使用LiDAR姿态初始化LiDAR面元地图，再初始化点对面元的对应关系.
图8 LI-calib流程
Fig.8 LI-calib process
批处理优化是使用LiDAR和IMU量测将标定问题转化成基于图的优化问题，并假设所有测量结果均具有独立的高斯噪声.
最后利用优化中当前的最佳估计状态更新面元图，点对平面数据关联，并迭代地优化估计状态.通过连续时间批次优化，状态估计变得更加精确.
通过采集一小段运动充分的数据，即可获得LiDAR/IMU外参标定结果.为了使得结果具备可信度，总共采集5组数据并进行实验，实验的过程如图9所示，其中彩色的为程序所提取的面特征，白色的线条为提取的线特征.
图9 LiDAR/IMU外参标定过程
Fig.9 LiDAR/IMU extrinsic calibration process
在外参估计实验中，估计所得的外参稳定性非常重要，而内符合精度以估计的最似然估值为比对基准，主要反映离散度，故可以很好地用来作为评判稳定性的指标.对同一个LiDAR/IMU设备进行5次外参标定实验，然后统计5个结果的均值和标准差来反映内符合精度.结果如表3所示，数据格式为:均值±标准差.
表3 LiDAR/IMU外参标定结果
Table3 LiDAR/IMU extrinsic calibration results
4.3 定位实验
通过对比LINS-GNSS的结果和RTK/INS的结果，可以获得如图10和表4所示的结果.图10左边为LINS-GNSS估计轨迹与真值轨迹对比，轨迹为在ENU坐标系下的轨迹，即图中的X轴对应东方向，Y轴对应北方向，Z轴对应高程方向，图10右边为LINS-GNSS三维误差序列.表4为LINS-GNSS的3D误差各项统计指标.
表4 LINS-GNSS定位结果误差统计
Table4 Error statistics of LINS-GNSS positioning results
表4中:max误差表示误差的最大值，可以反映定位的鲁棒性; mean表示误差的均值，median表示误差的中位数，rmse表示均方根误差，这三类误差一般用来反映定位的精度; std表示误差的标准差，可以反映定位的稳定性.从表4可以看出，在变电站环境中，LINS-GNSS的算法能很好地获得定位结果，其定位最大误差在0.5 m以内，定位误差的均方差为0.211 m，满足大部分变电站内机器人的定位需求.
图10 LINS-GNSS轨迹对比和误差分析
Fig.10 LINS-GNSS trajectory comparison and error analysis
LIO-SAM在这个实验中为对比实验组，其定位结果的统计值如表5所示.
表5 LIO-SAM定位结果误差统计
Table5 Error statistics of LIO-SAM positioning results
对比表4和表5可以看出，本文所提的LINS-GNSS算法，比最先进的LIO-SAM算法在变电站环境下定位性能更优.
LINS-GNSS算法和LIO-SAM算法的ENU三轴误差和统计结果如图11和表6所示.
图11 LINS-GNSS和LIO-SAM东北天三轴误差分量序列
Fig.11 Sequences of triaxial error components in ENU of LINS-GNSS and LIO-SAM
表6 LINS-GNSS和LIO-SAM三轴RMSE统计
Table6 LINS-GNSS and LIO-SAM triaxial RMSE statistics
从图11和表6中可以看出，LINS-GNSS的三轴误差均优于LIO-SAM，验证了本文所提算法的有效性.
5 结论与未来展望
本文提出在变电站环境中融合GNSS/INS/LiDAR的组合导航定位算法——LINS-GNSS，并在真实变电站环境中进行测试.LINS-GNSS算法架构的创新点在于，其前端使用卡尔曼滤波紧耦合了LiDAR和IMU，后端用因子图优化松耦合了GNSS，具有松紧交替、滤波优化交替的特性.
为了使LINS-GNSS性能更好，本文事先对IMU的噪声参数以及LiDAR/IMU的外参进行了标定.最终结果表明，LINS-GNSS在本文实验所处变电站环境中可以达到最大误差0.5 m以内，均方根误差0.211 m的定位精度，比现有的最优GNSS/ INS/LiDAR组合导航算法LIO-SAM精度更高，充分证明了LINS-GNSS的有效性.
目前LiDAR和IMU是基于卡尔曼滤波的紧耦合，GNSS是基于优化的松耦合，未来可以将GNSS原始信息融入紧耦合滤波框架.目前GNSS通过RTK获取的定位结果，未来可以使用PPP或者PPP-RTK等GNSS定位技术.
参考文献
- [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].北京:电子工业出版社,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] 鄂盛龙,周刚,谭理庆,等.变电站环境下GNSS接收机性能及观测数据质量分析[J].全球定位系统,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,France.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

基本信息

中图分类号: V249.3
文献标识码: A
DOI: 10.13878/j.cnki.jnuist.2023.01.009

基金信息

南方电网有限责任公司科技项目(YNKJXM20191246)；
国家自然科学基金(61873163)；
上海市科技创新行动计划项目(20511103103)；

引用信息

稿件历史

收稿日期: 2022-01-05

参考文献

[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].北京:电子工业出版社,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] 鄂盛龙,周刚,谭理庆,等.变电站环境下GNSS接收机性能及观测数据质量分析[J].全球定位系统,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,France.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

Address：No. 219, Ningliu Road, Nanjing, Jiangsu Province

Postcode：210044

Phone：025-58731025

分享给微信好友或者朋友圈

使用微信“扫一扫”功能。

LINS-GNSS:滤波与优化耦合的GNSS/INS/LiDAR巡检机器人定位方法

LINS-GNSS:filter and optimization coupled GNSS/INS/LiDAR positioning method for inspection robot localization

摘要

Abstract

关键词

Keywords

0 引言

1 LINS-GNSS算法总体架构

2 LINS-GNSS前端算法

2.1 点云分割

2.2 特征提取

2.3 状态传递

(1)

(2)

(3)

(4)

(5)

(6)

(7)

(8)

2.4 状态更新

(9)

(10)

(11)

(12)

(13)

(14)

(15)

3 LINS-GNSS后端优化

3.1 后端建图

3.2 坐标转换

(16)

(17)

3.3 因子图优化模型

(18)

(19)

4 实验结果及分析

4.1 实验设置

4.2 标定实验

4.2.1 IMU噪声参数标定结果

4.2.2 LiDAR/IMU外参标定

4.3 定位实验

5 结论与未来展望

参考文献

基本信息

基金信息

引用信息

稿件历史

参考文献