CN117521006A - 一种基于增量学习的因子图多源信息融合方法 - Google Patents

一种基于增量学习的因子图多源信息融合方法 Download PDF

Info

Publication number
CN117521006A
CN117521006A CN202311467200.3A CN202311467200A CN117521006A CN 117521006 A CN117521006 A CN 117521006A CN 202311467200 A CN202311467200 A CN 202311467200A CN 117521006 A CN117521006 A CN 117521006A
Authority
CN
China
Prior art keywords
information
factor
factor graph
imu
variable
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
CN202311467200.3A
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 Science and Technology
Southeast University
Original Assignee
Nanjing University of Science and Technology
Southeast University
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 Science and Technology, Southeast University filed Critical Nanjing University of Science and Technology
Priority to CN202311467200.3A priority Critical patent/CN117521006A/zh
Publication of CN117521006A publication Critical patent/CN117521006A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/214—Generating training patterns; Bootstrap methods, e.g. bagging or boosting
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/217—Validation; Performance evaluation; Active pattern learning techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
    • G06F18/2415—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches based on parametric or probabilistic models, e.g. based on likelihood ratio or false acceptance rate versus a false rejection rate
    • G06F18/24155—Bayesian classification
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/29—Graphical models, e.g. Bayesian networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/40—Software arrangements specially adapted for pattern recognition, e.g. user interfaces or toolboxes therefor
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N5/00—Computing arrangements using knowledge-based models
    • G06N5/01—Dynamic search techniques; Heuristics; Dynamic trees; Branch-and-bound
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N5/00—Computing arrangements using knowledge-based models
    • G06N5/04—Inference or reasoning models
    • G06N5/041—Abduction

Landscapes

  • Engineering & Computer Science (AREA)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Computational Linguistics (AREA)
  • Computing Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Probability & Statistics with Applications (AREA)
  • Human Computer Interaction (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于增量学习的因子图多源信息融合方法。方法为：首先对无人车搭载的传感器和程控电源进行自检，匹配相应的因子图框架；然后采集传感器量测信息，同步进行导航参数解算，并传输到中央处理器；接着设定先验信息，建立传感器的因子图模型和对应的代价函数；当有新的观测信息时，将受新添加因子影响的部分转换回因子图，并将与新的测量相关联的因子添加进去，加入训练集进行增量学习，并通过选择变量的消元顺序，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络；如果还有新的观测信息，则重复进行因子添加；否则输出导航信息。本发明具有适用性强、计算效率高、抗扰能力强、可维护性和扩展性强的优点。

Description

一种基于增量学习的因子图多源信息融合方法
技术领域
本发明属于组合导航与多源信息融合技术领域，特别是一种基于增量学习的因子图多源信息融合方法。
背景技术
在现代军事环境中，卫星拒止行为的出现给卫星通信和导航***带来了威胁和干扰，严重影响了相关应用的连续性和可靠性。随着信息技术的发展和应用的广泛，多源信息融合成为解决现实世界问题的重要手段。多源信息融合是指将来自不同传感器、不同平台或不同技术的多个信息源进行集成和分析，以获得更全面、准确和可靠的信息。
通常组合导航的滤波器都采用卡尔曼滤波，传统卡尔曼滤波是基于当前时刻的测量和先前时刻的状态估计进行更新，滤波器对新的测量信息的响应存在一定的滞后性，无法实时地反映***的变化，对于快速变化的***或需要及时响应的应用，传统卡尔曼滤波无法满足实时性的要求。
与基于卡尔曼滤波的多源融合方法不同，因子图的框架能够将众多不同频率、不同误差的传感器合并起来进行应用，如IMU传感器更新频率较快，而如相机、雷达等更新频率较慢。根据量测信息对导航状态变量的影响，因子图将未知变量结点和已知测量值的关系进行编码，融合来自不同的、非同步的传感器观测值的问题就变成在因子图中连接测量值定义的因子与相应结点的问题，还可以将因子图框架扩展到更多新加入的传感器。因子图优化的优势便是具备对多源导航信息的即插即用能力，基于因子图优化的多源融合方法保留了历史信息，通过对多个时刻的状态进行联合估计，提升了估计精度，但是该方法存在计算量大、延迟的问题。
发明内容
本发明的目的在于提供一种计算量小、实时性好、能够达到实时估计的要求、分类精度高、增量学习效率高的多源信息融合方法。
实现本发明目的的技术解决方案为：一种基于增量学习的因子图多源信息融合方法，包括以下步骤：
步骤1、对无人车搭载的传感器和程控电源进行自检，识别当前环境，匹配相应的因子图框架；
步骤2、实时采集传感器量测信息，同步进行导航参数解算，并传输到中央处理器；
步骤3、建立不同导航传感器的因子图模型和对应的代价函数；
步骤4、如果有新的观测信息，将受新添加因子影响的部分转换回因子图，并将与新的测量相关联的因子添加进去，加入训练集进行增量学习，选择特征项并设定初始权值，动态调整置信度因子并修正代价函数；
步骤5、选择具有最小未消元维度的变量作为下一个要消元的变量，通过选择变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断；
步骤6、如果还有新的观测信息，则返回步骤4，否则得到状态的最优估计，输出导航信息。
进一步地，步骤1中所述的传感器，包括惯导IMU、激光雷达Lidar、视觉Visual、磁传感器Mag、里程计Odo、高度计Alt和超声波雷达Ultra。
进一步地，步骤1所述的对无人车搭载的传感器和程控电源进行自检，识别当前环境，匹配相应的因子图框架，具体如下：
步骤1.1、将各传感器通过工装安装在车体上，接通电源进行预热，进行导航参数初始化，在监控终端实时显示定位信息，对无人车搭载的传感器和程控电源进行自检；
步骤1.2、根据无人车的任务需求和所处环境，判断传感器类型和对应的可行性，决定相应的因子图框架。
进一步地，步骤3中，建立不同导航传感器的因子图模型，具体如下：
因子图表示为：G＝(F,X,E)，其中F为因子节点集合，因式分解中的局部函数fi∈F；X为变量节点集合，全局多元函数中的变量xj∈X；E为边缘连线，表示因子节点fi与变量节点xj连接边的集合，eij∈E；
因子图G定义为函数f(X)的因式分解：
f(X)＝Πifi(Xi)
其中Xi是与因子节点fi有关的所有变量节点的集合，
在因子图模型中，变量节点到因子节点或者因子节点到变量节点的是通过信息交互的，节点的变化和增删通过信息传递反应；
采用因子图这类概率图模型，描述导航状态变量的联合概率分布，将导航状态最大后验估计的求解转化为因子图的推断，计算方法如下所示：
式中，x＝argmaxf(x)表示求满足使f(x)最大时的x的值；Xj:k＝{Xj,Xj+1,...,Xk}表示导航***tj时刻到tk时刻之间的所有状态量，Zj:k＝{Zj,Zj+1,...,Zk}表示tj时刻到tk时刻这一时间段内所接收到的所有测量值；P(Xj:k|Zj:k)表示在已知所有观测量Zj,Zj+1,...,Zk下估计状态Xj,Xj+1,...,Xk的概率函数；
将无人车的导航状态抽象为变量节点，将来自不同导航传感器的异步异构量测信息抽象为因子节点，xk代表***的导航状态，f代表各传感器量测信息；
fprior表示先验信息，fIMU表示来自IMU的量测信息，与tk时刻和tk+1时刻的导航状态相关；fMag、fAlt、fOdo、fVisual、fLidar和fUltra分别表示来自Mag、Alt、Odo、Visual、Lidar和Ultra的量测信息。
进一步地，步骤3中，建立不同导航传感器对应的代价函数，具体如下：
步骤3.1、设定先验信息；
步骤3.2、对惯导IMU的观测信息在预积分的区间内进行预积分处理，添加惯导IMU因子节点fIMU和偏差因子节点fbias；
步骤3.3、对其它传感器的观测信息，直接添加相应的因子节点。
进一步地，步骤3.1所述的设定先验信息，具体如下：
无人车定位因子图模型的先验信息包括车辆初始位置和初始速度，对于某些变量x∈X0，先验信息为均值为μx、方差为∑x的高斯分布模型，先验因子fprior(x)表示为：
式中，d(x)为x∈X0的先验因子的代价函数。
进一步地，步骤3.2所述的对惯导IMU的观测信息在预积分的区间内进行预积分处理，添加惯导IMU因子节点fIMU和偏差因子节点fbias，具体如下：
预测导航状态通过以下连续非线性微分方程描述：
式中，xk为当前导航状态，fb和ωb分别表示加速度计和陀螺仪测量的比力和角速度，h(·)为量测函数，进行预测得到初始值xk+1；ck为IMU校准参数，的时间传播用其非线性模型来描述：
对于给定的IMU测量值IMU的常规因子为：
式中，d(·)表示相应的代价函数；
IMU测量值和当前预估值xk,ck用来预测状态xk+1，该预测值和当前估计xk+1之间的差值是因子表示的误差函数fIMU(xk+1,xk,ck)；
与IMU误差计算模型相关的偏置因子fbias为：
其中，ck+1,ck在因子图中表示为变量节点；
新的IMU测量的运动状态更新公式为：
其中，分别为时刻tm-1到时刻tm的由姿态、速度、位置更新，下标m和m-1分别表示时刻tm和时刻tm-1，下标wi(m-1)表示固定在i-frame(惯性坐标系)上的历史框架w-frame(世界坐标系)，下标bi(m-1)表示固定在i-frame(惯性坐标系)上的历史框架b-frame(载体坐标系)，表示由下标向上标的坐标系转换，/>表示在时刻tm-1b-frame相对于w-frame的旋转，四元数/>或者旋转矩阵/>是地球自转引起的，I表示单位矩阵，g表示重力加速度，/>是由地心惯性坐标系映射到世界坐标系的角速度，Δtm-1,m＝tm-tm-1表示IMU采样时间间隔；
为了消除初始运动状态的依赖性，在时刻tk-1将b-frame固定到e-frame(地球坐标系)，即be(k-1)，推导tk-1时刻的精细化预积分模型的参考框架：
其中，和/>分别为姿态预积分、速度预积分和位置预积分，上标表示映射目标坐标系，下标表示映射初始坐标系，在精细预积分中使用姿态项/>是因为其需要绝对姿态来考虑地球自转，/>和/>分别是速度预积分和位置预积分的科氏矫正项，R是旋转矩阵。
进一步地，步骤3.3所述的对其它传感器的观测信息，直接添加相应的因子节点，具体如下：
步骤3.3.1、激光雷达Lidar的因子图模型为：
zLidar＝H(xi-1,xi)+nLidar
式中，zLidar是激光雷达当前量测值，H(·)是量测函数，xi-1是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，nLidar为量测噪声；
激光雷达Lidar的代价函数为：
fLidar(xi-1,xi)＝d[zLidar i-H(xi-1,xi)]
其中，d(·)是代价函数；
步骤3.3.2、超声波雷达Ultra的因子图模型为：
zUltra＝H(xi-1,xi)+nUltra
式中，zUltra是激光雷达当前量测值，H(·)是量测函数，xi-1是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，nUltra为量测噪声；
超声波雷达Ultra的代价函数为：
fUltra(xi-1,xi)＝d(zUltra i-H(xi-1,xi))
步骤3.3.3、双目相机Visual因子的因子图模型为：
其中，为tk时刻深度相机量测值，hVisual(·)为量测函数，nVisual为量测噪声；
双目相机Visual的代价函数为：
步骤3.3.4、磁传感器Mag因子的因子图模型为：
其中，为tk时刻磁传感器量测值，hMag为量测函数，nMag为量测噪声；
磁传感器Mag的代价函数为：
步骤3.3.5、里程计Odo因子的因子图模型为：
其中，为tk时刻里程计量测值，hOdo为量测函数，nOdo为量测噪声；
里程计Odo的代价函数为：
步骤3.3.6、高度计Alt因子的因子图模型为：
其中，为tk时刻高度计量测值，hAlt为量测函数，nAlt为量测噪声；
高度计Alt的代价函数为：
进一步地，步骤4所述的如果有新的观测信息，将受新添加因子影响的部分转换回因子图，并将与新的测量相关联的因子添加进去，加入训练集进行增量学习，选择特征项并设定初始权值，动态调整置信度因子并修正代价函数，具体如下：
步骤4.1、域增量学习的定义为：
P(X(t))≠P(X(t+1))
Y(t)＝Y(t+1)P(Y(t))＝P(Y(t+1))
其中，X(t)，Y(t)分别为任务t的数据样本集和类别标签集，上标t、t+1表示任务类别，P(X(t))和P(Y(t))分别表示任务t数据概率分布和标签概率分布；
步骤4.2、传感器因子信息dj归属于类别Ci的概率为，1≤i≤n：
其中，P(Ci)为类Ci条件概率，其值为属于类别Ci的因子信息数与总的训练因子信息数的比值，P(tk|ci)为特征项tk在类别Ci中出现的概率；
步骤4.3、在增量贝叶斯模型中，设定事件发生的先验概率为C，P(C|T0)表示其概率密度函数，其中T0表示先验信息，依据贝叶斯原理，当有新的样本S进入训练集，根据先验信息P(C|T0)，经计算得出后验信息P(C|S,T0)的公式为：
其中，P(S|C,T0)表示已知先验信息和先验概率的条件下样本发生的概率密度函数，P(C|T0)表示已知先验信息的条件下先验概率发生的概率密度函数，P(S|T0)表示已知先验信息的条件下样本发生的概率密度函数；
其中：
P(S|T0)＝∫[P(S|C,T0)P(C|T0)]dc
其中，P(S|C)表示先验概率发生的条件下样本发生的概率密度函数；
贝叶斯增量学习模型综合先验信息和与新增的样本信息S，得到后验信息T1，即：
后验信息(T1)＝先验信息(T0)+新增样本信息(S)。
进一步地，步骤5所述的选择具有最小未消元维度的变量作为下一个要消元的变量，通过选择变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断，具体如下：
步骤5.1、使用启发式排序中的最小维度法，选择一个具有最小未消元维度的变量作为下一个要消元的变量；
所述的启发式排序，具体如下：
①计算原始序列中每个元素对其他元素的比较次数，比较次数基于欧氏距离来定义；
②初始化排序顺序为原始序列的顺序；
③对于每个元素，计算该元素与其他元素的平均比较次数，即统计该元素在当前排序顺序下与其他元素的比较次数，并求平均值；
④选择平均比较次数最小的元素，并放置在当前排序顺序中的最前面；
⑤重复上述步骤，直到所有元素排序完毕；
步骤5.2、临时的因子图通过选择选择一个具有最小未消元维度的变量作为下一个要消元的变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断。
与现有技术相比，本发明的显著优势在于：(1)在根据复杂战场环境动态变换时，可以通过增加或剔除因子实现即插即用的功能，适用性强；(2)采用iSAM2优化工具箱，在增量因子图中增加滑动窗口，限制参与优化的因子信息，只使用最近窗口内的历史信息对下一时刻的量测信息进行预估，在新数据到达时，仅更新受到影响的特征和因子，而不是对整个模型进行全局更新，减小了计算量，提高了计算效率；(3)采用动态置信度因子的优先级策略方法，根据数据源的特性来衡量对于融合任务的贡献，通过最大化分类置信度，利用微平均F1均值实时监控并评估分类器性能，动态调整为数据源置信度因子，来改善分类器学***方作为惩罚项，以控制参数的大小，避免了数据噪声对模型的过度拟合；(5)采用不依赖外部信息，隐蔽性好的自主式惯导为核心搭配不同传感器进行信息融合，提高了抗干扰能力；(6)采用Ubuntu操作***，适用于各种通用计算需求，ROS采用模块化的架构，提高了***的可维护性和扩展性。
附图说明
图1为本发明一种基于增量学习的因子图多源信息融合方法的流程示意图。
图2为本发明实施例中基于增量学习的因子图多源信息融合软件的流程示意图。
图3为本发明实施例中因子图的一般模型的结构示意图。
图4为本发明实施例中各传感器因子节点与变量节点的结构示意图。
图5为本发明实施例中增量贝叶斯模型的结构示意图。
具体实施方式
下面结合附图和具体实施例，对本发明做进一步的详细说明。
本发明的目的在于提供一种结合增量学习的因子图嵌入式多源信息融合方法，能在Ubuntu环境下以惯导为核心，包括激光雷达、视觉、磁传感器、里程计、高度计、超声波雷达等传感器实现多源信息融合。
结合图1，本发明一种基于增量学习的因子图多源信息融合方法，包括以下步骤：
步骤1、对无人车搭载的传感器和程控电源进行自检，确保正常工作和安全性，识别当前环境，匹配相应的因子图框架，具体如下：
步骤1.1、将各传感器通过工装安装在车体上，包括惯导IMU、激光雷达Lidar、视觉Visual、磁传感器Mag、里程计Odo、高度计Alt和超声波雷达Ultra，接通电源进行预热，进行导航参数初始化，在监控终端实时显示定位信息，对无人车搭载的传感器和程控电源进行自检，确保正常工作；
步骤1.2、根据无人车的任务需求和所处环境，判断传感器类型和对应的可行性，决定相应的因子图框架。。
步骤2、实时采集传感器量测信息，同步进行导航参数解算，并传输到中央处理器，具体如下：
步骤3、设定先验信息，建立不同导航传感器的因子图模型和对应的代价函数f(X)，具体如下：
步骤3.1、设定先验信息，具体如下：
多源导航信息处理***的因子图G为：
G＝(F,X,E)
其中，F为因子节点集合，因式分解中的局部函数fi∈F；X为变量节点集合，全局多元函数中的变量xj∈X；E为边缘连线集合，因子节点fi与变量节点xj连接边eij∈E；
将因子图G定义为函数f(X)的因式分解：
f(X)＝Πifi(Xi) (1)
其中Xi是与因子节点fi有关的所有变量节点的集合，
因子图的一般模型如图3所示，在因子图模型中，变量节点到因子节点或者因子节点到变量节点是通过信息交互的，因此节点的变化和增删可以通过信息传递反应。
采用因子图模型描述导航状态变量的联合概率分布，将导航状态最大后验估计的求解转化为因子图的推断，计算方法为：
式中，x＝argmaxf(x)表示求满足使f(x)最大时的x的值；Xj:k＝{Xj,Xj+1,...,Xk}表示导航***tj时刻到tk时刻之间的所有状态量，Zj:k＝{Zj,Zj+1,...,Zk}表示对应时间段内所接收到的所有测量值；P(Xj:k|Zj:k)表示在已知所有观测量Zj,Zj+1,...,Zk下估计状态Xj,Xj+1,...,Xk的概率函数。
无人车定位因子图模型采用先验信息表示先验因子，包括车辆初始位置、初始速度等。先验信息fprior(x)定义为均值为μx、方差为∑x的高斯分布模型，表示为：
式中d(x)为x∈X0的先验因子的代价函数。
步骤3.2、对IMU的观测信息在预积分的区间内进行预积分处理，添加IMU因子节点fIMU和偏差因子节点fbias，具体如下：
预测导航状态xk可以通过以下连续非线性微分方程在概念上描述：
式中，fb和ωb分别表示加速度计和陀螺仪测量的比力和角速度，h(·)为量测函数；由ck表示的IMU校准参数，由于根据假定的IMU误差模型校正IMU测量fb和ωb，这种IMU误差模型通常与导航状态的估计相结合进行估计；式(4)的线性化使用状态空间表示，其中包含适当的雅可比矩阵和过程噪声，设定该过程噪声为零均值高斯噪声。
一般情况下，ck的时间传播可以用它自己的非线性模型来描述，如随机游走：
ck＝gc(ck+1) (5)
对于给定的IMU测量值IMU的常规因子为：
式中：d[·]表示相应的代价函数，h(·)为量测函数；
IMU测量值和当前预估值xk,ck用来预测状态xk+1，该预测值和当前估计xk+1之间的差值是因子表示的误差函数；
当向图中增加新的可变节点xk+1，需要为其设置一个合理的初始值，与IMU误差计算模型相关的偏置因子由下式给出：
其中ck+1,ck在因子图中表示为变量节点。由于IMU校准参数ci的典型缓慢动态，因子f偏差和实际校准节点可以以显著低于IMU速率的频率添加到因子图中。
对于新的IMU测量，使用公式(8)来更新运动状态，包括姿态、速度和位置：
其中，分别为时刻tm-1到时刻tm的由姿态、速度、位置更新，下标m和m-1分别表示时刻tm和时刻tm-1，下标wi(m-1)表示固定在i-frame(惯性坐标系)上的历史框架w-frame(世界坐标系)，下标bi(m-1)表示固定在i-frame(惯性坐标系)上的历史框架b-frame(载体坐标系)，表示由下标向上标的坐标系转换，/>表示在时刻tm-1b-frame相对于w-frame的旋转，四元数/>或者旋转矩阵/>是地球自转引起的，I表示单位矩阵，g表示重力加速度，/>是由地心惯性坐标系向世界坐标系转换的角速率，Δtm-1,m表示由时刻m-1到时刻m。
由于IMU运动积分依赖于初始运动状态，因此不能有效地应用于FGO框架。当线性化的点或偏差估计发生变化时，采用预积分理论对模型进行重新表述，以避免多次重复积分。为了消除初始运动状态的依赖性，在时刻tk-1将b-frame固定到e-frame，即be(k-1)，定义为精细化预积分模型的参考框架：
其中，和/>分别为姿态预积分、速度预积分和位置预积分，上标表示映射目标坐标系，下标表示映射初始坐标系，在精细预积分中使用姿态项/>是因为其需要绝对姿态来考虑地球自转，/>和/>分别是速度预积分和位置预积分的科氏矫正项，R是旋转矩阵。
步骤3.3、对其它传感器的观测信息，直接添加相应的因子节点，具体如下：
步骤3.3.1、激光雷达Lidar的因子图模型和代价函数：
根据实际测量方程和先验路标点设置，可在多个层面上整合激光雷达传感器观量测值，每个测量描述两个时间实例之间地平面上位置和方向的变化，它的实现为在时刻的位姿和时刻的位姿之间的相对姿态，不影响高度、横滚和俯仰，所以激光雷达可以被视为二元因子，因此测量模型为：
zLidar＝H(xi-1,xi)+nLidar (10)
式中，zLidar是激光雷达当前量测值，xi-1是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，nLidar为上下两个时刻的差值；H(·)是激光雷达相对位姿测量函数，表示载体两个时刻姿态的关系，因此激光雷达Lidar的代价函数为：
fLidar(xi-1,xi)＝d(zi-H(xi-1,xi)) (11)
其中，d(·)是代价函数。
步骤3.3.2、超声波雷达Ultra的因子图模型为：
zUltra＝H(xi-1,xi)+nUltra (10)
式中，zUltra是激光雷达当前量测值，H(·)是量测函数，xi-1是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，nUltra为量测噪声；
超声波雷达Ultra的代价函数为：
fUltra(xi-1,xi)＝d(zUltra i-H(xi-1,xi)) (11)
其中，d(·)是代价函数。
步骤3.3.3、双目相机Visual的因子图模型和代价函数：
双目相机Visual的量测方程为：
其中，为tk时刻深度相机量测值，hVisual为量测函数，nVisual为量测噪声；
双目相机Visual的代价函数为：
其中，d(·)是代价函数，双目相机因子也是一元因子，在因子图中和对应时刻的状态变量xk相连；
步骤3.3.4、磁传感器Mag的因子图模型和代价函数：
磁传感器Mag的量测方程为：
其中，为tk时刻磁传感器量测值，hMag为量测函数，nMag为量测噪声；
磁传感器Mag的代价函数为：
其中，d(·)是代价函数，磁传感器因子也是一元因子，在因子图中和对应时刻的状态变量xk相连；
步骤3.3.5、里程计Odo的因子图模型和代价函数：
里程计Odo的量测方程为：
其中，为tk时刻里程计量测值，hOdo为量测函数，nOdo为量测噪声；
里程计Odo的代价函数为：
其中，d(·)是代价函数，里程计因子也是一元因子，在因子图中和对应时刻的状态变量xk相连；
步骤3.3.6、高度计Alt的因子图模型和代价函数：
高度计Alt的量测方程为：
其中，为tk时刻高度计量测值，hAlt为量测函数，nAlt为量测噪声；
高度计Alt的代价函数为：
其中，d(·)是代价函数，高度计因子也是一元因子，在因子图中和对应时刻的状态变量xk相连。
因子图框架已经将无人车的导航状态抽象为变量节点，将无人车的导航状态抽象为变量节点，将来自不同导航传感器的异步异构量测信息抽象为因子节点将来自不同导航传感器的异步异构量测信息抽象为因子节点搭建出来，软件流程图如图2所示：
1)创建IMU测量对象并初始化：创建IMU测量对象，使用GTSAM提供的PreintegratedImuMeasurements类，将IMU的测量数据传递给该对象，并设置好IMU的噪声参数，包含噪声协方差矩阵和偏差；然后创建IMU预积分器IMUPreintegrator对象，用于预积分IMU测量数据并计算预积分因子；初始化状态向量和噪声协方差矩阵。
2)创建优化参数对象：是否使用优化工具箱ISAM2进行判断，否则就选用最小二乘法Levenberg Marquardt，若使用就创建一个优化参数对象，设置对应因子图的框架及接入的传感器类型，并设置优化参数ISAM2Params。添加先验因素PriorFactor到因子图框架中，构建IMU因子噪声模型。
3)实时发布融合：基于创建ROS节点和回调函数，订阅惯导的话题/imu/data，添加IMU预积分因子imu_preintergrated，构建IMU因子，回调函数循环等待其他传感器的topic/lidar_info、vis_info等，添加相应的传感器因子加入图中。
步骤4、如果有新的观测信息，将受新添加因子影响的部分转换回因子图，并将与新的测量相关联的因子添加进去，加入训练集进行增量学习，选择特征项并设定初始权值，动态调整置信度因子并修正代价函数，具体如下：
步骤4.1、针对增量学习中的灾难性遗忘问题，即当模型对新任务进行学习时会遗忘之前任务所学习的知识，结合增量因子图框架的灵活性，选择利用动态架构来解决灾难性遗忘问题，其优势在于可以为不同任务动态分配结构或参数，能够进一步提高模型可扩展性和自适应能力。根据学习场景、任务状况的特征，采取学习过程为域增量学习domain-incremental learning：不同时刻到达的数据属于同一任务的相同类别，但输入数据的分布发生了变化，不同时间片上的数据属于不同的域。在该场景下，在训练阶段提供任务标识，推理阶段不提供任务标识，并且提供共享标识。
域增量学习的基本设定为：
P(X(t))≠P(X(t+1)) (20)
Y(t)＝Y(t+1)P(Y(t))＝P(Y(t+1)) (21)
其中，X(t)，Y(t)分别为任务t的数据样本集和类别标签集，上标t、t+1表示任务类别，P(X(t))和P(Y(t))分别表示任务t数据概率分布和标签概率分布。
步骤4.2、朴素贝叶斯分类器是一种基于贝叶斯定理和特征条件独立假设的概率分类模型。在传统的朴素贝叶斯模型中，每个特征被视为相互独立的，而且每个特征对分类结果的影响具有相同的权重，但是在某些情况下，不同的特征可能对分类结果具有不同的重要性或权重。通过引入样本权重，加权朴素贝叶斯模型可以更好地适应不平衡数据集或给予某些样本更高的重要性，可以用于各种分类任务，并且可以与其他技术，如特征选择、正则化等结合使用来提高分类器的性能，传感器因子信息dj归属于类别Ci的概率为，其中1≤i≤n的概率可以用下面的公式求解：
其中，P(Ci)为类Ci条件概率，其值为属于类别Ci的因子信息数与总的训练因子信息数的比值，P(tk|ci)为特征项tk在类别Ci中出现的概率。
步骤4.3、当训练样本的数据不完备或者面临大量信息涌入的情况时，朴素贝叶斯分类算法预测的样本类型可能会不准确，针对以上问题，可以引入增量学习的方法，通过逐步更新类别的先验概率和条件概率，使得改进朴素贝叶斯算法更好地处理新的数据并达到更准确的分类结果。
在增量贝叶斯模型中，设事件发生的先验概率用C表示，P(C|T0)表示其概率密度函数，其中T0表示先验信息，依据贝叶斯原理，当有新的样本S进入训练集，根据先验信息P(C|T0)，经计算得出后验信息P(C|S,T0)的公式为：
其中，P(S|C,T0)表示已知先验信息和先验概率的条件下样本发生的概率密度函数，P(C|T0)表示已知先验信息的条件下先验概率发生的概率密度函数，P(S|T0)表示已知先验信息的条件下样本发生的概率密度函数。
其中：
P(S|T0)＝∫[P(S|C,T0)P(C|T0)]dc (24)
其中，P(S|C)表示先验概率发生的条件下样本发生的概率密度函数。
由图5可以看出，贝叶斯增量学习模型综合了先验信息和与新增的样本信息S，最终得到后验信息，即：
后验信息(T1)＝先验信息(T0)+新增信息(S) (25)
贝叶斯增量学习实质是一个动态的过程，它通过不断修正和更新先验信息来适应新增样本的信息。当新的样本到达时，之前积累的后验信息会立即转化为当前的先验信息。关键的前提是后验信息必须与先验信息具有相同的分布。这意味着新增样本的分布与已有样本的分布是一致的。只有在后验信息与先验信息分布相同的情况下，才能使用当前的后验信息作为下一次迭代的先验信息；如果分布不一致，那么就不能直接使用当前的后验信息作为先验信息进行下一次的学习，可能需要采取特定的方法来解决分布不一致的问题，例如领域自适应、特征选择或特征变换等技术。
针对增量学习的性能可以通过以下几个方面进行评估：
(a)准确性：模型的准确性是通过考虑矩阵I的对角元素，实现对训练集和测试集的平均精度进行考虑。准确性A可以经过下式计算：
式中，给定训练测试样本精度矩阵I，其中每个元素Ii,j∈RT×T为观察任务i的最后一个样本得到的模型在任务j上的测试分类精度。
(b)遗忘率：为了评估模型的遗忘程度，表示是在任务i上训练模型后对任务j的遗忘程度，定义如下：
其中ai,j为模型在任务i上训练后，在任务j上的分类准确率，任务T的平均遗忘率FT度量定义为：
(c)计算效率：任务之间的平均计算效率为fCE：
其中，Ops(Dt)是指在Dt进行训练时所需要的乘法和加法运算总次数；而Ops↑↓(Dt)指Dt进行一次正向和反向传播所需要的运算次数。为了避免Ops↑↓(Dt)过小，对其统一乘以一个大于1的缩放因子ε，与学习Dt所需要的epoch次数相关。
步骤5、选择具有最小未消元维度的变量作为下一个要消元的变量，通过选择变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断，具体如下：
引入带有滑动窗口的优化工具箱ISAM2，根据融合需求和计算能力，将滑动窗口设置为10，保留部分历史信息，得到优化后的融合结果iSAM2->calculateEstimate，若不使用则得到最小二乘法后的优化结果optimizer，获取的优化结果用以更新状态向量和噪声协方差矩阵，然后循环订阅惯导的话题数据。
信息融合问题已经使用因子图的表示方式推导出来，并且定义了一些常用传感器的因子。新添加因子影响的部分转换回因子图，并将与新的测量相关联的新的因子添加进去，得到一个临时的因子图。
因子图编码的非线性优化问题可以通过在标准高斯-牛顿非线性优化器中重复线性化来解决。从变量组Λk的一个初始估计值开始，高斯牛顿法通过线性化***找到估计的修正值Δ满足：
式中，为当前线性化点/>的稀疏雅可比矩阵，/>为给定所有测量值Zk的残差。雅克比矩阵等效于线性化的因子图，且其块结构反映了因子图的结构。
求解更新值需要利用QR分解或Cholesky分解雅克比矩阵为一个上三角矩阵的形式。对于上述的极值问题，将式(22)对求偏微分并使其等于零，得到：
JΔ＝b (31)
式(23)可通过矩阵求逆得到，但是当数据量较大时，计算量变得很大，因此一般利用矩阵分解来求解。设定矩阵J可QR分解为J＝QR，则有QTJ＝[R 0]T且QTb＝[d c]T，根据式(23)，有RΔ＝d。由于R为上三角矩阵，因此可通过回代法求得Δ。得到Δ后，线性化点更新到一个新的估计值
***每接收到一个新的传感器测量值就在图中产生一个新的因子，这等效于在线性化最小二乘问题的测量雅克比矩阵中增加新的一行，通常雅克比矩阵发生变化后，就需要从头对新的雅克比矩阵进行分解，但这样随着时间的增长计算量会越来越大。为防止构建的模型过度复杂，结合机器学***衡，从而实现特征选择，使得权重更加平衡及准确，降低参数的复杂度。
在因子图的框架中，优化是通过变量消除来完成。为了计算边缘概率或条件概率，首先确定变量消除的顺序，然后从因子图中依次消除各个变量。对于要消除的变量，首先找到包含该变量的所有因子，然后将这些因子合并为一个新的因子，以减少计算的复杂度；在合并因子时，可能会导致因子图中形成和弦。和弦是一个在图中连接了多个非相邻节点的路径。形成和弦后，可以将因子图中的变量和连接关系映射到和弦贝叶斯网络中。在和弦贝叶斯网络中，节点表示随机变量，边表示变量之间的依赖关系。形成和弦贝叶斯网，贝叶斯网络中只有很少一部分会因为新增加的因子而发生变化，因此并不需要每次都从头开始计算，可以更高效地计算目标变量的边缘概率或条件概率，通过对图的拓扑结构进行推断。
因此，当新的测量值到来时，只需要关注贝叶斯网络中受到影响的部分。一旦识别出贝叶斯网络中受到影响的部分，就将该部分换回因子图的形式，并加上新的因子，然后将产生的因子图重新进行变量消除；再将变量消除得到的贝叶斯网络融合到原来未受影响的贝叶斯网络中。
在增量平滑因子图中，通过将窗口内的数据构建成图的节点，并在节点之间建立关系，来表示数据之间的条件依赖关系。通过滑动窗口内的历史数据，以及新数据的连接，可以更新增量平滑因子图中的节点的估计值，避免在新数据到达时重新计算整个模型。滑动窗口可以减少过去残差的影响，提高动态估计的能力，通过保持一定数量的残差减少计算量和数据存储量。
步骤6、如果还有新的观测信息，则重复步骤4，否则结束，得到状态的最优估计，输出导航信息。
以上仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种基于增量学习的因子图多源信息融合方法，其特征在于，包括以下步骤：
步骤1、对无人车搭载的传感器和程控电源进行自检，识别当前环境，匹配相应的因子图框架；
步骤2、实时采集传感器量测信息，同步进行导航参数解算，并传输到中央处理器；
步骤3、建立不同导航传感器的因子图模型和对应的代价函数；
步骤4、如果有新的观测信息，将受新添加因子影响的部分转换回因子图，并将与新的测量相关联的因子添加进去，加入训练集进行增量学习，选择特征项并设定初始权值，动态调整置信度因子并修正代价函数；
步骤5、选择具有最小未消元维度的变量作为下一个要消元的变量，通过选择变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断；
步骤6、如果还有新的观测信息，则返回步骤4，否则得到状态的最优估计，输出导航信息。
2.根据权利要求1所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤1中所述的传感器，包括惯导IMU、激光雷达Lidar、视觉Visual、磁传感器Mag、里程计Odo、高度计Alt和超声波雷达Ultra。
3.根据权利要求2所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤1所述的对无人车搭载的传感器和程控电源进行自检，识别当前环境，匹配相应的因子图框架，具体如下：
步骤1.1、将各传感器通过工装安装在车体上，接通电源进行预热，进行导航参数初始化，在监控终端实时显示定位信息，对无人车搭载的传感器和程控电源进行自检；
步骤1.2、根据无人车的任务需求和所处环境，判断传感器类型和对应的可行性，决定相应的因子图框架。
4.根据权利要求3所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤3中，建立不同导航传感器的因子图模型，具体如下：
因子图表示为：G＝(F,X,E)，其中F为因子节点集合，因式分解中的局部函数fi∈F；X为变量节点集合，全局多元函数中的变量xj∈X；E为边缘连线，表示因子节点fi与变量节点xj连接边的集合，eij∈E；
因子图G定义为函数f(X)的因式分解：
f(X)＝Πifi(Xi)
其中Xi是与因子节点fi有关的所有变量节点的集合，
在因子图模型中，变量节点到因子节点或者因子节点到变量节点的是通过信息交互的，节点的变化和增删通过信息传递反应；
采用因子图这类概率图模型，描述导航状态变量的联合概率分布，将导航状态最大后验估计的求解转化为因子图的推断，计算方法如下所示：
式中，x＝argmaxf(x)表示求满足使f(x)最大时的x的值；Xj:k＝{Xj,Xj+1,...,Xk}表示导航***tj时刻到tk时刻之间的所有状态量，Zj:k＝{Zj,Zj+1,...,Zk}表示tj时刻到tk时刻这一时间段内所接收到的所有测量值；P(Xj:k|Zj:k)表示在已知所有观测量Zj,Zj+1,...,Zk下估计状态Xj,Xj+1,...,Xk的概率函数；
将无人车的导航状态抽象为变量节点，将来自不同导航传感器的异步异构量测信息抽象为因子节点，xk代表***的导航状态，f代表各传感器量测信息；
fprior表示先验信息，fIMU表示来自IMU的量测信息，与tk时刻和tk+1时刻的导航状态相关；fMag、fAlt、fOdo、fVisual、fLidar和fUltra分别表示来自Mag、Alt、Odo、Visual、Lidar和Ultra的量测信息。
5.根据权利要求4所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤3中，建立不同导航传感器对应的代价函数，具体如下：
步骤3.1、设定先验信息；
步骤3.2、对惯导IMU的观测信息在预积分的区间内进行预积分处理，添加惯导IMU因子节点fIMU和偏差因子节点fbias；
步骤3.3、对其它传感器的观测信息，直接添加相应的因子节点。
6.根据权利要求5所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤3.1所述的设定先验信息，具体如下：
无人车定位因子图模型的先验信息包括车辆初始位置和初始速度，对于某些变量x∈X0，先验信息为均值为μx、方差为∑x的高斯分布模型，先验因子fprior(x)表示为：
式中，d(x)为x∈X0的先验因子的代价函数。
7.根据权利要求6所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤3.2所述的对惯导IMU的观测信息在预积分的区间内进行预积分处理，添加惯导IMU因子节点fIMU和偏差因子节点fbias，具体如下：
预测导航状态通过以下连续非线性微分方程描述：
式中，xk为当前导航状态，fb和ωb分别表示加速度计和陀螺仪测量的比力和角速度，h(·)为量测函数，进行预测得到初始值xk+1；ck为IMU校准参数，的时间传播用其非线性模型来描述：
对于给定的IMU测量值IMU的常规因子为：
式中，d(·)表示相应的代价函数；
IMU测量值和当前预估值xk,ck用来预测状态xk+1，该预测值和当前估计xk+1之间的差值是因子表示的误差函数fIMU(xk+1,xk,ck)；
与IMU误差计算模型相关的偏置因子fbias为：
其中，ck+1,ck在因子图中表示为变量节点；
新的IMU测量的运动状态更新公式为：
其中，分别为时刻tm-1到时刻tm的由姿态、速度、位置更新，下标m和m-1分别表示时刻tm和时刻tm-1，下标wi(m-1)表示固定在i-frame(惯性坐标系)上的历史框架w-frame(世界坐标系)，下标bi(m-1)表示固定在i-frame(惯性坐标系)上的历史框架b-frame(载体坐标系)，表示由下标向上标的坐标系转换，/>表示在时刻tm-1b-frame相对于w-frame的旋转，四元数/>或者旋转矩阵/>是地球自转引起的，I表示单位矩阵，g表示重力加速度，/>是由地心惯性坐标系映射到世界坐标系的角速度，Δtm-1,m＝tm-tm-1表示IMU采样时间间隔；
为了消除初始运动状态的依赖性，在时刻tk-1将b-frame固定到e-frame(地球坐标系)，即be(k-1)，推导tk-1时刻的精细化预积分模型的参考框架：
其中，和/>分别为姿态预积分、速度预积分和位置预积分，上标表示映射目标坐标系，下标表示映射初始坐标系，在精细预积分中使用姿态项/>是因为其需要绝对姿态来考虑地球自转，/>和/>分别是速度预积分和位置预积分的科氏矫正项，R是旋转矩阵。
8.根据权利要求7所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤3.3所述的对其它传感器的观测信息，直接添加相应的因子节点，具体如下：
步骤3.3.1、激光雷达Lidar的因子图模型为：
zLidar＝H(xi-1,xi)+nLidar
式中，zLidar是激光雷达当前量测值，H(·)是量测函数，xi-1是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，nLid ar为量测噪声；
激光雷达Lidar的代价函数为：
fLidar(xi-1,xi)＝d[zLidar i-H(xi-1,xi)]
其中，d(·)是代价函数；
步骤3.3.2、超声波雷达Ultra的因子图模型为：
zUltra＝H(xi-1,xi)+nUltra
式中，zUltra是激光雷达当前量测值，H(·)是量测函数，xi-1是第i-1时刻的位姿状态，xi是第i时刻的位姿状态，nUlt ra为量测噪声；
超声波雷达Ultra的代价函数为：
fUltra(xi-1,xi)＝d(zUltra i-H(xi-1,xi))
步骤3.3.3、双目相机Visual因子的因子图模型为：
其中，为tk时刻深度相机量测值，hVisual(·)为量测函数，nVisual为量测噪声；
双目相机Visual的代价函数为：
步骤3.3.4、磁传感器Mag因子的因子图模型为：
其中，为tk时刻磁传感器量测值，hMag为量测函数，nMag为量测噪声；
磁传感器Mag的代价函数为：
步骤3.3.5、里程计Odo因子的因子图模型为：
其中，为tk时刻里程计量测值，hOdo为量测函数，nOdo为量测噪声；
里程计Odo的代价函数为：
步骤3.3.6、高度计Alt因子的因子图模型为：
其中，为tk时刻高度计量测值，hAlt为量测函数，nAlt为量测噪声；
高度计Alt的代价函数为：
。
9.根据权利要求8所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤4所述的如果有新的观测信息，将受新添加因子影响的部分转换回因子图，并将与新的测量相关联的因子添加进去，加入训练集进行增量学习，选择特征项并设定初始权值，动态调整置信度因子并修正代价函数，具体如下：
步骤4.1、域增量学习的定义为：
P(X(t))≠P(X(t+1))
Y(t)＝Y(t+1)P(Y(t))＝P(Y(t+1))
其中，X(t)，Y(t)分别为任务t的数据样本集和类别标签集，上标t、t+1表示任务类别，P(X(t))和P(Y(t))分别表示任务t数据概率分布和标签概率分布；
步骤4.2、传感器因子信息dj归属于类别Ci的概率为，1≤i≤n：
其中，P(Ci)为类Ci条件概率，其值为属于类别Ci的因子信息数与总的训练因子信息数的比值，P(tk|ci)为特征项tk在类别Ci中出现的概率；
步骤4.3、在增量贝叶斯模型中，设定事件发生的先验概率为C，P(C|T0)表示其概率密度函数，其中T0表示先验信息，依据贝叶斯原理，当有新的样本S进入训练集，根据先验信息P(C|T0)，经计算得出后验信息P(C|S,T0)的公式为：
其中，P(S|C,T0)表示已知先验信息和先验概率的条件下样本发生的概率密度函数，P(C|T0)表示已知先验信息的条件下先验概率发生的概率密度函数，P(S|T0)表示已知先验信息的条件下样本发生的概率密度函数；
其中：
P(S|T0)＝∫[P(S|C,T0)P(C|T0)]dc
其中，P(S|C)表示先验概率发生的条件下样本发生的概率密度函数；
贝叶斯增量学习模型综合先验信息和与新增的样本信息S，得到后验信息T1，即：
后验信息(T1)＝先验信息(T0)+新增样本信息(S)。
10.根据权利要求9所述的基于增量学习的因子图多源信息融合方法，其特征在于，步骤5所述的选择具有最小未消元维度的变量作为下一个要消元的变量，通过选择变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断，具体如下：
步骤5.1、使用启发式排序中的最小维度法，选择一个具有最小未消元维度的变量作为下一个要消元的变量；
所述的启发式排序，具体如下：
①计算原始序列中每个元素对其他元素的比较次数，比较次数基于欧氏距离来定义；
②初始化排序顺序为原始序列的顺序；
③对于每个元素，计算该元素与其他元素的平均比较次数，即统计该元素在当前排序顺序下与其他元素的比较次数，并求平均值；
④选择平均比较次数最小的元素，并放置在当前排序顺序中的最前面；
⑤重复上述步骤，直到所有元素排序完毕；
步骤5.2、临时的因子图通过选择选择一个具有最小未消元维度的变量作为下一个要消元的变量的消元顺序，构建网络结构，从因子图中消去节点并构建条件概率表，得到表示变量条件依赖关系的贝叶斯网络，用于概率建模和推断。
CN202311467200.3A 2023-11-07 2023-11-07 一种基于增量学习的因子图多源信息融合方法 Pending CN117521006A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311467200.3A CN117521006A (zh) 2023-11-07 2023-11-07 一种基于增量学习的因子图多源信息融合方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311467200.3A CN117521006A (zh) 2023-11-07 2023-11-07 一种基于增量学习的因子图多源信息融合方法

Publications (1)

Publication Number Publication Date
CN117521006A true CN117521006A (zh) 2024-02-06

Family

ID=89754264

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311467200.3A Pending CN117521006A (zh) 2023-11-07 2023-11-07 一种基于增量学习的因子图多源信息融合方法

Country Status (1)

Country Link
CN (1) CN117521006A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117848332A (zh) * 2024-03-07 2024-04-09 北京理工大学前沿技术研究院 一种车载多源融合高精度定位***的imu噪声消除方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117848332A (zh) * 2024-03-07 2024-04-09 北京理工大学前沿技术研究院 一种车载多源融合高精度定位***的imu噪声消除方法
CN117848332B (zh) * 2024-03-07 2024-05-03 北京理工大学前沿技术研究院 一种车载多源融合高精度定位***的imu噪声消除方法

Similar Documents

Publication Publication Date Title
Indelman et al. Factor graph based incremental smoothing in inertial navigation systems
Agamennoni et al. An outlier-robust Kalman filter
Li et al. A robust graph optimization realization of tightly coupled GNSS/INS integrated navigation system for urban vehicles
Campbell et al. Distributed data fusion: Neighbors, rumors, and the art of collective knowledge
CN111337020A (zh) 引入抗差估计的因子图融合定位方法
CN110275193B (zh) 一种基于因子图的集群卫星协同导航方法
Dang et al. Cubature Kalman filter under minimum error entropy with fiducial points for INS/GPS integration
CN117521006A (zh) 一种基于增量学习的因子图多源信息融合方法
CN107727097B (zh) 基于机载分布式位置姿态测量***的信息融合方法和装置
Xu et al. A multi-sensor information fusion method based on factor graph for integrated navigation system
McIntyre A comprehensive approach to sensor management and scheduling
Bucci et al. Evaluation of UKF-based fusion strategies for autonomous underwater vehicles multisensor navigation
Wiktor et al. Collaborative multi-robot localization in natural terrain
CN113175931A (zh) 基于约束卡尔曼滤波的集群组网协同导航方法及***
Qian et al. IMM-UKF based land-vehicle navigation with low-cost GPS/INS
Ellingson et al. Relative navigation of fixed-wing aircraft in GPS-denied environments
CN116086445A (zh) 一种基于因子图优化的多源信息时延融合导航方法
Das et al. Review of factor graphs for robust GNSS applications
Fakoorian et al. Rose: Robust state estimation via online covariance adaption
Blackman et al. Multiple sensor data association and fusion in aerospace applications
Van Nam et al. Learning type-2 fuzzy logic for factor graph based-robust pose estimation with multi-sensor fusion
CN117685953A (zh) 面向多无人机协同定位的uwb与视觉融合定位方法及***
Li et al. A track-oriented approach to target tracking with random finite set observations
Bozorg et al. A decentralised navigation architecture
Poddar et al. Tuning of GPS aided attitude estimation using evolutionary algorithms

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