一种消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法

1.本发明涉及姿态控制技术领域，尤其涉及一种消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法。


背景技术：

2.惯性-地磁组合又被称为姿态和航向参考系统，目前已经成为一种用于确定载体运动姿态的有广阔应用前景的工具。一个惯性-地磁组合主要由三轴陀螺仪、三轴加速度计和三轴地磁传感器组成，这三种传感器均为微机电传感器。一种姿态解算算法需要被编程至惯性-地磁组合的内置微处理器中，用于融合上述三种传感器的输出数据，以便于获得准确的姿态估计。
3.经过多年的发展，目前已经提出了很多姿态解算算法，但是最为常用的算法为卡尔曼滤波器和互补滤波器。卡尔曼器算法包括线性卡尔曼滤波器、扩展卡尔曼滤波器、无迹卡尔曼滤波器和容积卡尔曼滤波器。
4.姿态通常用欧拉角、旋转矩阵或者四元数来描述。由于容易理解、形象直观、物理意义清晰，欧拉角在许多场合中受到青睐。因此，尽管旋转矩阵或者四元数，尤其是后者，被大多数姿态解算算法所采用，一旦这两种描述形式的姿态被估计出，它们往往也会被最终转化成欧拉角。众所周知，当前时刻的陀螺仪输出数据被用于预测下一时刻的欧拉角，而下一时刻的加速度计和地磁传感器输出数据分别被用于修正预测的水平姿态(包括俯仰角和滚转角)和方位角(也被称为航向角)。
5.现有的姿态解算方法一是构造新的4元化姿态描述方法，并采用互补滤波器来估计这种姿态。在这种4元化姿态描述中，前三个元素只能由陀螺仪和加速度计输出数据估计出，且不受地磁传感器输出数据的影响，而最后一个元素则只能由陀螺仪和地磁传感器输出数据估计出，且不受加速度计输出数据影响。可利用前三个元素解算出水平姿态，而利用最后一个元素解算出方位角。这种措施的主要问题是，用于实现姿态预测的姿态微分方程是非线性的，因而不得不采用一阶近似的方法来求解上述微分方程，由此导致的后果是，载体存在旋转运动时，姿态估计精度不佳。
6.现有的姿态解算方法二是重新构造一个参考向量，使得其垂直于重力加速度矢量，随后将新构造的参考矢量代替地磁场矢量。所采用的滤波器可以为扩展卡尔曼滤波器或者为扩展互补滤波器。这种措施的主要问题是，对于扩展卡尔曼滤波器而言，如果同时存在线加速度干扰和地磁干扰，那么地磁干扰仍然会对水平姿态估计精度造成负面影响。对于扩展互补滤波器而言，在载体在地磁场受到干扰的区域内做旋转运动时，姿态估计精度仍然不佳。
7.现有的姿态解算方法三是采用两步化的卡尔曼或者互补滤波器。第一步只采用陀螺仪和加速度计估计水平姿态，而第二步则利用水平姿态估计结果以及陀螺仪和地磁传感器输出数据进一步估计方位角。这种措施的主要问题是，两步卡尔曼滤波器的运行时间过长，且在受到地磁干扰时方位角容易发散；两步互补滤波器严重依赖于地磁干扰存在与否
的判断精度，如果判断错误，那么地磁干扰对水平姿态估计精度的影响依然存在。
8.现有的姿态解算方法四是类似于第三种措施，仍然采用两步式滤波器结构，但在第一步时转而估计水平偏差(定义为真实姿态与上个采样时刻姿态估计之间的偏差)，而在第二步时则估计方位角偏差。这种措施的主要问题是，与扩展卡尔曼滤波器类似，如果同时存在线加速度干扰和地磁干扰，那么地磁干扰仍然会对水平姿态估计精度造成负面影响。


技术实现要素：

9.针对现有方法的不足，本发明解决地磁干扰引起的地磁传感器输出异常造成水平姿态估计精度的大幅度下降问题。
10.本发明所采用的技术方案是：一种消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法包括以下步骤：
11.步骤一、在采样时刻k+1，利用采样时刻k时的三轴陀螺仪实现对姿态的预测；
12.进一步的，具体包括：
13.构建姿态的预测的四元数，公式为：
[0014][0015]
其中，q
k/k
和q
k+1/k
分别被称为采样时刻k时的姿态验后估计和采样时刻k+1时的姿态预测；四元数q
k/k
定义为q
k/k
＝[q1·
(k/k)
,q2·
(k/k)
,q3·
(k/k)
,q0·
(k/k)
]
t
；ts是采样时间；反对称阵[ωk×
]4×4定义为：
[0016][0017]
其中，ω
x
·k，ωy·k，和ωz·k分别为陀螺仪沿载体坐标系x、y和z轴向的输出值；φk被称为状态转移矩阵。
[0018]
步骤二、计算姿态预测误差协方差矩阵；
[0019]
进一步的，误差协方差矩阵的公式为：
[0020][0021]
其中，p
k/k
为采样时刻k时的验后估计误差协方差阵；qk为过程噪声协方差矩阵；
[0022][0023]
其中，σ
ω
为每轴向陀螺仪输出高斯噪声的标准差，矩阵ξk定义为：
[0024][0025]
步骤三、计算增益矩阵；
[0026]
进一步的，增益矩阵的公式为：
[0027]
[0028]
其中，为采样时刻k+1时的线性化后的观测矩阵；
[0029][0030]
旋转矩阵定义为：
[0031][0032]rk+1
为观测误差协方差阵，公式为：
[0033][0034]
其中，σg和σm分别为不存在线加速度干扰和地磁干扰时每轴向上的加速度计和陀螺仪输出高斯噪声的方差。
[0035]
步骤四、利用加速度计和地磁传感器的输出数据更新姿态预测；
[0036]
进一步的，具体包括：更新姿态预测公式为：
[0037][0038]
其中，q
k+1/k+1
被称为采样时刻k+1时的姿态验后估计；和分别为归一化后的重力加速度矢量和地磁场矢量在载体坐标系下的观测值；和分别为重力加速度矢量和地磁场矢量在载体坐标系i轴向上的归一化后的观测值；g和m分别为上述两个矢量在参考坐标系下的表示。
[0039]
步骤五、对更新姿态预测进行重构；
[0040]
进一步的，重构的姿态预测公式为：
[0041][0042]
其中，k
k+1
(:,1:3)和k
k+1
(:,4:6)为由增益矩阵k
k+1
的前三列和后三列元素构成的子矩阵；
[0043]
定义：
[0044][0045]
得到：
[0046]
步骤六、计算参量a(q
k+1/k+1
)和b(q
k+1/k+1
)；
[0047]
进一步的，参量a(q
k+1/k+1
)和b(q
k+1/k+1
)的公式为：
[0048][0049]
步骤七、构建增益调整矩阵；
[0050]
进一步的，增益调整矩阵的公式为：
[0051][0052]
其中，如果a≥b，则：
[0053][0054]
而如果a《b，则：
[0055][0056]
步骤八、重新计算采样时刻k+1时的姿态验后估计q
k+1/k+1
；
[0057]
进一步的，采样时刻k+1时的姿态验后估计公式为：
[0058][0059]
步骤九、计算k+1时刻的验后估计误差协方差阵；
[0060]
进一步的，k+1时刻的验后估计误差协方差阵的公式为：
[0061]
p
k+1/k+1
＝p
k+1/k-k
k+1hk+1
p
k+1/k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0062]
步骤十、计算采样时刻k+2时的姿态验后估计，并循环执行，直至用户终止。
[0063]
本发明的有益效果：
[0064]
1、通过在增益矩阵k
k+1
中引入增益调整矩阵λ
k+1
，使得地磁干扰不会对水平姿态的估计精度产生影响。
附图说明
[0065]
图1是本发明的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法流程图；
[0066]
图2是本发明的随时间变化曲线；
[0067]
图3是本发明的随时间变化曲线；
[0068]
图4是本发明的随时间变化曲线；
[0069]
图5是本发明的随时间变化曲线；
[0070]
图6是本发明的随时间变化曲线；
[0071]
图7是本发明的随时间变化曲线；
[0072]
图8是本发明的随时间变化曲线；
[0073]
图9是本发明的随时间变化曲线；
[0074]
图10是本发明的随时间变化曲线。
具体实施方式
[0075]
下面结合附图和实施例对本发明作进一步说明，此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
[0076]
如图1所示，一种消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法包括以下步骤：
[0077]
步骤一、在采样时刻k+1，利用采样时刻k时的三轴陀螺仪实现对姿态的预测，其中姿态预测用四元数表示：
[0078][0079]
其中，四元数q
k/k
定义为四元数q
k/k
的严格定义是q0·
(k/k)
+q1·
(k/k)
i+q2·
(k/k)
j+q3·
(k/k)
k，其中i，j，k分别为参考系的三个坐标轴，但是当其用于描述旋转时，其通常以向量形式出现；ts是采样时间；反对称阵[ωk×
]4×4定义为：
[0080][0081]
其中，ω
x
·k，ωy·k和ωz·k分别为陀螺仪沿载体坐标系x、y和z轴向的输出值；φk被称为状态转移矩阵；式(1)中，q
k/k
和q
k+1/k
分别被称为采样时刻k时的姿态验后估计和采样时刻k+1时的姿态预测。
[0082]
步骤二、计算姿态预测误差协方差矩阵p
k+1/k
；
[0083][0084]
其中，p
k/k
为采样时刻k时的验后估计误差协方差阵；qk为过程噪声协方差矩阵，由下式计算得到：
[0085]
[0086]
上式中，σ
ω
为每轴向陀螺仪输出高斯噪声的标准差；矩阵ξk定义为：
[0087][0088]
步骤三、由式(6)计算增益矩阵k
k+1
：
[0089][0090]
其中，为采样时刻k+1时的线性化后的观测矩阵，而：
[0091][0092]
式(7)中，旋转矩阵定义为：
[0093][0094]
需要注意的是，为简化起见，式(8)中等式右边的下标“k+1/k”已被略去；r
k+1
为观测误差协方差阵，由式9构成：
[0095][0096]
其中，σg和σm分别为不存在线加速度干扰和地磁干扰时每轴向上的加速度计和陀螺仪输出高斯噪声的方差。
[0097]
步骤四、由式(10)通过利用加速度计和地磁传感器的输出数据更新姿态预测：
[0098][0099]
其中，q
k+1/k+1
被称为采样时刻k+1时的姿态验后估计；和分别为归一化后的重力加速度矢量和地磁场矢量在载体坐标系下的观测值；和分别为重力加速度矢量和地磁场矢量在载体坐标系i轴向上的归一化后的观测值；g和m分别为上述两个矢量在参考坐标系下的表示；如果北西天坐标系(也被称为当地地理坐标系)被选为参考坐标系，那么g＝[0,0,-1]
t
、m＝[cosβ,0,-sinβ]
t
，其中，β为磁倾角。
[0100]
步骤五、将式(10)重写为：
[0101][0102]
其中，k
k+1
(:,1:3)和k
k+1
(:,4:6)为由增益矩阵k
k+1
的前三列和后三列元素构成的子矩阵。
[0103]
然后定义：
[0104][0105]
从而使得式(10)成为如下形式：
[0106][0107]
步骤六、由式(14)计算参量a(q
k+1/k+1
)和b(q
k+1/k+1
)，参量a(q
k+1/k+1
)和b(q
k+1/k+1
)是四元数，公式为：
[0108][0109]
步骤七、由式(15)构建增益调整矩阵λ
k+1
；
[0110][0111]
其中，如果a≥b，则：
[0112][0113]
而如果a《b，则：
[0114][0115]
步骤八、重新计算采样时刻k+1时的姿态验后估计q
k+1/k+1
：
[0116]
[0117]
步骤九、计算k+1时刻的验后估计误差协方差阵；
[0118]
p
k+1/k+1
＝p
k+1/k-k
k+1hk+1
p
k+1/k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0119]
步骤十、返回步骤一，开始计算采样时刻k+2时的姿态验后估计q
k+2/k+2
。
[0120]
步骤一至十循环进行，直至用户终止。需要注意的是，姿态初始验后估计q
0/0
通过双矢量姿态确定算法(如quaternion estimator quest算法)获得，而姿态初始验后估计误差协方差阵p
0/0
则随意设置，一般设置为单位阵。
[0121]
通过仿真方式验证了本发明方法与现有方法的对比结果，证明了本发明方法的有效性。其中现有方法包括扩展卡尔曼滤波器(ekf，2022年提出)、扩展互补滤波器(ecf，2020年提出)、偏差状态扩展卡尔曼滤波器(eekf，2021年提出)、及四参数互补滤波器(fcf，2020年提出)。
[0122]
传感器的输出模型定义为：
[0123][0124]
上式中，k1＝1000、k2＝2000、k3＝3000、k4＝4000、k5＝5000、kn＝6000；＝6000；＝6000；高斯噪声和的标准差分别设置为：σ
ω
＝0.006rad/s，σg＝0.008m/s2及σm＝0.001gauss；采样率f设置为100hz；表示载体真实旋转角速度，而且：
[0125][0126]
其中，分别为陀螺仪、加速度计和地磁传感器的输出零偏。fcf和ecf的增益分别设置为α＝8.6和k＝4.2，这两个增益是通过实验方法获得的最优值。
[0127]
仿真实验重复1000次；在每次重复实验中，和分别从具有边界参数[0,5.25]、[0,0.5]和[0,0.5]的三个均匀分布中随机抽取出。
和均从具有边界参数[0,0.001]的均匀分布中随机抽取出，初始姿态为随机生成。
[0128]
在每次重复试验中，每种方法均运行两次；在第一次运行时，三种传感器(陀螺仪、加速度计和地磁传感器)均用于姿态估计，而在第二次运行时，只有陀螺仪和加速度计用于姿态估计。
[0129]
使用指标(aae)评估每种方法的性能：
[0130][0131]
上式中，α＝γ,θ,ψ，其中γ、θ和ψ分别代表俯仰角、滚转角和方位角；和分别表示第j次重复运行过程中采样时刻k时的姿态估计值和姿态真实值。通过真实的旋转矩阵计算得到。需要注意的是，式(22)是在通过融合三种传感器的输出信息后最终来得到如果只是通过融合陀螺仪和加速度计的输出信息来得到那么式(22)所示的指标表示为图2～10给出了最终的仿真实验结果；图中，“grekf”表示本发明提出的改进卡尔曼滤波方法。
[0132]
从图2～4可以看出，fcf的姿态估计精度与旋转运动呈负相关。该方法构造了四个参数，并用这些参数表示姿态，然而这四个参数的微分方程是非线性的，因此需要用一阶近似的方法去求解上述非线性微分方程。由于上述近似求解方法，fcf在载体存在旋转运动时表现不佳。从图2～3可以看出，ecf在载体在铁磁材料附近(即存在地磁干扰区域)做旋转运动时性能不佳。
[0133]
从图2、3、5、6、8和9可以看出，当存在线性加速度干扰时，eekf、ekf和ecf的水平姿态(俯仰角和滚转角)的估计精度严重下降。为了避免地磁干扰对水平姿态估计所带来的负面影响，ekf和ecf均丢弃了地磁传感器输出矢量沿加速度计输出矢量方向的分量。然而，上述避免地磁干扰影响的思路是建立在不存在线加速度干扰的假设上的。因此很明显，一旦存在线加速度干扰，那么上述思路不可能有良好的性能表现，eekf以姿态偏差作为估计对象，姿态偏差定义为姿态估计值与真实值之间的偏差。eekf为两步式姿态估计方法，方位角偏差的估计被放在了第二步，并且需要充分利用第一步的俯仰角和滚转角偏差的估计结果。因此，线加速度干扰也会影响方位角偏差的估计结果。eekf方法随后利用式(23)实现姿态验后估计(以旋转矩阵表示)的计算：
[0134][0135]
其中，δγ
k+1
、δθ
k+1
和δψ
k+1
分别为俯仰角、滚转角和方位角偏差。很明显，受到线加速度干扰影响的方位角偏差也会进一步影响俯仰角和滚转角估计。通过这些图可以看出，fcf方法的水平姿态估计精度完全不受地磁干扰的影响，本发明提出的方法的水平姿态估计精度虽然也受到了地磁干扰的影响，但是相对于ekf、eekf和ecf方法，这一影响可以忽略不计。
[0136]
通过对比图4和7可以看出，如果不使用地磁传感器输出信息，那么fcf的方位角估计会快速发散，这意味着，fcf方法不能单独利用陀螺仪输出信息实现方位角的估计。结合图4和7也可以发现，如果不使用地磁传感器输出信息，那么在载体做旋转运动时，eekf的方
位角估计存在振荡现象。
[0137]
图4中本发明方法的方位估计精度在采样时间间隔[2000,4000]内比其他四种方法都要高；不难从原理角度解释这一结果；λi·
(k+1)
与(i＝1,2,3,4)之间存在反比例关系；很明显，如果有一个增量(减量)，那么λi·
(k+1)
必然会产生一个减量(增量)；因此增益调整矩阵λ
k+1
表现得像是一个低通滤波器，其能够抑制时变地磁干扰所带来的负面影响。
[0138]
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关工作人员完全可以在不偏离本项发明技术思想的范围内，进行多样的变更以及修改。本项发明的技术性范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。

技术特征：
1.一种消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，包括以下步骤：步骤一、在采样时刻k+1，利用采样时刻k时的三轴陀螺仪实现对姿态的预测；步骤二、计算姿态预测误差协方差矩阵；步骤三、计算增益矩阵；步骤四、利用加速度计和地磁传感器的输出数据更新姿态预测；步骤五、对更新姿态预测进行重构；步骤六、计算参量a(q
k+1/k+1
)和b(q
k+1/k+1
)；步骤七、构建增益调整矩阵；步骤八、重新计算采样时刻k+1时的姿态验后估计；步骤九、计算k+1时刻的验后估计误差协方差阵；步骤十、计算采样时刻k+2时的姿态验后估计，并循环执行，直至用户终止。2.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，步骤一具体包括：构建姿态的预测的四元数，公式为：其中，q
k/k
和q
k+1/k
分别被称为采样时刻k时的姿态验后估计和采样时刻k+1时的姿态预测；四元数q
k/k
定义为q
k/k
＝[q1·
(k/k)
,q2·
(k/k)
,q3·
(k/k)
,q0·
(k/k)
]
t
；t
s
是采样时间；反对称阵[ω
k
×
]4×4定义为：其中，ω
x
·
k
，ω
y
·
k
，和ω
z
·
k
分别为陀螺仪沿载体坐标系x、y和z轴向的输出值；φ
k
被称为状态转移矩阵。3.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，误差协方差矩阵的公式为：其中，p
k/k
为采样时刻k时的验后估计误差协方差阵；q
k
为过程噪声协方差矩阵；其中，σ
ω
为每轴向陀螺仪输出高斯噪声的标准差，矩阵ξ
k
定义为：4.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，增益矩阵的公式为：
其中，为采样时刻k+1时的线性化后的观测矩阵；旋转矩阵定义为：r
k+1
为观测误差协方差阵，公式为：其中，σ
g
和σ
m
分别为不存在线加速度干扰和地磁干扰时每轴向上的加速度计和陀螺仪输出高斯噪声的方差。5.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，步骤四具体包括：更新姿态预测公式为：其中，q
k+1/k+1
被称为采样时刻k+1时的姿态验后估计；和分别为归一化后的重力加速度矢量和地磁场矢量在载体坐标系下的观测值；和分别为重力加速度矢量和地磁场矢量在载体坐标系i轴向上的归一化后的观测值；g和m分别为重力加速度矢量和地磁场矢量在参考坐标系下的表示。6.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，重构的姿态预测公式为：其中，k
k+1
(:,1:3)和k
k+1
(:,4:6)为由增益矩阵k
k+1
的前三列和后三列元素构成的子矩阵；定义：得到：
7.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，参量a(q
k+1/k+1
)和b(q
k+1/k+1
)的公式为：8.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，增益调整矩阵的公式为：其中，如果a≥b，则：而如果a<b，则：9.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，采样时刻k+1时的姿态验后估计公式为：10.根据权利要求1所述的消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，其特征在于，k+1时刻的验后估计误差协方差阵的公式为：p
k+1/k+1
＝p
k+1/k-k
k+1
h
k+1
p
k+1/k
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)。

技术总结
本发明涉及姿态控制技术领域，尤其涉及一种消除地磁干扰对水平姿态影响的改进卡尔曼滤波方法，包括：在采样时刻k+1，利用采样时刻k时的三轴陀螺仪实现对姿态的预测；计算姿态预测误差协方差矩阵；计算增益矩阵；利用加速度计和地磁传感器的输出数据更新姿态预测；对更新姿态预测进行重构；计算参量A(q


技术研发人员：戎海龙 彭翠云 诸燕平 吕继东
受保护的技术使用者：常州大学
技术研发日：2023.04.26
技术公布日：2023/7/25