CN114396936B - 基于多项式优化的惯性与磁传感器姿态估计方法及系统 - Google Patents
基于多项式优化的惯性与磁传感器姿态估计方法及系统 Download PDFInfo
- Publication number
- CN114396936B CN114396936B CN202210032044.7A CN202210032044A CN114396936B CN 114396936 B CN114396936 B CN 114396936B CN 202210032044 A CN202210032044 A CN 202210032044A CN 114396936 B CN114396936 B CN 114396936B
- Authority
- CN
- China
- Prior art keywords
- accelerometer
- representing
- quaternion
- chebyshev
- attitude
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 35
- 238000005457 optimization Methods 0.000 title claims abstract description 28
- 239000011159 matrix material Substances 0.000 claims description 57
- 238000001514 detection method Methods 0.000 claims description 42
- 238000004422 calculation algorithm Methods 0.000 claims description 28
- 238000005259 measurement Methods 0.000 claims description 19
- PXFBZOLANLWPMH-UHFFFAOYSA-N 16-Epiaffinine Natural products C1C(C2=CC=CC=C2N2)=C2C(=O)CC2C(=CC)CN(C)C1C2CO PXFBZOLANLWPMH-UHFFFAOYSA-N 0.000 claims description 18
- 230000009466 transformation Effects 0.000 claims description 18
- 230000005484 gravity Effects 0.000 claims description 12
- 238000005070 sampling Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000021615 conjugation Effects 0.000 claims description 8
- 241000897276 Termes Species 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 6
- 238000013507 mapping Methods 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 101150089047 cutA gene Proteins 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 239000003550 marker Substances 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- 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
- 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/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Optimization (AREA)
- Pure & Applied Mathematics (AREA)
- Automation & Control Theory (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Databases & Information Systems (AREA)
- Algebra (AREA)
- Operations Research (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明提供了一种基于多项式优化的惯性与磁传感器姿态估计方法及系统，所述方法包括如下步骤：步骤S1：基于多项式优化的惯性与磁传感器姿态进行四元数估计；步骤S2：基于多项式优化的惯性与磁传感器姿态进行罗德里格斯向量估计；步骤S3：根据步骤S1和步骤S2的估计结果估计载体姿态。本发明精度高、鲁棒性强，尤其是在初始姿态完全未知的情况下，仍能正确估计姿态。
Description
技术领域
本发明涉及传感器的技术领域，具体地，涉及基于多项式优化的惯性与磁传感器姿态估计方法及系统。
背景技术
使用惯性与磁传感器估计载体姿态，在航空航天、无人系统以及人体姿态测量等领域有广泛的应用。惯性与磁传感器通常集成了三轴陀螺、三轴加速度计与三陀螺仪。一方面，通过积分陀螺仪输出的角速度可以获得载体的姿态，另一方面加速度计与磁力仪分别测量载体坐标系下的比力与地球磁场强度，从而与载体姿态建立关联。由于传感器的误差特性不能完全通过离线标定消除，且加速度计与磁力仪极其容易受到外部干扰。因此，需要将三种传感器融合使用，以期得到更好的姿态估计结果。
在公开号为CN104764451A的专利文献中公开了一种基于惯性和地磁传感器的目标姿态跟踪方法，无需任何参考标志物和特定跟踪环境即可实现姿态跟踪，方法简单易行；采用陀螺仪、加速度计以及地磁传感器分别采集目标当前姿态所对应的角速度、加速度和磁强度数据在三个敏感轴上的分量，利用加速度计和磁传感器校正陀螺仪的姿态跟踪结果，消除了漂移误差，从而提高跟踪结果的精度。
针对上述中的相关技术，发明人认为现有技术在初始姿态完全未知的情况下，不能正确估计姿态，因此，需要提出一种技术方案以改善上述技术问题。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种基于多项式优化的惯性与磁传感器姿态估计方法及系统。
根据本发明提供的一种基于多项式优化的惯性与磁传感器姿态估计方法，所述方法包括如下步骤：
步骤S1：基于多项式优化的惯性与磁传感器姿态进行四元数估计；
步骤S2：基于多项式优化的惯性与磁传感器姿态进行罗德里格斯向量估计；
步骤S3：根据步骤S1和步骤S2的估计结果估计载体姿态。
优选地，所述步骤S1中根据惯性与磁传感器测量方程进行估计：
其中，上标/下标b表示载体坐标系b系；e表示地球坐标系e系；i表示惯性坐标系i系；n表示导航坐标系n系；导航坐标系为北-天-东，分别表示b系相对于n系的姿态四元数及其导数；yg,ya,ym分别表示陀螺仪测量的角速度、加速度计测量的比力以及磁力仪测量的磁强度，gn，mn分别表示地球自转当地重力与磁场在n系下的表示，bg与ba分别表示陀螺仪与加速度计的零偏，ng～N(0,Rg)，na～N(0,Ra)，nm～N(0,Rm)分别表示陀螺仪、加速度计与磁力仪的高斯白噪声，表示四元数的乘法，上标*表示四元数的共轭，表示四元数的共轭；
加速度计干扰与磁干扰检测算法成立时，对应的传感器观测为有效观测：
||ya|-g|＜εa
|1-|ym||＜εm
其中，εa与εm分别表示加速度计干扰检测与磁干扰检测的阈值；|·|表示向量求模运算，g表示当地重力大小；
采样时间窗口为使用仿射变换，将时间窗口映射到τ∈[-1 1]：
其中，t0与分别表示区间的开始与结束时间，t表示仿射变换之前的真实采样时间，τ为仿射变换之后的时间。
优选地，所述步骤S1对陀螺观测插值，获得切比雪夫点处的角速度，记作切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，其中N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
其中，Nq为切比雪夫多项式的阶数；di为第i阶切比雪夫多项式对应的系数；表示切比雪夫系数组成的矩阵；
其中，Fi与表示第i阶切比雪夫多项式及其导数；
Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
姿态四元数以及陀螺仪与加速度计的零偏通过求解如下非线性优化问题获得：
满足约束
其中，下标表示初始时刻的状态量其中，分别表示初始时刻已知的姿态四元数、加速度计与陀螺零偏；与初始状态对应的初始方差表示为其中，分别表示初始时刻姿态四元数、加速度计与陀螺零偏的方差，diag(·)表示把矩阵块对角化；
Jv，Jz分别定义如下：
其中，wi通过对拉格朗日多项式积分获得，p和s分别表示经过加速度计干扰与磁干扰检测算检测之后的加速度计与磁力仪观测个数；
加权残差项ev,ea，em定义为：
上式中δψ0代表初始时刻的姿态误差，定义为其中，运算符[·]2:4表示取矩阵的第2～4行元素；权重矩阵Wg，Wa与Wm分别表示初始先验、陀螺仪、加速度计与磁力仪观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
优选地，所述步骤S1中算法的初始化假设初始姿态以及陀螺加表零偏完全已知，并通过求解如下齐次最小二乘获得：
满足约束|Bvec(D)|＝1
其中，vec(·)表示列向量化矩阵，上式中的矩阵定义为 表示kronecker积，In表示n维单位矩阵， 其中Aa元素里面的时刻t表示区间内通过加速度计干扰检测算法的所有加速度计测量时刻，Am元素里面的时刻表t表示区间通过磁检测算法的所有磁力仪测量时刻；
矩阵aa(t)，am(t)，ag(t)定义如下：
其中对于任意的四元数四元数操作符号定义为：
其中，(·)×表示三维向量对应的反对称矩阵。
优选地，所述步骤S2中将区间内的罗德里格斯向量增量及其导数使用切比雪夫多项式表示如下：
其中，Ng为切比雪夫多项式的阶数，hi为第i阶切比雪夫多项式对应的系数，表示切比雪夫系数组成的矩阵；
区间内的姿态四元数表示为：
其中，为区间初始时刻的姿态四元数；
将上述使用罗德里格斯向量表示的姿态四元数，代入到上述求解四元数切比雪夫系数的非线性最小二乘中，转化为如下所示的无约束最小二乘问题：
在优化得到区间内的姿态之后，使用上述方差预测与更新方法获得对应的方差，然后作为下一区间解算的初始方差，循环解算。
本发明还提供一种基于多项式优化的惯性与磁传感器姿态估计系统，所述系统包括如下模块：
模块M1：基于多项式优化的惯性与磁传感器姿态进行四元数估计；
模块M2：基于多项式优化的惯性与磁传感器姿态进行罗德里格斯向量估计；
模块M3：根据模块M1和模块M2的估计结果估计载体姿态。
优选地，所述模块M1中根据惯性与磁传感器测量方程进行估计：
其中，上标/下标b表示载体坐标系b系；e表示地球坐标系e系；i表示惯性坐标系i系；n表示导航坐标系n系；导航坐标系为北-天-东，分别表示b系相对于n系的姿态四元数及其导数；yg,ya,ym分别表示陀螺仪测量的角速度、加速度计测量的比力以及磁力仪测量的磁强度，gn，mn分别表示地球自转当地重力与磁场在n系下的表示，bg与ba分别表示陀螺仪与加速度计的零偏，ng～N(0,Rg)，na～N(0,Ra)，nm～N(0,Rm)分别表示陀螺仪、加速度计与磁力仪的高斯白噪声，表示四元数的乘法，上标*表示四元数的共轭，表示四元数的共轭；
加速度计干扰与磁干扰检测算法成立时，对应的传感器观测为有效观测：
||ya|-g|＜εa
|1-|ym||＜εm
其中，εa与εm分别表示加速度计干扰检测与磁干扰检测的阈值；|·|表示向量求模运算，g表示当地重力大小；
采样时间窗口为使用仿射变换，将时间窗口映射到τ∈[-1 1]：
其中，t0与分别表示区间的开始与结束时间，t表示仿射变换之前的真实采样时间，τ为仿射变换之后的时间。
优选地，所述模块M1对陀螺观测插值，获得切比雪夫点处的角速度，记作切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，其中N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
其中，Nq为切比雪夫多项式的阶数；di为第i阶切比雪夫多项式对应的系数；表示切比雪夫系数组成的矩阵；
其中，Fi与表示第i阶切比雪夫多项式及其导数；
Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
姿态四元数以及陀螺仪与加速度计的零偏通过求解如下非线性优化问题获得：
满足约束
其中，下标表示初始时刻的状态量其中，分别表示初始时刻已知的姿态四元数、加速度计与陀螺零偏；与初始状态对应的初始方差表示为其中，分别表示初始时刻姿态四元数、加速度计与陀螺零偏的方差，diag(·)表示把矩阵块对角化；
Jv，Jz分别定义如下：
其中，wi通过对拉格朗日多项式积分获得，p和s分别表示经过加速度计干扰与磁干扰检测算检测之后的加速度计与磁力仪观测个数；
加权残差项ev,ea，em定义为：
上式中δψ0代表初始时刻的姿态误差，定义为其中，运算符[·]2:4表示取矩阵的第2～4行元素；权重矩阵Wg，Wa与Wm分别表示初始先验、陀螺仪、加速度计与磁力仪观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
优选地，所述模块M1中算法的初始化假设初始姿态以及陀螺加表零偏完全已知，并通过求解如下齐次最小二乘获得：
满足约束|Bvec(D)|＝1
其中，vec(·)表示列向量化矩阵，上式中的矩阵定义为 表示kronecker积，In表示n维单位矩阵， 其中Aa元素里面的时刻t表示区间内通过加速度计干扰检测算法的所有加速度计测量时刻，Am元素里面的时刻表t表示区间通过磁检测算法的所有磁力仪测量时刻；
矩阵aa(t)，am(t)，ag(t)定义如下：
其中对于任意的四元数四元数操作符号定义为：
其中，(·)×表示三维向量对应的反对称矩阵。
优选地，所述模块M2中将区间内的罗德里格斯向量增量及其导数使用切比雪夫多项式表示如下：
其中，Ng为切比雪夫多项式的阶数，hi为第i阶切比雪夫多项式对应的系数，表示切比雪夫系数组成的矩阵；
区间内的姿态四元数表示为：
其中，为区间初始时刻的姿态四元数；
将上述使用罗德里格斯向量表示的姿态四元数，代入到上述求解四元数切比雪夫系数的非线性最小二乘中，转化为如下所示的无约束最小二乘问题：
在优化得到区间内的姿态之后，使用上述方差预测与更新系统获得对应的方差，然后作为下一区间解算的初始方差，循环解算。
与现有技术相比，本发明具有如下的有益效果：
本发明是极大验后估计下近似最优的状态估计方法，相比与传统的扩展Kalman滤波算法，没有线性化误差。估计精度高、鲁棒性强，尤其是在初始姿态完全未知的情况下，仍能正确估计姿态。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为本发明的流程原理图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
针对现有技术中的缺陷，本发明的目的是提供一种基于切比雪夫多项式优化的惯性与磁传感器姿态估计方法，包括基于多项式优化的惯性与磁传感器姿态四元数估计方法和基于多项式优化的惯性与磁传感器姿态罗德里格斯向量估计方法。
其中，惯性与磁传感器测量方程表示为：
其中，上标/下标b表示载体坐标系b系；
e表示地球坐标系e系；
i表示惯性坐标系i系；
n表示导航坐标系n系；
不失一般性，本发明定义导航坐标系为北-天-东，分别表示b系相对于n系的姿态四元数及其导数；yg,ya,ym分别表示陀螺仪测量的角速度、加速度计测量的比力以及磁力仪测量的磁强度，gn，mn分别表示地球自转，当地重力与磁场在n系下的表示，bg与ba分别表示陀螺仪与加速度计的零偏，ng～N(0,Rg)，na～N(0,Ra)，nm～N(0,Rm)分别表示陀螺仪、加速度计与磁力仪的高斯白噪声，表示四元数的乘法，上标*表示四元数的共轭，表示四元数的共轭。
上述加速度计与磁力仪的观测分别仅在干扰加速度与磁干扰小的时候使用。具体来说只有下述加速度计干扰与磁干扰检测算法成立的时候，对应的传感器观测才视为可用的有效观测：
||ya|-g|＜εa
|1-|ym||＜εm
其中，εa与εm分别表示加速度计干扰检测与磁干扰检测的阈值；
|·|表示向量求模运算，g表示当地重力大小。
假设当前采样时间窗口使用仿射变换，将时间窗口映射到τ∈[-1 1]：
其中，t0与分别表示区间的开始与结束时间，t表示仿射变换之前的真实采样时间，τ为仿射变换之后的时间。
接下来，本发明首先对陀螺观测插值，以获得切比雪夫点处的角速度，记作切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，其中N是切比雪夫多项式的阶数。
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
其中，Nq为切比雪夫多项式的阶数；
di为第i阶切比雪夫多项式对应的系数；
表示切比雪夫系数组成的矩阵。
其中Fi与表示第i阶切比雪夫多项式及其导数，Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
根据以上模型，姿态四元数以及陀螺仪与加速度计的零偏，可以通过求解如下非线性优化问题获得：
满足约束
其中，下标表示初始时刻的状态量其中，分别表示初始时刻已知的姿态四元数、加速度计与陀螺零偏。
与初始状态对应的初始方差表示为其中，分别表示初始时刻姿态四元数、加速度计与陀螺零偏的方差，diag(·)表示把矩阵块对角化。Jv，Jz分别定义如下：
上式中的近似项是由于使用了Clenshaw-Curtis数值积分近似，其中，wi通过对拉格朗日多项式积分获得，p和s分别表示经过加速度计干扰与磁干扰检测算检测之后，可用的加速度计与磁力仪观测个数。加权残差项ev,ea，em定义为：
上式中δψ0代表初始时刻的姿态误差，定义为其中，运算符[·]2:4表示取矩阵的第2～4行元素。权重矩阵Wg，Wa与Wm分别表示初始先验、陀螺仪、加速度计与磁力仪观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
上述非线性最小二乘问题需要良好的切比雪夫系数初值，以保证算法收敛。算法的初始化可以假设初始姿态以及陀螺加表零偏完全已知，并通过求解如下齐次最小二乘获得：
满足约束|Bvec(D)|＝1
其中vec(·)表示列向量化矩阵，上式中的矩阵定义为 表示kronecker积，In表示n维单位矩阵， 其中Aa元素里面的时刻t表示区间内通过加速度计干扰检测算法的所有加速度计测量时刻，Am元素里面的时刻表t表示区间通过磁检测算法的所有磁力仪测量时刻。矩阵aa(t)，am(t)，ag(t)定义如下：
其中对于任意的四元数四元数操作符号定义为：
其中，(·)×表示三维向量对应的反对称矩阵。
为了满足实时计算要求，上述非线性优化可以采用小时间窗口迭代完成。具体来说，在完成当前窗口状态估计之后，当前窗口结束时刻的状态被当作下一窗口计算开始时刻的先验状态。同时，还需要求解下一窗口状态的初始方差。本发明使用类似扩展Kalman滤波的策略递推求解姿态估计的方差。方差预测计算方法如下：
方差预测
其中，方差的预测步长定义为Pk与Pk-1分别是tk和tk-1时刻的状态方差，上式中矩阵的定义如下：
其中，Rba，Rbg分别表示陀螺零偏与加表零偏的随机游走噪声方差。
假设tk时刻有加速度计或者磁力仪的观测，状态的方差采用下式更新：
方差更新
其中，Rk代表加速度计与磁力仪的观测噪声方差，Hk代表线性化之后的观测噪声，对于加速度计观测表示为：
对于磁力仪观测表示为：
注意：矩阵Gk,Hk，Lk中的姿态由是上述非线性最小二乘获得的。当获得当前区间内的状态以及方差之后，当前区间结束时刻的状态和方差被当作初值做为下一个区间的先验初值与方差。
基于多项式优化的惯性与磁传感器姿态罗德里格斯向量估计方法，上述基于四元数的姿态求解算法，需要求解有约束的非线性最小二乘问题。本发明继续提出了一种基于多项式优化的惯性与磁传感器姿态罗德里格斯向量估计方法。具体而言，将区间内的罗德里格斯向量增量及其导数使用切比雪夫多项式表示如下：
其中，Ng为切比雪夫多项式的阶数，hi为第i阶切比雪夫多项式对应的系数，表示切比雪夫系数组成的矩阵。基于上述模型，区间内的姿态四元数可以表示为：
其中，为区间初始时刻的姿态四元数；
将上述使用罗德里格斯向量表示的姿态四元数，代入到上述求解四元数切比雪夫系数的非线性最小二乘中，转化为如下所示的无约束最小二乘问题：
在优化得到区间内的姿态之后，使用上述方差预测与更新方法获得对应的方差，然后作为下一区间解算的初始方差，循环解算。
本发明还提供一种基于多项式优化的惯性与磁传感器姿态估计系统，所述系统包括如下模块：
模块M1：基于多项式优化的惯性与磁传感器姿态进行四元数估计；根据惯性与磁传感器测量方程进行估计：
其中，上标/下标b表示载体坐标系b系；e表示地球坐标系e系；i表示惯性坐标系i系；n表示导航坐标系n系；导航坐标系为北-天-东，分别表示b系相对于n系的姿态四元数及其导数；yg,ya,ym分别表示陀螺仪测量的角速度、加速度计测量的比力以及磁力仪测量的磁强度，gn，mn分别表示地球自转当地重力与磁场在n系下的表示，bg与ba分别表示陀螺仪与加速度计的零偏，ng～N(0,Rg)，na～N(0,Ra)，nm～N(0,Rm)分别表示陀螺仪、加速度计与磁力仪的高斯白噪声，表示四元数的乘法，上标*表示四元数的共轭，表示四元数的共轭；
加速度计干扰与磁干扰检测算法成立时，对应的传感器观测为有效观测：
||ya|-g|＜εa
|1-|ym||＜εm
其中，εa与εm分别表示加速度计干扰检测与磁干扰检测的阈值；|·|表示向量求模运算，g表示当地重力大小；
采样时间窗口为使用仿射变换，将时间窗口映射到τ∈[-1 1]：
其中，t0与分别表示区间的开始与结束时间，t表示仿射变换之前的真实采样时间，τ为仿射变换之后的时间。
模块M1对陀螺观测插值，获得切比雪夫点处的角速度，记作切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，其中N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
其中，Nq为切比雪夫多项式的阶数；di为第i阶切比雪夫多项式对应的系数；表示切比雪夫系数组成的矩阵；
其中，Fi与表示第i阶切比雪夫多项式及其导数；
Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
姿态四元数以及陀螺仪与加速度计的零偏通过求解如下非线性优化问题获得：
满足约束
其中，下标表示初始时刻的状态量其中，分别表示初始时刻已知的姿态四元数、加速度计与陀螺零偏；与初始状态对应的初始方差表示为其中，分别表示初始时刻姿态四元数、加速度计与陀螺零偏的方差，diag(·)表示把矩阵块对角化；
Jv，Jz分别定义如下：
其中，wi通过对拉格朗日多项式积分获得，p和s分别表示经过加速度计干扰与磁干扰检测算检测之后的加速度计与磁力仪观测个数；
加权残差项ev,ea，em定义为：
上式中δψ0代表初始时刻的姿态误差，定义为其中，运算符[·]2:4表示取矩阵的第2～4行元素；权重矩阵Wg，Wa与Wm分别表示初始先验、陀螺仪、加速度计与磁力仪观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
模块M1中算法的初始化假设初始姿态以及陀螺加表零偏完全已知，并通过求解如下齐次最小二乘获得：
满足约束|Bvec(D)|＝1
其中，vec(·)表示列向量化矩阵，上式中的矩阵定义为 表示kronecker积，In表示n维单位矩阵， 其中Aa元素里面的时刻t表示区间内通过加速度计干扰检测算法的所有加速度计测量时刻，Am元素里面的时刻表t表示区间通过磁检测算法的所有磁力仪测量时刻；
矩阵aa(t)，am(t)，ag(t)定义如下：
其中对于任意的四元数四元数操作符号定义为：
其中，(·)×表示三维向量对应的反对称矩阵。
模块M2：基于多项式优化的惯性与磁传感器姿态进行罗德里格斯向量估计；优选地，将区间内的罗德里格斯向量增量及其导数使用切比雪夫多项式表示如下：
其中，Ng为切比雪夫多项式的阶数，hi为第i阶切比雪夫多项式对应的系数，表示切比雪夫系数组成的矩阵；
区间内的姿态四元数表示为：
其中，为区间初始时刻的姿态四元数；
将上述使用罗德里格斯向量表示的姿态四元数，代入到上述求解四元数切比雪夫系数的非线性最小二乘中，转化为如下所示的无约束最小二乘问题：
在优化得到区间内的姿态之后，使用上述方差预测与更新系统
模块M3：根据模块M1和模块M2的估计结果估计载体姿态。
获得对应的方差，然后作为下一区间解算的初始方差，循环解算。
本发明提出的基于切比雪夫多项式优化的惯性与磁传感器姿态估计方法的优点是：精度高、鲁棒性强，尤其是在初始姿态完全未知的情况下，仍能正确估计姿态。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
Claims (8)
1.一种基于多项式优化的惯性与磁传感器姿态估计方法，其特征在于，所述方法包括如下步骤：
步骤S1：基于多项式优化的惯性与磁传感器姿态进行四元数估计；
步骤S2：基于多项式优化的惯性与磁传感器姿态进行罗德里格斯向量估计；
步骤S3：根据步骤S1和步骤S2的估计结果估计载体姿态；
所述步骤S2中将区间内的罗德里格斯向量增量及其导数使用切比雪夫多项式表示如下：
其中，Ng为切比雪夫多项式的阶数，hi为第i阶切比雪夫多项式对应的系数，表示切比雪夫系数组成的矩阵；
区间内的姿态四元数表示为：
其中，为区间初始时刻的姿态四元数；
将上述使用罗德里格斯向量表示的姿态四元数，代入到求解四元数切比雪夫系数的非线性最小二乘中，转化为如下所示的无约束最小二乘问题：
在优化得到区间内的姿态之后，使用方差预测与更新方法获得对应的方差，然后作为下一区间解算的初始方差，循环解算。
2.根据权利要求1所述的基于多项式优化的惯性与磁传感器姿态估计方法，其特征在于，所述步骤S1中根据惯性与磁传感器测量方程进行估计：
其中，上标/下标b表示载体坐标系b系；e表示地球坐标系e系；i表示惯性坐标系i系；n表示导航坐标系n系；导航坐标系为北-天-东，分别表示b系相对于n系的姿态四元数及其导数；yg,ya,ym分别表示陀螺仪测量的角速度、加速度计测量的比力以及磁力仪测量的磁强度，gn，mn分别表示地球自转当地重力与磁场在n系下的表示，bg与ba分别表示陀螺仪与加速度计的零偏，ng～N(0,Rg)，na～N(0,Ra)，nm～N(0,Rm)分别表示陀螺仪、加速度计与磁力仪的高斯白噪声，表示四元数的乘法，上标*表示四元数的共轭，表示四元数的共轭；
加速度计干扰与磁干扰检测算法成立时，对应的传感器观测为有效观测：
||ya|-g|<εa
|1-|ym||<εm
其中，εa与εm分别表示加速度计干扰检测与磁干扰检测的阈值；|·|表示向量求模运算，g表示当地重力大小；
采样时间窗口为使用仿射变换，将时间窗口映射到τ∈[-1 1]：
其中，t0与分别表示区间的开始与结束时间，t表示仿射变换之前的真实采样时间，τ为仿射变换之后的时间。
3.根据权利要求1所述的基于多项式优化的惯性与磁传感器姿态估计方法，其特征在于，所述步骤S1对陀螺观测插值，获得切比雪夫点处的角速度，记作切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，其中N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
其中，Nq为切比雪夫多项式的阶数；di为第i阶切比雪夫多项式对应的系数；表示切比雪夫系数组成的矩阵；
其中，Fi与表示第i阶切比雪夫多项式及其导数；
Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
姿态四元数以及陀螺仪与加速度计的零偏通过求解如下非线性优化问题获得：
满足约束
其中，下标表示初始时刻的状态量其中，分别表示初始时刻已知的姿态四元数、加速度计与陀螺零偏；与初始状态对应的初始方差表示为其中，分别表示初始时刻姿态四元数、加速度计与陀螺零偏的方差，diag(·)表示把矩阵块对角化；
Jv，Jz分别定义如下：
其中，wi通过对拉格朗日多项式积分获得，p和s分别表示经过加速度计干扰与磁干扰检测算检测之后的加速度计与磁力仪观测个数；
加权残差项ev,ea，em定义为：
上式中δψ0代表初始时刻的姿态误差，定义为其中，运算符[·]2:4表示取矩阵的第2～4行元素；权重矩阵Wg，Wa与Wm分别表示初始先验、陀螺仪、加速度计与磁力仪观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
4.根据权利要求1所述的基于多项式优化的惯性与磁传感器姿态估计方法，其特征在于，所述步骤S1中算法的初始化假设初始姿态以及陀螺加表零偏完全已知，并通过求解如下齐次最小二乘获得：
满足约束|Bvec(D)|＝1
其中，vec(·)表示列向量化矩阵，上式中的矩阵定义为 表示kronecker积，In表示n维单位矩阵， 其中Aa元素里面的时刻t表示区间内通过加速度计干扰检测算法的所有加速度计测量时刻，Am元素里面的时刻表t表示区间通过磁检测算法的所有磁力仪测量时刻；
矩阵aa(t)，am(t)，ag(t)定义如下：
其中对于任意的四元数四元数操作符号定义为：
其中，(·)×表示三维向量对应的反对称矩阵。
5.一种基于多项式优化的惯性与磁传感器姿态估计系统，其特征在于，所述系统包括如下模块：
模块M1：基于多项式优化的惯性与磁传感器姿态进行四元数估计；
模块M2：基于多项式优化的惯性与磁传感器姿态进行罗德里格斯向量估计；
模块M3：根据模块M1和模块M2的估计结果估计载体姿态；
所述模块M2中将区间内的罗德里格斯向量增量及其导数使用切比雪夫多项式表示如下：
其中，Ng为切比雪夫多项式的阶数，hi为第i阶切比雪夫多项式对应的系数，表示切比雪夫系数组成的矩阵；
区间内的姿态四元数表示为：
其中，为区间初始时刻的姿态四元数；
将上述使用罗德里格斯向量表示的姿态四元数，代入到求解四元数切比雪夫系数的非线性最小二乘中，转化为如下所示的无约束最小二乘问题：
在优化得到区间内的姿态之后，使用方差预测与更新系统获得对应的方差，然后作为下一区间解算的初始方差，循环解算。
6.根据权利要求5所述的基于多项式优化的惯性与磁传感器姿态估计系统，其特征在于，所述模块M1中根据惯性与磁传感器测量方程进行估计：
其中，上标/下标b表示载体坐标系b系；e表示地球坐标系e系；i表示惯性坐标系i系；n表示导航坐标系n系；导航坐标系为北-天-东，分别表示b系相对于n系的姿态四元数及其导数；yg,ya,ym分别表示陀螺仪测量的角速度、加速度计测量的比力以及磁力仪测量的磁强度，gn，mn分别表示地球自转当地重力与磁场在n系下的表示，bg与ba分别表示陀螺仪与加速度计的零偏，ng～N(0,Rg)，na～N(0,Ra)，nm～N(0,Rm)分别表示陀螺仪、加速度计与磁力仪的高斯白噪声，表示四元数的乘法，上标*表示四元数的共轭，表示四元数的共轭；
加速度计干扰与磁干扰检测算法成立时，对应的传感器观测为有效观测：
||ya|-g|＜εa
|1-|ym||<εm
其中，εa与εm分别表示加速度计干扰检测与磁干扰检测的阈值；|·|表示向量求模运算，g表示当地重力大小；
采样时间窗口为使用仿射变换，将时间窗口映射到τ∈[-1 1]：
其中，t0与分别表示区间的开始与结束时间，t表示仿射变换之前的真实采样时间，τ为仿射变换之后的时间。
7.根据权利要求5所述的基于多项式优化的惯性与磁传感器姿态估计系统，其特征在于，所述模块M1对陀螺观测插值，获得切比雪夫点处的角速度，记作切比雪夫点定义为τj＝-cos(jπ/N),j＝0,1,…,N，其中N是切比雪夫多项式的阶数；
在一个解算时间窗口内，姿态四元数及其导数使用切比雪夫多项式表示为：
其中，Nq为切比雪夫多项式的阶数；di为第i阶切比雪夫多项式对应的系数；表示切比雪夫系数组成的矩阵；
其中，Fi与表示第i阶切比雪夫多项式及其导数；
Fi定义如下：
F0(τ)＝1,F1(τ)＝τ,
Fi+1(τ)＝2τFi(τ)-Fi-1(τ)for i≥1
姿态四元数以及陀螺仪与加速度计的零偏通过求解如下非线性优化问题获得：
满足约束
其中，下标表示初始时刻的状态量其中，分别表示初始时刻已知的姿态四元数、加速度计与陀螺零偏；与初始状态对应的初始方差表示为其中，分别表示初始时刻姿态四元数、加速度计与陀螺零偏的方差，diag(·)表示把矩阵块对角化；
Jv，Jz分别定义如下：
其中，wi通过对拉格朗日多项式积分获得，p和s分别表示经过加速度计干扰与磁干扰检测算检测之后的加速度计与磁力仪观测个数；
加权残差项ev,ea，em定义为：
上式中δψ0代表初始时刻的姿态误差，定义为其中，运算符[·]2:4表示取矩阵的第2～4行元素；权重矩阵Wg，Wa与Wm分别表示初始先验、陀螺仪、加速度计与磁力仪观测对应的权重矩阵，分别通过对相应方差的逆进行Cholesky分解获得，即
8.根据权利要求5所述的基于多项式优化的惯性与磁传感器姿态估计系统，其特征在于，所述模块M1中算法的初始化假设初始姿态以及陀螺加表零偏完全已知，并通过求解如下齐次最小二乘获得：
满足约束|Bvec(D)|＝1
其中，vec(·)表示列向量化矩阵，上式中的矩阵定义为 表示kronecker积，In表示n维单位矩阵， 其中Aa元素里面的时刻t表示区间内通过加速度计干扰检测算法的所有加速度计测量时刻，Am元素里面的时刻表t表示区间通过磁检测算法的所有磁力仪测量时刻；
矩阵aa(t)，am(t)，ag(t)定义如下：
其中对于任意的四元数四元数操作符号定义为：
其中，(·)×表示三维向量对应的反对称矩阵。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210032044.7A CN114396936B (zh) | 2022-01-12 | 2022-01-12 | 基于多项式优化的惯性与磁传感器姿态估计方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210032044.7A CN114396936B (zh) | 2022-01-12 | 2022-01-12 | 基于多项式优化的惯性与磁传感器姿态估计方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114396936A CN114396936A (zh) | 2022-04-26 |
| CN114396936B true CN114396936B (zh) | 2024-03-12 |
Family
ID=81230704
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210032044.7A Active CN114396936B (zh) | 2022-01-12 | 2022-01-12 | 基于多项式优化的惯性与磁传感器姿态估计方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114396936B (zh) |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107339987A (zh) * | 2017-04-21 | 2017-11-10 | 上海交通大学 | 一种基于函数迭代积分的刚体姿态解算方法 |
| CN108534774A (zh) * | 2018-03-21 | 2018-09-14 | 上海交通大学 | 基于函数迭代积分的刚体姿态解算方法及系统 |
| CN108827299A (zh) * | 2018-03-29 | 2018-11-16 | 南京航空航天大学 | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 |
| CN109724597A (zh) * | 2018-12-19 | 2019-05-07 | 上海交通大学 | 一种基于函数迭代积分的惯性导航解算方法及系统 |
| CN112066993A (zh) * | 2020-09-10 | 2020-12-11 | 南京航空航天大学 | 一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法 |
| CN112683269A (zh) * | 2020-12-07 | 2021-04-20 | 电子科技大学 | 一种附有运动加速度补偿的marg姿态计算方法 |
| CN113158459A (zh) * | 2021-04-20 | 2021-07-23 | 浙江工业大学 | 一种基于视觉和惯性信息融合的人体姿态估计方法 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10578457B2 (en) * | 2016-09-15 | 2020-03-03 | Syracuse University | Robust and stable autonomous vision-inertial navigation system for unmanned vehicles |
-
2022
- 2022-01-12 CN CN202210032044.7A patent/CN114396936B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107339987A (zh) * | 2017-04-21 | 2017-11-10 | 上海交通大学 | 一种基于函数迭代积分的刚体姿态解算方法 |
| CN108534774A (zh) * | 2018-03-21 | 2018-09-14 | 上海交通大学 | 基于函数迭代积分的刚体姿态解算方法及系统 |
| CN108827299A (zh) * | 2018-03-29 | 2018-11-16 | 南京航空航天大学 | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 |
| CN109724597A (zh) * | 2018-12-19 | 2019-05-07 | 上海交通大学 | 一种基于函数迭代积分的惯性导航解算方法及系统 |
| CN112066993A (zh) * | 2020-09-10 | 2020-12-11 | 南京航空航天大学 | 一种基于误差四元数三维矢量分布的高斯粒子滤波数据处理方法 |
| CN112683269A (zh) * | 2020-12-07 | 2021-04-20 | 电子科技大学 | 一种附有运动加速度补偿的marg姿态计算方法 |
| CN113158459A (zh) * | 2021-04-20 | 2021-07-23 | 浙江工业大学 | 一种基于视觉和惯性信息融合的人体姿态估计方法 |
Non-Patent Citations (1)
| Title |
|---|
| 基于MARG的两种实时姿态测量算法的分析与比较;李磊;鲍其莲;;电子测量技术(02);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114396936A (zh) | 2022-04-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109991636B (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| Ludwig et al. | Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data | |
| CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| Wu et al. | Generalized linear quaternion complementary filter for attitude estimation from multisensor observations: An optimization approach | |
| Huang et al. | Online initialization and automatic camera-IMU extrinsic calibration for monocular visual-inertial SLAM | |
| Valenti et al. | A linear Kalman filter for MARG orientation estimation using the algebraic quaternion algorithm | |
| Brossard et al. | Unscented Kalman filter on Lie groups for visual inertial odometry | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN108592945B (zh) | 一种惯性/天文组合系统误差的在线标定方法 | |
| CN112013836A (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 | |
| Eckenhoff et al. | High-accuracy preintegration for visual-inertial navigation | |
| CN108731676B (zh) | 一种基于惯性导航技术的姿态融合增强测量方法及系统 | |
| Wang et al. | Direction cosine matrix estimation with an inertial measurement unit | |
| CN105486312A (zh) | 一种星敏感器与高频角位移传感器组合定姿方法及系统 | |
| WO2022160391A1 (zh) | 磁力计信息辅助的mems陀螺仪标定方法及标定系统 | |
| CN109631894A (zh) | 一种基于滑动窗口的单目视觉惯性紧耦合方法 | |
| CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
| CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
| CN108871319B (zh) | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 | |
| CN115200578A (zh) | 基于多项式优化的惯性基导航信息融合方法及系统 | |
| CN113155152B (zh) | 基于李群滤波的相机与惯性传感器空间关系自标定方法 | |
| CN114764830A (zh) | 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法 | |
| CN112284379A (zh) | 一种基于非线性积分补偿的组合运动测量系统的惯性预积分方法 | |
| CN114396936B (zh) | 基于多项式优化的惯性与磁传感器姿态估计方法及系统 | |
| Liu et al. | LGC-Net: A lightweight gyroscope calibration network for efficient attitude estimation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |