CN115200578A - 基于多项式优化的惯性基导航信息融合方法及*** - Google Patents

基于多项式优化的惯性基导航信息融合方法及*** Download PDF

Info

Publication number
CN115200578A
CN115200578A CN202210898677.6A CN202210898677A CN115200578A CN 115200578 A CN115200578 A CN 115200578A CN 202210898677 A CN202210898677 A CN 202210898677A CN 115200578 A CN115200578 A CN 115200578A
Authority
CN
China
Prior art keywords
representing
navigation
chebyshev
state
variance
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
CN202210898677.6A
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.)
Shanghai Jiaotong University
Original Assignee
Shanghai Jiaotong 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 Shanghai Jiaotong University filed Critical Shanghai Jiaotong University
Priority to CN202210898677.6A priority Critical patent/CN115200578A/zh
Publication of CN115200578A publication Critical patent/CN115200578A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/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/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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Computing Systems (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Operations Research (AREA)
  • Probability & Statistics with Applications (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种基于多项式优化的惯性基导航信息融合方法及***，包括：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航。本发明在最大验后准则下是一种最优惯性基信息融合方法，克服了传统信息融合算法对***非线性所作的近似；本发明相比于传统导航信息融合算法，收敛速度更快，精度更高，稳定性更好。

Description

基于多项式优化的惯性基导航信息融合方法及***
技术领域
本发明涉及传感器技术领域，具体地，涉及一种基于多项式优化的惯性基导航信息融合方法及***。
背景技术
惯性基导航是指以惯性器件为核心，其他传感器为辅助构建的多传感器组合导航导航***。由于自主性强、不受外界干扰、运动信息丰富等特点，惯性基导航在航空航天、大地测量、无人***、装备平台等应用中发挥了重要作用。针对不同应用需求，常见的惯性基导航***中还包括GNSS卫星导航接收机、磁力仪、里程计、雷达、摄像机等多种辅助传感器。无论采用哪些测量设备构成惯性基导航***，都需要选择一个最优的信息融合策略将多种测量信息有效融合。
当前惯性基信息融合方法可以分为滤波和优化两大框架。基于滤波的方法，如扩展Kalman滤波、无迹Kalman滤波，由于需要对惯性基***的非线性模型/概率密度函数低阶近似/确定性采样，存在融合精度低，稳定性差等问题。基于非线性优化的信息融合方法，如因子图优化，为了降低计算量需要对惯导输出预积分，损失了惯导原始观测噪声的准高斯特性，导致信息融合算法也不是最优的。如何根据不同传感器测量特性，设计出精度更高、稳定性更好的信息融合算法，是惯性基组合导航***的核心模块。
需要指出的是，本发明提出的基于多项式优化的惯性基导航信息融合方法是专利“CN202210032043.2”与专利“CN202210032044.7”的延伸与推广。具体来说，专利“CN202210032043.2”提出的基于多项式优化的高精度惯性解算方法，可以看作是惯性基信息融合的状态递推部分(不含其他辅助传感器观测)。而发明“CN202210032044.7”提出的基于多项式优化的惯性与磁传感器姿态估计方法，仅考虑了磁力仪与惯性器件的信息融合，估计状态仅包含姿态与惯性器件常值零偏。本发明考虑了包含多种传感器、运动学约束的惯性基信息融合问题，估计状态也扩展到速度、位置、姿态、惯性器件的零偏、比力因子、交轴耦合系数以及其他辅助传感器中的未知参数。
专利文献CN101957204B(申请号：CN201010295640.1)公开了一种基于相互测距信息的机群惯性导航数据融合方法，该方法考虑到惯性导航***的定位误差基本上服从正态分布这一特征，利用机载数据链获得的机群各节点之间的相互测距信息和惯性导航数据，使用几何图形平移旋转的数据融合方法来估计机群各节点的惯性导航定位误差，可以将机群惯性导航位置精度提高1/2左右。但该发明仅针对特定机群的惯性导航数据融合，且并非最优导航信息融合方法。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于多项式优化的惯性基导航信息融合方法及***。
根据本发明提供的一种基于多项式优化的惯性基导航信息融合方法，包括：
步骤S1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
步骤S2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
步骤S3：求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航；
将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复步骤S1至步骤S4。
优选地，对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态关系如下：
Figure RE-GDA0003852513260000021
Figure RE-GDA0003852513260000022
其中，上标、下标b表示载体坐标系b系，e表示地球坐标系e系，i表示惯性坐标系i系，g表示陀螺坐标系；
Figure RE-GDA0003852513260000023
表示b系相对于e系的姿态四元数，
Figure RE-GDA0003852513260000024
表示b系相对于 e系的姿态旋转矩阵，
Figure RE-GDA0003852513260000025
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度地速度，γe表示e系下的地球重力向量；
Figure RE-GDA0003852513260000026
表示四元数乘法，*表示四元数的共轭；yg表示陀螺仪测量的角速度,ya表示加速度计测量的比力；bg表示陀螺仪的零偏，ba表示加速度计的零偏；ng～N(0,Rg)表示陀螺仪的高斯白噪声，na～N(0,Ra)表示加速度计的高斯白噪声，Rg表示陀螺仪的高斯白噪声协方差矩阵，Ra表示加速度计的高斯白噪声协方差矩阵；In表示n维单位矩阵；
Figure RE-GDA0003852513260000031
表示b系到g系的姿态旋转矩阵；Mg与 Ma是陀螺与加速度计的误差矩阵，包含比力因子和交轴耦合系数，表示为：
Figure RE-GDA0003852513260000032
其中，sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy， mgxz，mgyz表示陀螺仪的交轴耦合系数；
将其他辅助传感器包括卫星导航接收机、磁力仪、里程计、雷达、摄像机的观测模型统一表示为：
ys(t)＝h(x,Θ,t)+ns
其中，t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-GDA0003852513260000033
其中，
Figure RE-GDA0003852513260000034
表示A集合是B集合的子集，pe表示e系下载体的位置；Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集；
Figure RE-GDA0003852513260000035
运动约束模型统一表示为：
Figure RE-GDA0003852513260000039
其中，yc(t)表示为t时刻的约束伪观测，函数
Figure RE-GDA0003852513260000036
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为高斯白噪声的协方差矩阵；
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关，此外，观测模型与运动约束模型中的x能够包括多个时刻下的状态；
在当前采样时间窗口
Figure RE-GDA0003852513260000037
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-GDA0003852513260000038
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-GDA0003852513260000041
其中，τ为当前时间。
优选地，在所述步骤S1中：
对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000042
Figure RE-GDA0003852513260000043
其中，di为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000044
表示四元数切比雪夫系数组成的矩阵；
Figure RE-GDA0003852513260000045
其中Fi与
Figure RE-GDA0003852513260000046
表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000047
Figure RE-GDA0003852513260000048
Figure RE-GDA0003852513260000049
其中，pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA00038525132600000410
表示速度切比雪夫系数组成的矩阵，
Figure RE-GDA00038525132600000411
表示区间初始时刻的先验位置，
Figure RE-GDA00038525132600000412
其中，
Figure RE-GDA00038525132600000413
为i阶切比雪夫多项式的积分：
Figure RE-GDA0003852513260000051
优选地，在所述步骤S2中：
导航状态及传感器误差模型参数通过求解如下非线性优化问题获得：
Figure RE-GDA0003852513260000052
满足约束
Figure RE-GDA0003852513260000053
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置 pe(τ0)；初始时刻状态的先验值
Figure RE-GDA0003852513260000054
其中
Figure RE-GDA0003852513260000055
分别表示
Figure RE-GDA0003852513260000056
ve、pe的初始时刻先验；
Figure RE-GDA0003852513260000057
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-GDA0003852513260000058
Ma,0、Mg,0、Ω0分别为
Figure RE-GDA0003852513260000059
Ma， Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-GDA00038525132600000510
其中，δψ0代表初始时刻的姿态误差，定义为
Figure RE-GDA00038525132600000511
运算符[·]2∶4表示取矩阵的第2至4行元素；与初始状态对应的初始方差表示为
Figure RE-GDA00038525132600000512
δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定；目标函数中
Figure RE-GDA00038525132600000513
Jv，Jz分别定义如下：
Figure RE-GDA00038525132600000514
Figure RE-GDA00038525132600000515
Figure RE-GDA00038525132600000516
其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数；
Figure RE-GDA00038525132600000517
eg,ea，es，ec定义为：
Figure RE-GDA00038525132600000518
Figure RE-GDA00038525132600000519
Figure RE-GDA00038525132600000520
Figure RE-GDA00038525132600000521
Figure RE-GDA00038525132600000522
权重矩阵Wx0，Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，
Figure RE-GDA0003852513260000061
上述非线性优化采用小时间窗口迭代完成，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。
优选地，在所述步骤S3中：
递推求解状态估计的方差，具体的方差预测计算方法如下：
Figure RE-GDA0003852513260000066
其中方差的预测步长定义为
Figure RE-GDA0003852513260000062
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵；
tk时刻有观测、约束，状态的方差采用下式更新：
Figure RE-GDA0003852513260000063
其中，Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵；计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的；当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
根据本发明提供的一种基于多项式优化的惯性基导航信息融合***，包括：
模块M1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
模块M2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
模块M3：求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航；
将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复模块M1至模块M4。
优选地，对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态关系如下：
Figure RE-GDA0003852513260000064
Figure RE-GDA0003852513260000065
其中，上标、下标b表示载体坐标系b系，e表示地球坐标系e系，i表示惯性坐标系i系，g表示陀螺坐标系；
Figure RE-GDA0003852513260000071
表示b系相对于e系的姿态四元数，
Figure RE-GDA0003852513260000072
表示b系相对于 e系的姿态旋转矩阵，
Figure RE-GDA0003852513260000073
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度地速度，γe表示e系下的地球重力向量；
Figure RE-GDA00038525132600000711
表示四元数乘法，*表示四元数的共轭；yg表示陀螺仪测量的角速度,ya表示加速度计测量的比力；bg表示陀螺仪的零偏， ba表示加速度计的零偏；ng～N(0,Rg)表示陀螺仪的高斯白噪声，na～N(0,Ra)表示加速度计的高斯白噪声，Rg表示陀螺仪的高斯白噪声协方差矩阵，Ra表示加速度计的高斯白噪声协方差矩阵；In表示n维单位矩阵；
Figure RE-GDA0003852513260000074
表示b系到g系的姿态旋转矩阵；Mg与 Ma是陀螺与加速度计的误差矩阵，包含比力因子和交轴耦合系数，表示为：
Figure RE-GDA0003852513260000075
其中，sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy， mgxz，mgyz表示陀螺仪的交轴耦合系数；
将其他辅助传感器包括卫星导航接收机、磁力仪、里程计、雷达、摄像机的观测模型统一表示为：
ys(t)＝h(x,Θ,t)+ns
其中，t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-GDA0003852513260000076
其中，
Figure RE-GDA0003852513260000077
表示A集合是B集合的子集，pe表示e系下载体的位置；Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集；
Figure RE-GDA0003852513260000078
运动约束模型统一表示为：
Figure RE-GDA00038525132600000710
其中，yc(t)表示为t时刻的约束伪观测，函数
Figure RE-GDA0003852513260000079
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为高斯白噪声的协方差矩阵；
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关，此外，观测模型与运动约束模型中的x能够包括多个时刻下的状态；
在当前采样时间窗口
Figure RE-GDA0003852513260000081
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-GDA00038525132600000811
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-GDA0003852513260000082
其中，τ为当前时间。
优选地，在所述模块M1中：
对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000083
Figure RE-GDA0003852513260000084
其中，di为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000085
表示四元数切比雪夫系数组成的矩阵；
Figure RE-GDA0003852513260000086
其中Fi与
Figure RE-GDA0003852513260000087
表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000088
Figure RE-GDA0003852513260000089
Figure RE-GDA00038525132600000810
其中，pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000091
表示速度切比雪夫系数组成的矩阵，
Figure RE-GDA0003852513260000092
表示区间初始时刻的先验位置，
Figure RE-GDA0003852513260000093
其中，
Figure RE-GDA0003852513260000094
为i阶切比雪夫多项式的积分：
Figure RE-GDA0003852513260000095
优选地，在所述模块M2中：
导航状态及传感器误差模型参数通过求解如下非线性优化问题获得：
Figure RE-GDA0003852513260000096
满足约束
Figure RE-GDA0003852513260000097
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置 pe(τ0)；初始时刻状态的先验值
Figure RE-GDA0003852513260000098
其中
Figure RE-GDA0003852513260000099
分别表示
Figure RE-GDA00038525132600000910
ve、pe的初始时刻先验；
Figure RE-GDA00038525132600000911
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-GDA00038525132600000912
Ma,0、Mg,0、Ω0分别为
Figure RE-GDA00038525132600000913
Ma， Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-GDA00038525132600000914
其中，δψ0代表初始时刻的姿态误差，定义为
Figure RE-GDA00038525132600000915
运算符[·]2∶4表示取矩阵的第2至4行元素；与初始状态对应的初始方差表示为
Figure RE-GDA00038525132600000921
δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定；目标函数中
Figure RE-GDA00038525132600000920
Jv，Jz分别定义如下：
Figure RE-GDA00038525132600000916
Figure RE-GDA00038525132600000917
Figure RE-GDA00038525132600000918
其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数；
Figure RE-GDA00038525132600000919
eg,ea，es，ec定义为：
Figure RE-GDA0003852513260000101
Figure RE-GDA0003852513260000102
Figure RE-GDA0003852513260000103
Figure RE-GDA0003852513260000104
Figure RE-GDA0003852513260000105
权重矩阵
Figure RE-GDA0003852513260000106
Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，
Figure RE-GDA0003852513260000107
上述非线性优化采用小时间窗口迭代完成，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。
优选地，在所述模块M3中：
递推求解状态估计的方差，具体的方差预测计算方法如下：
Figure RE-GDA00038525132600001010
其中方差的预测步长定义为
Figure RE-GDA0003852513260000108
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵；
tk时刻有观测、约束，状态的方差采用下式更新：
Figure RE-GDA0003852513260000109
其中，Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵；计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的；当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
与现有技术相比，本发明具有如下的有益效果：
1、本发明在最大验后准则下是一种最优惯性基信息融合方法，克服了传统信息融合算法对***非线性所作的近似；
2、本发明相比于传统导航信息融合算法，收敛速度更快，精度更高，稳定性更好。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明流程图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
根据本发明提供的一种基于多项式优化的惯性基导航信息融合方法，如图1所示，包括：
步骤S1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
具体地，在所述步骤S1中：
对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000111
Figure RE-GDA0003852513260000112
其中，di为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000113
表示四元数切比雪夫系数组成的矩阵；
Figure RE-GDA0003852513260000114
其中Fi与
Figure RE-GDA0003852513260000115
表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000121
Figure RE-GDA0003852513260000122
Figure RE-GDA0003852513260000123
其中，pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000124
表示速度切比雪夫系数组成的矩阵，
Figure RE-GDA0003852513260000125
表示区间初始时刻的先验位置，
Figure RE-GDA0003852513260000126
其中，
Figure RE-GDA0003852513260000127
为i阶切比雪夫多项式的积分：
Figure RE-GDA0003852513260000128
步骤S2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
具体地，在所述步骤S2中：
导航状态及传感器误差模型参数通过求解如下非线性优化问题获得：
Figure RE-GDA0003852513260000129
满足约束
Figure RE-GDA00038525132600001210
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置 pe(τ0)；初始时刻状态的先验值
Figure RE-GDA00038525132600001211
其中
Figure RE-GDA00038525132600001212
分别表示
Figure RE-GDA00038525132600001213
ve、pe的初始时刻先验；
Figure RE-GDA00038525132600001214
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-GDA00038525132600001215
Ma,0、Mg,0、Ω0分别为
Figure RE-GDA00038525132600001216
Ma， Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-GDA00038525132600001217
其中，δψ0代表初始时刻的姿态误差，定义为
Figure RE-GDA00038525132600001218
运算符[·]2∶4表示取矩阵的第2至4行元素；与初始状态对应的初始方差表示为
Figure RE-GDA00038525132600001219
δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定；目标函数中
Figure RE-GDA00038525132600001220
Jv，Jz分别定义如下：
Figure RE-GDA0003852513260000131
Figure RE-GDA0003852513260000132
Figure RE-GDA0003852513260000133
其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数；
Figure RE-GDA0003852513260000134
eg,ea，es，ec定义为：
Figure RE-GDA0003852513260000135
Figure RE-GDA0003852513260000136
Figure RE-GDA0003852513260000137
Figure RE-GDA0003852513260000138
Figure RE-GDA0003852513260000139
权重矩阵
Figure RE-GDA00038525132600001310
Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，
Figure RE-GDA00038525132600001311
上述非线性优化采用小时间窗口迭代完成，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。
步骤S3：求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航；
具体地，在所述步骤S3中：
递推求解状态估计的方差，具体的方差预测计算方法如下：
Figure RE-GDA00038525132600001312
其中方差的预测步长定义为
Figure RE-GDA00038525132600001313
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵；
tk时刻有观测、约束，状态的方差采用下式更新：
Figure RE-GDA00038525132600001314
其中，Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵；计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的；当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复步骤S1至步骤S4。
具体地，对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态关系如下：
Figure RE-GDA0003852513260000141
Figure RE-GDA0003852513260000142
其中，上标、下标b表示载体坐标系b系，e表示地球坐标系e系，i表示惯性坐标系i系，g表示陀螺坐标系；
Figure RE-GDA0003852513260000143
表示b系相对于e系的姿态四元数，
Figure RE-GDA0003852513260000144
表示b系相对于 e系的姿态旋转矩阵，
Figure RE-GDA0003852513260000145
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度地速度，γe表示e系下的地球重力向量；
Figure RE-GDA0003852513260000149
表示四元数乘法，*表示四元数的共轭；yg表示陀螺仪测量的角速度,ya表示加速度计测量的比力；bg表示陀螺仪的零偏， ba表示加速度计的零偏；ng～N(0,Rg)表示陀螺仪的高斯白噪声，na～N(0,Ra)表示加速度计的高斯白噪声，Rg表示陀螺仪的高斯白噪声协方差矩阵，Ra表示加速度计的高斯白噪声协方差矩阵；In表示n维单位矩阵；
Figure RE-GDA0003852513260000146
表示b系到g系的姿态旋转矩阵；Mg与 Ma是陀螺与加速度计的误差矩阵，包含比力因子和交轴耦合系数，表示为：
Figure RE-GDA0003852513260000147
其中，sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy， mgxz，mgyz表示陀螺仪的交轴耦合系数；
将其他辅助传感器包括卫星导航接收机、磁力仪、里程计、雷达、摄像机的观测模型统一表示为：
ys(t)＝h(x,Θ,t)+ns
其中，t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-GDA0003852513260000148
其中，
Figure RE-GDA0003852513260000151
表示A集合是B集合的子集，pe表示e系下载体的位置；Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集；
Figure RE-GDA0003852513260000152
运动约束模型统一表示为：
Figure RE-GDA0003852513260000153
其中，yc(t)表示为t时刻的约束伪观测，函数
Figure RE-GDA0003852513260000154
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为高斯白噪声的协方差矩阵；
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关，此外，观测模型与运动约束模型中的x能够包括多个时刻下的状态；
在当前采样时间窗口
Figure RE-GDA00038525132600001510
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-GDA0003852513260000155
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-GDA0003852513260000156
其中，τ为当前时间。
根据本发明提供的一种基于多项式优化的惯性基导航信息融合***，包括：
模块M1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
具体地，在所述模块M1中：
对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000157
Figure RE-GDA0003852513260000158
其中，di为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000159
表示四元数切比雪夫系数组成的矩阵；
Figure RE-GDA0003852513260000161
其中Fi与
Figure RE-GDA0003852513260000162
表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为：
Figure RE-GDA0003852513260000163
Figure RE-GDA0003852513260000164
Figure RE-GDA0003852513260000165
其中，pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000166
表示速度切比雪夫系数组成的矩阵，
Figure RE-GDA0003852513260000167
表示区间初始时刻的先验位置，
Figure RE-GDA0003852513260000168
其中，
Figure RE-GDA0003852513260000169
为i阶切比雪夫多项式的积分：
Figure RE-GDA00038525132600001610
模块M2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
具体地，在所述模块M2中：
导航状态及传感器误差模型参数通过求解如下非线性优化问题获得：
Figure RE-GDA00038525132600001611
满足约束
Figure RE-GDA00038525132600001612
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置 pe(τ0)；初始时刻状态的先验值
Figure RE-GDA00038525132600001613
其中
Figure RE-GDA00038525132600001614
分别表示
Figure RE-GDA00038525132600001615
ve、pe的初始时刻先验；
Figure RE-GDA00038525132600001616
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-GDA00038525132600001617
Ma,0、Mg,0、Ω0分别为
Figure RE-GDA00038525132600001618
Ma， Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-GDA00038525132600001619
其中，δψ0代表初始时刻的姿态误差，定义为
Figure RE-GDA0003852513260000171
运算符[·]2∶4表示取矩阵的第2至4行元素；与初始状态对应的初始方差表示为PX0，δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定；目标函数中
Figure RE-GDA0003852513260000172
Jv，Jz分别定义如下：
Figure RE-GDA0003852513260000173
Figure RE-GDA0003852513260000174
Figure RE-GDA0003852513260000175
其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数；
Figure RE-GDA0003852513260000176
eg,ea，es，ec定义为：
Figure RE-GDA0003852513260000177
Figure RE-GDA0003852513260000178
Figure RE-GDA0003852513260000179
Figure RE-GDA00038525132600001710
Figure RE-GDA00038525132600001711
权重矩阵
Figure RE-GDA00038525132600001712
Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，
Figure RE-GDA00038525132600001713
上述非线性优化采用小时间窗口迭代完成，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。
模块M3：求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航；
具体地，在所述模块M3中：
递推求解状态估计的方差，具体的方差预测计算方法如下：
Figure RE-GDA00038525132600001714
其中方差的预测步长定义为
Figure RE-GDA00038525132600001715
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵；
tk时刻有观测、约束，状态的方差采用下式更新：
Figure RE-GDA0003852513260000181
其中，Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵；计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的；当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复模块M1至模块M4。
具体地，对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态关系如下：
Figure RE-GDA0003852513260000182
Figure RE-GDA0003852513260000183
其中，上标、下标b表示载体坐标系b系，e表示地球坐标系e系，i表示惯性坐标系i系，g表示陀螺坐标系；
Figure RE-GDA0003852513260000184
表示b系相对于e系的姿态四元数，
Figure RE-GDA0003852513260000185
表示b系相对于 e系的姿态旋转矩阵，
Figure RE-GDA0003852513260000186
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度地速度，γe表示e系下的地球重力向量；
Figure RE-GDA0003852513260000189
表示四元数乘法，*表示四元数的共轭；yg表示陀螺仪测量的角速度,ya表示加速度计测量的比力；bg表示陀螺仪的零偏， ba表示加速度计的零偏；ng～N(0,Rg)表示陀螺仪的高斯白噪声，na～N(0,Ra)表示加速度计的高斯白噪声，Rg表示陀螺仪的高斯白噪声协方差矩阵，Ra表示加速度计的高斯白噪声协方差矩阵；In表示n维单位矩阵；
Figure RE-GDA0003852513260000187
表示b系到g系的姿态旋转矩阵；Mg与 Ma是陀螺与加速度计的误差矩阵，包含比力因子和交轴耦合系数，表示为：
Figure RE-GDA0003852513260000188
其中，sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy， mgxz，mgyz表示陀螺仪的交轴耦合系数；
将其他辅助传感器包括卫星导航接收机、磁力仪、里程计、雷达、摄像机的观测模型统一表示为：
ys(t)＝h(x,Θ,t)+ns
其中，t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-GDA0003852513260000191
其中，
Figure RE-GDA0003852513260000192
表示A集合是B集合的子集，pe表示e系下载体的位置；Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集；
Figure RE-GDA0003852513260000193
运动约束模型统一表示为：
Figure RE-GDA0003852513260000194
其中，yc(t)表示为t时刻的约束伪观测，函数
Figure RE-GDA0003852513260000195
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为高斯白噪声的协方差矩阵；
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关，此外，观测模型与运动约束模型中的x能够包括多个时刻下的状态；
在当前采样时间窗口
Figure RE-GDA0003852513260000196
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-GDA0003852513260000197
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-GDA0003852513260000198
其中，τ为当前时间。
实施例2：
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
针对现有技术中的缺陷，本发明的目的是提供一种基于切比雪夫多项式优化的惯性基信息融合方法。所述方法包括：基于多项式优化的惯性基导航信息融合方法。
步骤S1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
步骤S2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
步骤S3：利用EKF方差预测与更新策略，求解区间内估计状态的方差
步骤S4：将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行相应的决策与控制，并最终应用于：无人***(无人机、无人车、机器人等)的自主定位与导航，航天器姿态估计与控制，人体运动估计与导航等场景
步骤S5：将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复步骤 S1-S4。
对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态有如下关系
Figure RE-GDA0003852513260000201
Figure RE-GDA0003852513260000202
其中上标/下标b表示载体坐标系(b系)，e表示地球坐标系(e系)，i表示惯性坐标系(i系)，g表示陀螺坐标系。
Figure RE-GDA0003852513260000203
表示b系相对于e系的姿态四元数，
Figure RE-GDA0003852513260000204
表示b系相对于e系的姿态旋转矩阵，
Figure RE-GDA0003852513260000205
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度(又称为地速度)，γe表示e系下的地球重力向量；
Figure RE-GDA0003852513260000208
表示四元数乘法， *表示四元数的共轭；yg,ya分别表示陀螺仪测量的角速度与加速度计测量的比力；bg与ba分别表示陀螺仪与加速度计的零偏；ng～N(0,Rg)，na～N(0,Ra)分别表示陀螺仪和加速度计的高斯白噪声，Rg与Ra为相应的白噪声协方差矩阵；In表示n维单位矩阵；上标，
Figure RE-GDA0003852513260000206
表示b系到g系的姿态旋转矩阵；Mg与Ma是陀螺与加速度计的误差矩阵(包含比力因子和交轴耦合系数)，表示为：
Figure RE-GDA0003852513260000207
其中sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy，mgxz， mgyz表示陀螺仪的交轴耦合系数。
将卫星导航接收机、磁力仪、里程计、雷达、摄像机等其他辅助传感器的观测模型统一表示为
ys(t)＝h(x,Θ,t)+ns
其中t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-GDA0003852513260000211
其中
Figure RE-GDA0003852513260000212
表示A集合是B集合的子集，pe表示e系下载体的位置。Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集
Figure RE-GDA0003852513260000213
可能存在的运动约束模型(如零速度、零位置、零角速度、地面车辆非完整运动约束、旋翼无人机动力学约束等)统一表示为：
Figure RE-GDA0003852513260000214
其中yc(t)表示为t时刻的约束伪观测，函数
Figure RE-GDA0003852513260000215
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为其协方差矩阵。
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关。例如，对于已标定比力因子、交轴耦合的惯性器件与GNSS信息融合问题，GNSS位置观测模型表示为
Figure RE-GDA0003852513260000216
其中
Figure RE-GDA0003852513260000217
Θ＝[ba T bg T lbT]T，lb表示GNSS接收机与惯性器件之间的杆臂；对于已标定比力因子、交轴耦合的惯性器件与零速约束融合问题，yc(t)＝0，
Figure RE-GDA0003852513260000218
Θ＝[ba T bg T]T。此外，观测模型与运动约束模型中的x可能包括多个时刻下的状态，如视觉、雷达的约束模型与多个时刻下的***状态都有关。
假设在当前采样时间窗口
Figure RE-GDA0003852513260000219
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-GDA00038525132600002110
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-GDA00038525132600002111
其中，τ为当前时间；
为了后续计算的需要，本发明首先对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数。
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为
Figure RE-GDA0003852513260000221
Figure RE-GDA0003852513260000222
其中di为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000223
表示四元数切比雪夫系数组成的矩阵；
Figure RE-GDA0003852513260000224
其中Fi与
Figure RE-GDA0003852513260000225
表示第i阶切比雪夫多项式及其导数，Fi定义如下
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
类似的，在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为
Figure RE-GDA0003852513260000226
Figure RE-GDA0003852513260000227
Figure RE-GDA0003852513260000228
其中pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-GDA0003852513260000229
表示速度切比雪夫系数组成的矩阵，
Figure RE-GDA00038525132600002210
表示区间初始时刻的先验位置，
Figure RE-GDA00038525132600002211
其中
Figure RE-GDA00038525132600002212
为i阶切比雪夫多项式的积分
Figure RE-GDA00038525132600002213
根据以上模型，导航状态及传感器误差模型参数可以通过求解如下非线性优化问题获得
Figure RE-GDA0003852513260000231
满足约束
Figure RE-GDA0003852513260000232
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置 pe(τ0)；初始时刻状态的先验值
Figure RE-GDA0003852513260000233
其中
Figure RE-GDA0003852513260000234
分别表示
Figure RE-GDA0003852513260000235
ve、pe的初始时刻先验；
Figure RE-GDA0003852513260000236
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-GDA0003852513260000237
Ma,0、Mg,0、Ω0分别为
Figure RE-GDA0003852513260000238
Ma， Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-GDA0003852513260000239
其中δψ0代表初始时刻的姿态误差，定义为
Figure RE-GDA00038525132600002310
运算符[·]2∶4表示取矩阵的第2～4行元素。与初始状态对应的初始方差表示为
Figure RE-GDA00038525132600002311
δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定。目标函数中
Figure RE-GDA00038525132600002312
Jv，Jz分别定义如下
Figure RE-GDA00038525132600002313
Figure RE-GDA00038525132600002314
Figure RE-GDA00038525132600002315
上式中的近似项是由于使用了Clenshaw-Curtis数值积分近似，其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数。
Figure RE-GDA00038525132600002316
Figure RE-GDA00038525132600002317
eg,ea，es，ec定义为
Figure RE-GDA00038525132600002318
Figure RE-GDA00038525132600002319
Figure RE-GDA00038525132600002320
Figure RE-GDA00038525132600002321
Figure RE-GDA00038525132600002322
权重矩阵
Figure RE-GDA00038525132600002323
Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
Figure RE-GDA00038525132600002324
需要指出的是，上述优化区间内的位置使用了初始先验位置加上速度切比雪夫多项式的积分表示，因此非线性优化估计状态中包括了初始先验位置；也可以将位置直接表示为切比雪夫多项式的形式，速度以及速度的导数分别使用多项式的一阶、二阶导数表示，此时非线性优化状态将不再包括初始先验位置。
为了满足实时计算要求，上述非线性优化可以采用小时间窗口迭代完成。具体来说，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。同时，还需要求解下一窗口状态的初始方差。本发明使用类似扩展Kalman滤波的策略递推求解状态估计的方差，具体的方差预测计算方法如下
Figure RE-GDA0003852513260000241
其中方差的预测步长定义为
Figure RE-GDA0003852513260000242
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵
假设tk时刻有观测/约束，状态的方差采用下式更新
Figure RE-GDA0003852513260000243
Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵。注意计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的。当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的***、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的***、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的***、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种基于多项式优化的惯性基导航信息融合方法，其特征在于，包括：
步骤S1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
步骤S2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
步骤S3：求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航；
将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复步骤S1至步骤S4。
2.根据权利要求1所述的基于多项式优化的惯性基导航信息融合方法，其特征在于：
对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态关系如下：
Figure RE-FDA0003852513250000011
Figure RE-FDA0003852513250000012
其中，上标、下标b表示载体坐标系b系，e表示地球坐标系e系，i表示惯性坐标系i系，g表示陀螺坐标系；
Figure RE-FDA0003852513250000013
表示b系相对于e系的姿态四元数，
Figure RE-FDA0003852513250000014
表示b系相对于e系的姿态旋转矩阵，
Figure RE-FDA0003852513250000015
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度，也称作地速度，γe表示e系下的地球重力向量；
Figure RE-FDA0003852513250000016
表示四元数乘法，*表示四元数的共轭；yg表示陀螺仪测量的角速度,ya表示加速度计测量的比力；bg表示陀螺仪的零偏，ba表示加速度计的零偏；ng～N(0,Rg)表示陀螺仪的高斯白噪声，na～N(0,Ra)表示加速度计的高斯白噪声，Rg表示陀螺仪的高斯白噪声协方差矩阵，Ra表示加速度计的高斯白噪声协方差矩阵；In表示n维单位矩阵；
Figure RE-FDA0003852513250000017
表示b系到g系的姿态旋转矩阵；Mg与Ma是陀螺与加速度计的误差矩阵，包含比力因子和交轴耦合系数，表示为：
Figure RE-FDA0003852513250000018
其中，sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy，mgxz，mgyz表示陀螺仪的交轴耦合系数；
将其他辅助传感器包括卫星导航接收机、磁力仪、里程计、雷达、摄像机的观测模型统一表示为：
ys(t)＝h(x,Θ,t)+ns
其中，t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-FDA0003852513250000021
其中，
Figure RE-FDA0003852513250000022
表示A集合是B集合的子集，pe表示e系下载体的位置；Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集；
Figure RE-FDA0003852513250000023
运动约束模型统一表示为：
Figure RE-FDA0003852513250000024
其中，yc(t)表示为t时刻的约束伪观测，函数
Figure RE-FDA0003852513250000025
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为高斯白噪声的协方差矩阵；
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关，此外，观测模型与运动约束模型中的x能够包括多个时刻下的状态；
在当前采样时间窗口
Figure RE-FDA0003852513250000026
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-FDA0003852513250000027
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-FDA0003852513250000028
其中，τ为当前时间。
3.根据权利要求1所述的基于多项式优化的惯性基导航信息融合方法，其特征在于，在所述步骤S1中：
对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
Figure RE-FDA0003852513250000031
Figure RE-FDA0003852513250000032
其中，di为第i阶切比雪夫多项式对应的系数，
Figure RE-FDA0003852513250000033
表示四元数切比雪夫系数组成的矩阵；
Figure RE-FDA0003852513250000034
其中Fi与
Figure RE-FDA0003852513250000035
表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为：
Figure RE-FDA0003852513250000036
Figure RE-FDA0003852513250000037
Figure RE-FDA0003852513250000038
其中，pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-FDA0003852513250000039
表示速度切比雪夫系数组成的矩阵，
Figure RE-FDA00038525132500000310
表示区间初始时刻的先验位置，
Figure RE-FDA00038525132500000311
其中，
Figure RE-FDA00038525132500000312
为i阶切比雪夫多项式的积分：
Figure RE-FDA00038525132500000313
4.根据权利要求1所述的基于多项式优化的惯性基导航信息融合方法，其特征在于，在所述步骤S2中：
导航状态及传感器误差模型参数通过求解如下非线性优化问题获得：
Figure RE-FDA00038525132500000314
满足约束
Figure RE-FDA00038525132500000315
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置pe(τ0)；初始时刻状态的先验值
Figure RE-FDA00038525132500000316
其中
Figure RE-FDA00038525132500000317
Figure RE-FDA00038525132500000318
分别表示
Figure RE-FDA0003852513250000041
ve、pe的初始时刻先验；
Figure RE-FDA0003852513250000042
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-FDA0003852513250000043
Ma,0、Mg,0、Ω0分别为
Figure RE-FDA0003852513250000044
Ma，Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-FDA0003852513250000045
其中，δψ0代表初始时刻的姿态误差，定义为
Figure RE-FDA0003852513250000046
运算符[·]2：4表示取矩阵的第2至4行元素；与初始状态对应的初始方差表示为
Figure RE-FDA0003852513250000047
δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定；目标函数中
Figure RE-FDA0003852513250000048
Jv，Jz分别定义如下：
Figure RE-FDA0003852513250000049
Figure RE-FDA00038525132500000410
Figure RE-FDA00038525132500000411
其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数；
Figure RE-FDA00038525132500000412
eg,ea，es，ec定义为：
Figure RE-FDA00038525132500000413
Figure RE-FDA00038525132500000414
Figure RE-FDA00038525132500000415
Figure RE-FDA00038525132500000416
Figure RE-FDA00038525132500000417
权重矩阵
Figure RE-FDA00038525132500000418
Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，
Figure RE-FDA00038525132500000419
上述非线性优化采用小时间窗口迭代完成，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。
5.根据权利要求1所述的基于多项式优化的惯性基导航信息融合方法，其特征在于，在所述步骤S3中：
递推求解状态估计的方差，具体的方差预测计算方法如下：
Figure RE-FDA00038525132500000420
其中方差的预测步长定义为
Figure RE-FDA0003852513250000051
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵；
tk时刻有观测、约束，状态的方差采用下式更新：
Figure RE-FDA0003852513250000052
其中，Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵；计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的；当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
6.一种基于多项式优化的惯性基导航信息融合***，其特征在于，包括：
模块M1：从当前惯导***中，获得陀螺仪和加速度计的测量数据，并对其进行插值，获得切比雪夫点处的角速度、比力；
模块M2：利用切比雪夫点处的角速度、比力，及其他辅助传感器的观测，求解关于切比雪夫系数及其他参数的非线性优化，获得导航状态估计结果；
模块M3：求解区间内估计状态的方差，将解算得到的导航状态估计结果与方差，传递给导航制导与控制模块，进行自主定位与导航；
将区间末的导航状态估计结果与方差当作下一区间的初始先验，重复模块M1至模块M4。
7.根据权利要求6所述的基于多项式优化的惯性基导航信息融合***，其特征在于：
对于未标定的惯性传感器，陀螺仪与加速度计的测量与导航状态关系如下：
Figure RE-FDA0003852513250000053
Figure RE-FDA0003852513250000054
其中，上标、下标b表示载体坐标系b系，e表示地球坐标系e系，i表示惯性坐标系i系，g表示陀螺坐标系；
Figure RE-FDA0003852513250000055
表示b系相对于e系的姿态四元数，
Figure RE-FDA0003852513250000056
表示b系相对于e系的姿态旋转矩阵，
Figure RE-FDA0003852513250000057
表示e系下的地球自转角速度，ve表示e系下载体相对于地球的速度地速度，γe表示e系下的地球重力向量；
Figure RE-FDA0003852513250000058
表示四元数乘法，*表示四元数的共轭；yg表示陀螺仪测量的角速度,ya表示加速度计测量的比力；bg表示陀螺仪的零偏，ba表示加速度计的零偏；ng～N(0,Rg)表示陀螺仪的高斯白噪声，na～N(0,Ra)表示加速度计的高斯白噪声，Rg表示陀螺仪的高斯白噪声协方差矩阵，Ra表示加速度计的高斯白噪声协方差矩阵；In表示n维单位矩阵；
Figure RE-FDA0003852513250000061
表示b系到g系的姿态旋转矩阵；Mg与Ma是陀螺与加速度计的误差矩阵，包含比力因子和交轴耦合系数，表示为：
Figure RE-FDA0003852513250000062
其中，sax，say，saz分别表示加速度计三轴的比力因子系数，maxy，maxz，mayz表示加速度计的交轴耦合系数；sgx，sgy，sgz分别表示陀螺仪三轴的比力因子系数，mgxy，mgxz，mgyz表示陀螺仪的交轴耦合系数；
将其他辅助传感器包括卫星导航接收机、磁力仪、里程计、雷达、摄像机的观测模型统一表示为：
ys(t)＝h(x,Θ,t)+ns
其中，t表示观测时间，ys(t)表示传感器在t时刻的观测；ns～N(0,Rs)表示高斯白噪声，Rs为其协方差矩阵；函数h(·)表示传感器观测模型；x为待估计的导航状态；
Figure RE-FDA0003852513250000063
其中，
Figure RE-FDA0003852513250000064
表示A集合是B集合的子集，pe表示e系下载体的位置；Θ表示惯性传感器模型参数以及其他辅助传感器相关未知参数Ω的子集；
Figure RE-FDA0003852513250000065
运动约束模型统一表示为：
Figure RE-FDA0003852513250000066
其中，yc(t)表示为t时刻的约束伪观测，函数
Figure RE-FDA0003852513250000067
表示约束模型，nc～N(0,Rc)表示高斯白噪声，Rc为高斯白噪声的协方差矩阵；
x与Θ的具体定义与使用的辅助传感器以及指定需要估计的导航状态有关，此外，观测模型与运动约束模型中的x能够包括多个时刻下的状态；
在当前采样时间窗口
Figure RE-FDA0003852513250000068
内，有陀螺测量的角速度yg(tk)或者角增量观测Δθ(tk)，加速度计测量的比力ya(tk)或者速度增量Δv(tk)，其中k＝0,1,…Nm，t0为采样时间窗口的开始时间，
Figure RE-FDA0003852513250000069
为采样时间窗口的结束时间；
使用仿射变换，将时间窗口映射到τ∈[-1 1]
Figure RE-FDA0003852513250000071
其中，τ为当前时间。
8.根据权利要求6所述的基于多项式优化的惯性基导航信息融合***，其特征在于，在所述模块M1中：
对陀螺和加速度计观测插值，以获得切比雪夫点处的角速度与比力，分别表示为yg(τj)，ya(τj)，其中切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
Figure RE-FDA0003852513250000072
Figure RE-FDA0003852513250000073
其中，di为第i阶切比雪夫多项式对应的系数，
Figure RE-FDA0003852513250000074
表示四元数切比雪夫系数组成的矩阵；
Figure RE-FDA0003852513250000075
其中Fi与
Figure RE-FDA0003852513250000076
表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
在一个解算时间窗口内，速度、速度导数以及位置使用切比雪夫多项式表示为：
Figure RE-FDA0003852513250000077
Figure RE-FDA0003852513250000078
Figure RE-FDA0003852513250000079
其中，pe为e系下载体的位置，ki为第i阶切比雪夫多项式对应的系数，
Figure RE-FDA00038525132500000710
表示速度切比雪夫系数组成的矩阵，
Figure RE-FDA00038525132500000711
表示区间初始时刻的先验位置，
Figure RE-FDA00038525132500000712
其中，
Figure RE-FDA00038525132500000713
为i阶切比雪夫多项式的积分：
Figure RE-FDA00038525132500000714
9.根据权利要求6所述的基于多项式优化的惯性基导航信息融合***，其特征在于，在所述模块M2中：
导航状态及传感器误差模型参数通过求解如下非线性优化问题获得：
Figure RE-FDA0003852513250000081
满足约束
Figure RE-FDA0003852513250000082
其中，优化参数包括切比雪夫系数矩阵D，K，传感器模型参数Θ以及初始位置pe(τ0)；初始时刻状态的先验值
Figure RE-FDA0003852513250000083
其中
Figure RE-FDA0003852513250000084
Figure RE-FDA0003852513250000085
分别表示
Figure RE-FDA0003852513250000086
ve、pe的初始时刻先验；
Figure RE-FDA0003852513250000087
ba,0、bg,0分别表示加速度计和陀螺仪零偏的初始时刻先验，
Figure RE-FDA0003852513250000088
Ma,0、Mg,0、Ω0分别为
Figure RE-FDA0003852513250000089
Ma，Mg和Ω的初始时刻先验；状态的初始误差δX0表示为：
Figure RE-FDA00038525132500000810
其中，δψ0代表初始时刻的姿态误差，定义为
Figure RE-FDA00038525132500000811
运算符[·]2：4表示取矩阵的第2至4行元素；与初始状态对应的初始方差表示为
Figure RE-FDA00038525132500000812
δX0包含的误差项由具体定义的导航状态x以及其他待估计参数Θ确定；目标函数中
Figure RE-FDA00038525132500000813
Jv，Jz分别定义如下：
Figure RE-FDA00038525132500000814
Figure RE-FDA00038525132500000815
Figure RE-FDA00038525132500000816
其中wi通过拉格朗日多项式积分获得，p和r分别表示优化区间内辅助传感器观测以及约束的个数；
Figure RE-FDA00038525132500000817
eg,ea，es，ec定义为：
Figure RE-FDA00038525132500000818
Figure RE-FDA00038525132500000819
Figure RE-FDA00038525132500000820
Figure RE-FDA00038525132500000821
Figure RE-FDA00038525132500000822
权重矩阵
Figure RE-FDA00038525132500000823
Wa，Wg，Ws，Wc分别表示初始先验、加速度计、陀螺仪、辅助传感器、约束观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，
Figure RE-FDA00038525132500000824
上述非线性优化采用小时间窗口迭代完成，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。
10.根据权利要求6所述的基于多项式优化的惯性基导航信息融合***，其特征在于，在所述模块M3中：
递推求解状态估计的方差，具体的方差预测计算方法如下：
Figure RE-FDA0003852513250000091
其中方差的预测步长定义为
Figure RE-FDA0003852513250000092
Pk与Pk-1分别是tk和tk-1时刻的状态方差，Lk、Gk、Qk分别表示tk时刻的Kalman滤波增益矩阵、噪声驱动矩阵以及过程噪声协方差矩阵；
tk时刻有观测、约束，状态的方差采用下式更新：
Figure RE-FDA0003852513250000093
其中，Hk代表线性化之后的观测/约束模型矩阵，Rk表示对应的观测/约束噪声协方差矩阵；计算矩阵Gk,Hk，Lk所需要的状态是上述非线性优化获得的；当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值作为下一个区间的先验初值与方差。
CN202210898677.6A 2022-07-28 2022-07-28 基于多项式优化的惯性基导航信息融合方法及*** Pending CN115200578A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210898677.6A CN115200578A (zh) 2022-07-28 2022-07-28 基于多项式优化的惯性基导航信息融合方法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210898677.6A CN115200578A (zh) 2022-07-28 2022-07-28 基于多项式优化的惯性基导航信息融合方法及***

Publications (1)

Publication Number Publication Date
CN115200578A true CN115200578A (zh) 2022-10-18

Family

ID=83584849

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210898677.6A Pending CN115200578A (zh) 2022-07-28 2022-07-28 基于多项式优化的惯性基导航信息融合方法及***

Country Status (1)

Country Link
CN (1) CN115200578A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115824224A (zh) * 2023-02-14 2023-03-21 河海大学 基于ahrs和dvl的水下机器人自主航位推算方法
CN116105731A (zh) * 2023-04-07 2023-05-12 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115824224A (zh) * 2023-02-14 2023-03-21 河海大学 基于ahrs和dvl的水下机器人自主航位推算方法
CN116105731A (zh) * 2023-04-07 2023-05-12 中国人民解放军国防科技大学 稀疏测距条件下的导航方法、装置、计算机设备及介质

Similar Documents

Publication Publication Date Title
CN110501024B (zh) 一种车载ins/激光雷达组合导航***的量测误差补偿方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN108592945B (zh) 一种惯性/天文组合***误差的在线标定方法
CN100516775C (zh) 一种捷联惯性导航***初始姿态确定方法
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及***
CN105203098A (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航***及方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111121766B (zh) 一种基于星光矢量的天文与惯性组合导航方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN111238535A (zh) 一种基于因子图的imu误差在线标定方法
CN113291493B (zh) 一种卫星多敏感器融合姿态确定方法和***
CN111189442A (zh) 基于cepf的无人机多源导航信息状态预测方法
CN111380516A (zh) 基于里程计测量信息的惯导/里程计车辆组合导航方法及***
CN106441291A (zh) 一种基于强跟踪sdre滤波的组合导航***及导航方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN111190207B (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN114935345A (zh) 一种基于模式识别的车载惯导安装角误差补偿方法
CN113375664B (zh) 基于点云地图动态加载的自主移动装置定位方法
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN112284379B (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