一种高动态和磁干扰环境下的姿态估计算法制造技术

本发明专利技术提供一种高动态和磁干扰环境下的姿态估计算法，该姿态估计算法包括第一层姿态估计算法、第二层姿态估计算法，其中第一层算法解算俯仰、滚转角，选用方向余弦矩阵的最后一行作为俯仰、滚转角的状态变量，选用该状态变量的误差状态在EKF框架下进行更新与校正；第二层算法解算航向角，选用方向余弦矩阵的第一列的前两个元素作为航向角的状态变量，同时为了避免磁干扰对姿态解算的影响，使用磁力计模长乘以模块磁场倾角增量再取对数函数的方法表征磁干扰的强度，并将磁干扰的强度和观测噪声相关联。解决了姿态估计算法在高动态环境下精度低、易发散，且姿态角受磁干扰影响的问题，有效提高了姿态估计的精度和鲁棒性。有效提高了姿态估计的精度和鲁棒性。有效提高了姿态估计的精度和鲁棒性。

【技术实现步骤摘要】
一种高动态和磁干扰环境下的姿态估计算法


[0001]本专利技术涉及一种姿态估计算法，具体涉及一种高动态和磁干扰环境下的姿态估计算法。

技术介绍

[0002]卡尔曼滤波是姿态估计中应用最为广泛的一种方法，传统卡尔曼滤波只使用于线性系统，而三维旋转运动由于其转动空间是流形，无论用哪种其数学描述方法都是在非线性空间中工作。为了适应非线性系统，提出使用扩展卡尔曼滤波(EKF)进行姿态估计，其核心思想就是利用泰勒级数，将非线性系统在工作点附近进行一阶泰勒展开并忽略高阶项。但是在高动态环境中，运动方程的模型噪声和陀螺仪噪声增大，观测方程的线性化点开始远离工作点，最终导致EKF的估计结果产生偏差或不一致，甚至会导致滤波器发散。
[0003]由于加速度计并未包含航向信息，要想获得无累计误差的航向角则需要引入磁力计，但载体在运动过程中，不可避免的会运动到有外部磁场干扰的环境，当距离干扰源比较近时干扰磁场的强度会远远大于地磁场强度，使得磁力计数据失去参考价值。

技术实现思路

[0004]为了解决上述问题，本专利技术针对传统的姿态估计算法在高动态环境下估计值容易发散，姿态角受磁干扰影响的问题，提出一种高动态和磁干扰环境下的姿态估计算法，其将俯仰角、滚转角和航向角分层解算，解决了姿态估计算法在高动态环境下精度低、易发散，且姿态角受磁干扰影响的问题，有效提高了姿态估计的精度和鲁棒性。
[0005]本专利技术是通过以下技术方案实现的，提供一种高动态和磁干扰环境下的姿态估计算法，包括以下步骤：
[0006]S1第一层姿态估计算法：基于线性化程度更高的误差状态解算俯仰角、滚转角，并选用方向余弦矩阵的最后一行作为俯仰角、滚转角状态变量，选用该状态变量的误差状态在EKF框架下进行更新与校正；
[0007]S2第二层姿态估计算法：选用方向余弦矩阵的第一列的前两个元素作为航向角的状态变量，并选用磁力计校正航向信息，使用磁力计模值和模块磁场倾角增量相乘再取对数函数的方法度量磁干扰强度。
[0008]特别的，所述S1具体按照以下步骤实施：
[0009]S11名义状态预测
[0010]选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵的最后一行r作为表达俯仰θ、滚转角γ的状态变量，并将陀螺仪的零偏b
g
当成状态变量估计，第一层的名义状态向量x1按如下公式表示：
[0011]x1＝[r b
g
]ꢀꢀꢀ
(1)，
[0012]公式(1)中，r按如下公式计算：
[0013]r＝[
‑
sinθ sinγcosθ cosγcosθ]ꢀꢀꢀ
(2)，
[0014]状态向量x1的运动方程按如下公式表示：
[0015][0016]公式(3)中，
b
Φ
k
为[t
k
‑1,t
k
]时间段内的等效旋转矢量，(
b
Φ
k
)
^
为
b
Φ
k
的反对称矩阵；
[0017]S12误差状态预测和校正
[0018]误差状态的运动方程为：
[0019][0020][0021]公式(4)中，δφ为r的误差状态，δb
g
为b
g
的误差状态，将公式(4)按矩阵形式表示为：
[0022][0023]公式(5)中，F
k
‑1为误差状态δx1的状态转移矩阵，其按照如下公式计算：
[0024][0025]误差状态的先验协方差的预测方程如下：
[0026][0027]于公式(7)中，Q为预测方程的过程噪声矩阵；
[0028]引入加速度计对预测结果进行校正，观测方程按如下公式表示：
[0029]z
1,k
＝h1(x
1,k
,v
k
)
ꢀꢀ
(8)，
[0030]公式(8)中，z
1,k
＝a
m
为加速度计测量值，v
k
为加速度计测量噪声,由于状态变量是选用姿态旋转矩阵的最后一行，所以h1(
·
)是一个线性函数，
[0031]根据链式求导法则，观测方程关于δx1的雅克比矩阵如下：
[0032][0033]公式(9)中，H
x1
＝[I
3 03]，
[0034]因此，误差状态的后验校正过程按如下公式表示：
[0035][0036][0037][0038]公式(10)中，R1为观测方程的噪声矩阵,I6为6*6的单位矩阵；
[0039]S13误差状态的注入和复位
[0040]将经过加速度计校正后的误差状态按如下公式注入到名义状态中：
[0041][0042][0043]公式(11)中，Exp(δφ)是误差状态到上SO(3)的指数映射，其按照如下公式表示：
[0044]Exp(δφ)＝I3+u
^
sinφ+(1
‑
cosφ)(u
^
)2ꢀꢀ
(12)，
[0045]将误差状态注入到名义状态之后，需要将误差状态清零，具体按如下公式操作：
[0046][0047][0048]特别的，所述S11中，需引入单子样+前一周期的等效旋转矢量补偿不可交换误差，具体按如下公式表示：
[0049][0050]公式(14)中，Φ
k
为等效旋转矢量，ΔΘ
k
为角增量，ΔΘ
k
＝ΔT
·
(
b
ω
m,k
‑
b
g,k
‑1)
^
。
[0051]特别的，所述S2具体按照以下步骤实施：
[0052]S21航向信息解算
[0053]选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵的第一列的前两个元素作为表达航向角的状态变量，其中第二层的状态向量x2为：
[0054][0055]运动方程按如下公式表示：
[0056][0057]公式(16)中，由和计算所得，
[0058]状态x2的先验协方差的预测方程按如下公式表示：
[0059][0060]S22航向信息校正
[0061]由于磁力计数据并不能直接校正状态x2，因此需将其转换到和状态x2同一量纲，状态x2的后验校正过程按如下公式表示：
[0062][0063][0064][0065]公式(18)中，
[0066]S23磁干扰的检测
[0067]使用模块磁场倾角增量和磁力计模值来评价磁干扰强度，具体评价函数按如下公式表示：
[0068]m
len
＝log(||
b
m
m
||s本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种高动态和磁干扰环境下的姿态估计算法，其特征在于，包括以下步骤：S1第一层姿态估计算法：基于线性化程度更高的误差状态解算俯仰角、滚转角，并选用方向余弦矩阵的最后一行作为俯仰角、滚转角状态变量，选用该状态变量的误差状态在EKF框架下进行更新与校正；S2第二层姿态估计算法：选用方向余弦矩阵的第一列的前两个元素作为航向角的状态变量，并选用磁力计校正航向信息，使用磁力计模值和模块磁场倾角增量相乘再取对数函数的方法度量磁干扰强度。2.根据权利要求1所述一种高动态和磁干扰环境下的姿态估计算法，其特征在于，所述S1具体按照以下步骤实施：S11名义状态预测选用从载体坐标系b系到导航坐标系n系的姿态旋转矩阵的最后一行r作为表达俯仰θ、滚转角γ的状态变量，并将陀螺仪的零偏b
g
当成状态变量估计，第一层的名义状态向量x1按如下公式表示：x1＝[r b
g
](1)，公式(1)中，r按如下公式计算：r＝[
‑
sinθ sinγcosθ cosγcosθ](2)，状态向量x1的运动方程按如下公式表示：公式(3)中，
b
Φ
k
为[t
k
‑1,t
k
]时间段内的等效旋转矢量，(
b
Φ
k
)^为
b
Φ
k
的反对称矩阵；S12误差状态预测和校正误差状态的运动方程为：误差状态的运动方程为：公式(4)中，δφ为r的误差状态，δb
g
为b
g
的误差状态，将公式(4)按矩阵形式表示为：公式(5)中，F
k
‑1为误差状态δx1的状态转移矩阵，其按照如下公式计算：误差状态的先验协方差的预测方程如下：于公式(7)中，Q为预测方程的过程噪声矩阵；引入加速度计对预测结果进行校正，观测方程按如下公式表示：z
1,k
＝h1(x
1,k
,v
k
)(8)，公式(8)中，z
1,k
＝a
m
为加速度计测量值，v
k
为加速度计测量噪声,由于状态变量是选用
姿态旋转矩阵的最后一行，所以h1(
·
)是一个线性函数，根据链式求导法则，观测方程关于δx1的雅克比矩阵如下：公式(9)中，H
x1
＝[I
3 03]，因此，误差状态的后验校正过程按如下公式表示：误差状态的后验校正过程按如下公式表示：误差状态的后验校正过程按如下公式表示：公式(10)中，R1为观...

【专利技术属性】
技术研发人员：孙伟，韩冬，陈龙，连杰，
申请(专利权)人：中煤科工集团西安研究院有限公司，
类型：发明
国别省市：