CN116989763A - 一种面向水陆两用无人***的融合定位与建图方法 - Google Patents
一种面向水陆两用无人***的融合定位与建图方法 Download PDFInfo
- Publication number
- CN116989763A CN116989763A CN202310520140.0A CN202310520140A CN116989763A CN 116989763 A CN116989763 A CN 116989763A CN 202310520140 A CN202310520140 A CN 202310520140A CN 116989763 A CN116989763 A CN 116989763A
- Authority
- CN
- China
- Prior art keywords
- data
- fusion
- gnss
- factor
- odometer
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 68
- 238000000034 method Methods 0.000 title claims abstract description 53
- 238000013507 mapping Methods 0.000 title claims abstract description 38
- 238000005457 optimization Methods 0.000 claims abstract description 40
- 238000005259 measurement Methods 0.000 claims abstract description 22
- 230000008878 coupling Effects 0.000 claims abstract description 17
- 238000010168 coupling process Methods 0.000 claims abstract description 17
- 238000005859 coupling reaction Methods 0.000 claims abstract description 17
- 230000000007 visual effect Effects 0.000 claims abstract description 15
- 238000012216 screening Methods 0.000 claims abstract description 13
- 238000001514 detection method Methods 0.000 claims description 33
- 230000006870 function Effects 0.000 claims description 15
- 238000007500 overflow downdraw method Methods 0.000 claims description 9
- 230000010354 integration Effects 0.000 claims description 8
- 239000011159 matrix material Substances 0.000 claims description 7
- 230000011218 segmentation Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 claims description 4
- 230000009467 reduction Effects 0.000 claims description 4
- 230000009012 visual motion Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 12
- 238000009826 distribution Methods 0.000 description 7
- 230000009466 transformation Effects 0.000 description 6
- 230000033001 locomotion Effects 0.000 description 5
- 238000010276 construction Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000015556 catabolic process Effects 0.000 description 3
- 238000006731 degradation reaction Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000003860 storage Methods 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 230000007774 longterm Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 241001270131 Agaricus moelleri Species 0.000 description 1
- 208000006440 Open Bite Diseases 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种面向水陆两用无人***的融合定位与建图方法，该方法包括获取GNSS数据并进行卫星筛选，得到过滤后的GNSS数据；获取视觉惯性数据并将视觉惯性数据与过滤后的GNSS数据进行紧耦合融合，得到全局位置；将激光雷达与惯性测量单元通过误差状态卡尔曼滤波器进行融合，得到融合里程计；获取融合里程计数据并结合GNSS数据进行二阶段因子图优化，得到三维点云地图。通过使用本发明，能够实现无人***在水陆两栖环境中的高精度定位和建图。本发明作为一种面向水陆两用无人***的融合定位与建图方法，可广泛应用于定位与建图的技术领域。
Description
技术领域
本发明涉及定位与建图的技术领域，尤其涉及一种面向水陆两用无人***的融合定位与建图方法。
背景技术
水陆两用无人***所应用的两栖环境作为一种复杂的多介质地形环境，其不仅包含单一的水、陆地形，还包括水陆交界地带，而在此类交界地带运行时，无人***常常面临岸边遮挡物影响定位精度、水陆切换不平滑等问题。水陆两用无人***在水陆交替环境下如何利用自身搭载的传感器在运动过程中实现对周围环境的实时定位与建图对于***在此类复杂环境下能否进行自主导航具有极其重要的意义。近年来，研究人员不断地对实时定位与建图技术进行改进，但应用场景大多针对陆地场景或某一水面环境等单一场景，而如何在水陆切换的多介质地形环境下实现鲁棒、高效的定位与建图仍存在很多技术空白。
因此，亟需研究一种面向水陆两栖环境的水陆两用无人***融合定位与建图方法。
发明内容
为了解决上述技术问题，本发明的目的是提供一种面向水陆两用无人***的融合定位与建图方法，实现无人***在水陆两栖环境中的高精度定位和建图。
本发明所采用的技术方案是：一种面向水陆两用无人***的融合定位与建图方法，包括以下步骤：
获取GNSS数据并进行卫星筛选，得到过滤后的GNSS数据；
获取视觉惯性数据并将视觉惯性数据与过滤后的GNSS数据进行紧耦合融合，得到全局定位信息；
将激光雷达与惯性测量单元通过误差状态卡尔曼滤波器进行融合，得到融合里程计；
结合全局定位信息和融合里程计的局部定位信息进行闭环检测，并对GNSS数据和融合里程计进行二阶段因子图优化，得到三维点云地图。
基于技术方案的一种可能实现的方式中，还包括：
基于流形上迭代误差状态卡尔曼的紧耦合融合方法对无人***进行位姿估计。
在该实现方式中，针对各个传感器差异性较大、优化时非线性因素影响***稳定性的问题，本实施例开发出一种基于流形上迭代误差状态卡尔曼的紧耦合融合方法，实现轻量、高效、多源融合的里程计，为控制***提供合适的地图信息和平滑的位姿信息。
基于技术方案的一种可能实现的方式中，所述获取GNSS数据并进行卫星筛选，得到过滤后的GNSS数据这一步骤，其具体包括：
获取GNSS数据并对GNSS数据进行降维处理，得到预处理后的GNSS数据；
获取仰视图像并对仰视图像进行分割处理，得到分割后的仰视图像；
将预处理后的GNSS数据投影到分割后的仰视图像，并利用分割好的仰视图像对照星历图像进行可见性筛选，得到过滤后的GNSS数据。
在该实现方式中，针对由于GNSS信号的多径效应引起的精度下降和GNSS失锁等问题，提出了一种自适应的卫星筛选方法，通过利用仰视图像、星历和GNSS卫星信号进行卫星可用性筛选。
基于技术方案的一种可能实现的方式中，所述获取仰视图像并对仰视图像进行分割处理，得到分割后的仰视图像这一步骤，其具体包括：
获取仰视图像；
设定初始全局阈值并基于初始全局阈值对仰视图像进行分割，得到初步分割图像；
计算初步分割图像的灰度平均值并作为新全局阈值，直至新全局阈值小于预设最小阈值，得到最终阈值；
基于最终阈值对仰视图像进行遮挡区域和未遮挡区域的分割，得到分割后的仰视图像。
在该实现方式中，为了对采集到的仰视图像进行分割处理，研究了一种自适应阈值的全局阈值分割方法，以获取水陆两用无人***仰视遮挡区域和未遮挡区域。
基于技术方案的一种可能实现的方式中，所述获取视觉惯性数据并将视觉惯性数据与过滤后的GNSS数据进行紧耦合融合，得到全局定位信息这一步骤，其具体包括：
获取惯性测量单元的测量数据并进行预积分，得到预积分结果；
获取相机的拍摄数据并进行纯视觉运动重建，得到重建结果；
基于预积分结果和重建结果进行视觉-惯性对齐并初始化，得到视觉惯性数据；
输入至滑窗优化器进行因子图优化，生成全局一致轨迹，得到全局定位信息。
该实现方式中，充分利用了相机和惯性测量单元的测量数据，提高了***定位的准确性。
基于技术方案的一种可能实现的方式中，所述滑窗优化器的表达式如下：
其中，表示集合/>的最优估计，/>表示k时刻滑窗内所有关键帧的集合，表示第i时刻与第j时刻之间视觉-惯性里程计的相对约束，/>表示第i时刻GNSS的相对约束，/>表示滑窗中第一个状态量的先验约束，∑VIo、∑GNSS、∑0分别表示各约束的协方差矩阵。
该实现方式中，针对GNSS和视觉惯性***存在精度数量级不匹配的问题，采用上述公式构建滑窗优化器，通过滑窗优化器，进行因子图优化，以获取全局一致性高的地图。
基于技术方案的一种可能实现的方式中，所述结合全局定位信息和融合里程计的局部定位信息进行闭环检测，并对GNSS数据和融合里程计进行二阶段因子图优化，得到三维点云地图这一步骤，其具体包括：
根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子并添加至因子图进行第一阶段二次因子图优化，得到初步地图；
结合全局信息和局部信息进行闭环检测并作为约束加入到初步地图，重置因子图进行第二阶段的二次因子图优化，得到三维点云地图。
该实现方式中，针对GNSS信号可能出现错误而导致点云地图出现畸变和退化的问题，设计出一种可大幅度减少GNSS离群值带来的影响的二阶段优化策略。
基于技术方案的一种可能实现的方式中，所述根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子，并添加至因子图进行第一阶段二次因子图优化，得到初步地图这一步骤，其具体包括：
根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子；
将融合里程计因子和GNSS因子添加至因子图，并选用柯西鲁棒核函数对第一次优化进行约束并进行卡方检测，得到第一次优化结果；
删除柯西鲁棒核函数后对第一次优化结果进行第一阶段第二次因子图优化，得到初步地图。
该实现方式中，为了最大程度上剔除离群值，选用柯西鲁棒核函数对第一阶段第一次优化进行约束并进行卡方检测。
基于技术方案的一种可能实现的方式中，所述引入闭环检测作为约束这一步骤，其具体包括：
对候选闭环进行伪闭环检测；
将融合里程计数据中的激光雷达数据、毫米波雷达数据和声呐数据映射为2D图像，并在2D图像上进行闭环检测，得到粗略位置；
通过固定因子递进调高2D图像的分辨率，在多张分辨率图像下进行闭环检测，对粗略位置进行修正，得到正确闭环；
将正确闭环作为约束加入到第二阶段的二次因子图优化中。
该实现方式中，针对长时间建图累计误差大的问题，提出了一种基于位置的多分辨率闭环检测方法，且相较于传统的基于外观的闭环检测算法，本发明提出的方法更易于实现且运行良好。
本发明方法有益效果是：本发明的视觉-卫导-惯导的全局定位方法可以提高全局定位的精度和稳定性，为水陆两用无人***在水陆介质环境下的自主导航、地图构建提供可靠保障；通过面向水陆交替场景的稳定建图方法可以提高点云地图的全局一致性和精度。
附图说明
图1是本发明一种面向水陆两用无人***的融合定位与建图方法的整体框架图
图2是本发明一种面向水陆两用无人***的融合定位与建图方法的步骤流程图；
图3是本发明具体实施例卫星可行性筛选的框架图；
图4是本发明具体实施例紧耦合融合的框架图；
图5是本发明具体实施例因子图优化示意图；
图6是本发明具体实施例多源融合里程计的框架图；
图7是本发明具体实施例多分辨率交叉正态分布点云配准方法的框架图；
图8是本发明具体实施例面向水陆交替场景的稳定建图方法的框架图；
图9是本发明具体实施例多分辨率闭环检测方法的示意图。
具体实施方式
下面结合附图和具体实施例对本发明做进一步的详细说明。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。
针对复杂的水陆两栖环境，亟需提高水陆两用机器人定位与建图的准确性、实时性和长期工作的稳定性。如图1所示，本发明通过设计基于视觉-卫导-惯导的全局定位方法、基于测距传感器的多分辨率地图紧耦合里程计以及面向水陆交替场景的稳定建图方法，达到提高水陆两用机器人在水陆两栖环境下定位与建图的鲁棒性和平滑性的目的。
如图2所示，本发明提供了一种面向水陆两用无人***的融合定位与建图方法，该方法包括以下步骤：
S1、基于视觉-卫导-惯导的全局定位方法；
针对水陆介质环境下GNSS由于多径效应、地形遮挡等因素造成其信号不佳或缺失以及长期运行导致累积误差无法消除的问题，本实施例提出一种基于视觉-卫导-惯导的全局定位方法，确保水陆两用无人***全局定位的精度。
S1.1、参照图3，卫星可行性筛选；
S1.1.1、获取GNSS数据并对GNSS数据进行降维处理，得到预处理后的GNSS数据；
对无人***获取的三维卫星数据，进行外部校准和偏航对准，为了能够和仰视分割后的图像进行匹配，对校准后的三维卫星数据进行降维，并进行本征校准和畸变化。
S1.1.2、获取仰视图像并对仰视图像进行分割处理，得到分割后的仰视图像；
为了对采集到的仰视图像进行分割处理，本实施例研究一种自适应阈值的全局阈值分割方法，在获取仰视图像后，首先为全局阈值选取初始值T(一般选平均灰度)，然后利用该阈值对图像进行分割，计算分割后两张图像的平均灰度m1和m2，并计算两个灰度的平均值作为新的全局阈值直到T小于设定的最小阈值dT，以获取水陆两用无人***仰视遮挡区域和未遮挡区域。
S1.1.3、将预处理后的GNSS数据投影到分割后的仰视图像，并利用分割好的仰视图像对照星历图像进行可见性筛选，得到过滤后的GNSS数据。
利用机载GNSS传感器和仰视摄像头的外参矩阵，将星历投影到分割好的仰视图像中，利用分割好的仰视图像对照星历图像进行可见性筛选，过滤掉遮挡的卫星，避免GNSS因遮挡导致的多径效应带来的影响，完成卫星直接视距信号和非视距信号的分离。
S1.2、参照图4，获取视觉惯性数据并将视觉惯性数据与过滤后的GNSS数据进行紧耦合融合，得到全局定位信息；
在概率的框架下将视觉惯性数据与GNSS观测数据进行紧耦合融合，一方面，对相机图像利用纯视觉运动重建(Structure from motion，简称SfM)方法进行处理，并与惯性测量单元的预积分结果进行视觉-惯性对齐，在利用视觉-惯性对齐完成初始化后对三维卫星坐标进行偏航对准，以便于完成卫星的可见性分析，这一过程充分利用了相机和惯性测量单元的测量数据，提高了***的准确性。同时，相机数据经过预处理后，与惯性测量单元数据、过滤后的GNSS数据一同输入滑窗优化器，进行因子图优化，以获取全局一致性高的地图。其中因子图优化示意图如图5所示，其中L1、L2指视觉特征点的逆深度。
针对GNSS和视觉惯性***存在精度数量级不匹配的问题，采用如下公式构建滑窗优化器：
其中，表示集合/>的最优估计；/>表示k时刻滑窗内所有关键帧的集合；表示第i时刻与第j时刻之间视觉-惯性里程计的相对约束；/>表示第i时刻GNSS的相对约束；/>表示滑窗中第一个状态量的先验约束；ΣVI、∑GNSS、∑0分别表示各约束的协方差矩阵。
S2、基于流形上迭代误差状态卡尔曼的紧耦合融合方法对无人***进行位姿估计；
针对各个传感器差异性较大、优化时非线性因素影响***稳定性的问题，本实施例开发出一种基于流形上迭代误差状态卡尔曼的紧耦合融合方法，实现轻量、高效、多源融合的里程计。基于流形上迭代误差卡尔曼主要通过对传感器数据进行预测与更新来实现融合，惯性测量单元预积分得到的是预测量，激光雷达、毫米波雷达、声呐三个测距传感器之间进行多分辨率交叉正态分布变换点云配准得到的是观测量，通过预测量、观测量、以及约束代入基于流形上迭代误差卡尔曼中，通过更新就可以得到当前状态值。
S2.1、利用惯性测量单元的高频特性，通过预积分快速获取***的预测值，作为***的先验概率。
S2.2、参照图7，将激光雷达、毫米波雷达、声呐三个测距传感器之间进行多分辨率交叉正态分布变换点云配准。
S2.2.1、将参考点云根据实际情况划分为指定大小的网格单元或体素。本实施例根据传感器的精度不同分别设置不同大小的激光雷达、毫米波雷达、声呐体素。
S2.2.2、假设参考点云中每个最小格子的点云都满足高斯分布，故可根据下式计算参考曲面所属点集的均值向量μ与协方差矩阵∑：
式中m为网格单元中数据点的个数，k表示一个索引值，k∈[1:m]，xk表示m个数据点中第k个位置点。
S2.2.3、当前传感器进行配准时，通过位姿变换矩阵将当前测距传感器数据帧PB投影到世界坐标系，与另外两个测距传感器对应的地图数据进行正态分布点云配准，投影公式如下所示：
S2.2.4、使当前扫描的点云以最大可能性匹配于第一步构建的参考正态分布点云模型，找到最佳的当前扫描位姿。
假设当前扫描的点云X＝{x1,…,xn}，参考扫描的曲面分布为D维正态分布，那么参考点云的概率密度函数为则使如下似然函数式取值最大的位姿变量即为最佳位姿T：
或者等效地，使如下负对数似然函数式取值最小的位姿变量T：
式中，fmw(T,xk)表示位姿T对第k个位置点xk进行空间变换的变换函数，而f(fmw(T,xk))表示点xk经T变换后所得位置点的概率密度函数，log(f(fmw(T,xk)))表示对f(fmw(T,xk))取对数。
S2.2.5、-logL的取值越小表明点集X中的点通过T变换到参考坐标系中后，该点位于参考扫描的曲面上的可能性越大。故根据牛顿迭代算法求解出
从而得到使当前点云与参考扫描点云相匹配的最佳变换位姿T。
充分考虑到传感器的退化情况，不断观测协方差，当协方差大于阈值时，检测全局定位情况，若全局定位未退化，加入全局约束作为观测，以减小协方差，保持***准确性；若全局定位已失效，为了保证协方差不发散，进行融合退化处理，利用惯性测量单元的陀螺仪进行预测，加速度计作为观测，在加速度计提供水平位置的绝对参考的同时，加入磁力计以提供一个正北方向的绝对参考，进行自动航向基准***(Automatic Heading ReferenceSystem)姿态解算，最低限度下保证里程计的平滑。
S2.3、基于流形上迭代误差状态卡尔曼的紧耦合融合方法。
基于流形上迭代误差状态卡尔曼的紧耦合融合方法的流程如下：
首先，获取当前状态的误差状态：
式中δp,δv,δθ,δba,δbg,δg分别表示位置向量、速度向量、角轴向量、惯性测量单位加速度计的偏移向量、陀螺仪的偏移向量以及重力加速度向量的误差。
然后，在误差状态δx中执行卡尔曼滤波器来估计误差状态的轨迹。
最后将更新后的误差状态δx添加回名义状态中作为本次卡尔曼滤波更新的估计值，即：
式中x表示当前状态，而表示x的名义状态，δx表示x的误差状态；δp、δv、δθ、δba、δbg、δg分别表示/>的误差状态；/>代表广义上的加法，/>表示四元数乘积，exp(δθ)表示将角轴向量映射到四元数旋转上。
同时在更新的过程中，不断切换工作点进行迭代更新。相比于其他状态估计技术，本实施例提出的紧耦合融合方法具有精度高、效率高和稳定性强的优势，克服了***运动方程和观测方程中非线性因素带来的影响，为非线性的水陆两用无人***提供了一个优雅的卡尔曼滤波框架。
针对水陆交替环境下传感器共视区间狭窄、松耦合融合方法无法提供一致性较高的里程计的问题，本实施例设计出一种基于测距传感器的多分辨率地图紧耦合里程计框架，将激光雷达、毫米波雷达、声呐与惯性传感器进行紧耦合融合，并搭建多分辨率地图，如图6所示。
S3、面向水陆交替场景的稳定建图方法。
针对GNSS信号可能出现错误而导致点云地图出现畸变和退化的问题，本实施例设计出一种可大幅度减少GNSS离群值带来的影响的二阶段优化策略。框架图如图8所示。
由于卫导***解算经纬度对GNSS信号有着一定的依赖性，且即使是在固定解时也可能输出错误的信息，因此，利用GNSS的信息矩阵来表征其信号的质量，GNSS因子的误差量可使用下述公式表示：
式中ygnss,k∈SE(3)为GNSS提供的6自由度位姿约束，xk∈SE(3)为k时刻的6自由度位姿，符号表示流形上的减法,Log运算是从SE(3)到se(3)的对数映射并转换到向量空间其中SE(3)表示特殊欧氏群，se(3)是SE(3)对应的李代数。
S3.1、将激光雷达与惯性测量单元通过误差状态卡尔曼滤波器进行融合，得到融合里程计；
将激光雷达与惯性测量单元通过误差状态卡尔曼滤波器进行融合，构成一个融合里程计。具体步骤为：首先，基于误差状态卡尔曼滤波器对惯性测量单元进行短时的快速估计，得到无人***载体的自估计信息；然后将这一自估计信息作为激光雷达的初始值，估计出激光雷达的位姿，从而实现融合。
这一融合里程计产生的相对运动误差可采用下述公式表示：
式中为融合里程计在第k,j时刻产生的相对运动误差，yk,j为激光雷达在第k,j时刻的相对测量值，xk和xj分别为第k时刻与第j时刻无人***的位姿。
S3.2、根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子并添加至因子图进行第一阶段二次因子图优化，得到初步地图。
因子图包括节点和边，其中节点指的是待优化变量，边就是指因子，可以对节点构成约束条件。融合里程计因子和GNSS因子就是分别通过GNSS数据和融合里程计数据得到，可以对因子图中的节点形成约束。
优化阶段一：
在获取融合里程计因子、GNSS因子后，添加到因子图进行第一阶段第一次因子图优化，同时，为了最大程度上剔除离群值，选用柯西鲁棒核函数对第一次优化进行约束并进行卡方检测。
其中柯西鲁棒核函数ρ的定义如下式：
式中c为控制参数，s为误差的平方项。
剔除离群值后，去掉柯西鲁棒核函数进行第一阶段第二次因子图优化，得到较为粗略的地图。
优化阶段二：
进行回环检测获取回环因子，重置因子图后，进行第二阶段的二次因子图优化，获得精度更高、鲁棒性更强的地图。整个建图过程可以建模为批量最大后验估计问题，具体数学模型如下式所示：
式中，rodom，rgnss，rloop分别为里程计约束残差、卫导约束残差、闭环约束残差，Σodom，Σgnss，Σloop分别为各约束对应的协方差矩阵，分别为里程计、闭环、卫导的关键帧集合，而/>则是机器人的位姿集合。
针对长时间建图累计误差大的问题，本实施例提出一种基于位置的多分辨率闭环检测方法。基于位置的闭环检测算法仅检查特定范围内(即20m以内)以防止发生错误的闭环。由于经优化阶段一已经获得了一个较为良好的GNSS测量值以保证***累积误差不超过搜索范围，相较于传统的基于外观的闭环检测算法，本实施例提出的方法更易于实现且运行良好。该方法流程图如图9所示。
本实施例提出的基于位置的多分辨率闭环检测方法的具体步骤如下：
首先，检查阈值范围内的候选闭环，以防止伪闭环的出现。通过比较当前全局定位信息与历史全局定位信息接近的就是候选闭环，伪闭环检测就是判断差值是否低于设定的阈值。当两者的差值大于阈值的话就舍去，这样做的话可以大量节省计算资源。
然后，采用多传感器多分辨率搜索的方法进行闭环检测，该方法将激光雷达、毫米波雷达、声呐的数据映射为2D图像，先在一张低分辨率的图像上进行闭环检测得到一个较为粗略的位置，然后通过固定的因子递进调高图片分辨率，在多张分辨率图像下进行闭环检测，将闭环作为约束加入到优化阶段二中。加入闭环检测后，当载体运行到先前到达过的区域时能为位姿图提供额外约束，提高轨迹的全局精度；同时，能对发生共视的物体做进一步优化，从而消除累积误差、减少地图重影。
一种面向水陆两用无人***的融合定位与建图装置：
至少一个处理器；
至少一个存储器，用于存储至少一个程序；
当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如上所述一种面向水陆两用无人***的融合定位与建图方法。
上述方法实施例中的内容均适用于本装置实施例中，本装置实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。
一种存储介质，其中存储有处理器可执行的指令，其特征在于：所述处理器可执行的指令在由处理器执行时用于实现如上所述一种面向水陆两用无人***的融合定位与建图方法。
上述方法实施例中的内容均适用于本存储介质实施例中，本存储介质实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。
以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。
Claims (9)
1.一种面向水陆两用无人***的融合定位与建图方法，其特征在于，包括以下步骤：
获取GNSS数据并进行卫星筛选，得到过滤后的GNSS数据；
获取视觉惯性数据并将视觉惯性数据与过滤后的GNSS数据进行紧耦合融合，得到全局定位信息；
将激光雷达与惯性测量单元通过误差状态卡尔曼滤波器进行融合，得到融合里程计；
结合全局定位信息和融合里程计的局部定位信息进行闭环检测，并对GNSS数据和融合里程计进行二阶段因子图优化，得到三维点云地图。
2.根据权利要求1所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，还包括：
基于流形上迭代误差状态卡尔曼的紧耦合融合方法对无人***进行位姿估计。
3.根据权利要求1所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，所述获取GNSS数据并进行卫星筛选，得到过滤后的GNSS数据这一步骤，其具体包括：
获取GNSS数据并对GNSS数据进行降维处理，得到预处理后的GNSS数据；
获取仰视图像并对仰视图像进行分割处理，得到分割后的仰视图像；
将预处理后的GNSS数据投影到分割后的仰视图像，并利用分割好的仰视图像对照星历图像进行可见性筛选，得到过滤后的GNSS数据。
4.根据权利要求3所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，所述获取仰视图像并对仰视图像进行分割处理，得到分割后的仰视图像这一步骤，其具体包括：
获取仰视图像；
设定初始全局阈值并基于初始全局阈值对仰视图像进行分割，得到初步分割图像；
计算初步分割图像的灰度平均值并作为新全局阈值，直至新全局阈值小于预设最小阈值，得到最终阈值；
基于最终阈值对仰视图像进行遮挡区域和未遮挡区域的分割，得到分割后的仰视图像。
5.根据权利要求1所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，所述获取视觉惯性数据并将视觉惯性数据与过滤后的GNSS数据进行紧耦合融合，得到全局定位信息这一步骤，其具体包括：
获取惯性测量单元的测量数据并进行预积分，得到预积分结果；
获取相机的拍摄数据并进行纯视觉运动重建，得到重建结果；
基于预积分结果和重建结果进行视觉-惯性对齐并初始化，得到视觉惯性数据；
输入至滑窗优化器进行因子图优化，生成全局一致轨迹，得到全局定位信息。
6.根据权利要求5所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，所述滑窗优化器的表达式如下：
其中，表示集合/>的最优估计，/>表示k时刻滑窗内所有关键帧的集合，/>表示第i时刻与第j时刻之间视觉-惯性里程计的相对约束，/>表示第i时刻GNSS的相对约束，/>表示滑窗中第一个状态量的先验约束，∑VIO、∑GNSS、∑0分别表示各约束的协方差矩阵。
7.根据权利要求1所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，所述结合全局定位信息和融合里程计的局部定位信息进行闭环检测，并对GNSS数据和融合里程计进行二阶段因子图优化，得到三维点云地图这一步骤，其具体包括：
根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子，并添加至因子图进行第一阶段二次因子图优化，得到初步地图；
结合全局信息和局部信息进行闭环检测并作为约束加入到初步地图，重置因子图进行第二阶段的二次因子图优化，得到三维点云地图。
8.根据权利要求7所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，所述根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子，并添加至因子图进行第一阶段二次因子图优化，得到初步地图这一步骤，其具体包括：
根据融合里程计数据和GNSS数据对应获取融合里程计因子和GNSS因子；
将融合里程计因子和GNSS因子添加至因子图，并选用柯西鲁棒核函数对第一次优化进行约束并进行卡方检测，得到第一次优化结果；
删除柯西鲁棒核函数后对第一次优化结果进行第一阶段第二次因子图优化，得到初步地图。
9.根据权利要求7所述一种面向水陆两用无人***的融合定位与建图方法，其特征在于，闭环检测作为约束这一步骤，其具体包括：
对候选闭环进行伪闭环检测；
将融合里程计数据中的激光雷达数据、毫米波雷达数据和声呐数据映射为2D图像，并在2D图像上进行闭环检测，得到粗略位置；
通过固定因子递进调高2D图像的分辨率，在多张分辨率图像下进行闭环检测，对粗略位置进行修正，得到正确闭环；
将正确闭环作为约束加入到第二阶段的二次因子图优化中。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310520140.0A CN116989763B (zh) | 2023-05-10 | 2023-05-10 | 一种面向水陆两用无人***的融合定位与建图方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310520140.0A CN116989763B (zh) | 2023-05-10 | 2023-05-10 | 一种面向水陆两用无人***的融合定位与建图方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116989763A true CN116989763A (zh) | 2023-11-03 |
| CN116989763B CN116989763B (zh) | 2024-07-19 |
Family
ID=88532871
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310520140.0A Active CN116989763B (zh) | 2023-05-10 | 2023-05-10 | 一种面向水陆两用无人***的融合定位与建图方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116989763B (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
| CN113340295A (zh) * | 2021-06-16 | 2021-09-03 | 广东工业大学 | 一种多个测距传感器的无人艇近岸实时定位与建图方法 |
| CN113837277A (zh) * | 2021-09-24 | 2021-12-24 | 东南大学 | 一种基于视觉点线特征优化的多源融合slam*** |
| CN114608561A (zh) * | 2022-03-22 | 2022-06-10 | 中国矿业大学 | 一种基于多传感器融合的定位与建图方法及*** |
| CN114966734A (zh) * | 2022-04-28 | 2022-08-30 | 华中科技大学 | 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法 |
| CN115479598A (zh) * | 2022-08-23 | 2022-12-16 | 长春工业大学 | 基于多传感器融合的定位与建图方法及紧耦合*** |
| CN115540849A (zh) * | 2022-08-31 | 2022-12-30 | 中国科学技术大学 | 高空作业平台激光视觉与惯导融合定位与建图装置及方法 |
| CN115657101A (zh) * | 2022-10-26 | 2023-01-31 | 北京航空航天大学 | 一种鱼眼相机辅助gnss-ins高精度导航定位的方法 |
| US20230118945A1 (en) * | 2021-11-02 | 2023-04-20 | Apollo Intelligent Driving Technology (Beijing) Co., Ltd. | Method of processing data for autonomous vehicle, electronic device, storage medium and autonomous vehicle |
-
2023
- 2023-05-10 CN CN202310520140.0A patent/CN116989763B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112304307A (zh) * | 2020-09-15 | 2021-02-02 | 浙江大华技术股份有限公司 | 一种基于多传感器融合的定位方法、装置和存储介质 |
| CN113340295A (zh) * | 2021-06-16 | 2021-09-03 | 广东工业大学 | 一种多个测距传感器的无人艇近岸实时定位与建图方法 |
| CN113837277A (zh) * | 2021-09-24 | 2021-12-24 | 东南大学 | 一种基于视觉点线特征优化的多源融合slam*** |
| US20230118945A1 (en) * | 2021-11-02 | 2023-04-20 | Apollo Intelligent Driving Technology (Beijing) Co., Ltd. | Method of processing data for autonomous vehicle, electronic device, storage medium and autonomous vehicle |
| CN114608561A (zh) * | 2022-03-22 | 2022-06-10 | 中国矿业大学 | 一种基于多传感器融合的定位与建图方法及*** |
| CN114966734A (zh) * | 2022-04-28 | 2022-08-30 | 华中科技大学 | 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法 |
| CN115479598A (zh) * | 2022-08-23 | 2022-12-16 | 长春工业大学 | 基于多传感器融合的定位与建图方法及紧耦合*** |
| CN115540849A (zh) * | 2022-08-31 | 2022-12-30 | 中国科学技术大学 | 高空作业平台激光视觉与惯导融合定位与建图装置及方法 |
| CN115657101A (zh) * | 2022-10-26 | 2023-01-31 | 北京航空航天大学 | 一种鱼眼相机辅助gnss-ins高精度导航定位的方法 |
Non-Patent Citations (1)
| Title |
|---|
| 徐昊玮;廉保旺;刘尚波;: "基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法", 兵工学报, no. 04, 15 April 2019 (2019-04-15) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116989763B (zh) | 2024-07-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11585662B2 (en) | Laser scanner with real-time, online ego-motion estimation | |
| CN113345018B (zh) | 一种动态场景下的激光单目视觉融合定位建图方法 | |
| CN111583369B (zh) | 一种基于面线角点特征提取的激光slam方法 | |
| CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
| CN106595659A (zh) | 城市复杂环境下多无人机视觉slam的地图融合方法 | |
| CN114424250A (zh) | 结构建模 | |
| US20110274343A1 (en) | System and method for extraction of features from a 3-d point cloud | |
| CN112950696B (zh) | 导航地图的生成方法及生成装置、电子设备 | |
| CN114359476A (zh) | 一种用于城市峡谷环境导航的动态3d城市模型构建方法 | |
| CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
| CN114648584B (zh) | 一种用于多源融合定位的鲁棒性控制方法和*** | |
| CN116879870A (zh) | 一种适用于低线束3d激光雷达的动态障碍物去除方法 | |
| Venable et al. | Large scale image aided navigation | |
| CN116681733B (zh) | 一种空间非合作目标近距离实时位姿跟踪方法 | |
| CN116147618B (zh) | 一种适用动态环境的实时状态感知方法及*** | |
| CN116989763B (zh) | 一种面向水陆两用无人***的融合定位与建图方法 | |
| CN115718303A (zh) | 一种面向动态环境的相机与固态激光雷达融合重定位方法 | |
| Falchetti et al. | Probability hypothesis density filter visual simultaneous localization and mapping | |
| Escourrou et al. | Ndt localization with 2d vector maps and filtered lidar scans | |
| Skjellaug | Feature-Based Lidar SLAM for Autonomous Surface Vehicles Operating in Urban Environments | |
| Weitbrecht | Monte Carlo localization in dynamic environments based on an automotive Lidar sensor cocoon | |
| CN117689588A (zh) | 一种二维-三维结构特征联合约束的激光slam方法 | |
| CN117421384A (zh) | 基于共视投影匹配的视觉惯性slam***滑窗优化方法 | |
| Zeng et al. | Entropy-based Keyframe Established and Accelerated Fast LiDAR Odometry and Mapping | |
| 최성혁 | Image Inconsistency Mitigation Algorithm for IMU/Vision Localization based on Aerial Scene Matching |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |