CN117250630A - 基于自适应核卡尔曼滤波的多传感器融合定位系统及方法 - Google Patents

基于自适应核卡尔曼滤波的多传感器融合定位系统及方法 Download PDF

Info

Publication number
CN117250630A
CN117250630A CN202311110775.XA CN202311110775A CN117250630A CN 117250630 A CN117250630 A CN 117250630A CN 202311110775 A CN202311110775 A CN 202311110775A CN 117250630 A CN117250630 A CN 117250630A
Authority
CN
China
Prior art keywords
camera
module
pose
laser radar
network
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
CN202311110775.XA
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.)
Northeastern University Qinhuangdao Branch
Original Assignee
Northeastern University Qinhuangdao Branch
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 Northeastern University Qinhuangdao Branch filed Critical Northeastern University Qinhuangdao Branch
Priority to CN202311110775.XA priority Critical patent/CN117250630A/zh
Publication of CN117250630A publication Critical patent/CN117250630A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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/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/1656—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 passive imaging devices, e.g. cameras
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本发明提供一种基于自适应核卡尔曼滤波的多传感器融合定位系统及方法，涉及自动驾驶车辆定位技术领域。基于自适应核卡尔曼滤波的多传感器融合定位系统包括传感器模块、基于深度学习的相机位姿估计模块、模型模块、自适应核卡尔曼滤波模块、求和模块和积分模块；本发明利用深度学习对相机序列和激光雷达序列处理获得额外的观测量，与GNSS观测量一同构成多观测量，提升了定位的精度和鲁棒性；针对融合数据时常使用的卡尔曼滤波器及其变体计算量大的缺点，本发明在融合众多传感器数据时采用自适应核卡尔曼滤波算法，该算法能够使定位精度和定位系统的实时性同时得到保证。

Description

基于自适应核卡尔曼滤波的多传感器融合定位系统及方法
技术领域
本发明涉及自动驾驶车辆定位技术领域，尤其涉及基于自适应核卡尔曼滤波的多传感器融合定位系统及方法。
背景技术
自动驾驶是人工智能领域的一种主流应用，也是最具前景的技术之一。其中精准和鲁棒的车辆定位对于自动驾驶车辆十分重要，定位输出的位置和姿态信息是实现导航、碰撞预警和自动泊车等功能的基础。由于自动驾驶技术无需人类来驾驶，所以理论上能够有效避免人类的驾驶失误，减少交通事故的发生，且能够提高公路的运输效率。因此，自动驾驶技术越来越受到重视。
在自动驾驶车辆定位技术领域，组合使用GNSS和IMU进行定位是较为成熟的解决方案。IMU对自身测量的三轴加速度和角速度积分估计位置和姿态，GNSS提供较高精度的位置观测以保证IMU的累计误差收敛。然而在城市峡谷、树林和隧道等环境中，GNSS信号可能被遮挡甚至中断，此时仅依靠IMU的车辆定位结果精度和鲁棒性较差。同时常采用卡尔曼滤波及其各种变体处理GNSS和IMU的数据，但是这些滤波器的精度和实时性很难同时得到保证。
因此现有技术需要一种利用车载传感器构造观测量，与GNSS和IMU一同组成多传感器融合定位系统的方法，来解决车辆定位精度和鲁棒性差的问题，并且需要设计一种新的滤波器，来保证定位系统良好的实时性。
发明内容
针对上述现有技术存在的不足，本发明提供一种基于自适应核卡尔曼滤波的多传感器融合定位系统及方法，旨在解决车辆定位精度和鲁棒性差的问题，同时保证定位系统具有良好的实时性。
为了解决上述问题，本发明第一方面提供一种基于自适应核卡尔曼滤波的多传感器融合定位系统，包括传感器模块、基于深度学习的相机位姿估计模块、模型模块、自适应核卡尔曼滤波模块、求和模块和积分模块；
所述传感器模块，包括IMU、相机、激光雷达和GPS四种传感器；所述IMU用于测量三轴加速度和角速度并将测量得到的三轴加速度和角速度发送给模型模块和求和模块；所述相机用于获取相机序列并将相机序列发送给基于深度学习的相机位姿估计模块；所述激光雷达用于获取激光雷达序列并将激光雷达序列发送给基于深度学习的相机位姿估计模块；所述GPS用于接收位置和速度观测数据并将接收到的位置和速度观测数据发送给模型模块；
所述基于深度学习的相机位姿估计模块，用于接收相机发送的相机序列和激光雷达发送的激光雷达序列；利用神经网络对相机序列和激光雷达序列进行处理，得到相机的位姿；将最终得到相机的位姿发送给模型模块；
所述模型模块，用于接收传感器模块发送的三轴加速度和角速度；接收传感器模块发送的位置和速度观测数据；接收基于深度学习的相机位姿估计模块发送的相机的位姿；对三轴加速度和角速度进行处理得到误差状态传递方程并将其发送给自适应核卡尔曼滤波模块；对相机的位姿、位置和速度观测数据进行处理得到观测方程并将其发送给自适应核卡尔曼滤波模块；
所述自适应核卡尔曼滤波模块，用于接收模型模块发送的误差状态传递方程和观测方程；利用自适应核卡尔曼滤波算法得到误差状态向量并将其发送给求和模块；
所述积分模块，用于接收传感器模块发送的三轴加速度和角速度；对三轴加速度和角速度进行积分推算得到位置速度和姿态信息并将位置速度和姿态信息发送给求和模块；
所述求和模块，用于接收积分模块发送的位置速度和姿态信息；接收模型模块发送的误差状态向量；将误差状态向量与积分推算得到的位置速度和姿态信息进行广义求和，输出最终的位置速度和姿态信息。
所述基于深度学习的相机位姿估计模块进一步包括：
视觉网络，包括基于ResNet18网络的第一编码器和一个第一全连接层模块；所述基于ResNet18网络的第一编码器用于接收相机发送的相机序列并对相机序列进行相机特征提取，得到图像角点特征并将其发送给第一全连接层模块和注意力模块；所述第一全连接层模块包括两个串联的全连接层，用于接收基于ResNet18网络的第一编码器发送的图像角点特征，接收注意力模块发送的图像角点特征的重要性权重，通过两个全连接层完成位姿回归，得到相机位姿并将其发送给位姿融合网络和相机损失函数模块；
深度网络，包括基于Unet架构的一对编码器和解码器，用于接收相机发送的相机序列；预测相机序列中RGB图像每像素的深度，构成预测深度地图D并将其发送给相机损失函数模块；
测距网络，包括基于ResNet18网络的第二编码器和第二全连接层模块，所述基于ResNet18网络的第二编码器，用于接收激光雷达发送的激光雷达序列并对激光雷达序列进行特征提取得到激光雷达特征并将其发送给第二全连接层模块和注意力模块；所述第二全连接层模块包括两个串联的全连接层，用于接收基于ResNet18网络的第二编码器发送的激光雷达特征；接收注意力模块发送的激光雷达特征的重要性权重，通过两个全连接层完成位姿回归得到激光雷达位姿并将其发送给激光雷达损失函数模块和位姿融合网络；
掩膜网络，包括基于Unet架构的一对编码器和解码器，用于接收激光雷达发送的激光雷达序列；检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜M并将其发送给激光雷达损失函数模块；
融合模块包括注意力模块和位姿融合网络，所述注意力模块由基于ResNet18网络的第三编码器和SoftMax激活函数组成，该模块用于接收第一全连接层模块发送的相机位姿和第二全连接层发送的激光雷达位姿；生成图像角点特征的重要性权重和激光雷达特征的重要性权重；将图像角点特征的重要性权重发送给第一全连接层模块，将激光雷达特征的重要性权重发送给第二全连接层模块；所述位姿融合网络是多层感知机(multilayer-perceptron，MLP)，用于接收第一全连接层模块发送的相机位姿和第二全连接层发送的激光雷达位姿；将相机位姿和激光雷达位姿进行融合，输出最终的融合相机位姿；
相机损失函数模块，用于接收第一全连接层发送的相机位姿、深度网络发送的深度地图和位姿融合网络发送的融合相机位姿；计算相机的损失函数；所述相机的损失函数包括光度误差和深度误差；得到相机损失函数后为其中的光度误差和深度误差分配缩放因子；
激光雷达损失函数模块，用于接收第二全连接层发送的激光雷达位姿、掩模网路膜网络发送的预测掩膜和位姿融合网络发送的融合相机位姿；计算激光雷达的强度损失函数和预测掩膜的交叉熵损失函数；得到激光雷达损失函数后为其中的强度损失和掩膜损失分配缩放因子。
所述模型模块进一步包括：
误差状态传递模型，用于接收IMU发送的三轴加速度和角速度；通过对IMU建模得到误差状态传递方程并发送给自适应核卡尔曼滤波模块；
观测模型，包括相机位置姿态观测和GPS位置速度观测，用于接收融合网络发送的相机位姿和GPS发送的位置速度观测数据，将融合相机位姿和GPS发送的位置速度观测数据组成纳入观测方程中，并将最终的观测方程发送给自适应核卡尔曼滤波模块。
本发明第二方面提供一种基于自适应核卡尔曼滤波的多传感器融合定位方法，该方法包括以下步骤：
步骤1对车载传感器进行标定，所述标定包括空间标定和时间标定；所述车载传感器指IMU、相机、激光雷达和GPS；
步骤2利用标定好的激光雷达获取激光雷达序列；利用标定好的相机获取相机序列；
步骤3根据激光雷达序列和相机序列，估计相机的位姿，得到融合相机位姿；
步骤3.1将相机序列输入视觉网络和深度网络，通过视觉网络进行特征提取，得到相机序列的图像角点特征，通过深度网络预测相机序列中RGB图像每像素的深度，得到深度地图；将激光雷达序列输入测距网络和掩膜网络，通过测距网络提取特征，得到激光雷达特征，通过掩膜网络检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜；
步骤3.2将得到的图像角点特征和激光雷达特征输入基于ResNet18网络的第三编码器和SoftMax激活函数，得到图像角点特征的重要性权重和激光雷达特征的重要性权重；
步骤3.3将得到的图像角点特征的重要性权重分配给图像角点特征，利用第一全连接层模块进行位姿回归得到相机位姿；将得到的激光雷达特征的重要性权重分配给激光雷达特征，利用第二全连接层模块进行位姿回归得到激光雷达位姿；
步骤3.4将得到的相机位姿和激光雷达位姿输入多层感知机，得到所估计的融合相机位姿；
步骤3.5根据相机位姿、深度地图和估计的融合相机位姿计算相机的损失函数；根据激光雷达位姿和融合相机位姿计算激光雷达的强度损失函数；根据预测掩膜和融合相机位姿计算预测掩膜的交叉熵损失函数；所述相机的损失函数包括光度误差和深度误差；
所述相机的损失函数为：
Lc＝Lp+λsLs(D) (1)
其中，Lc表示相机的损失函数；Lp表示光度误差；Ls(D)表示深度误差，其中的参数D表示深度地图；λs表示为深度误差分配缩放因子；
所述激光雷达的强度损失函数为：
其中，Ll表示激光雷达的强度损失函数；Ms(xt)表示从源帧到目标帧的误差权重；It(xt)表示目标帧集合；表示重建的源帧集合；xt表示目标帧；S表示激光雷达序列的总帧数；s表示激光雷达序列的第一帧；
所述预测掩膜的交叉熵损失函数为：
其中，Lm表示预测掩膜的交叉熵损失函数；P(Ms(xt)＝1)表示当目标帧的预测掩膜为1时的交叉熵；
步骤3.6对相机的损失函数、激光雷达的强度损失函数和预测掩膜的交叉熵损失函数分配缩放因子，得到总的损失函数；
所述总的损失函数为：
L＝Ll+λcLc+λmLm (4)
其中，L表示总的损失函数；λc和λm分别为相机的损失的缩放因子和预测掩膜的交叉熵损失的缩放因子；
步骤3.7通过总的损失函数计算损失值，若损失值未达到极小值则通过反向传播去更新各个网络权值，返回步骤3.4；若损失值达到极小值则执行步骤3.8；
步骤3.8选取总的损失函数最小的融合相机位姿作为最终的融合相机位姿。
步骤4对IMU建模并构建误差状态传递模型；
步骤4.1对IMU建模得到IMU的动态微分方程；
步骤4.2定义误差状态向量，并将误差状态向量改写为关于误差状态向量的微分方程组的矩阵形式；所述误差状态向量包括导航状态误差和传感器误差；
步骤4.3根据线性系统理论，将矩阵形式的微分方程组转化，得到离散的系统误差状态传递模型。
步骤5根据融合相机位姿和GPS接收的位置和速度观测数据，得到误差状态的观测模型；所述相机位姿包括的相机位置和相机的姿态；
步骤5.1接收融合相机位姿和GPS接收到导航坐标系下位置和速度，建立相机位置、相机的姿态、GPS接收的位置和GPS接收的速度四个观测量的估计值或测量值的关系式；
所述四个观测量的估计值或测量值关系如下：
其中，和分别表示导航坐标系下的融合相机的位置的估计值和姿态的估计值、GPS接收到导航坐标系下的位置信息观测值和GPS接收到导航坐标系下的速度观测值；和分别是IMU到相机杆臂和IMU到GPS的杆臂，表示n系下IMU的位置，表示导航坐标系下IMU的速度，表示从导航坐标系到IMU坐标系的旋转矩阵，表示从IMU坐标系到相机坐标系的旋转矩阵，和分别是相机姿态的估计噪声、相机位置的估计噪声、GPS位置的估计噪声和GPS速度的估计噪声；ωi表示角速度的真实值；
步骤5.2根据各观测量的估计值与实际值，得到误差状态的观测模型；
所述误差状态的观测模型为：
其中，为观测量残差，为观测量残差关于误差状态向量的雅可比矩阵，为观测噪声向量；ξ是误差状态向量，vk是观测噪声向量，为融合相机姿态观测残差，为融合相机位置观测残差，为GPS位置观测残差，为GPS速度观测残差；分别表示融合相机姿态观测、融合相机位置观测、GPS位置观测、GPS速度观测的雅可比矩阵；分别表示融合相机姿态观测、融合相机位置观测、GPS位置观测、GPS速度观测的观测噪声。
步骤6利用自适应核卡尔曼滤波算法对误差状态向量进行滤波，得到下一时刻的误差状态向量；
步骤6.1对误差状态向量的原始粒子进行核均值嵌入操作，得到生成粒子；
步骤6.2利用步骤6.1的误差状态向量生成粒子和步骤4.3的误差状态传递模型进行预测，得到预测的下一时刻先验误差状态向量的生成粒子；
步骤6.3根据k时刻的观测和利用误差状态的观测模型修正步骤6.2中计算的先验误差状态向量的生成粒子，得到更新的后验误差状态向量。
步骤7对IMU输出的三轴加速度和角速度进行积分推算得到位置、速度和姿态信息；
步骤8将下一时刻更新的后验误差状态向量与积分推算得到位置、速度和姿态信息进行广义求和，得到最终的位置速度估计和姿态估计。
本发明提出的一种基于自适应核卡尔曼滤波的多传感器融合定位系统及方法，与现有技术相比较具有如下有益效果：
考虑到GNSS信号可能被遮挡甚至中断，此时观测信息缺失会使仅依靠IMU的车辆定位误差迅速发散，本发明利用深度学习对相机序列和激光雷达序列处理获得额外的观测量，与GNSS观测量一同构成多观测量，提升了定位的精度和鲁棒性；针对融合数据时常使用的卡尔曼滤波器及其变体计算量大的缺点，本发明在融合众多传感器数据时采用自适应核卡尔曼滤波算法，该算法能够使定位精度和定位系统的实时性同时得到保证。
附图说明
图1为本发明实施例中基于自适应核卡尔曼滤波的多传感器融合定位系统框图；
图2为本发明实施例中的基于深度学习相机位姿估计模块的示意图；
图3为本发明实施例中的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法的流程图；
图4为本发明实施例中的车载传感器位置关系示意图。
具体实施方式
为了使本发明的技术方案及其优点更加清晰，下面将结合附图，对本申请中的技术方案进行描述。此处所描述的具体实时例仅用以解释本发明，并不用于限定本发明的范围。
本实施方式的基于自适应核卡尔曼滤波的多传感器融合定位系统，如图1所示，包括传感器模块、基于深度学习的相机位姿估计模块、模型模块、自适应核卡尔曼滤波模块、求和模块和积分模块。
所述传感器模块，包括IMU、相机、激光雷达和GPS四种传感器。所述IMU用于测量三轴加速度和角速度并将测量得到的三轴加速度和角速度发送给模型模块和求和模块；所述相机用于获取相机序列并将相机序列发送给基于深度学习的相机位姿估计模块；所述激光雷达用于获取激光雷达序列并将激光雷达序列发送给基于深度学习的相机位姿估计模块；所述GPS用于接收位置和速度观测数据并将接收到的位置和速度观测数据发送给模型模块；
所述基于深度学习的相机位姿估计模块，用于接收相机发送的相机序列和激光雷达发送的激光雷达序列；利用神经网络对相机序列和激光雷达序列进行处理，得到相机的位姿；将最终得到相机的位姿发送给模型模块；
所述模型模块，用于接收传感器模块发送的三轴加速度和角速度；接收传感器模块发送的位置和速度观测数据；接收基于深度学习的相机位姿估计模块发送的相机的位姿；对三轴加速度和角速度进行处理得到误差状态传递方程并将其发送给自适应核卡尔曼滤波模块；对相机的位姿、位置和速度观测数据进行处理得到观测方程并将其发送给自适应核卡尔曼滤波模块；
所述自适应核卡尔曼滤波模块，用于接收模型模块发送的误差状态传递方程和观测方程；利用自适应核卡尔曼滤波算法得到误差状态向量并将其发送给求和模块；
所述积分模块，用于接收传感器模块发送的三轴加速度和角速度；对三轴加速度和角速度进行积分推算得到位置速度和姿态信息并将位置速度和姿态信息发送给求和模块；
所述求和模块，用于接收积分模块发送的位置速度和姿态信息；接收模型模块发送的误差状态向量；将误差状态向量与积分推算得到的位置速度和姿态信息进行广义求和，输出最终的位置速度和姿态信息；
如图2所示，基于深度学习的相机位姿估计模块进一步包括：
视觉网络，包括基于ResNet18网络的第一编码器和一个第一全连接层模块；所述基于ResNet18网络的第一编码器用于接收相机发送的相机序列并对相机序列进行相机特征提取，得到图像角点特征并将其发送给第一全连接层模块和注意力模块；所述第一全连接层模块包括两个串联的全连接层，用于接收基于ResNet18网络的第一编码器发送的图像角点特征，接收注意力模块发送的图像角点特征的重要性权重，通过两个全连接层完成位姿回归，得到相机位姿并将其发送给位姿融合网络和相机损失函数模块；
深度网络，包括基于Unet架构的一对编码器和解码器，用于接收相机发送的相机序列；预测相机序列中RGB图像每像素的深度，构成预测深度地图D并将其发送给相机损失函数模块；
测距网络，包括基于ResNet18网络的第二编码器和第二全连接层模块，所述基于ResNet18网络的第二编码器，用于接收激光雷达发送的激光雷达序列并对激光雷达序列进行特征提取得到激光雷达特征并将其发送给第二全连接层模块和注意力模块；所述第二全连接层模块包括两个串联的全连接层，用于接收基于ResNet18网络的第二编码器发送的激光雷达特征；接收注意力模块发送的激光雷达特征的重要性权重，通过两个全连接层完成位姿回归得到激光雷达位姿并将其发送给激光雷达损失函数模块和位姿融合网络；
掩膜网络，包括基于Unet架构的一对编码器和解码器，用于接收激光雷达发送的激光雷达序列；检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜M并将其发送给激光雷达损失函数模块；
所述融合模块包括注意力模块和位姿融合网络，所述注意力模块由基于ResNet18网络的第三编码器和SoftMax激活函数组成，该模块用于接收第一全连接层模块发送的相机位姿和第二全连接层发送的激光雷达位姿；生成图像角点特征的重要性权重和激光雷达特征的重要性权重；将图像角点特征的重要性权重发送给第一全连接层模块，将激光雷达特征的重要性权重发送给第二全连接层模块；所述位姿融合网络是多层感知机(multilayer-perceptron，MLP)，用于接收第一全连接层模块发送的相机位姿和第二全连接层发送的激光雷达位姿；将相机位姿和激光雷达位姿进行融合，输出最终的融合相机位姿。需要说明的是，由于激光雷达和相机的空间位置已提前标定，输出的融合位姿既可以表示为融合相机位姿，也能表示为融合激光雷达位姿，本申请以融合相机位姿为例。
相机损失函数模块，用于接收第一全连接层发送的相机位姿、深度网络发送的深度地图和位姿融合网络发送的融合相机位姿；计算相机的损失函数；所述相机的损失函数包括光度误差和深度误差；得到相机损失函数后为其中的光度误差和深度误差分配缩放因子；
激光雷达损失函数模块，用于接收第二全连接层发送的激光雷达位姿、掩模网路膜网络发送的预测掩膜和位姿融合网络发送的融合相机位姿；计算激光雷达的强度损失函数和预测掩膜的交叉熵损失函数；得到激光雷达损失函数后为其中的强度损失和掩膜损失分配缩放因子；
所述模型模块进一步包括：
误差状态传递模型，用于接收IMU发送的三轴加速度和角速度；通过对IMU建模得到误差状态传递方程并发送给自适应核卡尔曼滤波模块；
观测模型，包括相机位置姿态观测和GPS位置速度观测，用于接收融合网络发送的相机位姿和GPS发送的位置速度观测数据，将融合相机位姿和GPS发送的位置速度观测数据组成纳入观测方程中，并将最终的观测方程发送给自适应核卡尔曼滤波模块。
本实施方式的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法，如图3所示，该方法包括以下步骤：
步骤1对车载传感器进行标定，所述标定包括空间标定和时间标定；所述车载传感器指IMU、相机、激光雷达和GPS；
本实施方式中车载传感器指IMU、相机、激光雷达和GPS；空间标定是指用固结于传感器自身的坐标系代表传感器的位置和姿态，用旋转矩阵R和平移向量t描述各个传感器坐标系之间的空间关系；时间标定是指由于各个传感器的频率不同，必须将各个传感器的时间戳划定到统一时间轴上，使各个传感器接收的数据能够整齐划一。
步骤2利用标定好的激光雷达获取激光雷达序列；利用标定好的相机获取相机序列；
步骤3根据激光雷达序列和相机序列，估计相机的位姿，得到融合相机位姿；
步骤3.1将相机序列输入视觉网络和深度网络，通过视觉网络进行特征提取，得到相机序列的图像角点特征，通过深度网络预测相机序列中RGB图像每像素的深度，得到深度地图；将激光雷达序列输入测距网络和掩膜网络，通过测距网络提取特征，得到激光雷达特征，通过掩膜网络检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜。
步骤3.2将得到的图像角点特征和激光雷达特征输入基于ResNet18网络的第三编码器和SoftMax激活函数，得到图像角点特征的重要性权重和激光雷达特征的重要性权重；
本实施方式中重要性权重均介于[0,1]。
步骤3.3将得到的图像角点特征的重要性权重分配给图像角点特征，利用第一全连接层模块进行位姿回归得到相机位姿；将得到的激光雷达特征的重要性权重分配给激光雷达特征，利用第二全连接层模块进行位姿回归得到激光雷达位姿；
步骤3.4将得到的相机位姿和激光雷达位姿输入多层感知机，得到所估计的融合相机位姿；
步骤3.5根据相机位姿、深度地图和估计的融合相机位姿计算相机的损失函数；根据激光雷达位姿和融合相机位姿计算激光雷达的强度损失函数；根据预测掩膜和融合相机位姿计算预测掩膜的交叉熵损失函数；所述相机的损失函数包括光度误差和深度误差；
所述相机的损失函数为：
Lc＝Lp+λsLs(D) (1)
其中，Lc表示相机的损失函数；Lp表示光度误差；Ls(D)表示深度误差，其中的参数D表示深度地图；λs表示为深度误差分配缩放因子。
所述激光雷达的强度损失函数为：
其中，Ll表示激光雷达的强度损失函数；Ms(xt)表示从源帧到目标帧的误差权重；It(xt)表示目标帧集合；表示重建的源帧集合；xt表示目标帧；S表示激光雷达序列的总帧数；s表示激光雷达序列的第一帧。
所述预测掩膜的交叉熵损失函数为：
其中，Lm表示预测掩膜的交叉熵损失函数；P(Ms(xt)＝1)表示当目标帧的预测掩膜为1时的交叉熵。
步骤3.6对相机的损失函数、激光雷达的强度损失函数和预测掩膜的交叉熵损失函数分配缩放因子，得到总的损失函数；
本实施方式中所述总的损失函数为：
L＝Ll+λcLc+λmLm (4)
其中，L表示总的损失函数；λc和λm分别为相机的损失的缩放因子和预测掩膜的交叉熵损失的缩放因子。
步骤3.7通过总的损失函数计算损失值，若损失值未达到极小值则通过反向传播去更新各个网络权值，返回步骤3.4；若损失值达到极小值则执行步骤3.8；
步骤3.8选取总的损失函数最小的融合相机位姿作为最终的融合相机位姿；
本实施方式中通过总的损失函数计算损失值，通过反向传播去更新各个网络权值，来降低真实值与预测值之间的损失，也就是使总的损失函数值变小，选取总的损失函数最小的融合相机位姿作为最终的融合相机位姿。
步骤4对IMU建模并构建误差状态传递模型；
步骤4.1对IMU建模得到IMU的动态微分方程；
IMU配备有陀螺仪和加速度计可分别测量三轴加速度和角速度，测量值伴随着偏置和噪声，本实施方式中所述IMU可建模为：
其中，分别表示IMU角速度和比力的测量值；ωi，fi分别表示角速度和比力的真实值，为陀螺仪的偏置，为加速度计的偏置，ng为陀螺仪的噪声，nf为加速度计的噪声，下标g表示陀螺仪，下标f表示加速度计；描述了均值为0方差为∑的高斯分布；上标i表示该值是IMU坐标系下定义的。
本实施方式中所述IMU的动态微分方程有：
其中，代表从IMU坐标系到导航坐标系的旋转矩阵，n表示导航坐标系；分别表示IMU在导航坐标系下的速度和位置；gn表示导航坐标系下重力向量；(·)×表示3×3的反对称矩阵；为的导数；为的导数；为的导数。
本实施方式中选择东北天(East-North-Up,ENU)坐标系作为导航坐标系。
步骤4.2定义误差状态向量，并将误差状态向量改写为关于误差状态向量的微分方程组的矩阵形式；所述误差状态向量包括导航状态误差和传感器误差；
用于滤波的状态向量定义为误差状态向量，包括导航状态误差和传感器误差,本实施方式中所述误差状态向量为：
在误差状态向量ξ中，前三项为导航状态误差，ξR，ξv和ξp分别表示姿态误差、速度误差和位置误差；后两项为传感器误差，表示陀螺仪的估计误差，且 加速度计的估计误差，且 表示估计值。
将误差状态向量中姿态误差、速度误差和位置误差写成微分方程组有：
其中，为ξR的导数；为ξv导数；为ξp的导数；
然后将所述微分方程组合并为矩阵形式:
其中，表示求导后的误差状态向量；ξ表示误差状态向量，Ni＝[ng,nf,03,1,nbg,nbf]T表示噪声向量，ng为陀螺仪的噪声，nf为加速度计的噪声，nbg和nbf分别为陀螺仪偏置的随机游走噪声和加速度计偏置的随机游走噪声；表示噪声向量Nf的系数矩阵，为15×15的单位矩阵，是误差状态向量的系数矩阵：
步骤4.3根据线性系统理论，将矩阵形式的微分方程组转化，得到离散的系统误差状态传递模型；
所述系统误差状态传递模型为：
ξk+1＝Φk+1|kξk+uk (11)
其中，ξk+1表示k+1时刻的误差状态向量；表示离散时间噪声协方差；Φk+1|k表示转移矩阵，k表示离散时间戳；
由于IMU采样频率较高，在100Hz左右，故可做采样时间很小的假设，在此假设下可做如下近似：
其中，Δt表示IMU采样时间；tk表示k时刻；tk+1表示k+1时刻；
为了方便计算，对做泰勒展开，通常展开到前三项：
其中，I为单位矩阵；
步骤5根据融合相机位姿和GPS接收的位置和速度观测数据，得到误差状态的观测模型；所述融合相机位姿包括的相机位置和相机的姿态；
步骤5.1接收融合相机位姿和GPS接收到导航坐标系下位置和速度，建立相机位置、相机的姿态、GPS接收的位置和GPS接收的速度四个观测量的估计值或测量值的关系式；
本实施方式中四个观测量分别为导航坐标系下的融合相机的位置和相机的姿态GPS接收到导航坐标系下的位置信息导航坐标系下的速度其中下标C表示该信息由相机与激光雷达观测得到，下标G表示该信息由GPS观测得到。
所述四个观测量的估计值或测量值关系如下：
其中，和分别表示导航坐标系下的融合相机的位置的估计值和姿态的估计值、GPS接收到导航坐标系下的位置信息观测值和GPS接收到导航坐标系下的速度观测值；和分别是IMU到相机杆臂和IMU到GPS的杆臂，其位置关系如图4所示，表示n系下IMU的位置，表示导航坐标系下IMU的速度，表示从导航坐标系到IMU坐标系的旋转矩阵，表示从IMU坐标系到相机坐标系的旋转矩阵，和分别是相机姿态的估计噪声、相机位置的估计噪声、GPS位置的估计噪声和GPS速度的估计噪声。
步骤5.2根据各观测量的估计值与实际值，得到误差状态的观测模型；
本将各观测量的估计值与实际值作差构造观测量残差各观测量残差关于误差状态向量的雅可比矩阵观测噪声向量所以观测模型为：
其中，ξ是误差状态向量，vk是观测噪声向量，为融合相机姿态观测残差，为融合相机位置观测残差，为GPS位置观测残差，为GPS速度观测残差；分别表示融合相机姿态观测、融合相机位置观测、GPS位置观测、GPS速度观测的雅可比矩阵；分别表示融合相机姿态观测、融合相机位置观测、GPS位置观测、GPS速度观测的观测噪声。
步骤6利用自适应核卡尔曼滤波算法对误差状态向量进行滤波，得到下一时刻的误差状态向量；
步骤6.1对误差状态向量的原始粒子进行核均值嵌入操作，得到生成粒子。
具体地说，k-1时刻的误差状态向量的粒子和其在和空间的映射分别为：和同样给定k-1时刻的后验权重向量和正定的后验权重矩阵使用原始粒子得到k-1时刻后验的经验核均值和协方差算子：
其中
使用二次核，将数据空间粒子映射在高维空间中，则可以用ξk-1和表示经验核均值，从中提取和
作为重要性均值，作为重要性方差，根据重要性分布生成粒子：
其中，表示k-1时刻的生成粒子，其在核空间映射为
步骤6.2利用步骤6.1的误差状态向量生成粒子和步骤4.3的误差状态传递模型进行预测，得到预测的下一时刻先验误差状态向量的生成粒子。
从k-1时刻生成粒子预测k时刻粒子：
其中，表示预测的下一时刻先验误差状态向量的生成粒子，f(·)表示步骤4.3中的误差状态传递模型，是过程噪声，k时刻先验粒子的特征映射为
步骤6.3根据k时刻的观测和利用误差状态的观测模型修正步骤6.2中计算的先验误差状态向量的生成粒子，得到更新的后验误差状态向量。
观测模型为：
其中，h(·)表示步骤5中的观测方程，表示观测噪声，由步骤5中观测量残差生成的的k时刻观测粒子为其特征映射为
k时刻后验误差状态向量为：
其中，Qk表示核卡尔曼增益，ξk +为下一时刻的后验误差状态向量，ξk -为下一时刻的先验验误差状态向量，为从ξk到yk的概率分布。
步骤7对IMU输出的三轴加速度和角速度进行积分推算得到位置、速度和姿态信息；
步骤8将下一时刻更新的后验误差状态向量与积分推算得到位置、速度和姿态信息进行广义求和，得到最终的位置速度估计和姿态估计；
所述位置速度估计和姿态估计为：
其中，p，v，R分别为最终的位置估计、速度估计和姿态估计，ξp,ξv,ξR为误差状态向量ξ中的元素，分别为由IMU积分推算得到的位置、速度和姿态的初步估计值。

Claims (8)

1.一种基于自适应核卡尔曼滤波的多传感器融合定位系统，其特征在于，包括传感器模块、基于深度学习的相机位姿估计模块、模型模块、自适应核卡尔曼滤波模块、求和模块和积分模块；
所述传感器模块，包括IMU、相机、激光雷达和GPS四种传感器；所述IMU用于测量三轴加速度和角速度并将测量得到的三轴加速度和角速度发送给模型模块和求和模块；所述相机用于获取相机序列并将相机序列发送给基于深度学习的相机位姿估计模块；所述激光雷达用于获取激光雷达序列并将激光雷达序列发送给基于深度学习的相机位姿估计模块；所述GPS用于接收位置和速度观测数据并将接收到的位置和速度观测数据发送给模型模块；
所述基于深度学习的相机位姿估计模块，用于接收相机发送的相机序列和激光雷达发送的激光雷达序列；利用神经网络对相机序列和激光雷达序列进行处理，得到相机的位姿；将最终得到相机的位姿发送给模型模块；
所述模型模块，用于接收传感器模块发送的三轴加速度和角速度；接收传感器模块发送的位置和速度观测数据；接收基于深度学习的相机位姿估计模块发送的相机的位姿；对三轴加速度和角速度进行处理得到误差状态传递方程并将其发送给自适应核卡尔曼滤波模块；对相机的位姿、位置和速度观测数据进行处理得到观测方程并将其发送给自适应核卡尔曼滤波模块；
所述自适应核卡尔曼滤波模块，用于接收模型模块发送的误差状态传递方程和观测方程；利用自适应核卡尔曼滤波算法得到误差状态向量并将其发送给求和模块；
所述积分模块，用于接收传感器模块发送的三轴加速度和角速度；对三轴加速度和角速度进行积分推算得到位置速度和姿态信息并将位置速度和姿态信息发送给求和模块；
所述求和模块，用于接收积分模块发送的位置速度和姿态信息；接收模型模块发送的误差状态向量；将误差状态向量与积分推算得到的位置速度和姿态信息进行广义求和，输出最终的位置速度和姿态信息。
2.根据权利要求1所述的基于自适应核卡尔曼滤波的多传感器融合定位系统，其特征在于，所述基于深度学习的相机位姿估计模块进一步包括：
视觉网络，包括基于ResNet18网络的第一编码器和一个第一全连接层模块；所述基于ResNet18网络的第一编码器用于接收相机发送的相机序列并对相机序列进行相机特征提取，得到图像角点特征并将其发送给第一全连接层模块和注意力模块；所述第一全连接层模块包括两个串联的全连接层，用于接收基于ResNet18网络的第一编码器发送的图像角点特征，接收注意力模块发送的图像角点特征的重要性权重，通过两个全连接层完成位姿回归，得到相机位姿并将其发送给位姿融合网络和相机损失函数模块；
深度网络，包括基于Unet架构的一对编码器和解码器，用于接收相机发送的相机序列；预测相机序列中RGB图像每像素的深度，构成预测深度地图D并将其发送给相机损失函数模块；
测距网络，包括基于ResNet18网络的第二编码器和第二全连接层模块，所述基于ResNet18网络的第二编码器，用于接收激光雷达发送的激光雷达序列并对激光雷达序列进行特征提取得到激光雷达特征并将其发送给第二全连接层模块和注意力模块；所述第二全连接层模块包括两个串联的全连接层，用于接收基于ResNet18网络的第二编码器发送的激光雷达特征；接收注意力模块发送的激光雷达特征的重要性权重，通过两个全连接层完成位姿回归得到激光雷达位姿并将其发送给激光雷达损失函数模块和位姿融合网络；
掩膜网络，包括基于Unet架构的一对编码器和解码器，用于接收激光雷达发送的激光雷达序列；检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜M并将其发送给激光雷达损失函数模块；
融合模块包括注意力模块和位姿融合网络，所述注意力模块由基于ResNet18网络的第三编码器和SoftMax激活函数组成，该模块用于接收第一全连接层模块发送的相机位姿和第二全连接层发送的激光雷达位姿；生成图像角点特征的重要性权重和激光雷达特征的重要性权重；将图像角点特征的重要性权重发送给第一全连接层模块，将激光雷达特征的重要性权重发送给第二全连接层模块；所述位姿融合网络是多层感知机multilayer-perceptron，MLP，用于接收第一全连接层模块发送的相机位姿和第二全连接层发送的激光雷达位姿；将相机位姿和激光雷达位姿进行融合，输出最终的融合相机位姿；
相机损失函数模块，用于接收第一全连接层发送的相机位姿、深度网络发送的深度地图和位姿融合网络发送的融合相机位姿；计算相机的损失函数；所述相机的损失函数包括光度误差和深度误差；得到相机损失函数后为其中的光度误差和深度误差分配缩放因子；
激光雷达损失函数模块，用于接收第二全连接层发送的激光雷达位姿、掩模网路膜网络发送的预测掩膜和位姿融合网络发送的融合相机位姿；计算激光雷达的强度损失函数和预测掩膜的交叉熵损失函数；得到激光雷达损失函数后为其中的强度损失和掩膜损失分配缩放因子。
3.根据权利要求1所述的基于自适应核卡尔曼滤波的多传感器融合定位系统，其特征在于，所述模型模块进一步包括：
误差状态传递模型，用于接收IMU发送的三轴加速度和角速度；通过对IMU建模得到误差状态传递方程并发送给自适应核卡尔曼滤波模块；
观测模型，包括相机位置姿态观测和GPS位置速度观测，用于接收融合网络发送的相机位姿和GPS发送的位置速度观测数据，将融合相机位姿和GPS发送的位置速度观测数据组成纳入观测方程中，并将最终的观测方程发送给自适应核卡尔曼滤波模块。
4.基于权利要求1所述的基于自适应核卡尔曼滤波的多传感器融合定位系统，实现的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法，其特征在于，包括如下步骤：
步骤1对车载传感器进行标定，所述标定包括空间标定和时间标定；所述车载传感器指IMU、相机、激光雷达和GPS；
步骤2利用标定好的激光雷达获取激光雷达序列；利用标定好的相机获取相机序列；
步骤3根据激光雷达序列和相机序列，估计相机的位姿，得到融合相机位姿；
步骤4对IMU建模并构建误差状态传递模型；
步骤5根据融合相机位姿和GPS接收的位置和速度观测数据，得到误差状态的观测模型；所述相机位姿包括的相机位置和相机的姿态；
步骤6利用自适应核卡尔曼滤波算法对误差状态向量进行滤波，得到下一时刻的误差状态向量；
步骤7对IMU输出的三轴加速度和角速度进行积分推算得到位置、速度和姿态信息；
步骤8将下一时刻更新的后验误差状态向量与积分推算得到位置、速度和姿态信息进行广义求和，得到最终的位置速度估计和姿态估计。
5.根据权利要求4所述的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法，其特征在于，所述步骤3具体包括：
步骤3.1将相机序列输入视觉网络和深度网络，通过视觉网络进行特征提取，得到相机序列的图像角点特征，通过深度网络预测相机序列中RGB图像每像素的深度，得到深度地图；将激光雷达序列输入测距网络和掩膜网络，通过测距网络提取特征，得到激光雷达特征，通过掩膜网络检测激光雷达序列的连续帧中的一致性区域，得到预测掩膜；
步骤3.2将得到的图像角点特征和激光雷达特征输入基于ResNet18网络的第三编码器和SoftMax激活函数，得到图像角点特征的重要性权重和激光雷达特征的重要性权重；
步骤3.3将得到的图像角点特征的重要性权重分配给图像角点特征，利用第一全连接层模块进行位姿回归得到相机位姿；将得到的激光雷达特征的重要性权重分配给激光雷达特征，利用第二全连接层模块进行位姿回归得到激光雷达位姿；
步骤3.4将得到的相机位姿和激光雷达位姿输入多层感知机，得到所估计的融合相机位姿；
步骤3.5根据相机位姿、深度地图和估计的融合相机位姿计算相机的损失函数；根据激光雷达位姿和融合相机位姿计算激光雷达的强度损失函数；根据预测掩膜和融合相机位姿计算预测掩膜的交叉熵损失函数；所述相机的损失函数包括光度误差和深度误差；
所述相机的损失函数为：
Lc＝Lp+λsLs(D) (1)
其中，Lc表示相机的损失函数；Lp表示光度误差；Ls(D)表示深度误差，其中的参数D表示深度地图；λs表示为深度误差分配缩放因子；
所述激光雷达的强度损失函数为：
其中，Ll表示激光雷达的强度损失函数；Ms(xt)表示从源帧到目标帧的误差权重；It(xt)表示目标帧集合；表示重建的源帧集合；xt表示目标帧；S表示激光雷达序列的总帧数；s表示激光雷达序列的第一帧；
所述预测掩膜的交叉熵损失函数为：
其中，Lm表示预测掩膜的交叉熵损失函数；P(Ms(xt)＝1)表示当目标帧的预测掩膜为1时的交叉熵；
步骤3.6对相机的损失函数、激光雷达的强度损失函数和预测掩膜的交叉熵损失函数分配缩放因子，得到总的损失函数；
所述总的损失函数为：
L＝Ll+λcLc+λmLm (4)
其中，L表示总的损失函数；λc和λm分别为相机的损失的缩放因子和预测掩膜的交叉熵损失的缩放因子；
步骤3.7通过总的损失函数计算损失值，若损失值未达到极小值则通过反向传播去更新各个网络权值，返回步骤3.4；若损失值达到极小值则执行步骤3.8；
步骤3.8选取总的损失函数最小的融合相机位姿作为最终的融合相机位姿。
6.根据权利要求4所述的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法，其特征在于，所述步骤4具体包括：
步骤4.1对IMU建模得到IMU的动态微分方程；
步骤4.2定义误差状态向量，并将误差状态向量改写为关于误差状态向量的微分方程组的矩阵形式；所述误差状态向量包括导航状态误差和传感器误差；
步骤4.3根据线性系统理论，将矩阵形式的微分方程组转化，得到离散的系统误差状态传递模型。
7.根据权利要求4所述的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法，其特征在于，所述步骤5具体包括：
步骤5.1接收融合相机位姿和GPS接收到导航坐标系下位置和速度，建立相机位置、相机的姿态、GPS接收的位置和GPS接收的速度四个观测量的估计值或测量值的关系式；
所述四个观测量的估计值或测量值关系如下：
其中，和分别表示导航坐标系下的融合相机的位置的估计值和姿态的估计值、GPS接收到导航坐标系下的位置信息观测值和GPS接收到导航坐标系下的速度观测值；和分别是IMU到相机杆臂和IMU到GPS的杆臂，表示n系下IMU的位置，表示导航坐标系下IMU的速度，表示从导航坐标系到IMU坐标系的旋转矩阵，表示从IMU坐标系到相机坐标系的旋转矩阵，和分别是相机姿态的估计噪声、相机位置的估计噪声、GPS位置的估计噪声和GPS速度的估计噪声；ωi表示角速度的真实值；
步骤5.2根据各观测量的估计值与实际值，得到误差状态的观测模型；
所述误差状态的观测模型为：
其中，为观测量残差，为观测量残差关于误差状态向量的雅可比矩阵，为观测噪声向量；ξ是误差状态向量，vk是观测噪声向量，为融合相机姿态观测残差，为融合相机位置观测残差，为GPS位置观测残差，为GPS速度观测残差；分别表示融合相机姿态观测、融合相机位置观测、GPS位置观测、GPS速度观测的雅可比矩阵；分别表示融合相机姿态观测、融合相机位置观测、GPS位置观测、GPS速度观测的观测噪声。
8.根据权利要求4所述的基于自适应核卡尔曼滤波的多传感器融合车辆定位方法，其特征在于，所述步骤6具体包括：
步骤6.1对误差状态向量的原始粒子进行核均值嵌入操作，得到生成粒子；步骤6.2利用步骤6.1的误差状态向量生成粒子和步骤4.3的误差状态传递模型进行预测，得到预测的下一时刻先验误差状态向量的生成粒子；
步骤6.3根据k时刻的观测和利用误差状态的观测模型修正步骤6.2中计算的先验误差状态向量的生成粒子，得到更新的后验误差状态向量。
CN202311110775.XA 2023-08-31 2023-08-31 基于自适应核卡尔曼滤波的多传感器融合定位系统及方法 Pending CN117250630A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311110775.XA CN117250630A (zh) 2023-08-31 2023-08-31 基于自适应核卡尔曼滤波的多传感器融合定位系统及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311110775.XA CN117250630A (zh) 2023-08-31 2023-08-31 基于自适应核卡尔曼滤波的多传感器融合定位系统及方法

Publications (1)

Publication Number Publication Date
CN117250630A true CN117250630A (zh) 2023-12-19

Family

ID=89128547

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311110775.XA Pending CN117250630A (zh) 2023-08-31 2023-08-31 基于自适应核卡尔曼滤波的多传感器融合定位系统及方法

Country Status (1)

Country Link
CN (1) CN117250630A (zh)

Similar Documents

Publication Publication Date Title
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
US11519729B2 (en) Vision-aided inertial navigation
CN109887057B (zh) 生成高精度地图的方法和装置
US10295365B2 (en) State estimation for aerial vehicles using multi-sensor fusion
CN104729506B (zh) 一种视觉信息辅助的无人机自主导航定位方法
CN113252033B (zh) 基于多传感器融合的定位方法、定位系统及机器人
CN110196443A (zh) 一种飞行器的容错组合导航方法及系统
KR20210111180A (ko) 위치 추적 방법, 장치, 컴퓨팅 기기 및 컴퓨터 판독 가능한 저장 매체
JP2009294214A (ja) 地形構造を用いたナビゲーション方法及びシステム
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN111308415B (zh) 一种基于时间延迟的在线估计位姿的方法和设备
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN116929338B (zh) 地图构建方法、设备及存储介质
US11860287B2 (en) System and method for detecting outliers in GNSS observations
CN116202509A (zh) 一种面向室内多层建筑的可通行地图生成方法
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
Ćwian et al. GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments
Wang et al. Micro aerial vehicle navigation with visual-inertial integration aided by structured light
CN117268408A (zh) 一种激光slam定位方法及系统
CN117031513A (zh) 一种道路及附属物的实时监测定位方法、系统及装置
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN116182855A (zh) 一种弱光强环境下仿复眼偏振视觉无人机组合导航方法
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN117250630A (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