CN114046790A - 一种因子图双重回环的检测方法 - Google Patents

一种因子图双重回环的检测方法 Download PDF

Info

Publication number
CN114046790A
CN114046790A CN202111232146.5A CN202111232146A CN114046790A CN 114046790 A CN114046790 A CN 114046790A CN 202111232146 A CN202111232146 A CN 202111232146A CN 114046790 A CN114046790 A CN 114046790A
Authority
CN
China
Prior art keywords
loop detection
factor graph
sensor
laser radar
point cloud
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.)
Pending
Application number
CN202111232146.5A
Other languages
English (en)
Inventor
田志宇
曾庆化
孙克诚
刘建业
邵晨
李一能
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202111232146.5A priority Critical patent/CN114046790A/zh
Publication of CN114046790A publication Critical patent/CN114046790A/zh
Pending legal-status Critical Current

Links

Images

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/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/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
    • 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)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种因子图双重回环的检测方法，获取激光雷达、卫星、IMU等多源传感器采集的量测信息，依据自适应因子图多源信息融合导航算法获取状态信息，基于状态约束进行粗回环检测，若通过，则利用激光雷达点云图进行精确回环检测，若双重回环检测都通过，则提取状态信息进行因子图回环修正；若未通过，则继续检测下一时刻节点。本发明可以应用于城区定位、车辆测速、姿态对准等场景；本发明鲁棒性好且时间复杂度低，可缩短因子图回环检测时间。

Description

一种因子图双重回环的检测方法
技术领域
本发明属于因子图多源信息融合导航算法领域，特别涉及一种因子图双重回环的检测方法。
背景技术
因子图是一种双向概率图模型，内部包含两种节点类型：一种是变量节点，代表全局多元函数中的变量；一种是因子节点，代表因式分解中的局部函数。每个局部函数只与全局函数的部分变量相关，当且仅当变量是局部函数的自变量时，该因子节点与之对应的变量节点间存在一条连接边。因子图作为一种分析问题的图形工具在很多领域都得到了应用，例如：信号处理、人工智能、组合导航等。在组合导航领域，因子图多源信息融合导航算法以其传感器信息丰富、即插即用等特点成为了研究领域热门。例如结合车辆传感器以及城市环境信息完成城区导航定位，或者在强气流干扰下排除故障传感器实现飞机鲁棒导航等。
在组合导航过程中，位姿的估计往往存在递推的过程，即由上一帧位姿计算当前帧位姿，这一过程中误差发散会造成较大的累计误差，回环检测可有效修正此类推算误差。回环检测应用于激光雷达、视觉SLAM领域，它能使导航***准确识别曾经去过的位置，通过辨识过去的位置信息，来解决位姿推算的漂移问题。例如在城市峡谷或室内等缺乏可靠几何式定位信息情况下进行多源信息融合导航，推算式传感器误差随时间漂移是影响导航精度的首要因素，传统的自适应因子算法难以满足实际应用场景。
发明内容
为了解决上述现有技术存在的问题，本发明提供了一种引入状态约束的因子图双重回环检测方法。
为了实现上述目的，本发明提供了一种因子图双重回环的检测方法，具体包括如下步骤：
步骤1：获取卫星、惯导以及激光雷达传感器量测信息；
步骤2：将***状态变量定义为因子图的变量节点，将传感器量测信息定义为因子节点，再利用自适应方法改善误差模型，从而构建自适应因子图模型，通过因子图优化方法获取当前时刻状态变量，经统计分析提取状态变量分类特征，对比各时刻分类特征进行粗回环检测，若通过，则锁定精确回环检测的关键帧；
步骤3：将经过步骤2粗回环检测的激光雷达量测信息进行神经网络训练，获得适用于预定场景的网络结构，从而利用网络对步骤2锁定的两个关键帧激光雷达量测信息进行点云类特征提取，获取分类特征，构建两帧间相似函数进行精确回环检测，若通过，则确认两个时刻完成回环检测；
步骤4：利用步骤3的激光雷达量测信息获取状态变换信息添加回环因子修正。
进一步的，所述步骤1中的传感器量测信息包括卫星解算的经纬度信息、惯导传感器输出的比力和角速度信息以及激光雷达输出的点云信息。
进一步的，所述步骤2中针对因子图构型以及传感器的不同选型，当前时刻状态变量可选取例如经纬度、水平速度、姿态等多种信息作为约束条件，本发明实例选取经度和纬度的位置信息为例。
进一步的，所述步骤2具体为：
步骤2.1：利用步骤1的量测信息构建因子图模型，并在因子图中添加自适应方法修正误差模型进行优化；
步骤2.2：利用当前时刻状态变量进行粗回环检测，采用的搜索策略为KD树查找。查找相对于匹配点距离最近的k个点，计算k个点相对于匹配点的时间间隔，若时间间隔大于预设值且间隔距离小于预设值，则完成回环检测，进入步骤3；否则结束回环检测，等待下一时刻输入重新执行。
进一步的，所述步骤2中自适应因子图模型为
Figure BDA0003316399570000021
式中X*为待优化状态变量，
Figure BDA0003316399570000022
为目标函数f(x)取最小值时的变量值x，
Figure BDA0003316399570000023
为第1～i时刻的目标函数总和，
Figure BDA0003316399570000024
为目标函数f(x)的马氏范数，
Figure BDA0003316399570000025
为权重函数，hk(Xi)为i时刻第k个传感器状态预测量，zi,k为i时刻第k个传感器量测量，根据各传感器的残差
Figure BDA0003316399570000026
调整传感器因子节点权重。具体权重函数
Figure BDA0003316399570000027
如下
Figure BDA0003316399570000028
式中exp(x)为指数函数，
Figure BDA0003316399570000029
表示传感器的残差值，r0表示传感器残差阈值，传感器的定位精度越高，r0值越低，即权重变化越小。γ为表征各传感器定位精度的参数，γ决定权重变化的快慢。
进一步的，所述步骤3具体为：
步骤3.1：利用步骤1获得的激光雷达点云数据进行PointNet训练，获得适用于预设环境的网络；
步骤3.2：利用PointNet网络对步骤2获得的两个匹配点进行特征提取，获得点云图对应的m个分类特征；
步骤3.3：针对m个分类特征计算汉明距离进行相似度判断，若相似度达到预设值以上，则完成精确回环检测，进入下一步回环修正；否则结束回环检测，等待下一时刻输入重新执行。
进一步的，所述步骤3中的PointNet方法具体为一种点云处理网络，可直接输入无序点云进行处理，学习点云数据的全局特征，具体步骤为：
步骤3.1.1：输入变换，利用空间变换网络(STN)对齐输入点云数据；
步骤3.1.2：点云处理，利用多层感知器(MLP)处理点云数据,将3维数据升维至64维提取点云特征；
步骤3.1.3：特征变换，利用STN对齐输入特征；
步骤3.1.4：特征处理，利用MLP处理特征数据，将64维数据升维至1024维；
步骤3.1.5：对称函数处理，利用最大池化对特征进行融合生成1*1024维全局特征；
步骤3.1.6：特征分类，针对输入的1024维点云特征利用MLP进行学习，输出为m个分类特征。
进一步的，所述步骤4具体为：
步骤4.1：利用激光雷达点云信息计算迭代最近点法(ICP)，获取两个时刻间的位姿变换信息和对应误差函数；
步骤4.2：将两个时刻间的位姿变换信息和对应误差函数输入因子图中，建立回环因子节点，进而实现全局优化。
本发明相对于现有技术的有益效果是：本发明对于常规的应用场景具有较好的效果，可应用于卫星拒止下车载组合导航以及机载多源传感器测速、姿态修正等领域，使用过程具备高鲁棒性、高可靠性，在缺乏可靠几何式定位信息的场景中，本发明利用状态约束减小了回环检测的时间复杂度，本发明实现了引入状态约束的因子图回环检测功能。
附图说明
图1为本发明整体算法流程图；
图2为引入状态约束的因子图结构示意图；
具体实施方式
构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。
下面结合附图对本发明作进一步说明：
如图1所示，本发明公开了一种因子图双重回环的检测方法，包含如下具体步骤：
步骤1：获取卫星、惯导以及激光雷达传感器量测信息；
具体的，传感器量测信息包括卫星解算的经纬度信息、惯导传感器输出的比力和角速度信息以及激光雷达输出的点云信息。在构建因子图模型前，需对不同传感器量测信息进行预处理，包括各因子节点时间匹配及IMU预积分。因子节点匹配采用激光雷达的采集频率10Hz作为基准，IMU数据采集频率为100Hz，通过预积分计算出相邻两节点间的相对位姿实现同步，GPS数据采集频率为1Hz,GPS数据利用时间戳进行匹配。
步骤2：将***状态变量定义为因子图的变量节点，将传感器量测信息定义为因子节点，再利用自适应方法改善误差模型，从而构建自适应因子图模型，通过因子图优化方法获取当前时刻状态变量，经统计分析提取状态变量分类特征，对比各时刻分类特征进行粗回环检测，若通过，则锁定精确回环检测的关键帧，继续执行步骤3；若未通过，则执行下一时刻步骤1；
针对因子图构型以及传感器的不同选型，当前时刻状态变量可选取例如经纬度、水平速度、姿态等多种信息作为约束条件，本发明实例选取经度和纬度的位置信息为例。
所述步骤2具体为：
步骤2.1：利用步骤1的量测信息构建因子图模型，并在因子图中添加自适应方法修正误差模型进行优化；
步骤2.2：利用当前时刻状态变量进行粗回环检测，采用的搜索策略为KD树查找。查找相对于匹配点距离最近的k个点，计算k个点相对于匹配点的时间间隔，若时间间隔大于预设值且间隔距离小于预设值，则完成回环检测，进入步骤3；否则结束回环检测，等待下一时刻输入重新执行。
具体的，在本实施例中国，步骤2中自适应因子图模型为
Figure BDA0003316399570000041
式中X*为待优化状态变量，
Figure BDA0003316399570000042
为目标函数f(x)取最小值时的变量值x，
Figure BDA0003316399570000043
为第1～i时刻的目标函数总和，
Figure BDA0003316399570000044
为目标函数f(x)的马氏范数，
Figure BDA0003316399570000045
为权重函数，hk(Xi)为i时刻第k个传感器状态预测量，zi,k为i时刻第k个传感器量测量，根据各传感器的残差
Figure BDA0003316399570000046
调整传感器因子节点权重。具体权重函数
Figure BDA0003316399570000047
如下
Figure BDA0003316399570000051
式中exp(x)为指数函数，
Figure BDA0003316399570000052
表示传感器的残差值，r0表示传感器残差阈值，传感器的定位精度越高，r0值越低，即权重变化越小。γ为表征各传感器定位精度的参数，γ决定权重变化的快慢。在发明实例中，车载组合导航实验所采用的惯导***为Xsens Mti-10系列传感器，陀螺常值漂移为18°/h。激光雷达传感器采用了HDL-32E，实时点云匹配误差能低至0.2cm，有效范围为80m。GPS接收机采用了常用的u-blox M8T型号，由于城区多路径效应导致卫星定位精度较低，平均定位精度15～20m。在设计自适应权重时，参考传感器性能进行调整后，获得激光雷达、IMU、卫星传感器的自适应权重为2,4,8。
另外，步骤2中采用的搜索策略为KD树查找。查找相对于匹配点位置距离最近的k个点，计算k个点相对于匹配点的时间间隔，若时间间隔大于预设值且间隔距离小于预设值，则完成回环检测，进入下一步精确回环检测；否则结束回环检测，等待下一时刻输入重新执行。
步骤3：将激光雷达量测信息进行神经网络训练，获得适用于预定场景的网络结构，从而利用网络对步骤2锁定的两个关键帧激光雷达量测信息进行点云类特征提取，获取分类特征，构建两帧间相似函数进行精确回环检测，若通过，则确认两个时刻完成回环检测，继续执行步骤4；若未通过，则执行下一时刻步骤1；
具体的，步骤3分为以下步骤：
步骤3.1：利用步骤1获得的激光雷达点云数据进行PointNet训练，获得适用于预设环境的网络；
步骤3.2：利用PointNet网络对步骤2获得的两个匹配点进行特征提取，获得点云图对应的m个分类特征；
步骤3.3：针对m个分类特征计算汉明距离进行相似度判断，若相似度达到预设值以上，则完成精确回环检测，进入下一步回环修正；否则结束回环检测，等待下一时刻输入重新执行。
另外，在本实施例中，所述步骤3中的PointNet方法具体为一种点云处理网络，可直接输入无序点云进行处理，学习点云数据的全局特征，具体步骤为：
步骤3.1.1：输入变换，利用空间变换网络(STN)对齐输入点云数据，网络生成一个仿射变换矩阵来对点云的旋转、平移等变化进行规范化处理，输入为原始点云数据，输出为一个3*3的旋转矩阵；
步骤3.1.2：点云处理，利用多层感知器(MLP)处理点云数据,将n*3的点云输入，针对每个点采用64*64的两层感知器来进行处理，将每个点的信息进行学习提取,将3维数据升维至64维提取点云特征；
步骤3.1.3：特征变换，针对输入的64维点云特征采用STN网络进行对齐；
步骤3.1.4：特征处理，利用MLP处理特征数据，将64维数据升维至1024维；
步骤3.1.5：对称函数处理，利用最大池化对n*1024的点云特征融合生成1*1024维的特征进行描述；
步骤3.1.6：特征分类，针对输入的1024维点云特征利用MLP进行学习，输出为m个分类特征，代表分类的类别，每个类别对应点云分类得分。
步骤4：利用经过步骤3的激光雷达量测信息获取状态变换信息添加回环因子修正。
具体的，在本实施例中，步骤4具体为：
步骤4.1：输入激光雷达点云信息利用迭代最近点法(ICP)配准两个时刻间的点云数据，通过最小二乘法迭代计算最优的坐标变化，即旋转矩阵R和平移矢量t，使得误差函数最小，从而获取两个时刻间的位姿变换信息和对应误差函数；
步骤4.2：将两个时刻间的位姿变换信息和对应误差函数输入因子图中，在两个匹配时刻间建立回环因子节点，进而实现全局优化。
所述步骤2～4中因子图结构如图2所示，初始时刻添加先验因子节点
Figure BDA0003316399570000061
为第i时刻变量节点添加与其连接的GPS一元因子节点
Figure BDA0003316399570000062
与相邻两时刻变量节点连接的IMU二元因子节点
Figure BDA0003316399570000063
以及相邻两时刻变量节点连接的Lidar二元因子节点
Figure BDA0003316399570000064
双重回环检测方法具体为：先进行粗回环检测，即利用GPS位置状态信息检测是否满足GPS回环要求
Figure BDA0003316399570000065
再进行精确回环检测，即利用激光雷达点云信息检测是否满足Lidar回环要求
Figure BDA0003316399570000066
若通过双重回环检测，则添加一个回环因子fback进行修正；否则回环检测失败，等待下一时刻输入重新执行。
另外，本发明实施例还提供一种计算机可读存储介质，其中，该计算机可读存储介质可存储有程序，该程序执行时包括上述方法实施例中记载的任何因子图双重回环的检测方法的部分或全部步骤。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储器中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储器中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储器包括：U盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储器中，存储器可以包括：闪存盘、只读存储器(英文：Read-Only Memory，简称：ROM)、随机存取器(英文：Random Access Memory，简称：RAM)、磁盘或光盘等。
上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (8)

1.一种因子图双重回环的检测方法，其特征在，包括：
步骤一、获取卫星、惯导以及激光雷达传感器量测信息；
步骤二、将***状态变量定义为因子图的变量节点，将所述激光雷达传感器量测信息定义为因子节点，再利用自适应方法改善误差模型，从而构建自适应因子图模型，通过因子图优化方法获取当前时刻状态变量，经统计分析提取状态变量分类特征，对比各时刻分类特征进行粗回环检测，从而锁定精确回环检测的关键帧；
步骤三、将经过步骤二粗回环检测的激光雷达量测信息进行神经网络训练，获得适用于预定场景的网络结构，从而利用神经网络对所述关键帧的激光雷达量测信息进行点云类特征提取，获取分类特征，构建两帧间相似函数进行精确回环检测，若通过，则确认两个时刻完成回环检测；
步骤四、利用经过步骤三精确回环检测的激光雷达量测信息获取状态变换信息添加回环因子修正。
2.根据权利要求1所述的检测方法，其特征在于：所述步骤二中针对因子图构型以及传感器的不同选型，当前时刻状态变量的约束条件包括经纬度、水平速度、姿态中的一种或多种。
3.根据权利要求1所述的检测方法，其特征在于：所述步骤二包括：
利用步骤一的量测信息构建因子图模型，并在因子图中添加自适应方法修正误差模型进行优化；
利用当前时刻状态变量进行粗回环检测，采用的搜索策略为KD树查找；查找相对于匹配点距离最近的k个点，计算k个点相对于匹配点的时间间隔，若时间间隔大于预设值且间隔距离小于预设值，则完成回环检测。
4.根据权利要求3所述的检测方法，其特征在于，所述步骤二中自适应因子图模型为
Figure FDA0003316399560000011
式中X*为待优化状态变量，
Figure FDA0003316399560000012
为目标函数f(x)取最小值时的变量值x，
Figure FDA0003316399560000013
为第1～i时刻的目标函数总和，
Figure FDA0003316399560000014
为目标函数f(x)的马氏范数，
Figure FDA0003316399560000015
为权重函数，hk(Xi)为i时刻第k个传感器状态预测量，zi,k为i时刻第k个传感器量测量，根据各传感器的残差
Figure FDA0003316399560000016
调整传感器因子节点权重。
5.根据权利要求4所述的检测方法，其特征在于：所述权重函数
Figure FDA0003316399560000017
如下
Figure FDA0003316399560000021
式中exp(x)为指数函数，
Figure FDA0003316399560000022
表示传感器的残差值，r0表示传感器残差阈值，传感器的定位精度越高，r0值越低，即权重变化越小，γ为表征各传感器定位精度的参数。
6.根据权利要求1所述的检测方法，其特征在于，所述步骤三包括：
利用步骤一获得的激光雷达点云数据进行PointNet训练，获得适用于预设环境的网络；利用PointNet网络对步骤三获得的两个匹配点进行特征提取，获得点云图对应的m个分类特征；
针对m个分类特征计算汉明距离进行相似度判断，若相似度达到预设值以上，则完成精确回环检测，进入下一步回环修正；否则结束回环检测，等待下一时刻输入重新执行。
7.根据权利要求1所述的检测方法，其特征在于，所述步骤四包括：
利用激光雷达点云信息计算迭代最近点法，获取两个时刻间的位姿变换信息和对应误差函数；
将两个时刻间的位姿变换信息和对应误差函数输入因子图中，建立回环因子节点，进而实现全局优化。
8.一种计算机可读存储介质，其特征在于，包括：所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7任一项所述的一种因子图双重回环的检测方法的步骤。
CN202111232146.5A 2021-10-22 2021-10-22 一种因子图双重回环的检测方法 Pending CN114046790A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111232146.5A CN114046790A (zh) 2021-10-22 2021-10-22 一种因子图双重回环的检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111232146.5A CN114046790A (zh) 2021-10-22 2021-10-22 一种因子图双重回环的检测方法

Publications (1)

Publication Number Publication Date
CN114046790A true CN114046790A (zh) 2022-02-15

Family

ID=80205682

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111232146.5A Pending CN114046790A (zh) 2021-10-22 2021-10-22 一种因子图双重回环的检测方法

Country Status (1)

Country Link
CN (1) CN114046790A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115326068A (zh) * 2022-10-17 2022-11-11 北京理工大学 激光雷达-惯性测量单元融合里程计设计方法及***
CN115685292A (zh) * 2023-01-05 2023-02-03 中国人民解放军国防科技大学 一种多源融合导航***的导航方法和装置

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190219401A1 (en) * 2018-01-12 2019-07-18 The Trustees Of The University Of Pennsylvania Probabilistic data association for simultaneous localization and mapping
CN110274588A (zh) * 2019-06-19 2019-09-24 南京航空航天大学 基于无人机集群信息的双层嵌套因子图多源融合导航方法
CN111325794A (zh) * 2020-02-23 2020-06-23 哈尔滨工业大学 一种基于深度卷积自编码器的视觉同时定位与地图构建方法
CN111337020A (zh) * 2020-03-06 2020-06-26 兰州交通大学 引入抗差估计的因子图融合定位方法
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及***
CN113432600A (zh) * 2021-06-09 2021-09-24 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及***

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20190219401A1 (en) * 2018-01-12 2019-07-18 The Trustees Of The University Of Pennsylvania Probabilistic data association for simultaneous localization and mapping
CN110274588A (zh) * 2019-06-19 2019-09-24 南京航空航天大学 基于无人机集群信息的双层嵌套因子图多源融合导航方法
CN111325794A (zh) * 2020-02-23 2020-06-23 哈尔滨工业大学 一种基于深度卷积自编码器的视觉同时定位与地图构建方法
CN111337020A (zh) * 2020-03-06 2020-06-26 兰州交通大学 引入抗差估计的因子图融合定位方法
CN112304307A (zh) * 2020-09-15 2021-02-02 浙江大华技术股份有限公司 一种基于多传感器融合的定位方法、装置和存储介质
CN112907491A (zh) * 2021-03-18 2021-06-04 中煤科工集团上海有限公司 一种适用于井下巷道的激光点云回环检测方法及***
CN113432600A (zh) * 2021-06-09 2021-09-24 北京科技大学 基于多信息源的机器人即时定位与地图构建方法及***

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
倪志康;厉茂海;林睿;孙立宁;刘仕琦;: "基于三维激光雷达与RTK融合的SLAM研究", 制造业自动化, no. 07, 25 July 2020 (2020-07-25) *
周雅婧 等: "基于因子图的组合导航算法研究", 《第十一届中国卫星导航年会论文集——S10 PNT体系与多源融合导航》, 23 November 2020 (2020-11-23) *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115326068A (zh) * 2022-10-17 2022-11-11 北京理工大学 激光雷达-惯性测量单元融合里程计设计方法及***
CN115685292A (zh) * 2023-01-05 2023-02-03 中国人民解放军国防科技大学 一种多源融合导航***的导航方法和装置

Similar Documents

Publication Publication Date Title
Clark et al. Vinet: Visual-inertial odometry as a sequence-to-sequence learning problem
Yin et al. 3d lidar-based global localization using siamese neural network
CN109682382B (zh) 基于自适应蒙特卡洛和特征匹配的全局融合定位方法
CN109800689B (zh) 一种基于时空特征融合学习的目标跟踪方法
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN113313763B (zh) 一种基于神经网络的单目相机位姿优化方法及装置
CN110631588B (zh) 一种基于rbf网络的无人机视觉导航定位方法
CN114046790A (zh) 一种因子图双重回环的检测方法
US11830218B2 (en) Visual-inertial localisation in an existing map
Yin et al. Pse-match: A viewpoint-free place recognition method with parallel semantic embedding
Wen et al. CAE-RLSM: Consistent and efficient redundant line segment merging for online feature map building
CN113532438B (zh) 一种大初始定位误差下的改进iccp地形匹配方法
Goldman et al. Robust epipolar geometry estimation using noisy pose priors
CN113112547A (zh) 机器人及其重定位方法、定位装置及存储介质
CN117053779A (zh) 一种基于冗余关键帧去除的紧耦合激光slam方法及装置
Zhou et al. Place recognition and navigation of outdoor mobile robots based on random Forest learning with a 3D LiDAR
Ali et al. A life-long SLAM approach using adaptable local maps based on rasterized LIDAR images
Morell et al. 3d maps representation using gng
Le Barz et al. Absolute geo-localization thanks to Hidden Markov Model and exemplar-based metric learning
Roznovjak Feature reprojection as image alignment score in GNSS-free UAV localization
Ravindranath et al. 3D-3D Self-Calibration of Sensors Using Point Cloud Data
Mäkelä Deep Feature UAV Localization in Urban Areas and Agricultural Fields and Forests
Mukherjee Design of Real Time System for SLAM and Semantic Mapping
Ling et al. Benchmarking Classical and Learning-Based Multibeam Point Cloud Registration
CN115265560A (zh) 基于视觉slam的高精定位方法、***及存储介质

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