一种多传感器信息融合的自主导航方法及系统技术方案

本发明专利技术公开了一种多传感器信息融合的自主导航方法及系统，包括：初步筛选

【技术实现步骤摘要】
一种多传感器信息融合的自主导航方法及系统


[0001]本专利技术属于融合导航
，具体涉及一种多传感器信息融合的自主导航方法及系统
。

技术介绍

[0002]导航定位技术在人类的社会
、
经济和军事活动中越来越发挥着重要的支撑作用
。
随着自动驾驶
、
无人机和机器人等技术的兴起，高精度
、
高可靠性
、
高可用性的低成本导航定位需求得到空前增长
。
同时定位与建图（
Simultaneous Localization and Mapping, SLAM
）所关注的问题是载有传感器的机器人在未知的环境中定位并构建环境地图，是机器人感知自身状态和外部环境的关键技术，通常采用视觉
、
激光等环境传感器，并结合捷联惯导（
SINS
，
Strapdown Inertial Navigation System
）进行相对定位与建立地图
。
在移动机器人实现自动化操作时，先决条件便是准确地估计自身的位置和姿态，并感知所处环境
。
在准确地感知自身位姿和环境后，机器人才能进行自主决策
、
规划和控制，完成自动化操作
。
然而，
SLAM
递推不可避免地造成了误差累积，使其无法在大规模
、
长距离场景下给出可靠的位姿与地图
。
为解决这一问题，一方面可以检测轨迹闭环，通过回环约束消除部分累积误差；另一方面则通过在开阔环境融合全球导航卫星系统（
Global Navigation Satellite System, GNSS
）的绝对观测信息，消除累积误差，提高定位定姿与建图精度，进而提升已建图区域的定位定姿精度
。
[0003]多源融合导航定位与建图方法多基于因子图优化设计，其采用预积分方法提供历元间导航参数的变化约束，并将其余传感器观测因子化，通过因子图优化解算导航参数，利用估计的位姿将点云投影至世界坐标系完成建图
。
该方法较传统的滤波方案有精度高
、
可扩展性高等特点，是目前导航定位与建图的主流方法
。
[0004]目前，基于因子图优化的多源融合导航定位方法仍存在以下问题：1）现有方法仅融合视觉或激光单个环境传感器，尚未构建统一的多源融合导航定位因子图模型；2）可分布式部署；或现有的分布式节点方法依赖于主节点，容灾能力差；3）现有方法多采用传统预积分方法，未考虑地球自转
、
重力变化带来的影响，无法发挥高精度惯性导航器件应有的价值；4）现有方法中激光雷达多采用传统的点到点匹配方式，无法估算协方差，其观测难以融合
。

技术实现思路

[0005]针对以上问题，本专利技术给出了一种多传感器信息融合的自主导航方法，通过分布式进程设计，融合视觉
、
激光
、
惯导
、GNSS
观测信息与已建图区域的匹配定位信息，获取载体高精度位置与姿态，实现根据任务需求灵活部署，且无需人工干预，实现载体自主导航
。
[0006]为达到上述目的，本专利技术方法的技术方案为：
一种多传感器信息融合的自主导航方法，包括以下步骤：步骤
S1
：接收
GNSS、SINS、
视觉和激光雷达原始观测数据并进行初步筛选；步骤
S2
：接收初步筛选后的
GNSS、SINS、
视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将
SINS
观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；步骤
S3
：接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹
、
点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；步骤
S4
：接收步骤
S3
中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿
、
点云及方差信息
。
[0007]进一步地，所述步骤
S1
中设置4个独立的线程并行处理不同传感器，包括
GNSS
线程
、SINS
线程
、
视觉线程和激光雷达线程；其中，
GNSS
线程接收
GNSS
原始观测数据，包括伪距
、
相位和多普勒，并采用实时动态
RTK
定位模型建模观测值，选取不同卫星导航系统高度角最高的卫星为基准，剔除可视卫星少于4的卫星导航系统；并通过卫星
PRN
号匹配基准站和流动站之间的共视卫星；
SINS
线程根据视觉
/
激光相邻两次观测选取积分区间，匹配区间内的所有
SINS
观测值存入队列；视觉线程通过
ORB
角点提取算法提取影像中的特征信息，通过光流法匹配方法匹配不同影像间的同名特征点，当可用特征点少于阈值时，重新提取图像中
ORB
角点；激光雷达线程从原始激光雷达点云中提取平面和边缘点，其中原始点云中低曲率和高曲率的点分别对应于数据环境中的平面和线，随后将其参数化；通过计算不同帧间的线
‑
线距离与面
‑
面距离匹配同名线
/
面特征；上述激光点云曲率表示如下：
[0008]式中，为该点在统一扫描线的前后五个点的集合，；
、
分别为集合中点
i
和点
j
的深度，若
c
大于阈值则为线特征，反之则为面特征
。
[0009]进一步地，提取激光雷达点云中线和面的步骤如下：对于属于第
i
条扫描线的点，首先确定在扫描线中的最近点，然后利用和计算一条线，并在条扫描线中找到的最近点，若与线之间的距离小于给定阈值，则使用，和拟合新线，这个过程重复进行，直到不能添加更多的点为止，其中，
L
为所有线属性点的集合；
[0010]在提取平面时，从第一条扫描线开始，首先找出在第
i
条扫描线中的每个片段
P
中的点，其
k
个最近的点位于
P本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.
一种多传感器信息融合的自主导航方法，其特征在于，包括以下步骤：步骤
S1
：接收
GNSS、SINS、
视觉和激光雷达原始观测数据并进行初步筛选；步骤
S2
：接收初步筛选后的
GNSS、SINS、
视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将
SINS
观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；步骤
S3
：接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹
、
点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；步骤
S4
：接收步骤
S3
中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿
、
点云及方差信息
。2.
根据权利要求1所述的一种多传感器信息融合的自主导航方法，其特征在于，所述步骤
S1
中设置4个独立的线程并行处理不同传感器，包括
GNSS
线程
、SINS
线程
、
视觉线程和激光雷达线程；其中，
GNSS
线程接收
GNSS
原始观测数据，包括伪距
、
相位和多普勒，并采用实时动态
RTK
定位模型建模观测值，选取不同卫星导航系统高度角最高的卫星为基准，剔除可视卫星少于4的卫星导航系统；并通过卫星
PRN
号匹配基准站和流动站之间的共视卫星；
SINS
线程根据视觉
/
激光相邻两次观测选取积分区间，匹配区间内的所有
SINS
观测值存入队列；视觉线程通过
ORB
角点提取算法提取影像中的特征信息，通过光流法匹配方法匹配不同影像间的同名特征点，当可用特征点少于阈值时，重新提取图像中
ORB
角点；激光雷达线程从原始激光雷达点云中提取平面和边缘点，其中原始点云中低曲率和高曲率的点分别对应于数据环境中的平面和线，随后将其参数化；通过计算不同帧间的线
‑
线距离与面
‑
面距离匹配同名线
/
面特征；上述激光点云曲率表示如下：式中，为该点在统一扫描线的前后五个点的集合，；
、
分别为集合中点
i
和点
j
的深度，若
c
大于阈值则为线特征，反之则为面特征
。3.
根据权利要求2所述的一种多传感器信息融合的自主导航方法，其特征在于，提取激光雷达点云中线和面的步骤如下：对于属于第
i
条扫描线的点，首先确定在扫描线中的最近点，然后利用和计算一条线，并在条扫描线中找到的最近点，若与线之间的距离小于给定阈值，则使用，和拟合新线，这个过程重复进行，直到不能添加更多的点为止，其中，
L
为所有线属性点的集合；
在提取平面时，从第一条扫描线开始，首先找出在第
i
条扫描线中的每个片段
P
中的点，其
k
个最近的点位于
P
中但不在第
i
条扫描线中，然后，使用
SVD
算法将这些点拟合到
P
中的一个平面上，接着，该平面可以由其方向向量
n
表示，该向量是与
A
的最小特征值相关联的特征向量，计算所有点到平面的距离之和，并排除大于设定阈值的平面
。4.
根据权利要求1所述的一种多传感器信息融合的自主导航方法，其特征在于，所述步骤
S2
包括如下子步骤：步骤
S2.1
：并行接收初步筛选后的
GNSS、SINS、
视觉和激光雷达观测数据；步骤
S2.2
：通过插值方法将
SINS
观测数据对齐至区间起止位置，并通过顾及地球自转和区域重力变化的预积分方法对观测值积分，得到历元间相对位姿，得到
SINS
解算信息；步骤
S2.3
：利用历元间相对位姿，将前一帧激光点云中的线面投影至当前帧，通过设定阈值判断线到线的距离与面到面的距离，并剔除大于阈值的线或面，得到精化后的激光雷达观测值，即激光雷达解算信息；步骤
S2.4
：通过随机采样一致方法
RANSAC
计算基础矩阵和单应矩阵，以剔除视觉观测中的错误匹配，得到精化后的视觉匹配观测值，即视觉解算信息；步骤
S2.5
：将高度角低于设置阈值的卫星观测值剔除，得到精化后的共视观测卫星，即
GNSS
解算信息
。5.
根据权利要求4所述的一种多传感器信息融合的自主导航方法，其特征在于，所述步骤
S2.2
具体实施步骤如下：步骤
S2.21
：根据低频观测传感器采样时间确定积分间隔，设定低频观测传感器相邻两次观测时间分别为和，在此间隔内有
N
个
SINS
量测，即积分所需的观测值，积分区间即为；步骤
S2.22
：采用考虑地球自转的姿态双子样更新算法积分陀螺输出；进一步计算科里奥利力补偿的速度
、
位置在积分区间内的增量；步骤
S2.223
：通过预积分误差模型传播不确定度，为预积分观测值在多传感器融合中提供权值；设定时刻的姿态预积分为，则时刻的姿态预积分为：世界坐标系的姿态增量约束为：式中，表示相对惯性系固定的历史世界系，即历史的
e
系；表示
IMU
...

【专利技术属性】
技术研发人员：朱锋，许卓，张小红，
申请(专利权)人：武汉大学，
类型：发明
国别省市：