CN117451043A - 一种数-模混合估计的多源融合定位方法及系统 - Google Patents

一种数-模混合估计的多源融合定位方法及系统 Download PDF

Info

Publication number
CN117451043A
CN117451043A CN202311793765.0A CN202311793765A CN117451043A CN 117451043 A CN117451043 A CN 117451043A CN 202311793765 A CN202311793765 A CN 202311793765A CN 117451043 A CN117451043 A CN 117451043A
Authority
CN
China
Prior art keywords
factor
observation
driving module
uncertainty
imu
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
Application number
CN202311793765.0A
Other languages
English (en)
Other versions
CN117451043B (zh
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.)
Wuhan University WHU
Original Assignee
Wuhan University WHU
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 Wuhan University WHU filed Critical Wuhan University WHU
Priority to CN202311793765.0A priority Critical patent/CN117451043B/zh
Publication of CN117451043A publication Critical patent/CN117451043A/zh
Application granted granted Critical
Publication of CN117451043B publication Critical patent/CN117451043B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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

Abstract

一种数‑模混合估计的多源融合定位方法及系统，涉及多传感器融合定位技术领域，其利用IMU数据驱动模块回归传播调整因子、IMU不确定度因子和伪观测因子；利用LiDAR数据驱动模块计算场景特征退化率；根据IMU不确定度因子和场景特征退化率确定融合定位系统的不确定度；根据传播调整因子动态调整模型驱动传播预测时的噪声协方差，当场景特征退化率超过阈值时，仅使用伪观测因子进行观测更新，并结合不确定度进行位姿估计。本申请通过基于数据驱动的方法为基于模型驱动的滤波器系统提供自适应的参数调整，来适应不同场景和不同运动状态的变化。增强了系统在复杂场景和运动下的鲁棒性，综合考虑了传感器数据和系统模型的不确定度，能提供更可靠的结果置信度。

Description

一种数-模混合估计的多源融合定位方法及系统
技术领域
本申请涉及融合定位技术领域，具体涉及一种数-模混合估计的多源融合定位方法及系统。
背景技术
在当今自动驾驶和机器人领域，实现准确、鲁棒的定位是关键问题之一。随着自主移动平台的迅速发展，特别是在复杂和未知环境中，高精度的定位和导航成为了确保安全和可靠性的关键任务。为了实现这一目标，同时构建未知环境的三维地图并进行实时定位，即同步定位与地图构建（SLAM），已经成为一种广泛采用的策略。
光探测和测距（LiDAR）传感器由于其能够直接提供高精度的三维信息，受到了自动驾驶和机器人领域的广泛应用。相较于基于相机的方法，LiDAR-SLAM方法在光照条件不稳定的情况下仍然能够实现准确感知，因此在场景感知中具备更高的鲁棒性。然而，当环境中的场景特征退化或缺失时，传统的LiDAR定位方法容易产生漂移误差，导致定位不准确。
为了克服这些局限性，近年来，紧密耦合LiDAR-IMU融合技术受到了广泛研究。该技术将激光雷达和惯性测量单元（IMU）的测量数据结合，利用IMU的加速度计和陀螺仪数据来提供姿态信息和运动状态的估计，从而弥补LiDAR在一些情况下的不足。然而，现有的LiDAR-IMU融合方法主要基于滤波或优化技术进行定位估计。这些方法通常仅使用传感器的几何信息，如深度、加速度、陀螺仪等，未充分挖掘传感器数据的高层次观测特征信息。而基于模型驱动的方法虽然具有较高的稳定性和效率，但过于依赖领域知识和模型假设的准确性，并且通常需要手动调整参数。同时，现有的定位方法很少考虑到结果的不确定度问题。当传感器故障发生或者环境缺乏足够的特征时，由于缺少不确定度估计，系统可能无条件地接受不准确的预测结果，从而导致依赖该输出的系统容易出现故障。
发明内容
本申请提供一种数-模混合估计的多源融合定位方法及系统，其可自适应调整滤波器参数，增强了定位系统在复杂场景和运动状态下的鲁棒性，且综合考虑了传感器数据和模型的不确定度，能提供更可靠的结果置信度。
第一方面，本申请实施例提供一种数-模混合估计的多源融合定位方法，所述一种数-模混合估计的多源融合定位方法包括：
根据IMU数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；
基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；
根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度；
根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，并结合不确定度以进行位姿估计。
结合第一方面，在一种实施方式中，所述基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率，包括：
同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；
将二维距离图像输入至LiDAR数据驱动模块的LiDAR全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。
结合第一方面，在一种实施方式中，所述根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度，包括：
根据公式：，计算所述不确定度U，其中，a 2 为IMU不确定度因子，P为场景特征退化率，σ为基于EKF模型驱动的协方差。
结合第一方面，在一种实施方式中，所述根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，包括：
根据公式：Q bw ′ = Q bw × 10^(a 1 w )，更新陀螺仪噪声b w 的噪声协方差Q bw ；
根据公式：Q ba ′ = Q ba × 10^(a 1 a )，更新加速度计噪声b a 的噪声协方差Q ba ；
其中，a 1 w 为陀螺仪传播调整因子，a 1 a 为加速度计传播调整因子。
结合第一方面，在一种实施方式中，所述当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，包括：
构造速度约束：
y k hor = h hor (x k ) + n k hor
y k ver = h ver (x k ) + n k ver
其中，y k hor 和y k ver 分别为伪测量值y k 水平方向和垂直方向的分量，h(·)为EKF模型驱动模块的观测函数，n k 为服从高斯分布的第k个观测噪声，对应的协方差矩阵为R 2×2 ，且R 2 ×2 满足：
R ’ 11 = R 11 × 10 ^ (a 3 hor )
R ’ 22 = R 22 × 10 ^ (a 3 ver )
其中，a 3 hor 为伪观测因子水平方向的分量，a 3 ver 为伪观测因子垂直方向的分量；
使用伪测量值[y k hor , y k ver ] =[0,0]为状态更新提供过滤器。
第二方面，本申请实施例提供一种数-模混合估计的多源融合定位系统，所述数-模混合估计的多源融合定位系统包括：
IMU数据驱动模块，其用于采集连续多帧采集的陀螺仪的角速度和加速度计的加速度，以回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；
LiDAR数据驱动模块，其基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；
EKF模型驱动模块，其根据所述传播调整因子动态调整传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行观测更新；
所述EKF模型驱动模块还根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度。
结合第二方面，在一种实施方式中，所述LiDAR数据驱动模块基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率，包括：
同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；
将二维距离图像输入至LiDAR数据驱动模块的LiDAR全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。
结合第二方面，在一种实施方式中，所述EKF模型驱动模块根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度，包括：
根据公式：，计算所述不确定度U，其中，a 2 为IMU不确定度因子，P为场景特征退化率，σ为基于EKF模型驱动的协方差。
结合第二方面，在一种实施方式中，所述EKF模型驱动模块根据所述传播调整因子动态调整传播预测时的噪声协方差，包括：
根据公式：Q bw ′ = Q bw × 10^(a 1 w )，更新陀螺仪噪声b w 的噪声协方差Q bw ；
根据公式：Q ba ′ = Q ba × 10^(a 1 a )，更新加速度计噪声b a 的噪声协方差Q ba ；
其中，a 1 w 为陀螺仪传播调整因子，a 1 a 为加速度计传播调整因子。
结合第二方面，在一种实施方式中，所述当场景特征退化率超过设定阈值时，EKF模型驱动模块仅使用所述伪观测因子进行观测更新，包括：
构造速度约束：
y k hor = h hor (x k ) + n k hor
y k ver = h ver (x k ) + n k ver
其中，y k hor 和y k ver 分别为伪测量值y k 水平方向和垂直方向的分量，h(·)为EKF模型驱动模块的观测函数，n k 为服从高斯分布的第k个观测噪声，对应的协方差矩阵为R 2×2 ，且R 2 ×2 满足：
R ’ 11 = R 11 × 10 ^ (a 3 hor )
R ’ 22 = R 22 × 10 ^ (a 3 ver )
其中，a 3 hor 为伪观测因子水平方向的分量，a 3 ver 为伪观测因子垂直方向的分量；
使用伪测量值[y k hor , y k ver ] =[0,0]为状态更新提供过滤器。
本申请实施例提供的技术方案带来的有益效果至少包括：
本申请中的数-模混合估计的多源融合定位方法，其根据IMU数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度；根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，并结合不确定度以进行位姿估计。
和现有技术相比，本申请将LiDAR和IMU的数据驱动与基于EKF的模型驱动相结合，充分发挥两者的优势，实现更准确的定位；通过IMU数据驱动模块实时感知运动状态，以及通过LiDAR数据驱动模块感知场景特征变化，进而通过两者共同作用来自适应调整基于模型驱动的滤波器参数，也即通过基于数据驱动的方法为基于模型驱动的滤波器系统提供自适应的参数调整，来适应不同场景和不同运动状态的变化。增强了定位系统在复杂场景和运动下的鲁棒性，此外，还引入了不确定度模型，综合考虑传感器数据和系统模型的不确定度，提供更可靠的结果置信度。
附图说明
图1为本申请数-模混合估计的多源融合定位方法一实施例的流程示意图；
图2为本申请数-模混合估计的多源融合定位系统的结构框图；
图3为本申请IMU数据驱动模块的结构框图；
图4为本申请LiDAR数据驱动模块的结构框图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本申请的说明书和权利要求书及上述附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。术语“第一”、“第二”和“第三”等描述，是用于区分不同的对象等，其不代表先后顺序，也不限定“第一”、“第二”和“第三”是不同的类型。
在本申请实施例的描述中，“示例性的”、“例如”或者“举例来说”等用于表示作例子、例证或说明。本申请实施例中被描述为“示例性的”、“例如”或者“举例来说”的任何实施例或设计方案不应被解释为比其它实施例或设计方案更优选或更具优势。确切而言，使用“示例性的”、“例如”或者“举例来说”等词旨在以具体方式呈现相关概念。
在本申请实施例的描述中，除非另有说明，“/”表示或的意思，例如，A/B可以表示A或B；文本中的“和/或”仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如，A和/或B，可以表示：单独存在A，同时存在A和B，单独存在B这三种情况，另外，在本申请实施例的描述中，“多个”是指两个或多于两个。
在本申请实施例描述的一些流程中，包含了按照特定顺序出现的多个操作或步骤，但是应该理解，这些操作或步骤可以不按照其在本申请实施例中出现的顺序来执行或并行执行，操作的序号仅用于区分开各个不同的操作，序号本身不代表任何的执行顺序。另外，这些流程可以包括更多或更少的操作，并且这些操作或步骤可以按顺序执行或并行执行，并且这些操作或步骤可以进行组合。
为使本申请的目的、技术方案和优点更加清楚，下面将结合附图对本申请实施方式作进一步地详细描述。
第一方面，本申请实施例还提供一种数-模混合估计的多源融合定位方法。
本发明的构思在于通过IMU数据驱动模块实时感知运动状态，以及通过LiDAR数据驱动模块感知场景特征变化，进而通过两者共同作用来自适应调整基于模型驱动的滤波器参数，也即通过基于数据驱动的方法为基于模型驱动的滤波器系统提供自适应的参数调整，来适应不同场景和不同运动状态的变化。此外，还综合考虑了传感器数据和系统模型的不确定度，以提供更可靠的结果置信度。
具体实现时，一实施例中，参照图1，图1为本申请数-模混合估计的多源融合定位方法一实施例的流程示意图。如图1所示，数-模混合估计的多源融合定位方法包括：
S1、根据IMU数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子。
S2、基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率。
S3、根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度。
S4、根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，并结合不确定度以进行位姿估计。
下面结合数-模混合估计的多源融合定位系统的框架来对上述步骤做出进一步介绍：
本申请的数-模混合估计的多源融合定位系统的框架可参见图2至图4所示，主要包括三个部分：IMU数据驱动、LiDAR数据驱动、基于EKF的模型驱动状态估计。整个系统的输入是LiDAR和IMU数据，输出每个时间点的位置和对应的不确定度。对于IMU，本申请设计了一个IMU数据驱动状态编码器，可以实时感知运动状态并自适应调整EKF噪声参数。对于LiDAR，本申请提出了一个轻量级的LiDAR数据驱动场景观测编码器网络，用于提取场景中点云的全局特征观测值，并计算不同场景下的特征退化率。对于EKF模型驱动，整体框架基于扩展卡尔曼滤波EKF，使用两个数据驱动的结果动态调整不同运动状态和场景下的EKF模型参数。对于不确定度估计，本发明提出了一种不确定度表征方式，它可以同时表达传感器数据和模型的不确定度，并基于本发明的不确定度，构造了一个损失函数来训练整个网络结构。
具体而言，对于IMU数据驱动：
本申请的初衷是自适应调整不同运动状态下传感器之间的融合参数。因此，本申请设计了一个IMU数据驱动状态编码器，充分利用高频IMU中的加速度计和陀螺仪来感知运动状态特征。本申请的IMU数据驱动状态编码器主要是通过构建一个轻量级的深度神经网络来学习原始IMU传感器数据质量。与LiDAR传感器相比，IMU具有更少的数据量，并且本申请只使用较少的网络参数和层数来构建编码器，以避免过拟合。如图3所示，输入为IMU在时间上连续多帧的陀螺仪的角速度w和加速度计的加速度a，最后回归了当前时刻的三类变量：传播调整因子、IMU不确定度因子和伪观测因子。
传播调整因子：在融合系统中，本申请使用基于EKF的滤波模型来估计实时姿态。用IMU数据预测了EKF从第k-1步到第k步的状态传播过程。然而，现有的EKF融合模型大多假设传播过程中的噪声协方差Q为常数，难以适应不同的运动状态和IMU数据质量。因此，在本申请的IMU数据驱动状态编码器中，本申请输出当前时刻的传播过程调整因子a 1 ，然后使用以下公式实时自适应调整协方差:
Q' = Q × 10 ^ (a 1 )
其中，Q'为自适应更新后的协方差，通过tanh激活函数得到。因此，在不同的运动状态下，本申请可以通过调整函数10 a1 来动态地增加或减少当前时刻的协方差。
不确定度估计因子：现有的里程估计方法，无论是模型驱动还是数据驱动，很少考虑预测位姿对应的不确定度估计。这些系统无法确定预测位置的准确性，而这种假设在不同的运动状态下往往是不正确的，并可能导致灾难性的后果。为了测量IMU数据本身的不确定度，本申请在IMU噪声估计编码器中回归了IMU不确定度因子a 2 ，并结合LiDAR数据对整个融合系统的不确定度建模。
伪观测因子：考虑到地面移动平台横向和垂直速度几乎为零的固有特性，可以构建伪观测来更新EKF。在本申请的IMU噪声估计编码器中，除了计算传播调整因子和不确定因子外，本申请还回归了一个伪观测因子a 3 来调整EKF更新过程中水平和垂直零速度的置信度，因此因子a 3 在两个方向上都包含两个分量，一个水平方向(a 3 hor )和一个垂直方向（a 3 ver ）。因此，本申请构造了一个速度约束:
y k hor = h hor (x k ) + r k hor
y k ver = h ver (x k ) + r k ver
其中，y k hor 和y k ver 分别为伪测量值y k 水平方向和垂直方向的分量，h(·)为EKF模型驱动模块的观测函数，n k 为服从高斯分布的第k个观测噪声，对应的协方差矩阵为R 2×2 ，且R 2 ×2 满足：
R ’ 11 = R 11 × 10 ^ (a 3 hor )
R ’ 22 = R 22 × 10 ^ (a 3 ver )
其中，a 3 hor 为伪观测因子水平方向的分量，a 3 ver 为伪观测因子垂直方向的分量。
然后，使用伪测量值[y k hor , y k ver ] =[0,0]为状态更新提供过滤器。
与直线运动相比，水平方向的零速度观测噪声在转弯时具有更大的不确定度。因此，该方法可以很好地应用于不同运动状态下的自适应动态调整。
对于激光雷达数据驱动：
在本申请的LiDAR数据驱动中，本申请设计了一个轻量级的全局特征观测编码器，通过提取不同激光之间的变化特征来提供LiDAR场景观测，并计算当前时刻的场景特征退化率P，如图3所示。此外，本申请用场景特征退化率来表示LiDAR原始数据的整个系统不确定度，并将场景特征退化率P和IMU不确定度因子a 2 混合，构建了整个融合系统的不确定度模型。
距离图像投影：为了使本申请的网络结构轻量化，计算速度快，从而满足移动平台的实时性要求。通过球面投影将三维原始点云转换为二维距离图像表示，然后利用基于图像的计算机视觉网络提取点云特征。对于任意给定的LiDAR传感器参数，点云和距离图像之间存在确定的投影变换关系，每个点p i = (x, y, z)通过以下方式转换为球坐标和图像坐标(u, v):
其中，r点p i 的深度值，f = f up + f down 为传感器固定固有垂直视场角，w, h为二维距离图像的宽度和高度。接下来，本申请将尺寸为1×h×w的距离图像送到全局观测编码器中。
LiDAR数据驱动场景观测编码器：作为智能系统中的视觉传感器，LiDAR场景的观测对融合定位至关重要。然而，现有的LiDAR-SLAM方法大多基于LOAM，通过计算曲率提取点云的局部平面和角特征，这在退化或复杂场景下难以提取有效且鲁棒的特征。因此，本申请设计了一种LiDAR数据驱动场景观测编码器，从帧的角度提取当前激光的全局观测特征，比局部特征具有更好的容错率。此外，在面对不同场景时，本申请的编码器模块还对当前场景特征退化率P进行了回归，并将其作为LiDAR原始数据的一个不确定因子。
在上一步中，本申请通过球面投影将三维点云转换为二维距离图像表示，作为本申请全局观测特征编码器模块的输入。基于现有的点云特征提取器，同时输入当前第k次和第k-1次的点云数据对，提取当前时刻的全局观测特征和场景特征退化率。最后通过全连通层(FC)回归一维场景特征退化率。
对于卡尔曼滤波模型驱动：
在里程计状态估计中，本申请使用基于扩展卡尔曼滤波(EKF)的模型驱动来融合LiDAR和IMU。与传统的卡尔曼滤波不同，其模型参数在不同的运动状态和场景下是不变的。该模型基于两个数据驱动模块，可以自适应调整滤波器参数。
动力学模型：其运动学动态离散时间非线性模型可表示为:
其中，x k 表示第k步待估计状态，u k 为输入，w k 为过程噪声，假设其为均值为零的高斯分布，协方差矩阵为Q k 。为了融合来自其他传感器的数据，需要使用它们的观测值来更新状态x k ，即：
y k = h(x k ) + n k
其中y k 为传感器数据的观测值，n k 为观测噪声并服从零均值高斯分布，协方差为R k 。在本申请的系统中，本申请使用IMU来预测运动传播，并使用LiDAR作为观测构造约束来更新状态。
本申请定义第k时刻的状态x k 如下：x k = (r k , v k , p k , b w k , b a k )，其中，r k 为方向和姿态信息，v k 为速度信息，p k 为位置信息，b w k 为陀螺仪噪声，b a k 为加速度计噪声。
传播预测：在传播预测中，本申请使用IMU陀螺仪的角速度w和加速度计的加速度a进行从到x k 的状态预测。假设预测噪声为中心高斯噪声w k ~ N(0, Qk)，其中协方差矩阵Q k 反映信息的置信度，Q k 越大，置信度越低。从状态x 0 ~ N(x 0 , Q 0 )的初始高斯置信度开始，其中x 0 表示初始状态估计，协方差矩阵Q 0 表示x 0 对应的不确定度，EKF在预测与更新之间交替进行。在传播步骤中，令/>，则协方差矩阵P k 更新为:
其中是f(·)关于/>的雅可比矩阵。然而，在本申请的传播过程中，噪声协方差/>不是一成不变的。本申请使用IMU数据驱动模块计算每次的传播调整因子a 1 ，并根据下面的公式来更新陀螺仪噪声b w 和加速度计噪声b a 的相应噪声协方差：
Q bw ′ = Q bw × 10^(a 1 w )
Q ba ′ = Q ba × 10^(a 1 a )
其中，a 1 w 为陀螺仪传播调整因子，a 1 a 为加速度计传播调整因子。
观测更新：EKF需要在传播和更新之间交替进行，本申请的更新主要是通过LiDAR观测数据0速度为观测构建约束方程，将状态从先验修正到后验。在融合系统的观测更新中，本申请在传统的平面、角特征观测基础上，增加了基于LiDAR数据驱动的全局特征观测和伪特征观测。LiDAR数据驱动模块感知当前场景的全局特征观测值，计算场景特征退化率P。当场景特征出现严重退化时，LiDAR数据具有副作用，无法与EKF融合。因此，本申请只使用伪观测值进行卡尔曼滤波更新。对于地面移动平台，本申请可以假设它们的横向和垂直速度近似为零，因为这个伪观测值可以用来构建观测方程。因此，在不同的场景和运动状态下，本申请使用构建的两个数据驱动模块为基于EKF的模型驱动提供动态自适应参数调整。
对于不确定度估计：
现有的定位方法很少考虑位姿估计所对应的不确定度或置信度，导致系统在不同复杂场景或运动状态下的状态估计存在严重偏差，仍然认为结果是正确的并接受它。即使有一些方法在现有任务中增加不确定度，如分割、检测和姿态估计，但不确定度主要以协方差表示，不能同时考虑模型和传感器数据的不确定度。为解决上述问题，本申请中设计了一种利用数字模拟驱动来表示不确定度U的建模方法，其公式为:
其中a 2 为IMU数据驱动的IMU不确定度因子，P为LiDAR数据驱动的场景特征退化率，反映了传感器原始数据的不确定度，σ为基于EKF模型驱动的协方差。通过构造该表征方式，本申请同时考虑了传感器数据和模型的不确定度，并提供了与整个融合系统的姿态估计相对应的结果置信度。
综上所述，本申请中的数-模混合估计的多源融合定位方法，其根据IMU数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度；根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，并结合不确定度以进行位姿估计。
和现有技术相比，本申请将LiDAR和IMU的数据驱动与基于EKF的模型驱动相结合，充分发挥两者的优势，实现更准确的定位；通过IMU数据驱动模块实时感知运动状态，以及通过LiDAR数据驱动模块感知场景特征变化，进而通过两者共同作用来自适应调整基于模型驱动的滤波器参数，也即通过基于数据驱动的方法为基于模型驱动的滤波器系统提供自适应的参数调整，来适应不同场景和不同运动状态的变化。增强了定位系统在复杂场景和运动下的鲁棒性，此外，还引入了不确定度模型，综合考虑传感器数据和系统模型的不确定度，提供更可靠的结果置信度。
第二方面，本申请实施例还提供一种数-模混合估计的多源融合定位系统。
一实施例中，参照图2至图4所示，数-模混合估计的多源融合定位系统包括：
IMU数据驱动模块，其用于采集连续多帧采集的陀螺仪的角速度和加速度计的加速度，以回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；
LiDAR数据驱动模块，其基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；
EKF模型驱动模块，其根据所述传播调整因子动态调整传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行观测更新；
所述EKF模型驱动模块还根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度。
进一步地，一实施例中，所述LiDAR数据驱动模块基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率，包括：
同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；
将二维距离图像输入至LiDAR数据驱动模块的LiDAR全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。
进一步地，一实施例中，所述EKF模型驱动模块根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度，包括：
根据公式：，计算所述不确定度U，其中，a 2 为IMU不确定度因子，P为场景特征退化率，σ为基于EKF模型驱动的协方差。
进一步地，一实施例中，所述EKF模型驱动模块根据所述传播调整因子动态调整传播预测时的噪声协方差，包括：
根据公式：Q bw ′ = Q bw × 10^(a 1 w )，更新陀螺仪噪声b w 的噪声协方差Q bw ；
根据公式：Q ba ′ = Q ba × 10^(a 1 a )，更新加速度计噪声b a 的噪声协方差Q ba ；
其中，a 1 w 为陀螺仪传播调整因子，a 1 a 为加速度计传播调整因子。
进一步地，一实施例中，所述当场景特征退化率超过设定阈值时，EKF模型驱动模块仅使用所述伪观测因子进行观测更新，包括：
构造速度约束：
y k hor = h hor (x k ) + n k hor
y k ver = h ver (x k ) + n k ver
其中，y k hor 和y k ver 分别为伪测量值y k 水平方向和垂直方向的分量，h(·)为EKF模型驱动模块的观测函数，n k 为服从高斯分布的第k个观测噪声，对应的协方差矩阵为R 2×2 ，且R 2 ×2 满足：
R ’ 11 = R 11 × 10 ^ (a 3 hor )
R ’ 22 = R 22 × 10 ^ (a 3 ver )
其中，a 3 hor 为伪观测因子水平方向的分量，a 3 ver 为伪观测因子垂直方向的分量；
使用伪测量值[y k hor , y k ver ] =[0,0]为状态更新提供过滤器。
其中，上述数-模混合估计的多源融合定位系统中各功能模块与上述数-模混合估计的多源融合定位方法实施例中各步骤相对应，其功能和实现过程在此处不再一一赘述。
以上仅为本申请的优选实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (10)

1.一种数-模混合估计的多源融合定位方法，其特征在于：所述一种数-模混合估计的多源融合定位方法包括：
根据IMU数据驱动模块连续多帧采集的陀螺仪的角速度和加速度计的加速度，回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；
基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；
根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度；
根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，并结合不确定度以进行位姿估计。
2.如权利要求1所述的数-模混合估计的多源融合定位方法，其特征在于，所述基于相邻两次采集的点云数据，利用LiDAR数据驱动模块提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率，包括：
同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；
将二维距离图像输入至LiDAR数据驱动模块的LiDAR全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。
3.如权利要求2所述的数-模混合估计的多源融合定位方法，其特征在于，所述根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度，包括：
根据公式：，计算所述不确定度U，其中，a 2 为IMU不确定度因子，P为场景特征退化率，σ为基于EKF模型驱动的协方差。
4.如权利要求1所述的数-模混合估计的多源融合定位方法，其特征在于，所述根据所述传播调整因子动态调整EKF模型驱动模块传播预测时的噪声协方差，包括：
根据公式：Q bw ′= Q bw × 10^(a 1 w )，更新陀螺仪噪声b w 的噪声协方差Q bw ；
根据公式：Q ba ′= Q ba × 10^(a 1 a )，更新加速度计噪声b a 的噪声协方差Q ba ；
其中，a 1 w 为陀螺仪传播调整因子，a 1 a 为加速度计传播调整因子。
5.如权利要求1所述的数-模混合估计的多源融合定位方法，其特征在于，所述当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行EKF模型驱动模块的观测更新，包括：
构造速度约束：
y k hor = h hor (x k ) + n k hor
y k ver = h ver (x k ) + n k ver
其中，y k hor 和y k ver 分别为伪测量值y k 水平方向和垂直方向的分量，h(·)为EKF模型驱动模块的观测函数，n k 为服从高斯分布的第k个观测噪声，对应的协方差矩阵为R 2×2 ，且R 2×2 满足：
R ’ 11 = R 11 × 10 ^ (a 3 hor )
R ’ 22 = R 22 × 10 ^ (a 3 ver )
其中，a 3 hor 为伪观测因子水平方向的分量，a 3 ver 为伪观测因子垂直方向的分量；
使用伪测量值[y k hor , y k ver ] =[0,0]为状态更新提供过滤器。
6.一种数-模混合估计的多源融合定位系统，其特征在于：所述数-模混合估计的多源融合定位系统包括：
IMU数据驱动模块，其用于采集连续多帧采集的陀螺仪的角速度和加速度计的加速度，以回归当前时刻的传播调整因子、IMU不确定度因子和伪观测因子；
LiDAR数据驱动模块，其基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率；
EKF模型驱动模块，其根据所述传播调整因子动态调整传播预测时的噪声协方差，且当场景特征退化率超过设定阈值时，仅使用所述伪观测因子进行观测更新；
所述EKF模型驱动模块还根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度。
7.如权利要求6所述的数-模混合估计的多源融合定位系统，其特征在于，所述LiDAR数据驱动模块基于相邻两次采集的点云数据，提取不同激光之间的变化特征以提供LiDAR场景全局观测，并计算当前时刻的场景特征退化率，包括：
同时输入当前第k次和第k-1次的点云数据对，并通过球面投影将三维点云转换为二维距离图像表示；
将二维距离图像输入至LiDAR数据驱动模块的LiDAR全局观测编码器，从帧的角度提取当前激光的全局观测特征，并计算当前时刻的场景特征退化率。
8.如权利要求7所述的数-模混合估计的多源融合定位系统，其特征在于，所述EKF模型驱动模块根据所述IMU不确定度因子、场景特征退化率以及基于EKF模型驱动的协方差，确定IMU数据驱动模块、LiDAR数据驱动模块和EKF模型驱动模块的不确定度，包括：
根据公式：，计算所述不确定度U，其中，a 2 为IMU不确定度因子，P为场景特征退化率，σ为基于EKF模型驱动的协方差。
9.如权利要求6所述的数-模混合估计的多源融合定位系统，其特征在于，所述EKF模型驱动模块根据所述传播调整因子动态调整传播预测时的噪声协方差，包括：
根据公式：Q bw ′= Q bw × 10^(a 1 w )，更新陀螺仪噪声b w 的噪声协方差Q bw ；
根据公式：Q ba ′= Q ba × 10^(a 1 a )，更新加速度计噪声b a 的噪声协方差Q ba ；
其中，a 1 w 为陀螺仪传播调整因子，a 1 a 为加速度计传播调整因子。
10.如权利要求6所述的数-模混合估计的多源融合定位系统，其特征在于，所述当场景特征退化率超过设定阈值时，EKF模型驱动模块仅使用所述伪观测因子进行观测更新，包括：
构造速度约束：
y k hor = h hor (x k ) + n k hor
y k ver = h ver (x k ) + n k ver
其中，y k hor 和y k ver 分别为伪测量值y k 水平方向和垂直方向的分量，h(·)为EKF模型驱动模块的观测函数，n k 为服从高斯分布的第k个观测噪声，对应的协方差矩阵为R 2×2 ，且R 2×2 满足：
R ’ 11 = R 11 × 10 ^ (a 3 hor )
R ’ 22 = R 22 × 10 ^ (a 3 ver )
其中，a 3 hor 为伪观测因子水平方向的分量，a 3 ver 为伪观测因子垂直方向的分量；
使用伪测量值[y k hor , y k ver ] =[0,0]为状态更新提供过滤器。
CN202311793765.0A 2023-12-25 2023-12-25 一种数-模混合估计的多源融合定位方法及系统 Active CN117451043B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311793765.0A CN117451043B (zh) 2023-12-25 2023-12-25 一种数-模混合估计的多源融合定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311793765.0A CN117451043B (zh) 2023-12-25 2023-12-25 一种数-模混合估计的多源融合定位方法及系统

Publications (2)

Publication Number Publication Date
CN117451043A true CN117451043A (zh) 2024-01-26
CN117451043B CN117451043B (zh) 2024-03-15

Family

ID=89595184

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311793765.0A Active CN117451043B (zh) 2023-12-25 2023-12-25 一种数-模混合估计的多源融合定位方法及系统

Country Status (1)

Country Link
CN (1) CN117451043B (zh)

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050251328A1 (en) * 2004-04-05 2005-11-10 Merwe Rudolph V D Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion
CN109459033A (zh) * 2018-12-21 2019-03-12 哈尔滨工程大学 一种多重渐消因子的机器人无迹快速同步定位与建图方法
DE102018222166A1 (de) * 2018-12-18 2020-06-18 Robert Bosch Gmbh Verfahren zum Ermitteln eines Integritätsbereichs
CN111780755A (zh) * 2020-06-30 2020-10-16 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法
CN115014325A (zh) * 2022-06-01 2022-09-06 齐鲁工业大学 一种基于鲁棒容积点更新框架的组合导航数据融合方法
CN115265551A (zh) * 2021-08-26 2022-11-01 北京理工大学 一种无人驾驶复杂场景下的多传感器融合定位方法和系统
CN115540860A (zh) * 2022-09-26 2022-12-30 福建（泉州）哈工大工程技术研究院 一种多传感器融合位姿估计算法
CN115855048A (zh) * 2023-01-31 2023-03-28 思灵机器人科技(哈尔滨)有限公司 一种多传感器融合位姿估计方法
CN116678421A (zh) * 2023-06-12 2023-09-01 深圳沧穹科技有限公司 基于多模块ble发射装置的多源融合定位方法及系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050251328A1 (en) * 2004-04-05 2005-11-10 Merwe Rudolph V D Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion
DE102018222166A1 (de) * 2018-12-18 2020-06-18 Robert Bosch Gmbh Verfahren zum Ermitteln eines Integritätsbereichs
CN109459033A (zh) * 2018-12-21 2019-03-12 哈尔滨工程大学 一种多重渐消因子的机器人无迹快速同步定位与建图方法
CN111780755A (zh) * 2020-06-30 2020-10-16 南京理工大学 一种基于因子图和可观测度分析的多源融合导航方法
CN115265551A (zh) * 2021-08-26 2022-11-01 北京理工大学 一种无人驾驶复杂场景下的多传感器融合定位方法和系统
CN115014325A (zh) * 2022-06-01 2022-09-06 齐鲁工业大学 一种基于鲁棒容积点更新框架的组合导航数据融合方法
CN115540860A (zh) * 2022-09-26 2022-12-30 福建（泉州）哈工大工程技术研究院 一种多传感器融合位姿估计算法
CN115855048A (zh) * 2023-01-31 2023-03-28 思灵机器人科技(哈尔滨)有限公司 一种多传感器融合位姿估计方法
CN116678421A (zh) * 2023-06-12 2023-09-01 深圳沧穹科技有限公司 基于多模块ble发射装置的多源融合定位方法及系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
YEHONATHAN LITMAN 等: "Invariant Filter Based Preintegration for Addressing the Visual Observability Problem", 《2018 IEEE MIT UNDERGRADUATE RESEARCH TECHNOLOGY CONFERENCE (URTC)》, 4 November 2020 (2020-11-04), pages 1 - 5 *
刘进一;杜岳峰;张硕;朱忠祥;毛恩荣;陈雨;: "基于GNSS/MIMU/DR的农业机械组合导航定位方法", 农业机械学报, no. 1, 15 October 2016 (2016-10-15) *
路丹晖;周文晖;龚小谨;刘济林;: "视觉和IMU融合的移动机器人运动解耦估计", 浙江大学学报(工学版), no. 06, 15 June 2012 (2012-06-15) *
陈帅 等: "置信检验自适应联邦卡尔曼滤波及其水下机器人组合导航应用", 《中国舰船研究》, vol. 17, no. 1, 28 February 2022 (2022-02-28), pages 203 - 211 *

Also Published As

Publication number Publication date
CN117451043B (zh) 2024-03-15

Similar Documents

Publication Publication Date Title
EP3940421A1 (en) Positioning method and device based on multi-sensor fusion
JP6739463B2 (ja) 回転翼型小型飛行体（ｍａｖ）での屋内環境及び屋外環境における安定自律飛行のためのマルチセンサの融合
CN109211251B (zh) 一种基于激光和二维码融合的即时定位与地图构建方法
Achtelik et al. Autonomous navigation and exploration of a quadrotor helicopter in GPS-denied indoor environments
Leishman et al. Relative navigation approach for vision-based aerial GPS-denied navigation
CN110546459A (zh) 具有数据融合的机器人跟踪导航
CN107015238A (zh) 基于三维激光雷达的无人车自主定位方法
Chambers et al. Perception for a river mapping robot
WO2018081366A1 (en) Vision-aided inertial navigation with loop closure
CN112083726A (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN112197770A (zh) 一种机器人的定位方法及其定位装置
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN113899363A (zh) 车辆的定位方法、装置及自动驾驶车辆
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN112378397A (zh) 无人机跟踪目标的方法、装置及无人机
CN112945233B (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN114264301A (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN112652001B (zh) 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统
CN117451043B (zh) 一种数-模混合估计的多源融合定位方法及系统
CN112380933A (zh) 无人机识别目标的方法、装置及无人机
Emter et al. Stochastic cloning for robust fusion of multiple relative and absolute measurements
CN114723920A (zh) 一种基于点云地图的视觉定位方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
Conway et al. Vision-based Velocimetry over Unknown Terrain with a Low-Noise IMU
CN117346768B (zh) 同时适用于室内外的多传感器融合感知定位方法

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