CN110095115B - 一种基于地磁信息更新的载体航姿测量方法 - Google Patents

一种基于地磁信息更新的载体航姿测量方法 Download PDF

Info

Publication number
CN110095115B
CN110095115B CN201910480822.7A CN201910480822A CN110095115B CN 110095115 B CN110095115 B CN 110095115B CN 201910480822 A CN201910480822 A CN 201910480822A CN 110095115 B CN110095115 B CN 110095115B
Authority
CN
China
Prior art keywords
time
measurement
information
state vector
measuring
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
Application number
CN201910480822.7A
Other languages
English (en)
Other versions
CN110095115A (zh
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.)
Hefei Institutes of Physical Science of CAS
Original Assignee
Hefei Institutes of Physical Science of CAS
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 Hefei Institutes of Physical Science of CAS filed Critical Hefei Institutes of Physical Science of CAS
Priority to CN201910480822.7A priority Critical patent/CN110095115B/zh
Publication of CN110095115A publication Critical patent/CN110095115A/zh
Application granted granted Critical
Publication of CN110095115B publication Critical patent/CN110095115B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/06—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving measuring of drift angle; involving correction for drift

Landscapes

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

Abstract

本发明涉及一种基于地磁信息更新的载体航姿测量方法，其包括以下步骤：初始化；磁偏角的计算；下一时刻的数据获取；下一时刻航姿信息的测量；磁偏角信息的更新；测量方法的持续执行。本发明基于加速度计、陀螺仪和磁传感器测量数据，利用离散化的卡尔曼滤波算法，实时估计和更新磁偏角信息，避免了磁性矿物、钢筋等铁磁性材料的存在导致磁偏角变化而引起的航向角信息扭曲和不可靠问题。

Description

一种基于地磁信息更新的载体航姿测量方法
技术领域
本发明涉及载体航姿测量技术领域，具体来说是一种基于地磁信息更新的载体航姿测量方法。
背景技术
航姿测量方法是指测量航向角ψ、俯仰角θ和翻滚角
Figure GDA0002100714430000011
的方法。根据航向角ψ测量原理不同，目前航姿测量方法主要有：天文测量方法、卫星导航测量方法、惯性测量方法和地磁测量方法。
天文测量***利用对天体的测量实现航姿信息输出，受大气影响大、环境适应性差。卫星导航测量方法基于卫星导航***定位技术，利用双天线测量航向和俯仰角、或三天线解算航姿，由于天线安装位置误差、高程差的存在等影响，会降低其精度，且一般精度随基线的加长而提高，不适于在小尺寸设备上使用；卫星数据输出频率一般不超过10Hz，响应速度较慢，无法给出高频或实时航姿信息；另外，卫星导航测量方法需要接受导航卫星信号的特点决定了其存在以下局限性：(1)信号弱，容易受到干扰和欺骗；(2)由于衰减严重难以到达室内、丛林以及水下和地面；(3)导航卫星存在被攻击的危险。惯性测量方法依赖高精度惯性器件，即通过高精度天陀螺仪感知地球自转方法，实现航向角观测，其体积大、成本高，通常需要大量***电路进行集成，以上这些缺点大大限制了其在更多领域的应用，特别是在对低成本小体积轻质量等要求较高的民用领域。
地磁测量方法是指充分利用地球上无处不在具有方向性的地磁场，通过测量磁北方向，在结合磁北与真北之间的夹角即磁偏角，获取航姿信息。与天文测量方法、惯性测量方法以及卫星导航测量方法相比，地磁测量方法具有环境适应性强、成本低、无源、自主等优势。基于地磁信息的航姿测量方法目前主要有地磁匹配、地磁滤波以及电子罗盘三类方式。地磁匹配和地磁滤波这两种方式都依赖预先存储的高精度地磁图。然而，地磁场包含复杂多变，难以预测异常场，高精度地磁图往往难以获取；尤其是近地表区域，异常场的影响更觉明显，导致地磁匹配和地磁滤波的航姿测量方式的使用受到限制。
电子罗盘这种测量航姿的方式不需要高精度地磁图，其往往利用磁偏角在一定区域方位内变化不大、近似保持不变的特性，获取航向角ψ信息；同时，采用加速度计感知重力场信息实现俯仰角θ和翻滚角φ测量，实现航向角ψ测量过程的姿态补偿。对于载体运动过程中，载体加速度对重力场信息检测造成干扰，通常引入微机电(MEMS)陀螺仪，并采用滤波算法，以实现动态过程中的航姿测量。然而，外界磁环境的变化如磁性矿物、含钢筋等铁磁性的建筑的存在，都会导致磁偏角发生变化，会扭曲电子罗盘方式获取的航向角信息。
那么如何基于地磁信息的实时更新，实现对磁偏角的实时校准，准确进行航姿测量方法进行成为急需解决的技术问题。
发明内容
本发明的目的是为了解决现有技术中外界磁环境对磁偏角缺陷，提供一种基于地磁信息更新的载体航姿测量方法来解决上述问题。
为了实现上述目的，本发明的技术方案如下：
一种基于地磁信息更新的载体航姿测量方法，包括以下步骤：
初始化：获取当前时刻t＝k的俯仰角θk、翻滚角
Figure GDA0002100714430000021
和航向角ψk信息；
磁偏角的计算：利用当前时刻t＝k的磁传感器量测数据，解算量测磁航向角αm,k；根据当前时刻的航向角ψk和量测磁航向角αm,k，计算磁偏角Dk，其计算公式如下：
Dk＝ψk-αm,k；
下一时刻的数据获取：获取下一时刻t＝k+1的加速度计、陀螺仪和磁传感器的量测数据；
下一时刻航姿信息的测量：利用离散化卡尔曼滤波方法解算下一时刻t＝k+1的俯仰角θk+1、翻滚角
Figure GDA0002100714430000022
和航向角ψk+1以及量测磁航向角αm,k+1；
磁偏角信息的更新：根据下一时刻t＝k+1的航向角ψk+1和量测磁航向角αm,k+1，更新下一时刻t＝k+1时的磁偏角Dk+1；
测量方法的持续执行：返回下一时刻的数据获取步骤，获取时刻t＝k+2的数据信息，并进行时刻t＝k+2时磁偏角的更新。
所述下一时刻航姿信息的测量步骤中，离散化卡尔曼滤波方法为离散化扩展卡尔曼滤波方法；所述下一时刻航姿信息的测量包括以下步骤：
建立离散化的状态方程和量测方程，其表达式如下：
Figure GDA0002100714430000031
Figure GDA0002100714430000032
Zk+1＝h(Xk+1,k+1)+vk+1＝Xk+1+vk+1， (3)
Figure GDA0002100714430000033
Figure GDA0002100714430000034
Figure GDA0002100714430000035
ψm,k+1＝αm,k+1+Dk， (7)
Figure GDA0002100714430000036
其中：状态向量
Figure GDA0002100714430000037
Figure GDA0002100714430000038
表示当前时刻t＝k的载体坐标系相对于当地水平坐标系的航向角ψk、俯仰角θk和翻滚角
Figure GDA0002100714430000039
k表示离散时间点；
函数f表征下一时刻k+1的状态向量xk+1与当前时刻k的状态向量xk之间的关系；
Figure GDA0002100714430000041
表示角度的时间变化率；dt为t＝k与t＝k+1之间的时间间隔；
Figure GDA0002100714430000042
和
Figure GDA0002100714430000043
分别为k时刻载体坐标系中标定后陀螺仪在x、y和z轴上方向上量测数据；
函数h表征下一时刻k+1的状态量xk+1与量测量zk+1两者的关系；
Figure GDA0002100714430000044
表示下一时刻k+1的载体坐标系相对于当地水平坐标系的航向角ψk+1、俯仰角θk+1和翻滚角
Figure GDA0002100714430000045
的量测量；
Figure GDA0002100714430000046
和
Figure GDA0002100714430000047
分别为下一时刻k+1载体坐标系中标定后加速度传感器在x，y和z轴上方向上量测数据；
Figure GDA0002100714430000048
和
Figure GDA0002100714430000049
分别为下一时刻k+1载体坐标系中标定后磁传感器在x，y和z轴上方向上量测数据；
Figure GDA00021007144300000410
和
Figure GDA00021007144300000411
分别为
Figure GDA00021007144300000412
和
Figure GDA00021007144300000413
转换到地理坐标系l三个轴xl、yl和zl上得到量测数据；wk和vk+1分别为***过程噪声和量测噪声，其统计特性如下：
Figure GDA00021007144300000414
其中，k、j表示离散时间点，Q为***过程噪声协方差矩阵，βkj为Kronecker符号，R为量测噪声协方差矩阵；
设定忽略***过程噪声wk和量测噪声vk+1，
由方程(1)和(2)获得状态向量和量测向量的先验估计
Figure GDA00021007144300000415
和
Figure GDA00021007144300000416
其表示如下：
Figure GDA00021007144300000417
Figure GDA00021007144300000418
其中，
Figure GDA00021007144300000419
为状态向量在时刻k的后验估计值；
由此得到近似呈线性的状态方程和量测方程为：
Figure GDA00021007144300000420
Figure GDA00021007144300000421
其中，Xk+1和Zk+1分别为状态向量和量测向量在下一时刻k+1的真实值；
B为函数f关于x偏导的Jacobian矩阵，
Figure GDA00021007144300000422
W为函数f关于w偏导的Jacobian矩阵，
Figure GDA0002100714430000051
H为函数h关于x偏导的Jacobian矩阵，
Figure GDA0002100714430000052
V为函数h关于v偏导的Jacobian矩阵，
Figure GDA0002100714430000053
构建一组完整的离散化扩展卡尔曼滤波方法迭代过程，其表示如下：
Figure GDA0002100714430000054
Figure GDA0002100714430000055
其中，
Figure GDA0002100714430000056
为k+1时刻状态向量的先验协方差矩阵，Pk为k时刻状态向量的协方差矩阵；
k+1时刻的量测新息
Figure GDA0002100714430000057
为：
Figure GDA0002100714430000058
结合方程(4)和(16)，可得到，
Figure GDA0002100714430000059
离散化扩展卡尔曼滤波方法测量更新方程如下：
令，
Figure GDA00021007144300000510
其中，(γ1,γ2,γ3)为传感器特性相关的参数，取正数；
Rk+1＝f(η1,η2,η3)R， (19)
其中，f(η1,η2,η3)为(η1,η2,η3)相关的函数。
Figure GDA0002100714430000061
Figure GDA0002100714430000062
Figure GDA0002100714430000063
其中，Kk为k时刻的滤波增益矩阵，Pk为k时刻的状态向量的协方差矩阵；
k+1时刻状态向量Xk+1近似如下：
Figure GDA0002100714430000064
所述下一时刻航姿信息的测量步骤中，离散化卡尔曼滤波方法为离散化无迹卡尔曼滤波方法；所述下一时刻航姿信息的测量包括以下步骤：
构建Sigma散点集：
Figure GDA0002100714430000065
其中，
Figure GDA0002100714430000066
为状态向量在时刻k的后验估计值；Pk为k时刻的状态向量的协方差矩阵；n为状态向量维数；λ＝α2(n+κ)-n为获取给定分布高阶矩信息的自由参数，使得n+κ＝3，10-4≤α≤1，代表矩阵(n+λ)Pk-1平方根的第i行或第i列；
设定离散化无迹卡尔曼滤波方法的时间更新方程如下：
Figure GDA0002100714430000067
Figure GDA0002100714430000068
Figure GDA0002100714430000069
其中，
Figure GDA0002100714430000071
为状态向量在时刻k+1的先验估计值；
Figure GDA0002100714430000072
为时刻k+1的状态向量的协方差矩阵的先验估计值；
Figure GDA0002100714430000073
和
Figure GDA0002100714430000074
分别UT变化计算状态向量和协方差矩阵的先验估计值所用的加权值；
Figure GDA0002100714430000075
其中，β包含状态分布的先验信息；
结合方程Rk+1＝f(η1,η2,η3)R，f(η1,η2,η3)为(η1,η2,η3)相关的函数，离散化无迹卡尔曼滤波方法的测量更新方程如下：
Figure GDA0002100714430000076
Figure GDA0002100714430000077
Figure GDA0002100714430000078
Figure GDA0002100714430000079
Figure GDA00021007144300000710
Figure GDA00021007144300000711
Figure GDA00021007144300000712
得到k+1时刻状态向量Xk+1的近似值，如下：
Figure GDA00021007144300000713
所述磁偏角信息的更新包括以下步骤：
根据时刻k+1的后验估计航向角
Figure GDA00021007144300000714
和量测磁航向角αm,k+1，解算时刻k+1的磁偏角计算
Figure GDA00021007144300000715
更新磁偏角信息，并存储时刻k+1的磁偏角Dk+1。
有益效果
本发明的一种基于地磁信息更新的载体航姿测量方法，与现有技术相比基于加速度计、陀螺仪和磁传感器测量数据，利用离散化的卡尔曼滤波算法，实时估计和更新磁偏角信息，避免了磁性矿物、钢筋等铁磁性材料的存在导致磁偏角变化而引起的航向角信息扭曲和不可靠问题。
本发明不依赖预存的高精地地磁图，而是基于地磁信息实时更新的航姿测量方法，实现对磁偏角的实时校准，从而补偿外界磁环境航姿信息的扭曲，达到提高航姿测量可靠性和精度的目的。
附图说明
图1为本发明的方法顺序图；
图2为载体坐标系b、局域水平坐标系l和地理坐标系g关系示意图。
具体实施方式
为使对本发明的结构特征及所达成的功效有更进一步的了解与认识，用以较佳的实施例及附图配合详细的说明，说明如下：
如图1所示，本发明所述的一种基于地磁信息更新的载体航姿测量方法，其包括以下步骤：
第一步，初始化：载体处于静止状态，基于外界输入，获取当前时刻t＝k的航向角ψk信息；基于校准后三轴加速度计量测数据，获取当前时刻t＝k的俯仰角θk翻滚角
Figure GDA0002100714430000081
信息，计算公式如下：
Figure GDA0002100714430000082
Figure GDA0002100714430000083
其中，θm,k和
Figure GDA0002100714430000084
分别为载体处于静止状态时，当前时刻t＝k的俯仰角和翻滚角的量测信息；
Figure GDA0002100714430000085
和
Figure GDA0002100714430000086
分别为当前时刻t＝k时校准后三轴加速度传感器量测数据投影到载体坐标系b三个轴xb、yb和zb上得到量测数据。
第二步，磁偏角的计算。
利用当前时刻t＝k的俯仰角θk翻滚角
Figure GDA0002100714430000091
信息，如图2所示，将当前时刻t＝k的载体敏感轴上磁传感器量测数据载体坐标系b转换到局域水平坐标系l，转换公式如下：
Figure GDA0002100714430000092
其中，
Figure GDA0002100714430000093
和
Figure GDA0002100714430000094
分别为当前时刻t＝k时校准后三轴磁传感器量测数据投影到载体坐标系b三个轴xb、yb和zb上得到量测数据；
Figure GDA0002100714430000095
和
Figure GDA0002100714430000096
分别为
Figure GDA0002100714430000097
和
Figure GDA0002100714430000098
转换到局域水平坐标系l三个轴xl、yl和zl上得到量测数据。
由此，可以计算当前时刻t＝k的量测磁航向角αm,k，计算公式如下：
Figure GDA0002100714430000099
根据当前时刻t＝k的航向角ψk和量测磁航向角αm,k信息，解算当前时刻的磁偏角：
Dk＝ψk-αm,k。
第三步，下一时刻的数据获取：获取下一时刻t＝k+1的加速度计、陀螺仪和磁传感器的量测数据。
第四步，下一时刻航姿信息的测量。
利用离散化卡尔曼滤波方法解算下一时刻t＝k+1的俯仰角θk+1、翻滚角
Figure GDA00021007144300000910
和航向角ψk+1以及量测磁航向角αm,k+1；
建立离散化的卡尔曼滤波的状态方程和量测方程，其表达式如下：
Figure GDA0002100714430000101
Figure GDA0002100714430000102
Zk+1＝h(Xk+1,k+1)+vk+1＝Xk+1+vk+1， (3)
Figure GDA0002100714430000103
Ψm,k+1＝αm,k+1+Dk，
其中：状态向量
Figure GDA0002100714430000104
表示当前时刻t＝k的载体坐标系相对于当地水平坐标系的航向角ψk、俯仰角θk和翻滚角
Figure GDA0002100714430000105
k表示离散时间点；
函数f表征下一时刻k+1的状态向量xk+1与当前时刻k的状态向量xk之间的关系；
Figure GDA0002100714430000106
表示角度的时间变化率；dt为t＝k与t＝k+1之间的时间间隔；
Figure GDA0002100714430000107
和
Figure GDA0002100714430000108
分别为k时刻载体坐标系中标定后陀螺仪在x，y和z轴上方向上量测数据；
函数h表征下一时刻k+1的状态量xk+1与量测量zk+1两者的关系；
Figure GDA0002100714430000109
表示下一时刻k+1的载体坐标系相对于当地水平坐标系的航向角ψk+1、俯仰角θk+1和翻滚角
Figure GDA00021007144300001010
的量测量；wk和vk+1分别为***过程噪声和量测噪声，其统计特性如下：
Figure GDA00021007144300001011
其中，k、j表示离散时间点，Q为***过程噪声协方差矩阵，δkj为Kronecker符号，R为量测噪声协方差矩阵。
作为本发明的第一种实施方式，当离散化卡尔曼滤波方法采用扩展卡尔曼滤波时，
设定忽略***过程噪声wk和量测噪声vk+1，
由离散化的卡尔曼滤波的状态方程和量测方程获得状态向量和量测向量的先验估计
Figure GDA0002100714430000111
和
Figure GDA0002100714430000112
其表示如下：
Figure GDA0002100714430000113
Figure GDA0002100714430000114
其中，
Figure GDA0002100714430000115
为状态向量在时刻k的后验估计值；
由此得到近似呈线性的状态方程和量测方程为：
Figure GDA0002100714430000116
Figure GDA0002100714430000117
其中，Xk+1和Zk+1分别为状态向量和量测向量在下一时刻k+1的真实值；
B为函数f关于x偏导的Jacobian矩阵，
Figure GDA0002100714430000118
W为函数f关于w偏导的Jacobian矩阵，
Figure GDA0002100714430000119
H为函数h关于x偏导的Jacobian矩阵，
Figure GDA00021007144300001110
V为函数h关于v偏导的Jacobian矩阵，
Figure GDA00021007144300001111
构建一组完整的离散化扩展卡尔曼滤波方法迭代过程，其表示如下：
Figure GDA00021007144300001112
Figure GDA00021007144300001113
其中，
Figure GDA00021007144300001114
为k+1时刻状态向量的先验协方差矩阵，Pk为k时刻状态向量的协方差矩阵；
k+1时刻的量测新息
Figure GDA00021007144300001115
为：
Figure GDA0002100714430000121
离散化扩展卡尔曼滤波方法测量更新方程如下：
令，
Figure GDA0002100714430000122
其中，(γ1,γ2,γ3)为传感器特性相关的参数，取正数；
Rk+1＝f(η1,η2,η3)R，
其中，f(η1,η2,η3)为(η1,η2,η3)相关的函数。
Figure GDA0002100714430000123
Figure GDA0002100714430000124
Figure GDA0002100714430000125
其中，Kk为k时刻的滤波增益矩阵，Pk为k时刻的状态向量的协方差矩阵；
k+1时刻状态向量Xk+1近似如下：
Figure GDA0002100714430000126
作为本发明的第二种实施方式，当离散化卡尔曼滤波方法采用无迹卡尔曼滤波时：
构建Sigma散点集：
Figure GDA0002100714430000131
其中，
Figure GDA0002100714430000132
为状态向量在时刻k的后验估计值；Pk为k时刻的状态向量的协方差矩阵；n为状态向量维数；λ＝α2(n+κ)-n为获取给定分布高阶矩信息的自由参数，使得n+κ＝3，10-4≤α≤1，代表矩阵(n+λ)Pk-1平方根的第i行或第i列；
设定离散化无迹卡尔曼滤波方法的时间更新方程如下：
Figure GDA0002100714430000133
Figure GDA0002100714430000134
Figure GDA0002100714430000135
其中，
Figure GDA0002100714430000136
为状态向量在时刻k+1的先验估计值；
Figure GDA0002100714430000137
为时刻k+1的状态向量的协方差矩阵的先验估计值；
Figure GDA0002100714430000138
和
Figure GDA0002100714430000139
分别UT变化计算状态向量和协方差矩阵的先验估计值所用的加权值；
Figure GDA00021007144300001310
其中，β包含状态分布的先验信息；
结合方程Rk+1＝f(η1,η2,η3)R，f(η1,η2,η3)为(η1,η2,η3)相关的函数，离散化无迹卡尔曼滤波方法的测量更新方程如下：
Figure GDA00021007144300001311
Figure GDA00021007144300001312
Figure GDA00021007144300001313
Figure GDA00021007144300001314
Figure GDA0002100714430000141
Figure GDA0002100714430000142
Figure GDA0002100714430000143
得到k+1时刻状态向量Xk+1的近似值，如下：
Figure GDA0002100714430000144
第五步，磁偏角信息的更新。
根据下一时刻t＝k+1的航向角ψk+1和量测磁航向角αm,k+1，更新下一时刻t＝k+1时的磁偏角Dk+1；
Dk+1＝ψk+1-αm,k+1。
第六步，测量方法的持续执行：返回下一时刻的数据获取步骤，获取时刻t＝k+2的数据信息，并进行时刻t＝k+2时磁偏角的更新。
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。

Claims (4)

1.一种基于地磁信息更新的载体航姿测量方法，其特征在于，包括以下步骤：
11)初始化：获取当前时刻t＝k的俯仰角θk、翻滚角
Figure FDA0002751201320000011
和航向角ψk信息；
12)磁偏角的计算：利用当前时刻t＝k的磁传感器量测数据，解算量测磁航向角αm.k；根据当前时刻的航向角ψk和量测磁航向角αm,k，计算磁偏角Dk，其计算公式如下：
Dk＝ψk-αm,k；
13)下一时刻的数据获取：获取下一时刻t＝k+1的加速度计、陀螺仪和磁传感器的量测数据；
14)下一时刻航姿信息的测量：利用离散化卡尔曼滤波方法解算下一时刻t＝k+1的俯仰角θk+1、翻滚角
Figure FDA0002751201320000012
和航向角ψk+1以及量测磁航向角αm,k+1；
15)磁偏角信息的更新：根据下一时刻t＝k+1的航向角ψk+1和量测磁航向角αm,k+1，更新下一时刻t＝k+1时的磁偏角Dk+1；
16)测量方法的持续执行：返回下一时刻的数据获取步骤，获取时刻t＝k+2的数据信息，并进行时刻t＝k+2时磁偏角的更新。
2.根据权利要求1所述的一种基于地磁信息更新的载体航姿测量方法，其特征在于，所述下一时刻航姿信息的测量步骤中，离散化卡尔曼滤波方法为离散化扩展卡尔曼滤波方法；所述下一时刻航姿信息的测量包括以下步骤：
21)建立离散化的状态方程和量测方程，其表达式如下：
Figure FDA0002751201320000013
Figure FDA0002751201320000014
Zk+1＝h(Xk+1,k+1)+vk+1＝Xk+1+vk+1， (3)
Figure FDA0002751201320000021
Figure FDA0002751201320000022
Figure FDA0002751201320000023
ψm,k+1＝αm,k+1+Dk， (7)
Figure FDA0002751201320000024
其中：状态向量
Figure FDA0002751201320000025
Figure FDA0002751201320000026
表示当前时刻t＝k的载体坐标系相对于当地水平坐标系的航向角ψk、俯仰角θk和翻滚角
Figure FDA0002751201320000027
k表示离散时间点；
函数f表征下一时刻k+1的状态向量xk+1与当前时刻k的状态向量xk之间的关系；
Figure FDA0002751201320000028
表示角度的时间变化率；dt为t＝k与t＝k+1之间的时间间隔；
Figure FDA0002751201320000029
和
Figure FDA00027512013200000210
分别为k时刻载体坐标系中标定后陀螺仪在x，y和z轴上方向上量测数据；
函数h表征下一时刻k+1的状态向量xk+1与量测量zk+1两者的关系；
Figure FDA00027512013200000211
表示下一时刻k+1的载体坐标系相对于当地水平坐标系的航向角ψk+1、俯仰角θk+1和翻滚角
Figure FDA00027512013200000212
的量测量；
Figure FDA00027512013200000213
和
Figure FDA00027512013200000214
分别为下一时刻k+1载体坐标系中标定后加速度传感器在x、y和z轴上方向上量测数据；
Figure FDA00027512013200000215
和
Figure FDA00027512013200000216
分别为下一时刻k+1载体坐标系中标定后磁传感器在x、y和z轴上方向上量测数据；
Figure FDA00027512013200000217
和
Figure FDA00027512013200000218
分别为
Figure FDA00027512013200000219
和
Figure FDA00027512013200000220
转换到地理坐标系l三个轴xl、yl和zl上得到量测数据；wk和vk+1分别为***过程噪声和量测噪声，其统计特性如下：
Figure FDA0002751201320000031
其中，k、j表示离散时间点，Q为***过程噪声协方差矩阵，δkj为Kronecker符号，R为量测噪声协方差矩阵；
22)设定忽略***过程噪声wk和量测噪声vk+1，
由方程(1)-(4)获得状态向量和量测向量的先验估计
Figure FDA0002751201320000032
和
Figure FDA0002751201320000033
其表示如下：
Figure FDA0002751201320000034
Figure FDA0002751201320000035
其中，
Figure FDA0002751201320000036
为状态向量在时刻k的后验估计值；
由此得到近似呈线性的状态方程和量测方程为：
Figure FDA0002751201320000037
Figure FDA0002751201320000038
其中，Xk+1和Zk+1分别为状态向量和量测向量在下一时刻k+1的真实值；
B为函数f关于x偏导的Jacobian矩阵，
Figure FDA0002751201320000039
W为函数f关于w偏导的Jacobian矩阵，
Figure FDA00027512013200000310
H为函数h关于x偏导的Jacobian矩阵，
Figure FDA00027512013200000311
V为函数h关于v偏导的Jacobian矩阵，
Figure FDA0002751201320000041
23)构建一组完整的离散化扩展卡尔曼滤波方法迭代过程，其表示如下：
Figure FDA0002751201320000042
Figure FDA0002751201320000043
其中，
Figure FDA0002751201320000044
为k+1时刻状态向量的先验协方差矩阵，Pk为k时刻状态向量的协方差矩阵；
k+1时刻的量测新息
Figure FDA0002751201320000045
为：
Figure FDA0002751201320000046
结合方程(4)和(16)，可得到，
Figure FDA0002751201320000047
24)离散化扩展卡尔曼滤波方法测量更新方程如下：
令，
Figure FDA0002751201320000048
其中，(γ1,γ2,γ3)为传感器特性相关的参数，取正数；
Rk+1＝f(η1,η2,η3)R， (19)
其中，f(η1,η2,η3)为(η1,η2,η3)相关的函数；
Figure FDA0002751201320000051
Figure FDA0002751201320000052
Figure FDA0002751201320000053
其中，Kk为k时刻的滤波增益矩阵，Pk为k时刻的状态向量的协方差矩阵；
25)k+1时刻状态向量Xk+1近似如下：
Figure FDA0002751201320000054
3.根据权利要求2所述的一种基于地磁信息更新的载体航姿测量方法，其特征在于，所述下一时刻航姿信息的测量步骤中，离散化卡尔曼滤波方法为离散化无迹卡尔曼滤波方法；所述下一时刻航姿信息的测量包括以下步骤：
31)构建Sigma散点集：
Figure FDA0002751201320000055
其中，
Figure FDA0002751201320000056
为状态向量在时刻k的后验估计值；Pk为k时刻的状态向量的协方差矩阵；n为状态向量维数；λ＝α2(n+κ)-n为获取给定分布高阶矩信息的自由参数，使得n+κ＝3，10-4≤α≤1，
Figure FDA0002751201320000057
代表矩阵(n+λ)Pk-1平方根的第i行或第i列；
32)设定离散化无迹卡尔曼滤波方法的时间更新方程如下：
Figure FDA0002751201320000058
Figure FDA0002751201320000059
Figure FDA0002751201320000061
其中，
Figure FDA0002751201320000062
为状态向量在时刻k+1的先验估计值；
Figure FDA00027512013200000612
为时刻k+1的状态向量的协方差矩阵的先验估计值；Wi (m)和Wi (c)分别为UT变化计算状态向量和协方差矩阵的先验估计值所用的加权值；
Figure FDA0002751201320000063
其中，β包含状态分布的先验信息；
33)结合方程Rk+1＝f(η1,η2,η3)R，f(η1,η2,η3)为(η1,η2,η3)相关的函数，离散化无迹卡尔曼滤波方法的测量更新方程如下：
Figure FDA0002751201320000064
Figure FDA0002751201320000065
Figure FDA0002751201320000066
Figure FDA0002751201320000067
Figure FDA0002751201320000068
Figure FDA0002751201320000069
Figure FDA00027512013200000610
34)得到k+1时刻状态向量Xk+1的近似值，如下：
Figure FDA00027512013200000611
4.根据权利要求1所述的一种基于地磁信息更新的载体航姿测量方法，其特征在于，所述磁偏角信息的更新包括以下步骤：
41)根据时刻k+1的后验估计航向角
Figure FDA0002751201320000071
和量测磁航向角αm,k+1，解算时刻k+1的磁偏角计算
Figure FDA0002751201320000072
42)更新磁偏角信息，并存储时刻k+1的磁偏角Dk+1。
CN201910480822.7A 2019-06-04 2019-06-04 一种基于地磁信息更新的载体航姿测量方法 Active CN110095115B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910480822.7A CN110095115B (zh) 2019-06-04 2019-06-04 一种基于地磁信息更新的载体航姿测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910480822.7A CN110095115B (zh) 2019-06-04 2019-06-04 一种基于地磁信息更新的载体航姿测量方法

Publications (2)

Publication Number Publication Date
CN110095115A CN110095115A (zh) 2019-08-06
CN110095115B true CN110095115B (zh) 2020-12-25

Family

ID=67450164

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910480822.7A Active CN110095115B (zh) 2019-06-04 2019-06-04 一种基于地磁信息更新的载体航姿测量方法

Country Status (1)

Country Link
CN (1) CN110095115B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111895994A (zh) * 2020-06-29 2020-11-06 西北工业大学 一种基于磁趋势航向策略的地磁仿生导航方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1908584A (zh) * 2006-08-23 2007-02-07 北京航空航天大学 一种捷联惯性导航***初始姿态确定方法
CN108955671A (zh) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 一种基于磁偏角、磁倾角的卡尔曼滤波导航方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备
CN109443342A (zh) * 2018-09-05 2019-03-08 中原工学院 新型自适应卡尔曼无人机姿态解算方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109405820A (zh) * 2018-09-05 2019-03-01 中原工学院 无人机航姿监测***

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1908584A (zh) * 2006-08-23 2007-02-07 北京航空航天大学 一种捷联惯性导航***初始姿态确定方法
CN108955671A (zh) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 一种基于磁偏角、磁倾角的卡尔曼滤波导航方法
CN109443342A (zh) * 2018-09-05 2019-03-08 中原工学院 新型自适应卡尔曼无人机姿态解算方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
EKF和UKF在惯导初始对准中的应用研究;陈焕清 等;《长江工程职业技术学院学报》;20090630;第26卷(第2期);第40-43页 *
基于无迹卡尔曼滤波的四旋翼无人飞行器姿态估计算法;朱岩 等;《测试技术学报》;20141231;第28卷(第3期);第194-198页 *

Also Published As

Publication number Publication date
CN110095115A (zh) 2019-08-06

Similar Documents

Publication Publication Date Title
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN109556631B (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航***对准方法
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
US9417091B2 (en) System and method for determining and correcting field sensors errors
CN106500693B (zh) 一种基于自适应扩展卡尔曼滤波的ahrs算法
CN111947652A (zh) 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法
CN110174123B (zh) 一种磁传感器实时标定方法
CN112923924B (zh) 一种锚泊船舶姿态与位置监测方法及***
CN109000639B (zh) 乘性误差四元数地磁张量场辅助陀螺的姿态估计方法及装置
CN108225312B (zh) 一种gnss/ins松组合中杆臂估计以及补偿方法
CN111649747A (zh) 一种基于imu的自适应ekf姿态测量改进方法
Canciani et al. An analysis of the benefits and difficulties of aerial magnetic vector navigation
CN110095115B (zh) 一种基于地磁信息更新的载体航姿测量方法
Tomaszewski et al. Concept of AHRS algorithm designed for platform independent IMU attitude alignment
CN112284388B (zh) 一种无人机多源信息融合导航方法
CN110375773B (zh) Mems惯导***姿态初始化方法
CN110160530B (zh) 一种基于四元数的航天器姿态滤波方法
CN116380052A (zh) 一种基于uwb融合imu的室内定位方法
CN113551669B (zh) 基于短基线的组合导航定位方法及装置
CN115523919A (zh) 一种基于陀螺漂移优化的九轴姿态解算方法
Mathiassen et al. A low cost navigation unit for positioning of personnel after loss of GPS position
Tomaszewski et al. Analysis of the noise parameters and attitude alignment accuracy of INS conducted with the use of MEMS-based integrated navigation system
Shou et al. Micro-satellite attitude determination: using Kalman filtering of magnetometer data approach
Goryanina et al. Recursive least squares method for identification of MEMS orientation sensors parameters

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