CN117268373A - 一种多传感器信息融合的自主导航方法及系统 - Google Patents
一种多传感器信息融合的自主导航方法及系统 Download PDFInfo
- Publication number
- CN117268373A CN117268373A CN202311549558.0A CN202311549558A CN117268373A CN 117268373 A CN117268373 A CN 117268373A CN 202311549558 A CN202311549558 A CN 202311549558A CN 117268373 A CN117268373 A CN 117268373A
- Authority
- CN
- China
- Prior art keywords
- pose
- observation
- point cloud
- information
- line
- 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
- 238000000034 method Methods 0.000 title claims abstract description 126
- 230000004927 fusion Effects 0.000 title claims abstract description 34
- 230000008569 process Effects 0.000 claims abstract description 36
- 238000012216 screening Methods 0.000 claims abstract description 16
- 238000004364 calculation method Methods 0.000 claims abstract description 15
- 238000004891 communication Methods 0.000 claims abstract description 3
- 230000010354 integration Effects 0.000 claims description 43
- 230000000007 visual effect Effects 0.000 claims description 37
- 238000013507 mapping Methods 0.000 claims description 21
- 238000005259 measurement Methods 0.000 claims description 13
- 230000005484 gravity Effects 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 9
- 230000000694 effects Effects 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 9
- 238000005457 optimization Methods 0.000 claims description 8
- 239000000284 extract Substances 0.000 claims description 6
- 238000007670 refining Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 241001061260 Emmelichthys struhsakeri Species 0.000 claims description 3
- 206010034719 Personality change Diseases 0.000 claims description 3
- 238000004458 analytical method Methods 0.000 claims description 3
- 238000010276 construction Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 238000012795 verification Methods 0.000 claims description 3
- 230000015556 catabolic process Effects 0.000 claims description 2
- 238000006731 degradation reaction Methods 0.000 claims description 2
- 238000012986 modification Methods 0.000 description 6
- 230000004048 modification Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000004075 alteration Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 230000007850 degeneration Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000001575 pathological effect Effects 0.000 description 1
- 238000011084 recovery 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/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
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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
-
- 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)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种多传感器信息融合的自主导航方法及系统，包括：初步筛选GNSS、SINS、视觉和激光雷达原始观测数据；并生成多传感器精化后的解算信息；构建各传感器观测因子，得到解算后的位姿及点云；利用视觉和激光观测信息判断轨迹闭环，同时构建位姿序列集削弱轨迹闭合误差并精化视觉/激光点云，最后融合视觉/激光点云得到全局一致的高精度点云地图；采用地图匹配在已建图区域导航定位，从而有效降低系统计算负荷；采用四个进程将上述方法分布式部署于不同终端，进程间采用ZeroMQ通信；本发明可以得到实时高精度的定位定姿结果，无需人工干预，实现自主导航定位。
Description
技术领域
本发明属于融合导航技术领域，具体涉及一种多传感器信息融合的自主导航方法及系统。
背景技术
导航定位技术在人类的社会、经济和军事活动中越来越发挥着重要的支撑作用。随着自动驾驶、无人机和机器人等技术的兴起，高精度、高可靠性、高可用性的低成本导航定位需求得到空前增长。同时定位与建图（Simultaneous Localization and Mapping,SLAM）所关注的问题是载有传感器的机器人在未知的环境中定位并构建环境地图，是机器人感知自身状态和外部环境的关键技术，通常采用视觉、激光等环境传感器，并结合捷联惯导（SINS，Strapdown Inertial Navigation System）进行相对定位与建立地图。在移动机器人实现自动化操作时，先决条件便是准确地估计自身的位置和姿态，并感知所处环境。在准确地感知自身位姿和环境后，机器人才能进行自主决策、规划和控制，完成自动化操作。然而，SLAM递推不可避免地造成了误差累积，使其无法在大规模、长距离场景下给出可靠的位姿与地图。为解决这一问题，一方面可以检测轨迹闭环，通过回环约束消除部分累积误差；另一方面则通过在开阔环境融合全球导航卫星系统（Global Navigation SatelliteSystem, GNSS）的绝对观测信息，消除累积误差，提高定位定姿与建图精度，进而提升已建图区域的定位定姿精度。
多源融合导航定位与建图方法多基于因子图优化设计，其采用预积分方法提供历元间导航参数的变化约束，并将其余传感器观测因子化，通过因子图优化解算导航参数，利用估计的位姿将点云投影至世界坐标系完成建图。该方法较传统的滤波方案有精度高、可扩展性高等特点，是目前导航定位与建图的主流方法。
目前，基于因子图优化的多源融合导航定位方法仍存在以下问题：
1）现有方法仅融合视觉或激光单个环境传感器，尚未构建统一的多源融合导航定位因子图模型；
2）可分布式部署；或现有的分布式节点方法依赖于主节点，容灾能力差；
3）现有方法多采用传统预积分方法，未考虑地球自转、重力变化带来的影响，无法发挥高精度惯性导航器件应有的价值；
4）现有方法中激光雷达多采用传统的点到点匹配方式，无法估算协方差，其观测难以融合。
发明内容
针对以上问题，本发明给出了一种多传感器信息融合的自主导航方法，通过分布式进程设计，融合视觉、激光、惯导、GNSS观测信息与已建图区域的匹配定位信息，获取载体高精度位置与姿态，实现根据任务需求灵活部署，且无需人工干预，实现载体自主导航。
为达到上述目的，本发明方法的技术方案为：
一种多传感器信息融合的自主导航方法，包括以下步骤：
步骤S1：接收GNSS、SINS、视觉和激光雷达原始观测数据并进行初步筛选；
步骤S2：接收初步筛选后的GNSS、SINS、视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将SINS观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；
步骤S3：接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹、点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；
步骤S4：接收步骤S3中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿、点云及方差信息。
进一步地，所述步骤S1中设置4个独立的线程并行处理不同传感器，包括GNSS线程、SINS线程、视觉线程和激光雷达线程；
其中，GNSS线程接收GNSS原始观测数据，包括伪距、相位和多普勒，并采用实时动态RTK定位模型建模观测值，选取不同卫星导航系统高度角最高的卫星为基准，剔除可视卫星少于4的卫星导航系统；并通过卫星PRN号匹配基准站和流动站之间的共视卫星；
SINS线程根据视觉/激光相邻两次观测选取积分区间，匹配区间内的所有SINS观测值存入队列；
视觉线程通过ORB角点提取算法提取影像中的特征信息，通过光流法匹配方法匹配不同影像间的同名特征点，当可用特征点少于阈值时，重新提取图像中ORB角点；
激光雷达线程从原始激光雷达点云中提取平面和边缘点，其中原始点云中低曲率和高曲率的点分别对应于数据环境中的平面和线，随后将其参数化；通过计算不同帧间的线-线距离与面-面距离匹配同名线/面特征；上述激光点云曲率表示如下：
式中，为该点在统一扫描线的前后五个点的集合，/>；/>、/>分别为集合/>中点i和点j的深度，若c大于阈值则为线特征，反之则为面特征。
进一步地，提取激光雷达点云中线和面的步骤如下：
对于属于第i条扫描线的点，首先确定在/>扫描线中/>的最近点，然后利用/>和/>计算一条线，并在/>条扫描线中找到/>的最近点/>，若/>与线之间的距离小于给定阈值，则使用/>，/>和/>拟合新线，这个过程重复进行，直到不能添加更多的点为止，其中，L为所有线属性点的集合；
在提取平面时，从第一条扫描线开始，首先找出在第i条扫描线中的每个片段P中的点，其k个最近的点位于P中但不在第i条扫描线中，然后，使用SVD算法将这些点拟合到P中的一个平面上，接着，该平面可以由其方向向量n表示，该向量是与A的最小特征值相关联的特征向量，计算所有点到平面的距离之和，并排除大于设定阈值的平面。
进一步地，所述步骤S2包括如下子步骤：
步骤S2.1：并行接收初步筛选后的GNSS、SINS、视觉和激光雷达观测数据；
步骤S2.2：通过插值方法将SINS观测数据对齐至区间起止位置，并通过顾及地球自转和区域重力变化的预积分方法对观测值积分，得到历元间相对位姿，得到SINS解算信息；
步骤S2.3：利用历元间相对位姿，将前一帧激光点云中的线面投影至当前帧，通过设定阈值判断线到线的距离与面到面的距离，并剔除大于阈值的线或面，得到精化后的激光雷达观测值，即激光雷达解算信息；
步骤S2.4：通过随机采样一致方法RANSAC计算基础矩阵和单应矩阵，以剔除视觉观测中的错误匹配，得到精化后的视觉匹配观测值，即视觉解算信息；
步骤S2.5：将高度角低于设置阈值的卫星观测值剔除，得到精化后的共视观测卫星，即GNSS解算信息。
进一步地，所述步骤S2.2具体实施步骤如下：
步骤S2.21：根据低频观测传感器采样时间确定积分间隔，设定低频观测传感器相邻两次观测时间分别为和/>，在此间隔内有N个SINS量测，即积分所需的观测值，积分区间即为/>；
步骤S2.22：采用考虑地球自转的姿态双子样更新算法积分陀螺输出；进一步计算考虑圆锥效应和划桨效应补偿的速度、位置在积分区间内的增量；
步骤S2.223：通过预积分误差模型传播不确定度，为预积分观测值在多传感器融合中提供权值；
设定时刻的姿态预积分为/>，则/>时刻的姿态预积分为：
世界坐标系的姿态增量约束为：
式中，表示相对惯性系固定的历史世界系，即历史的e系；/>表示IMU量测参考系；/>为相对惯性系固定的历史IMU量测参考系；/>为相对e系固定的历史IMU量测参考系；/>分别表示IMU积分区间起止时刻；/> 表示参考坐标系A至B的旋转矩阵；/>为地球自转补偿项，与其对应的旋转矢量的解析形式为：
其中，为积分时长，/>为地球自转角速度，若忽略地球旋转补偿项，上式退化为传统的姿态预积分，
设定时刻的速度预积分为/>，则/>时刻的速度预积分为：
上式中，为划桨效应补偿项，其中包含了科里奥利力补偿，
设定重力为常值，使用表示整个区间内的重力，则圆锥效应补偿项可表示为：
设定时刻的位置预积分为/>，则/>时刻的位置预积分为：
位置的科里奥利力补偿项近似形式为：
对积分区间内的所有IMU量测按照如上方式递推，可分别得到区间内顾及地球自转的位置、速度及姿态预积分约束/>、/>及/>。
进一步地，所述步骤S3包括如下子步骤：
步骤S3.1：判断当前区域是否有可用的地图，若已建图，则仅构建匹配定位因子；
步骤S3.2：判断当前轨迹是否回环；若存在轨迹闭环，则将精化后的位姿和点云更新至实时系统；
步骤S3.3：接收步骤S2中所述的各传感器解算信息，并将其因子化；同时，将各个观测因子插入因子图中，并设置滑动窗口求解待估计参数，并计算其验后协方差信息；
步骤S3.4：序贯更新GNSS观测值，首先判断当前历元是否有GNSS观测值，若有，则以滑窗解算结果为先验，融合GNSS观测值；若当前GNSS数据质量差、GNSS信号缺失，则不更新；
步骤S3.5：窗口滑动，通过边缘化移出窗口内最早一帧观测值，得到该帧的解算结果，及窗口内的视觉及激光点云。
进一步地，激光雷达采用参数化线面的因子化方式：
对于点到线距离观测，使用普朗克坐标将线特征参数化为，/>为直线的方向向量，/>为直线的中心点，则激光线因子表达为：
式中，为线特征点，/>为激光雷达中心在世界坐标系的位置，/>表示局部坐标系，设定/>，/>，/>，通过协方差传播法则确定/>的不确定性/>，根据方差传播定律，/>表达为：
其中：
对于平面而言，其观测因子为：
式中，为平面法向量；/>为平面中心；/>为投影到世界坐标系下的面特征点：
式中为激光雷达中心在局部坐标系的位置。同理根据方差传播定律可得点面观测值的不确定度/>：
其中：
IMU因子如前所述建模为顾及地球自转的预积分因子，GNSS因子和视觉观测因子则分别通过RTK和重投影误差建模。
进一步地，所述步骤4包括如下子步骤：
步骤4.1:初始化位姿序列集，接收步骤S3中解算后的位姿及点云，并将点云投影至视觉图像中，以其作为初值提取特征信息，将位姿插入位姿序列集中；
步骤4.2:采用DBoW对新插入的图像检测回环；若轨迹闭环，则以闭合差为约束，优化位姿，并将激光点云投影至全局坐标系；
步骤4.3:对回环轨迹构建重投影误差并固定位姿，精化视觉点云，最终将视觉点云投影至与激光点云相同的全局坐标系，并将二者融合；
步骤4.4:将精化的点云及位姿发送至实时系统，提高实施导航定位的可用性。
进一步地，准实时或离线建图系统接收实时系统发送的位姿及点云信息，通过词袋模型（DBoW）检测回环，并将该帧位姿加入至位姿序列集中，设定/>为轨迹上的一帧，在随后的第/>帧检测到轨迹闭环，此时回环约束表达为：
式中，、/>分别为第/>帧与第/>帧的相对位置及姿态变化；残差表达为：
而对于在回环轨迹中的所有相邻位姿，均有：
式中，、/>分别为第/>帧与第/>帧的相对位置及姿态变化，/>；残差表示为
则回环的总体目标函数为：
其中，为回环轨迹序列中所有的位姿，上述方法为位姿网平差，由此得到最优位姿序列，此后，对于位姿序列集/>中的所有位姿及视觉点云，匹配共同观测后建立重投影误差，进一步精化视觉点云，此时，位姿及视觉、激光点云均为最优解，将视觉及激光点云融合后即可得到高精度点云地图。
另一方面，本发明还提供一种多传感器信息融合的自主导航系统，包括：
原始观测数据初步筛选模块：其用于接收GNSS、SINS、视觉和激光雷达原始观测数据并进行初步筛选；
解算信息精化模块：其用于接收初步筛选后的GNSS、SINS、视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将SINS观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；
导航状态解算模块：其用于接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹、点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；
建图优化模块：其用于接收步骤S3中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿、点云及方差信息；
且上述模块间采用低延时通信方法ZeroMQ通信，各个模块可根据不同任务需求采用分布式进程部署。
所述多传感器信息融合的自主导航系统用于执行上述多传感器信息融合的自主导航方法中的步骤。
与现有技术相比，本发明具有如下有益效果：
1)充分发挥了传感器间互相补充的优势，建立了融合四类传感器的通用因子图模型；
2)各个进程任务明确，可灵活分布式部署，充分发挥可用的计算资源，提高解算效率；
3)采用顾及地球自转和区域重力变化的预积分因子和，充分发挥惯性器件性能，提高历元间约束的精度；
4)采用参数化线面的激光雷达观测因子，解决了传统的点到点匹配方法协方差无法估计的难题，降低了激光观测的融合难度。
附图说明
图1为本发明实例的多传感器融合导航数据处理流程图；
图2为本发明实例的多传感器数据关联流程图；
图3为本发明实例的多源观测信息精化流程图；
图4为本发明实例的实时导航处理流程图；
图5为本发明实例的严密预积分算法流程图；
图6为本发明实例的参数化线/面的激光雷达观测值处理流程图；
图7为本发明实例的融合视觉/激光点云的高精度建图算法流程；
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本发明普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
本发明是一种融合多传感器观测信息的高精度实时导航方法，包含实时定位定姿算法与准实时/离线建图算法，如图1所示。其中，实时定位定姿算法由多传感器观测数据关联、多源观测信息精化和实时导航处理三个模块组成，各模块采用分布式进程设计，可灵活部署于不同终端；同时，实时定位定姿算法为建图算法提供高精度的位姿及点云初值。准实时/离线建图算法判断轨迹闭环，通过闭环约束校正位姿序列，进而精化点云，得到高精度地图；此外，建图算法判断当前区域建图情况为实时导航算法提供匹配定位信息，减轻导航系统资源占用。
以下将结合图1对本发明中各关键模块及其实时方案展开详细叙述。
一、多传感器观测数据关联
单一传感器观测数据存在大量冗余，无法直接用于导航定位，如视觉图像中仅有少量的特征点能够用于定位。因此，首先需要匹配与观测模型相符的观测数据。由于传感器间相互独立，本方法首先设置4个独立的线程并行处理不同传感器观测数据，如图2所示，具体实施方案如下所述：
1.GNSS线程，GNSS采用实时动态（RTK）定位模型，选取不同卫星导航系统高度角最高的卫星为基准，剔除可视卫星少于4的卫星导航系统；并匹配基准站和流动站之间的共视卫星；
2.SINS线程，根据视觉/激光相邻两次观测选取积分区间，将区间内的所有SINS观测值存入队列；
3.视觉线程，通过光流法匹配方法匹配同名特征点，当可用特征点少于阈值时，重新提取图像中ORB角点；
4.激光雷达线程，从原始点云中提取平面和边缘点，其中原始点云中低曲率和高曲率的点分别对应于数据环境中的平面和线，随后将其参数化；通过计算不同帧间的线-线距离与面-面距离匹配同名线/面特征；上述激光点云曲率定义如下：
式中，为该点在统一扫描线的前后五个点的集合，/>；/>、/>分别为集合/>中点i和点j的深度。若c大于阈值则为线特征，反之则为面特征。
最后，将上述筛选后的多源观测信息通过ZeroMQ发送至数据精化进程。提取激光雷达点云中线和面的步骤如下所述。对于属于第i条扫描线的点，首先确定在/>扫描线中/>的最近点。然后利用/>和/>计算一条线，并在/>条扫描线中找到/>的最近点/>。若/>与线之间的距离小于给定阈值，则使用/>，/>和/>拟合新线。这个过程重复进行，直到不能添加更多的点为止。其中，L为所有线属性点的集合。
在提取平面时，从第一条扫描线开始，首先找出在第i条扫描线中的每个片段P中的点，其k个最近的点位于P中但不在第i条扫描线中。然后，使用SVD算法将这些点拟合到P中的一个平面上。接着，该平面可以由其方向向量n表示，该向量是与A的最小特征值相关联的特征向量。为了在提取过程中排除病态平面，本方法计算所有点到平面的距离之和，并排除大于设定阈值的平面。
二、多源观测信息精化
通过数据关联初步筛选了观测值，但其中仍存在粗差观测值和错误匹配，进而导致融合解算收敛慢、融合结果次优。因此，在融合解算前需根据先验条件剔除粗差和错误匹配。其中，由于SINS高频观测特性，可为其他传感器相邻观测间提供良好的约束条件，以剔除观测值中存在的粗差。因此，本发明首先将积分区间内的SINS观测值积分，通过其提供的相对位姿剔除视觉与激光雷达中的错误匹配，算法流程如图3所示，具体实施步骤如下所述：
步骤1，通过ZeroMQ并行接收GNSS共视卫星、积分区间内所有SINS观测值、视觉同名特征点和激光雷达同名线/面特征；
步骤2，通过插值方法将SINS观测值对齐至区间起止位置，并通过顾及地球自转和区域重力变化的预积分方法对观测值积分，得到历元间相对位姿，得到SINS解算信息；
步骤3，利用历元间相对位姿，将前一帧激光点云中的线面投影至当前帧，通过设定阈值判断线到线的距离与面到面的距离，并剔除大于阈值的线或面，得到精化后的激光雷达观测值，即激光雷达解算信息；
步骤4，通过随机采样一致方法RANSAC计算基础矩阵和单应矩阵，以剔除视觉观测中的错误匹配，得到精化后的视觉匹配观测值，即视觉解算信息；
步骤5，设置阈值将高度角低于8度的卫星观测值剔除，得到精化后的共视观测卫星，即GNSS解算信息；
步骤6，将精化后的解算信息通过ZeroMQ发送至实时导航处理线程；
其中，传统的预积分方法忽略了地球自转与重力变化带来的影响。对于地球自转而言，其数值约为15，若不对其进行妥善建模和考虑，即使是在高精度的陀螺仪也会退化到战术级水平，无法发挥出其应有的价值。为了使SINS的测量精度在预积分算法中得以充分发挥，本方法采用的改进预积分算法充分考虑了地球自转与重力变化，并加入圆锥补偿和划桨效应补偿。算法流程如图5所示，具体实施步骤如下所述：
步骤1，根据低频观测传感器采样时间确定积分间隔。设低频观测传感器相邻两次观测时间分别为和/>，在此间隔内有N个SINS量测，即积分所需的观测值，积分区间即为；
步骤2，采用考虑地球自转的姿态双子样更新算法积分陀螺输出；进一步计算考虑了科里奥利力补偿的速度、位置在积分区间内的增量；
步骤3，通过预积分误差模型传播不确定度，为预积分观测值在多传感器融合中提供权值。
设时刻的姿态预积分为/>，则/>时刻的姿态预积分为：
为了与相对于世界坐标系（e系）的传感器观测信息相融合，实际需要的是e系的姿态增量约束，则有：
式中，表示相对惯性系固定的历史世界系，即历史的e系；/>表示IMU量测参考系；/>为相对惯性系固定的历史IMU量测参考系；/>为相对e系固定的历史IMU量测参考系；/>分别表示IMU积分区间起止时刻；/>表示参考坐标系A至B的旋转矩阵；为地球自转补偿项，与其对应的旋转矢量的解析形式为：
其中，为积分时长，/>为地球自转角速度。若忽略地球旋转补偿项，则退化为传统的姿态预积分。
设时刻的速度预积分为/>，则/>时刻的速度预积分为：
上式中，为划桨效应补偿项，其中包含了科里奥利力补偿。与在SINS传统机械编排算法中的计算方法一致，在此不再赘述。同时，考虑到积分区间很短（通常小于0.1s），重力可认为常值，使用/>表示整个区间内的重力，则圆锥效应补偿项可表示为：
而位置预积分是对速度预积分的再次积分，其形式较为简单。设时刻的位置预积分为/>，则/>时刻的位置预积分为：
位置的科里奥利力补偿项近似形式为：
对积分区间内的所有IMU量测按照如上方式递推，可分别得到区间内顾及地球自转的位置、速度及姿态预积分约束/>、/>及/>。
三、实时导航处理
目前，用于导航定位的传感器通常包括SINS、视觉、激光雷达和GNSS。一方面，尽管单一的传感器仍可用于导航定位，但上述传感器各具有局限性，无法实现全时全域的高精度导航定位与建图。如SINS误差会随时间累积；GNSS在树荫、高架、隧道等遮挡环境下定位精度下降甚至无法定位；视觉和激光易受环境纹理影响。另一方面，上述传感器间存在优势互补的特点，使得融合上述传感器能够显著提高定位精度，提高导航系统的可用性。时导航处理进程接收粗差剔除后的观测值，并根据传感器观测模型建立各传感实器观测因子，通过滑动窗口限制图结构增长的同时能够更充分地利用观测信息，算法流程如图4所示，具体实施步骤如下所述：
步骤1，接收建图系统发送的当前区域是否建图的信息，若已建图，则仅构建匹配定位因子；
步骤2，接收建图系统发送的回环检测信息；若存在轨迹闭环，则将精化后的位姿和点云更新至实时系统；
步骤3，通过ZeroMQ并行接收各传感器解算信息，并将其因子化；同时，将各个观测因子插入因子图中，并设置滑动窗口求解待估计参数，并计算其验后协方差信息；
步骤4，序贯更新GNSS观测值。首先判断当前历元是否有GNSS观测值。若有，则以滑窗解算结果为先验，融合GNSS观测值；若当前GNSS数据质量差、GNSS信号缺失，则不更新；
步骤5，窗口滑动，通过边缘化移出窗口内最早一帧观测值，得到该帧的解算结果，及窗口内的视觉及激光点云。
其中，激光雷达采用参数化线面的因子化方式，如图6所示。对于点到线距离观测，使用普朗克坐标将线特征参数化为。/>为直线的方向向量，/>为直线的中心点。则激光线因子表达为：
/>
式中，为线特征点，/>为激光雷达中心在世界坐标系的位置，/>表示局部坐标系，其余符号与上文一致。假设/>，/>，/>，可以通过协方差传播法则确定/>的不确定性/>。根据方差传播定律，/>表达为：
其中：
对于平面而言，其观测因子为：
式中，为平面法向量；/>为平面中心；/>为投影到世界坐标系下的面特征点：
式中为激光雷达中心在局部坐标系的位置。同理根据方差传播定律可得点面观测值的不确定度/>：
其中：
则激光因子可表达为：
SINS因子如前所述建模为顾及地球自转的预积分因子，GNSS因子和视觉观测因子则分别通过RTK和重投影误差建模。
四、后端建图优化
尽管实时系统利用多类传感器观测数据获得了导航定位参数的最优解，但在GNSS缺失时缺少绝对定位信息，导致SINS/视觉/激光雷达递推误差随导航距离累积，需要通过轨迹闭环约束削弱误差累积；此外，视觉和激光雷达丰富的环境观测信息可以用于建图，建好的地图存入数据库中。当载体进入建图区域时，则通过匹配定位获取载体高精度位置与姿态。其算法流程如图7所示，具体实施步骤如下所述：
步骤1，初始化位姿序列集，通过ZeroMQ接收上述实时导航模块发送的位姿及点云，并将点云投影至视觉图像中，以其作为初值提取特征信息，将位姿插入位姿序列集中；
步骤2，采用DBoW对新插入的图像检测回环；若轨迹闭环，则以闭合差为约束，优化位姿，并将激光点云投影至全局坐标系；
步骤3，对回环轨迹构建重投影误差并固定位姿，精化视觉点云，最终将视觉点云投影至与激光点云相同的全局坐标系，并将二者融合；
步骤4，将精化的点云及位姿发送至实时系统，提高实施导航定位的可用性。
准实时/离线建图系统接收实时系统发送的位姿及点云信息，通过词袋模型（DBoW）检测回环，并将该帧位姿加入至位姿序列集中。设/>为轨迹上的一帧，在随后的第帧检测到轨迹闭环，此时回环约束表达为：
式中，、/>分别为第/>帧与第/>帧的相对位置及姿态变化。残差表达为：
而对于在回环轨迹中的所有相邻位姿，均有：
式中，、/>分别为第/>帧与第/>帧的相对位置及姿态变化，/>。残差表示为
则回环的总体目标函数为：
其中，为回环轨迹序列中所有的位姿，上述方法为位姿网平差，由此得到最优位姿序列。此后，对于位姿序列集/>中的所有位姿及视觉点云，匹配共同观测后建立重投影误差，进一步精化视觉点云。此时，位姿及视觉、激光点云均为最优解，将视觉及激光点云融合后即可得到高精度点云地图。最后，将精化后的位姿及点云发送至实时系统，提高实时系统解算精度及可用性。
本文中所描述的具体实施例仅是对本发明精神作举例说明。本发明所属技术领域的技术人员可以对所描述的具体实施例做各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。
实施例2
本实施例提供一种多传感器信息融合的自主导航系统，包括：
原始观测数据初步筛选模块：其用于接收GNSS、SINS、视觉和激光雷达原始观测数据并进行初步筛选；
解算信息精化模块：其用于接收初步筛选后的GNSS、SINS、视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将SINS观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；
导航状态解算模块：其用于接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹、点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；
建图优化模块：其用于接收步骤S3中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿、点云及方差信息；
尽管已描述了本发明的优选实例，但本领域的技术人员一旦得知了基本的创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围所有的变更和修改。
显然，本领域的技术人员可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其同等技术的范围之类，则本发明也意图包含这些改动和变型在内。
其它未详细说明的部分均为现有技术。
Claims (10)
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量测参考系；为相对惯性系固定的历史IMU量测参考系；/>为相对e系固定的历史IMU量测参考系；/>分别表示IMU积分区间起止时刻；/> 表示参考坐标系A至B的旋转矩阵；为地球自转补偿项，与其对应的旋转矢量的解析形式为：
其中，为积分时长，/>为地球自转角速度，若忽略地球旋转补偿项，上式退化为传统的姿态预积分，
设定时刻的速度预积分为/>，则/>时刻的速度预积分为：
上式中，为划桨效应补偿项，其中包含了科里奥利力补偿，
设定重力为常值，使用表示整个区间内的重力，则圆锥效应补偿项可表示为：
设定时刻的位置预积分为/>，则/>时刻的位置预积分为：
位置的科里奥利力补偿项近似形式为：
对积分区间内的所有IMU量测按照如上方式递推，可分别得到区间内顾及地球自转的位置、速度及姿态预积分约束/>、/>及/>。
6.根据权利要求1所述的一种多传感器信息融合的自主导航方法，其特征在于，所述步骤S3包括如下子步骤：
步骤S3.1：判断当前区域是否有可用的地图，若已建图，则仅构建匹配定位因子；
步骤S3.2：判断当前轨迹是否回环；若存在轨迹闭环，则将精化后的位姿和点云更新至实时系统；
步骤S3.3：接收步骤S2中所述的各传感器解算信息，并将其因子化；同时，将各个观测因子插入因子图中，并设置滑动窗口求解待估计参数，并计算其验后协方差信息；
步骤S3.4：序贯更新GNSS观测值，首先判断当前历元是否有GNSS观测值，若有，则以滑窗解算结果为先验，融合GNSS观测值；若当前GNSS数据质量差、GNSS信号缺失，则不更新；
步骤S3.5：窗口滑动，通过边缘化移出窗口内最早一帧观测值，得到该帧的解算结果，及窗口内的视觉及激光点云。
7.根据权利要求6所述的一种多传感器信息融合的自主导航方法，其特征在于，激光雷达采用参数化线面的因子化方式：
对于点到线距离观测，使用普朗克坐标将线特征参数化为，/>为直线的方向向量，/>为直线的中心点，则激光线因子表达为：
式中，为线特征点，/>为激光雷达中心在世界坐标系的位置，/>表示局部坐标系，设定/>，/>，/>，通过协方差传播法则确定的不确定性/>，根据方差传播定律，/>表达为：
其中：
对于平面而言，其观测因子为：
式中，为平面法向量；/>为平面中心；/>为投影到世界坐标系下的面特征点：
式中为激光雷达中心在局部坐标系的位置，同理根据方差传播定律得点面观测值的不确定度/>：
其中：
IMU因子如前所述建模为顾及地球自转的预积分因子，GNSS因子和视觉观测因子则分别通过RTK和重投影误差建模。
8.根据权利要求6所述的一种多传感器信息融合的自主导航方法，其特征在于，所述步骤4包括如下子步骤：
步骤4.1:初始化位姿序列集，接收步骤S3中解算后的位姿及点云，并将点云投影至视觉图像中，以其作为初值提取特征信息，将位姿插入位姿序列集中；
步骤4.2:采用DBoW对新插入的图像检测回环；若轨迹闭环，则以闭合差为约束，优化位姿，并将激光点云投影至全局坐标系；
步骤4.3:对回环轨迹构建重投影误差并固定位姿，精化视觉点云，最终将视觉点云投影至与激光点云相同的全局坐标系，并将二者融合；
步骤4.4:将精化的点云及位姿发送至实时系统，提高实施导航定位的可用性。
9.根据权利要求8所述的一种多传感器信息融合的自主导航方法，其特征在于，采用词袋模型的位姿优化算法：
通过词袋模型（DBoW）检测回环，并将该帧位姿加入至位姿序列集中，设定/>为轨迹上的一帧，在随后的第/>帧检测到轨迹闭环，此时回环约束表达为：
式中，、/>分别为第/>帧与第/>帧的相对位置及姿态变化；残差表达为：
而对于在回环轨迹中的所有相邻位姿，均有：
式中，、/>分别为第/>帧与第/>帧的相对位置及姿态变化，/>；残差表示为
则回环的总体目标函数为：
其中，为回环轨迹序列中所有的位姿，上述方法为位姿网平差，由此得到最优位姿序列，此后，对于位姿序列集/>中的所有位姿及视觉点云，匹配共同观测后建立重投影误差，进一步精化视觉点云，此时，位姿及视觉、激光点云均为最优解，将视觉及激光点云融合后即可得到高精度点云地图。
10.一种多传感器信息融合的自主导航系统，其特征在于:包括：
原始观测数据初步筛选模块：其用于接收GNSS、SINS、视觉和激光雷达原始观测数据并进行初步筛选；
解算信息精化模块：其用于接收初步筛选后的GNSS、SINS、视觉和激光雷达观测数据，通过先验信息剔除粗差观测及错误匹配，并将SINS观测数据插值对齐至积分区间；由此生成多传感器精化后的解算信息；
导航状态解算模块：其用于接收精化后的解算信息并构建各传感器观测因子，同时判断当前区域是否建图，若当前环境已建图，则仅采用地图匹配解算载体位姿；反之，则通过滑动窗口限制图结构的增长的同时利用更多的观测数据；判断当前轨迹是否闭环，若轨迹闭环，则更新闭环后的轨迹、点云及方差信息更新；采用因子图优化加速解算并动态剔除粗差观测，实现实时高精度导航状态解算，得到解算后的位姿及点云；
建图优化模块：其用于接收步骤S3中解算后的的位姿和点云信息，并判断当前载体所处环境是否建图，若已建图则直接返回建图信息；若未建图，则构建位姿序列集，在位姿序列中通过词袋模型检测回环，若轨迹回环，则通过轨迹闭合差优化位姿，并进一步精化视觉点云，并融合全局坐标系下的视觉与激光点云进行建图，得到回环后的位姿、点云及方差信息；
根据不同任务需求，采用分布式进程将上述模块灵活部署于不同终端，进程间采用ZeroMQ通信；
所述多传感器信息融合的自主导航系统用于执行如权利要求1-9中任一项所述多传感器信息融合的自主导航方法中的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311549558.0A CN117268373B (zh) | 2023-11-21 | 2023-11-21 | 一种多传感器信息融合的自主导航方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311549558.0A CN117268373B (zh) | 2023-11-21 | 2023-11-21 | 一种多传感器信息融合的自主导航方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117268373A true CN117268373A (zh) | 2023-12-22 |
| CN117268373B CN117268373B (zh) | 2024-02-13 |
Family
ID=89204870
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311549558.0A Active CN117268373B (zh) | 2023-11-21 | 2023-11-21 | 一种多传感器信息融合的自主导航方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117268373B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110412635A (zh) * | 2019-07-22 | 2019-11-05 | 武汉大学 | 一种环境信标支持下的gnss/sins/视觉紧组合方法 |
| US20200309529A1 (en) * | 2019-03-29 | 2020-10-01 | Trimble Inc. | Slam assisted ins |
| CN112950781A (zh) * | 2021-03-19 | 2021-06-11 | 中山大学 | 特种场景的多传感器动态加权融合的点云地图构建方法 |
| US20220018962A1 (en) * | 2020-07-16 | 2022-01-20 | Beijing Tusen Weilai Technology Co., Ltd. | Positioning method and device based on multi-sensor fusion |
| EP3955030A1 (en) * | 2020-08-10 | 2022-02-16 | ANavS GmbH | Apparatus for precise positioning, mapping or object detection |
-
2023
- 2023-11-21 CN CN202311549558.0A patent/CN117268373B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20200309529A1 (en) * | 2019-03-29 | 2020-10-01 | Trimble Inc. | Slam assisted ins |
| CN110412635A (zh) * | 2019-07-22 | 2019-11-05 | 武汉大学 | 一种环境信标支持下的gnss/sins/视觉紧组合方法 |
| US20220018962A1 (en) * | 2020-07-16 | 2022-01-20 | Beijing Tusen Weilai Technology Co., Ltd. | Positioning method and device based on multi-sensor fusion |
| EP3955030A1 (en) * | 2020-08-10 | 2022-02-16 | ANavS GmbH | Apparatus for precise positioning, mapping or object detection |
| CN112950781A (zh) * | 2021-03-19 | 2021-06-11 | 中山大学 | 特种场景的多传感器动态加权融合的点云地图构建方法 |
Non-Patent Citations (2)
| Title |
|---|
| XINGXING LI ET.AL: "Progress and Achievements of Multi-sensor Fusion Navigation in China during 2019-2023", JOURNAL OF GEODESY AND GEOINFORMATION SCIENCE, vol. 6, no. 3, pages 102 - 114 * |
| 王雅仪 等: "线面特征结合的三维激光SLAM 算法及性能评估", 导航定位学报, vol. 9, no. 1, pages 122 - 129 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117268373B (zh) | 2024-02-13 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10295365B2 (en) | State estimation for aerial vehicles using multi-sensor fusion | |
| CN109059906B (zh) | 车辆定位方法、装置、电子设备、存储介质 | |
| Bijjahalli et al. | Advances in intelligent and autonomous navigation systems for small UAS | |
| EP3470787B1 (en) | Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav) | |
| US9071829B2 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
| WO2020048623A1 (en) | Estimation of a pose of a robot | |
| CN113406682A (zh) | 一种定位方法、装置、电子设备及存储介质 | |
| CN110824453A (zh) | 一种基于图像跟踪与激光测距的无人机目标运动估计方法 | |
| CN114019552A (zh) | 一种基于贝叶斯多传感器误差约束的定位置信度优化方法 | |
| Li et al. | Multi-GNSS PPP/INS/Vision/LiDAR tightly integrated system for precise navigation in urban environments | |
| Mostafa et al. | A smart hybrid vision aided inertial navigation system approach for UAVs in a GNSS denied environment | |
| Andert et al. | Optical-aided aircraft navigation using decoupled visual SLAM with range sensor augmentation | |
| Andert et al. | On the safe navigation problem for unmanned aircraft: Visual odometry and alignment optimizations for UAV positioning | |
| CN113436261B (zh) | 一种面向封闭园区自动驾驶的单目视觉惯性定位方法 | |
| CN115031718A (zh) | 一种多传感器融合的无人船同步定位与建图方法(slam)及系统 | |
| Wen et al. | 3D vision aided GNSS real-time kinematic positioning for autonomous systems in urban canyons | |
| Afia et al. | A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments | |
| Ćwian et al. | GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments | |
| CN117268373B (zh) | 一种多传感器信息融合的自主导航方法及系统 | |
| CN115523920B (zh) | 一种基于视觉惯性gnss紧耦合的无缝定位方法 | |
| CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
| Medagoda et al. | Water column current profile aided localisation combined with view-based SLAM for autonomous underwater vehicle navigation | |
| CN116105729A (zh) | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 | |
| Quist | UAV navigation and radar odometry | |
| Volden et al. | Development and experimental evaluation of visual-acoustic navigation for safe maneuvering of unmanned surface vehicles in harbor and waterway areas |
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 | ||
| GR01 | Patent grant |