CN116972834A - 多传感器融合定位方法、装置、系统及存储介质 - Google Patents

多传感器融合定位方法、装置、系统及存储介质 Download PDF

Info

Publication number
CN116972834A
CN116972834A CN202310619914.5A CN202310619914A CN116972834A CN 116972834 A CN116972834 A CN 116972834A CN 202310619914 A CN202310619914 A CN 202310619914A CN 116972834 A CN116972834 A CN 116972834A
Authority
CN
China
Prior art keywords
data
value
state
bias
wheel speed
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
CN202310619914.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.)
Shenzhen Yiqing Innovation Technology Co ltd
Original Assignee
Shenzhen Yiqing Innovation Technology Co ltd
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 Shenzhen Yiqing Innovation Technology Co ltd filed Critical Shenzhen Yiqing Innovation Technology Co ltd
Priority to CN202310619914.5A priority Critical patent/CN116972834A/zh
Publication of CN116972834A publication Critical patent/CN116972834A/zh
Pending legal-status Critical Current

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
    • 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/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
    • 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)
  • Navigation (AREA)

Abstract

本申请涉及定位技术领域，尤其涉及一种多传感器融合定位方法、系统及相关装置。本方法根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，通过获取全局定位数据和轮速计的数据，并基于惯性测量单元测量数据、全局定位数据和轮速计的数据，构建观测残差和雅可比矩阵。基于观测残差和所述雅可比矩阵采用误差状态卡尔曼滤波算法对下一时刻状态预测值进行更新，得到下一时刻状态值，最终输出位姿信息。本方法实现在不损失定位精度的情况下，同时降低计算资源的占用和计算耗时，提高机体定位的准确性和实时性。

Description

多传感器融合定位方法、装置、系统及存储介质
技术领域
本申请涉及定位技术领域，尤其涉及一种多传感器融合定位方法、系统及相关装置。
背景技术
随着机器人、无人机和无人驾驶技术的快速发展，为准确确定机体在环境中的位置和方向，对定位技术的要求越来越高。目前，常见定位技术包括全局定位系统(GPS)、惯性导航系统(INS)、视觉识别等。其中，GPS技术是一种广泛使用的全球卫星导航系统，具有高精度、高稳定性和全球覆盖的优势。但是，GPS技术在室内或密闭空间中信号受干扰而失效，而且无法提供高精度的位置测量。INS技术是一种基于惯性测量单元(IMU)和运动学模型的定位技术，具有无需外部信号、高精度和高可靠性的优点。但是，INS技术的传感器的误差和模型误差会随着时间的推移而积累，导致定位精度随时间的增加而降低。视觉识别技术是一种主动或被动式传感器，基于机器视觉和图像处理技术进行环境感知和目标识别，具有定位精度高、适用范围广的优势。但是，视觉识别技术对处理器和算法要求高，处理复杂场景时计算量过大导致实时性弱。
综上所述，现有定位技术可能存在精度不够高，或者，计算量过大导致实时性不强等缺点。
发明内容
本申请实施例提供一种多传感器融合定位方法、装置、系统及存储介质，实现在不损失定位精度的情况下，同时降低计算资源的占用和计算耗时，提高机体定位的准确性和实时性。
第一方面，本申请实施例提供一种多传感器融合定位方法、装置、系统及存储介质，该方法包括：
根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据、所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
根据下一时刻状态值，输出位姿信息。
在一些实施例中，所述状态值包括位置值、姿态值和速度值。
所述根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪的步骤，包括：
获取所述惯性测量单元输出的加速度计测量值和陀螺仪测量值；
获取所述惯性测量单元的加速度偏置和陀螺仪偏置；
根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置、所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值。
在一些实施例中，所述根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置和所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值，包括：
基于以下预测方程得到所述下一时刻状态预测值：
biasak+1＝bias_ak；
bias_gk+1＝bias_gk；
Scalek+1＝Scalek；
其中，P为位置值，V为速度值，R为姿态值，bias_a为加速度偏置，bias_g为陀螺仪偏置，Scale为速度系数，k和k+1分别代表上一时刻和下一时刻，G为重力向量，Δt为上一时刻和下一时刻之间的时间增量，acc为加速度计测量值，ω为陀螺仪测量值，M为全局地图坐标系，I为惯性测量单元坐标系。
在一些实施例中，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
位置的观测残差为：
其中，res_P为所述全局定位传感器的观测残差，measurement_P为所述全局定位传感器实际的观测值，为上一时刻状态的位置值；
位置的雅可比矩阵为：
位置观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
在一些实施例中，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
姿态的观测残差为：
其中，res_θ为全局姿态传感器的观测残差。measurement_θ为全局姿态传感器实际的观测值，为上一时刻状态的姿态值；
姿态的雅可比矩阵为：
姿态观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
在一些实施例中，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
速度的观测残差为：
其中，res_vel为所述速度的观测残差，new_measurement为所述校正后的轮速计的测量数据，为上一时刻状态值中的姿态数据，Scale为速度系数，/>为上一时刻状态值中的速度数据；
速度的雅可比矩阵为：
轮速计观测对姿态的雅可比矩阵为：
轮速计观测对速度系数的雅可比矩阵为：
轮速计观测对位置、加速度偏置、陀螺仪偏置的雅可比矩阵为：
在一些实施例中，所述方法还包括：
重复执行预测和更新，得到多个实时定位信息；
基于所述多个实时定位信息获得运动轨迹，输出所述运动轨迹。
第二方面，本申请提供一种多传感器融合定位装置，该装置包括：
预测模块,用于根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
构建模块，用于获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
更新模块，用于根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
输出模块，用于根据下一时刻状态值，输出位姿信息。
第三方面，本申请提供一种多传感器融合定位系统，该系统包括：惯性测量单元、全局位置传感器、全局姿态传感器、轮速计、至少一个第一处理器；以及与所述至少一个第一处理器通信连接的第一存储器；
所述第一存储器存储有可被所述至少一个第一处理器执行的指令，所述指令被所述至少一个第一处理器执行，以使所述至少一个第一处理器能够执行如上第一方面任一项所述的方法。
第四方面，本申请还提供了非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被计算机所执行时，使所述计算机执行如上第一方面的方法。
本发明实施例的有益效果：区别于现有技术，本申请实施例提供一种多传感器融合定位方法、装置、系统及存储介质，该方法通过综合利用惯性测量单元、全局定位数据和轮速计数据，实现对位姿信息的高精度估计。具体而言，该方法利用惯性测量单元的测量数据对下一时刻状态进行预测，同时结合全局定位数据和轮速计数据构建观测残差和雅可比矩阵。然后，采用误差状态卡尔曼滤波算法对下一时刻状态预测值进行更新，得到下一时刻状态值，最终输出位姿信息。实现了在不损失定位精度的情况下，同时降低计算资源的占用和计算耗时，提高机体定位的准确性和实时性。
附图说明
一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。
图1为本申请实施例提供的一种多传感器融合定位装置的结构示意图；
图2为本申请实施例提供的一种多传感器融合定位方法的流程示意图；
图3为本申请实施例提供的一种多传感器融合定位系统的结构示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本申请，并不用于限定本申请。
需要说明的是，如果不冲突，本发明实施例中的各个特征可以相互结合，均在本申请的保护范围之内。另外，虽然在装置示意图中进行了功能模块划分，在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于装置中的模块划分，或流程图中的顺序执行所示出或描述的步骤。此外，本文所采用的“第一”、“第二”、“第三”等字样并不对数据和执行次序进行限定，仅是对功能和作用基本相同的相同项或相似项进行区分。
除非另有定义，本说明书所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本说明书中在本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是用于限制本发明。本说明书所使用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组合。
此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
在对本申请进行详细说明之前，对本申请实施例中涉及的名词和术语进行说明，本申请实施例中涉及的名词和术语适用于如下的解释：
(1)误差状态卡尔曼滤波(Error State Kalman Filter，ESKF)是一种用于状态估计的算法，用于估计系统的状态和误差，以提高系统的精度和可靠性。在ESKF中，系统状态被表示为真实状态和误差状态的和，其中真实状态是指系统的实际状态，而误差状态是真实状态与估计值之间的差异。通过对误差状态的估计，ESKF可以抑制系统误差和噪声，提高状态估计的精度和可靠性。
ESKF主要包括两个阶段：预测阶段和更新阶段。在预测阶段，ESKF利用系统的运动模型和状态转移矩阵预测下一个时刻的状态向量和状态协方差矩阵，用于描述系统状态的不确定性。在更新阶段，ESKF利用观测模型和观测值计算状态向量和状态协方差矩阵的更新值，以修正状态向量和状态协方差矩阵，从而提高状态估计的精度和可靠性。
在本发明中，通过构建运行模型，对系统下一时刻状态进行预测，并对传感器测量值构建观测残差和雅可比矩阵，最后将采用ESKF算法来实现对系统状态和误差的估计，以提高系统的定位精度和可靠性。
在介绍本申请实施例前，先对本申请发明人所知晓的融合定位方法进行简单介绍，使得后续便于理解本申请实施例。
随着机器人、无人机和无人驾驶技术的快速发展，使用传感器测量机体定位信息的需求日益增加。通常使用的传感器包括惯性测量单元(IMU)、激光雷达、GPS、轮速计和相机等。融合技术可以将这些传感器的测量信息综合起来，以得到机体自身的定位状况。常用的融合技术包括优化方法和滤波方法。优化方法可以同时综合来自所有传感器反馈的信号来求解出最优的定位估计值，但由于综合的信息量较多，容易受到异常值干扰，对于非正态分布的误差表现较差，计算耗时较长，不利于实现实时定位。相比之下，滤波方法能够不断地通过高频的预测以及接收较为低频的观测信息来得到实时的定位状态估计值，并且由于滤波方法的技术框架较为简单、计算资源占用少、耗时短和反应迅速而被广泛使用。
在滤波方法中，常见的包括卡尔曼滤波和粒子滤波等，其中，卡尔曼滤波将系统的状态建模为高斯分布，并使用递归贝叶斯滤波算法来融合传感器测量值和系统动态模型，从而实现状态估计，但是无法处理传感器噪声，精度无法保证；粒子滤波使用一组状态粒子来表示状态的概率分布，并使用重要性抽样技术来更新和估计状态。粒子滤波可以处理非线性和非高斯的问题，但需要较大的粒子数才能达到较高的估计精度，并且计算复杂度较高。
因此，本发明提供一种多传感器融合定位方法，该方法基于误差状态卡尔曼滤波算法，相较于其他融合定位技术，误差状态卡尔曼滤波算法，可以对传感器的误差进行建模和校正，能够处理传感器噪声、模型不确定性等问题，从而提高定位的精度和准确性，且计算速度快，能够实现实时的定位。该方法通过获取惯性测量单元的测量数据，结合上一时刻状态值，构建运动预测方程，计算得到下一时刻状态预测值，读取多传感器的测量数据，构建每一个传感器的观测残差和雅可比矩阵，使用卡尔曼滤波算法融合计算多传感器的观测残差和雅可比矩阵，基于误差状态卡尔曼滤波算法对下一时刻状态预测值进行更新，最终输出定位信息。
下面结合说明书附图具体阐述本申请的技术方案。
请参阅图1，图1示例性示出了一种多传感器融合定位装置100，该装置包括预测模块101、构建模块102、更新模块103和输出模块104。
其中，
预测模块101,用于根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
构建模块102，用于获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
更新模块103，用于根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
输出模块104，用于根据下一时刻状态值，输出位姿信息。
请参阅图2，图2为本申请实施例提供的多传感器融合定位方法的流程示意图。该方法S100具体可以包括如下步骤：
S101：根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪。
在一些实施例中，步骤S101具体包括步骤S1011-S1013：
S1011：获取惯性测量单元输出的加速度计测量值和陀螺仪测量值。
惯性测量单元(Inertial Measurement Unit，简称IMU)是一种集成了多个惯性传感器的设备，可以测量物体的加速度和角速度。在本实施例中，惯性测量单元由加速度计和陀螺仪组成。
获取惯性测量单元输出的加速度计测量值和陀螺仪测量值需要通过惯性测量单元的接口进行读取，例如SPI接口、I2C接口、串口等。
S1012：获取惯性测量单元的加速度偏置和陀螺仪偏置。
惯性测量单元(IMU)中的加速度计和陀螺仪在使用中会产生一些偏差，称为加速度偏置和陀螺仪偏置，会导致定位误差。加速度偏置是由于加速度计的灵敏度和零点的误差，导致加速度计在静止状态下输出的值不为0。陀螺仪偏置则是由于陀螺仪在运动过程中温度变化和机械结构的非对称性等原因导致的误差。为了减小加速度偏置和陀螺仪偏置对定位精度的影响，需要对其进行校准。
在一些实施例中，可以将IMU保持静止，获取一系列加速度计和陀螺仪的读数，然后对这些读数进行统计分析，求出偏置值，并将其作为修正因子应用到IMU的输出值中，以提高定位的精度。
S1013：根据加速度计测量值、陀螺仪测量值、加速度偏置、陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值。
在定位领域中，预测是非常重要的步骤，因为定位系统中的传感器往往会存在延迟、噪声、漂移等问题，导致实时获取的定位数据可能存在误差，而通过预测，可以通过上一时刻的状态值和传感器测量数据预测下一个时刻的状态值，从而减少传感器误差带来的影响，提高定位的精度和稳定性。预测结果可以用于定位系统的路径规划、目标追踪、动态控制等应用中。
在一些实施例中，状态值包括位置值、姿态值和速度值，其中，速度值包括速度系数。构建如下预测方程，结合步骤S101得到加速度计测量值、陀螺仪测量值，步骤S102得到的加速度偏置、陀螺仪偏置，以及上一时刻的位置值、姿态值和速度值，根据预测方程预测下一时刻的状态：
bias_ak+1＝bias_ak；
bias_gk+1＝bias_gk；
Scalek+1＝Scalek；
其中，P为位置值，V为速度值，R为姿态值，bias_a为加速度偏置，bias_g为陀螺仪偏置，Scale为速度系数，k和k+1分别代表上一时刻和下一时刻，G为重力向量，Δt为上一时刻和下一时刻之间的时间增量，acc为加速度计测量值，ω为陀螺仪测量值，M为全局地图坐标系，I为惯性测量单元坐标系。
S102：获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据、所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据。
在多传感器融合定位中，观测残差和雅可比矩阵用于描述不同传感器测量数据对状态向量的贡献，其中，观测残差是指实际测量值与预测值之间的差异，雅可比矩阵则是描述这种差异对状态向量的影响程度的矩阵。通过构建观测残差和雅可比矩阵，可以将传感器的测量数据与状态值进行融合，得到更准确的状态值。
在一些实施例中，传感器包括全局位置传感器、全局姿态传感器和轮速计。
全局位置传感器可以提供精确位置信息，用于实现全局定位，例如，GPS(GlobalPositioning System)卫星定位系统、北斗卫星导航系统等。
在一些实施例中，位置的观测残差为：
其中，res_P为位置的观测残差，measurement_P为加速度计的测量数据，为全局位置数据；
位置的雅可比矩阵为：
位置观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
全局姿态传感器可以提供精确姿态信息，用于实现全局姿态测量和控制。通常，全局姿态传感器是多种传感器组合的统称，例如包括加速度计、陀螺仪、磁力计等一起组合使用，以计算得到姿态值。
在一些实施例中，姿态的观测残差为：
其中，res_θ为姿态的观测残差。measurement_θ为陀螺仪的测量数据，为全局姿态数据；
姿态的雅可比矩阵为：
姿态观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
轮速计是一种用于测量车辆轮速的传感器，可以根据车轮转动的角速度和方向，测量车辆的速度、位置和方向等信息。
在一些实施例中，为保证数据的准确性和一致性，轮速计的数据需要经过校正，校正公式为：
其中，new_measurement为校正后的速度观测值，measurement为轮速轮速计实际的观测值，[ωk+1]×为下一时刻陀螺仪测量值所对应的反对称矩阵，为姿态后轴中心与惯性测量单元之间的平移外参；
速度的观测残差的为：
其中，res_vel为速度的观测残差，new_measurement为校正后的速度观测值，为上一时刻状态的姿态值，Scale为速度系数，/>为上一时刻状态的速度值；
速度的雅可比矩阵为：
轮速计观测对姿态的雅可比矩阵为：
轮速计观测对速度系数的雅可比矩阵为：
轮速计观测对位置、加速度偏置、陀螺仪偏置的雅可比矩阵为：
S103：根据观测残差和雅可比矩阵，基于误差状态卡尔曼滤波算法对下一时刻状态预测值进行更新，得到下一时刻状态值。
下一时刻状态预测值由上一时刻状态值和惯性测量单元测量值预测得到，但是这个预测值存在误差，因此，需要对预测值进行修正，以提高精准度。
基于误差状态卡尔曼滤波算法，可以利用观测残差和雅可比矩阵，对下一时刻状态预测值进行修正，从而得到更准确的下一时刻状态值。具体地，误差状态卡尔曼滤波将观测残差和雅可比矩阵作为输入，利用卡尔曼增益计算出修正量，然后将修正量加到预测值上，得到更准确的下一时刻状态值。误差状态卡尔曼滤波算法的基本公式为：
其中，和ˇ分别代表先验值和后验值，/>为下一时刻状态的预测值，A为状态转移矩阵，用于表示状态在时间上的变化规律，xk为上一时刻的状态值，B为控制输入矩阵，用于表示外部控制对状态的影响，uk上一时刻的控制输入，/>为下一时刻状态预测值的协方差矩阵，Q为系统过程噪声协方差矩阵，用于表示状态转移中的随机干扰，Kk+1为下一时刻状态预测值与实际观测值之间的卡尔曼增益，/>为观测残差，H为观测雅可比矩阵，用于表示状态与观测值之间的映射关系，zk+1为下一时刻的观测值，/>为下一时刻的状态值，即对/>的最优估计，/>为下一时刻状态值的协方差矩阵，即对/>的最优估计，R为测量噪声协方差矩阵，用于表示观测值中的随机干扰，I为单位矩阵。
在误差状态卡尔曼滤波算法中，上述参数用于计算下一时刻的状态值和协方差矩阵，即和/>具体来说，首先根据状态转移矩阵A、上一时刻状态值xk和控制输入矩阵B、上一时刻控制输入uk，计算下一时刻状态的预测值/>然后根据预测值/>协方差矩阵/>和观测雅可比矩阵H，计算卡尔曼增益Kk+1。最后，利用卡尔曼增益Kk+1、观测残差和预测值/>更新状态值/>和协方差矩阵/>
通过这个步骤的更新，误差状态卡尔曼滤波能够不断地根据步骤S104得到的多传感器观测残差和雅可比矩阵来修正预测值，从而达到更准确地估计系统状态的目的。
S104：根据下一时刻状态值，输出位姿信息。
误差状态卡尔曼滤波的输出通常包括状态估计值、状态协方差矩阵、系统噪声协方差矩阵、观测噪声协方差矩阵、状态转移矩阵以及观测矩阵等，本领域技术人员可以根据具体需求选择相应的参数输出。
在一些实施例中，经过误差状态卡尔曼滤波算法的修正和更新，得到的一个更加准确的下一时刻状态值。根据下一时刻状态值，输出位姿信息，包括位置、速度、姿态等参数，这些参数可以用于定位和导航，且比直接从传感器测量得到的定位信息更准确。
在一些实施例中，在步骤S104之后，还包括步骤S105：重复执行预测和更新，得到多个实时定位信息；
基于多个实时定位信息获得运动轨迹，输出运动轨迹。
具体的，根据步骤S101-S104的方法，不断重复执行预测和更新操作，以获得随时间推进的多个实时定位信息。通过这些实时定位信息，可以获取移动对象在运动过程中的位置、速度和姿态等状态信息，进而可以根据运动状态信息计算出运动轨迹。最终，通过输出运动轨迹，可以实现实时对移动对象的精确定位和跟踪。
综上所述，本申请实施例提供一种多传感器融合定位方法，根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，通过获取全局定位数据和轮速计的数据，并基于惯性测量单元测量数据、全局定位数据和轮速计的数据，构建观测残差和雅可比矩阵。基于观测残差和所述雅可比矩阵采用误差状态卡尔曼滤波算法对下一时刻状态预测值进行更新，得到下一时刻状态值，最终输出位姿信息。本方法实现在不损失定位精度的情况下，忽略掉复杂的冗余参数的计算，降低了计算资源的占用和计算耗时，提高了机体定位的准确性和实时性。
在一些实施例中，本申请提供的一种多传感器融合定位方法，可以应用于一种多传感器融合定位系统，如图3所示，多传感器融合定位系统10包括惯性测量单元、全局定位传感器、全局姿态传感器、轮速计以及至少一个第一处理器11、以及与至少一个第一处理器11通信连接的第一存储器12(图3中以总线连接、一个处理器为例)。第一存储器存储12有可被至少一个第一处理器11执行的指令，指令被至少一个第一处理器11执行时，以使至少一个第一处理器11能够执行本申请任一个实施例提供的方法。
本领域的技术人员可以理解，图3所示的结构仅为示意，其并不对上述多传感器融合定位系统10的结构造成限定。例如，多传感器融合定位系统10还可以包括比图3中所示更多或者更少的组件，或者具有与图2所示不同的配置。
其中，所述处理器11用于提供计算和控制能力，以控制多传感器融合定位系统10执行相应任务，例如，控制所述多传感器融合定位系统10执行本申请任一实施例提供的方法。
可以理解的是，所述处理器11可以是通用处理器，包括中央处理器(CentralProcessing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital Signal Processing，DSP)、专用集成电路(Application SpecificIntegrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
所述存储器12作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态性计算机可执行程序以及模块，如本申请实施例中多传感器融合定位的方法对应的程序指令/模块。所述处理器11通过运行存储在存储器12中的非暂态软件程序、指令以及模块，可以实现上述任一方法实施例中的多传感器融合定位方法，所述存储器12可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器12还可以包括相对于处理器远程设置的存储器，这些远程存储器可以通过网络连接至处理器。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
本申请实施例还提供一种计算机可读存储介质，该计算机可读存储介质存储有计算机程序，计算机程序包括程序指令，程序指令当被计算机执行时使计算机执行如前述实施例中多传感器融合定位的方法。
需要说明的是，以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。
通过以上的实施方式的描述，本领域普通技术人员可以清楚地了解到各实施方式可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory,ROM)或随机存储记忆体(Random Access Memory,RAM)等。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (10)

1.一种多传感器融合定位方法，其特征在于，所述方法包括：
根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据、所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
根据下一时刻状态值，输出位姿信息。
2.根据权利要求1所述的方法，其特征在于，所述状态值包括位置值、姿态值和速度值。
所述根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪的步骤，包括：
获取所述惯性测量单元输出的加速度计测量值和陀螺仪测量值；
获取所述惯性测量单元的加速度偏置和陀螺仪偏置；
根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置、所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值。
3.根据权利要求2所述的方法，其特征在于，所述根据所述加速度计测量值、所述陀螺仪测量值、所述加速度偏置和所述陀螺仪偏置，对下一时刻状态进行预测，得到下一时刻状态预测值，包括：
基于以下预测方程得到所述下一时刻状态预测值：
bias_ak+1＝ias_ak；
bias_gk+1＝ias_gk；
Scalek+1＝calek；
其中，P为位置值，V为速度值，R为姿态值，bias_a为加速度偏置，bias_g为陀螺仪偏置，Scale为速度系数，k和k+1分别代表上一时刻和下一时刻，G为重力向量，Δt为上一时刻和下一时刻之间的时间增量，acc为加速度计测量值，ω为陀螺仪测量值，M为全局地图坐标系，I为惯性测量单元坐标系。
4.根据权利要求1-3任一项所述的方法，其特征在于，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
位置的观测残差为：
其中，res_P为所述位置的观测残差，measurement_P为所述加速度计的测量数据，为所述全局位置数据；
位置的雅可比矩阵为：
1 0 0
0 1 0
0 0 1；
位置观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
0 0 0
0 0 0
0 0 0。
5.根据权利要求4所述的方法，其特征在于，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
姿态的观测残差为：
其中，res_θ为所述姿态的观测残差。measurement_θ为所述陀螺仪的测量数据，为所述全局姿态数据；
姿态的雅可比矩阵为：
1 0 0
0 1 0
0 0 1；
姿态观测对速度、姿态、加速度偏置、陀螺仪偏置或速度系数的雅可比矩阵为：
0 0 0
0 0 0
0 0 0。
6.根据权利要求5所述的方法，其特征在于，所述获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵包括：
速度的观测残差为：
其中，res_vel为所述速度的观测残差，new_measurement为所述校正后的轮速计的测量数据，为上一时刻状态值中的姿态数据，Scale为速度系数，/>为上一时刻状态值中的速度数据；
速度的雅可比矩阵为：
轮速计观测对姿态的雅可比矩阵为：
轮速计观测对速度系数的雅可比矩阵为：
轮速计观测对位置、加速度偏置、陀螺仪偏置的雅可比矩阵为：
0 0 0
0 0 0
0 0 0。
7.根据权利要求1所述的方法，其特征在于，所述方法还包括：
重复执行预测和更新，得到多个实时定位信息；
基于所述多个实时定位信息获得运动轨迹，输出所述运动轨迹。
8.一种多传感器融合定位装置，其特征在于，包括：
预测模块,用于根据惯性测量单元测量数据对下一时刻状态进行预测，得到下一时刻状态预测值，其中，所述惯性测量单元包括加速度计和陀螺仪；
构建模块，用于获取全局定位数据和轮速计的数据，基于所述惯性测量单元测量数据所述全局定位数据和所述轮速计的数据，构建观测残差和雅可比矩阵，其中所述全局定位数据包括全局位置数据和全局姿态数据；
更新模块，用于根据所述观测残差和所述雅可比矩阵，基于误差状态卡尔曼滤波算法对所述下一时刻状态预测值进行更新，得到下一时刻状态值；
输出模块，用于根据下一时刻状态值，输出位姿信息。
9.一种多传感器融合定位系统，其特征在于，所述系统包括：惯性测量单元、全局位置传感器、全局姿态传感器、轮速计、至少一个第一处理器；以及与所述至少一个第一处理器通信连接的第一存储器；
所述第一存储器存储有可被所述至少一个第一处理器执行的指令，所述指令被所述至少一个第一处理器执行，以使所述至少一个第一处理器能够执行权利要求1-7任一项所述的方法。
10.一种非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被计算机所执行时，使所述计算机执行权利要求1-8任一项所述的方法。
CN202310619914.5A 2023-05-29 2023-05-29 多传感器融合定位方法、装置、系统及存储介质 Pending CN116972834A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310619914.5A CN116972834A (zh) 2023-05-29 2023-05-29 多传感器融合定位方法、装置、系统及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310619914.5A CN116972834A (zh) 2023-05-29 2023-05-29 多传感器融合定位方法、装置、系统及存储介质

Publications (1)

Publication Number Publication Date
CN116972834A true CN116972834A (zh) 2023-10-31

Family

ID=88475681

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310619914.5A Pending CN116972834A (zh) 2023-05-29 2023-05-29 多传感器融合定位方法、装置、系统及存储介质

Country Status (1)

Country Link
CN (1) CN116972834A (zh)

Similar Documents

Publication Publication Date Title
CN112577521B (zh) 一种组合导航误差校准方法及电子设备
CN112505737B (zh) 一种gnss/ins组合导航方法
CN111102978A (zh) 一种车辆运动状态确定的方法、装置及电子设备
CN109143304B (zh) 用于确定无人驾驶车辆位姿的方法和装置
CN114179825B (zh) 多传感器融合获取量测值置信度方法及自动驾驶车辆
CN112946681B (zh) 融合组合导航信息的激光雷达定位方法
CN114264301B (zh) 车载多传感器融合定位方法、装置、芯片及终端
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
CN113566850B (zh) 惯性测量单元的安装角度标定方法、装置和计算机设备
CN111197994B (zh) 位置数据修正方法、装置、计算机设备和存储介质
Taghizadeh et al. A low-cost integrated navigation system based on factor graph nonlinear optimization for autonomous flight
CN110375740B (zh) 车辆导航方法、装置、设备和存储介质
CN108981689B (zh) 基于dsp tms320c6748的uwb/ins组合导航系统
CN113917512B (zh) 自动驾驶车辆的定位方法、装置、电子设备及存储介质
CN116242373A (zh) 一种融合多源数据的高精度导航定位方法及系统
Valizadeh et al. Improvement of navigation accuracy using tightly coupled kalman filter
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
CN116972834A (zh) 多传感器融合定位方法、装置、系统及存储介质
CN114897942A (zh) 点云地图的生成方法、设备及相关存储介质
CN114705223A (zh) 多移动智能体在目标跟踪中的惯导误差补偿方法及系统
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN111811500A (zh) 目标对象的位姿估计方法、装置、存储介质和电子设备
CN117308925B (zh) 一种频谱地图惯导组合的导航方法、装置、设备和介质
CN115388914B (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