CN103017787A - 适用于摇摆晃动基座的初始对准方法 - Google Patents

Abstract

本发明提供的是一种适用于摇摆晃动基座的初始对准方法。在摇摆加晃动干扰基座条件下，实时地同步采集三轴陀螺的角速率信息和三轴加速度计的比力信息；进行粗对准；由惯导输出的航向、纵摇和横摇信息，得到载体坐标系和半固定坐标系之间的转化关系；将载体坐标系下的加速度信息投影到半固定坐标系下的三个轴上，并进行积分；用高通数字滤波器对信号进行处理；将已提取的船舶瞬时线运动速度信息作为速度基准，与惯导解算出的速度相减作差，以所得差值作为卡尔曼滤波的量测量，列写卡尔曼滤波的状态方程和量测方程；将卡尔曼滤波进行离散化，完成对准。本发明对准时间比以位置误差为量测量的对准方法要短；对准精度更高；环境适应性更强。

Description

适用于摇摆晃动基座的初始对准方法

技术领域

本发明涉及的是一种船用捷联惯导系统的初始对准方法，具体地说是一种适用于摇摆晃动基座的初始对准方法。

背景技术

相对于航天器和飞行器来说，船舶的摇摆、晃动（又称为“瞬时线运动”）同时存在的运动状态是其所特有的一种恶劣的运动状态。在瞬时线运动基座条件下，GPS和多普勒计程仪无法提供载体准确的瞬时线速度信息，导致卡尔曼滤波对准过程中量测量（即速度信息）不准确。如果忽略瞬时线运动，会对初始对准产生很大的影响。

对于摇摆，首先必定会使惯导系统产生角速度的输入，会引起数学平台中姿态矩阵

的变化，但

本身应该随着摇摆一起变化。因此不论载体系怎么摇摆，输入的角速度只会影响

的值，但不会影响

的准确性。船舶航行过程中的线运动包括两部分，一部分是由于船舶机动引起的线运动，另一部分是由于海浪作用引起的船舶瞬时线运动，在船舶航行过程中，由于瞬时线运动的幅度较小，在整个速度的量值中所占的比重很小，故在进行卡尔曼滤波对准的过程中可以将此部分忽略。但是，在系泊状态下，由于没有机动所引起的线运动，船舶始终处于受海浪作用引起的升沉和横荡、纵荡运动，即基本上处于瞬时线运动状态。此时，在进行卡尔曼滤波对准的过程中不可以将此部分运动忽略掉。不同海况下对于升沉横荡，可看作加速度作用于惯导系统，即系统中引入了加速度，加速度经过一次积分得到瞬时速度。对于此种瞬时线运动，目前国内还没有很好的测量方法（多普勒计程仪（DVL）和GPS在系泊状态下测速误差很大），进而导致在卡尔曼滤波对准过程中的速度量测量误差很大，对基于白噪声误差模型的卡尔曼滤波方法来说，相当于在量测量中引入了幅值很大的有色噪声，使得最终的估计结果发散。因此在瞬时线运动干扰基座条件下如何提取准确的瞬时线运动信息成为解决问题的关键所在。

发明内容

本发明的目的在于提供一种对准时间短、环境适应性强、对准精度高的适用于摇摆晃动基座的初始对准方法。

本发明的目的是这样实现的：

步骤一：在摇摆加晃动干扰基座条件下，实时地同步采集三轴陀螺的角速率信息和三轴加速度计的比力信息；

步骤二：进行粗对准；

步骤三：粗对准结束后，由惯导输出的航向、纵摇和横摇信息，得到载体坐标系和半固定坐标系之间的转化关系；

步骤四：将载体坐标系下的加速度信息投影到半固定坐标系下的三个轴上，并进行积分，得到半固定坐标系下速度的相关信息；

步骤五：用高通数字滤波器对信号进行处理，得到相对于舒勒周期的高频信号，即船舶瞬时线运动速度信号；

步骤六：将已提取的船舶瞬时线运动速度信息作为速度基准，与惯导解算出的速度相减作差，以所得差值作为卡尔曼滤波的量测量，列写卡尔曼滤波的状态方程和量测方程；

步骤七：将卡尔曼滤波进行离散化，完成对准。

在摇摆加晃动干扰基座条件下，多普勒计程仪和GPS无法提供准确的速度信息，导致以速度误差为量测量的卡尔曼初始对准最终不能收敛，为了解决这一问题，本发明在完成粗对准的基础上，使用基于惯性测量的方法，通过设计相应的数字滤波器，将瞬时线运动信息提取出来，并以此作为速度基准进行量测，完成初始对准。

本发明的优点主要体现在：

一、本发明所提出的方法和以位置误差为量测量的卡尔曼滤波方法相比较，由于速度误差和姿态误差之间的耦合程度比位置误差和姿态误差之间的耦合程度要紧密，因此对准时间也比以位置误差为量测量的对准方法要短。

二、由于在摇摆和晃动基座下GPS和DVL所测得的速度误差很大，不能用来作为基准，因此和传统的以速度误差为量测量的卡尔曼滤波方法相比，此方法的环境适应性更强。

三、由于作为速度基准的信息更为准确，和传统的以速度误差为量测量的卡尔曼滤波方法相比，此方法的对准精度更高。

四、由于作为速度基准的信息更为准确，和传统的以速度误差为量测量的卡尔曼滤波方法相比，滤波收敛速度更快，有效的缩短了对准时间。

附图说明

图1为原理框图；

图2为流程图；

图3为粗对准原理图；

图4为初始对准的效果图，其中虚线代表本方法，实线代表传统的卡尔曼滤波方法。

具体实施方式

下面举例对本发明作更详细的描述：

步骤一：在摇摆加晃动干扰基座条件下，实时地同步采集三轴陀螺的角速率信息和三轴加速度计的比力信息，完成捷联惯导系统的预热准备，时间约为2个小时；

步骤二：针对如何消除干扰加速度，选用相应的粗对准方案，并完成粗对准；

如何有效地去除由晃动引起的干扰加速度是粗对准的关键，因为载体的摇摆运动同样会导致加速度计所测的比力信息中产生变化的分量，其反映了载体由于正常的摇摆运动所产生的航向姿态的变化。因此不能滤掉由于摇摆引起的加速度的变化，即滤波处理不能在载体坐标系中进行。为了克服解析式粗对准的缺陷，保留摇摆产生的加速度变化而只滤除瞬时线运动干扰加速度，设计基座惯性系下的粗对准方法。

此方法应用惯性凝固假设，将比力信息投影到基座惯性坐标系中（此时加速度计的输出中不含由摇摆运动引起的加速度变化），得到重力加速度相对于惯性空间随着地球旋转而引起的方向变化信息，将其积分，正负干扰加速度被积分掉，求解姿态矩阵

定义一个新的坐标系：基座惯性坐标系i_b0，它是在t₀时刻将载体坐标系b经惯性凝固后得到的，其中t₀为粗对准的起始时刻。捷联矩阵可表示为：

$C_b^n=C_e^n{C}_i^e{C}_{i_{b0}}^i{C}_b^{i_{b0}}---\left(1\right)$

式中，

和

为t时刻描述i系、n系和e系之间关系的变换矩阵，可由船舶所在点地理位置及粗对准时间t确定；

为t时刻b系与i_bo系之间的变换矩阵，利用陀螺输出的b系相对i_bo系的角运动信息，通过捷联惯导姿态更新算法可以求得该矩阵；

为i_bo系与i系之间的变换矩阵，该矩阵是个常值阵，可由重力加速度与加速度计输出之间的转换关系求得，该矩阵的计算是求姿态矩阵

的关键之处。此方法将瞬时线运动摇摆基座下随时间变化的捷联姿态矩阵的求取转变成对不随时间变化的基座惯性坐标系的求取。

1）

和

的求取

在式（1）中，

是地球坐标系e和导航坐标系n间的方向余弦阵，可由载体所在地的纬度经度λ来确定。

是地球坐标系e与惯性坐标系i间的转换矩阵，可根据时间间隔Δt＝t-t₀确定。

$C_i^e=\left[\begin{array}{ccc}{\cos \mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& \sin {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& 0\\ -{\sin \mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& \cos {\mathrm{\ω}}_{\mathrm{ie}}\mathrm{\Δt}& 0\\ 0& 0& 1\end{array}\right]---\left(3\right)$

2）

的求取

是基座惯性坐标系i_b0和载体坐标系之间的变换矩阵，初始时刻

可以利用陀螺输出的b系相对i_b0系的角速率信息，通过四元素更新求解可以获得。

3）

的求取

为i系与i_b0系间的转换矩阵，该矩阵是常值矩阵。由于

都可以利用已知的信息进行计算，所以只要能得到的估计值便可以完成

的初步估算，从而完成粗对准，因此粗对准的任务由对

的求解变为对常值矩阵的求解。

在摇摆基座上，在坐标系i_b0内加速度计的输出的投影可表示如下：

${\tilde{f}}^{i_{b0}}=C_b^{i_{b0}}{\tilde{f}}^b---\left(4\right)$

式中，为加速度计的输出比力。在瞬时线运动条件下，加速度计能感测两种成分：瞬时线运动的干扰加速度和重力加速度，设

为加速度的偏差，那么加速度计的比力输出是：

${\tilde{f}}^b={-g}^b+a_D^b+{\▿}^b---\left(5\right)$

式中

是船舶纵荡、垂荡、横荡及高频干扰从而产生的干扰线运动加速度。将式(5)代入式(4)得：

${\tilde{f}}^{i_{b0}}=C_b^{i_{b0}}{\tilde{f}}^b=-C_b^{i_{b0}}{g}^b+C_b^{i_{b0}}(a_D^b+{\▿}^b)---\left(6\right)$

对式(4)两边在[t₀  t_k]区间内积分，得基座惯性系中的视速度为：

${\tilde{V}}^{i_{b0}}=\underset{t_0}{\overset{t_k}{\∫}}{C}_b^{i_{b0}}{\tilde{f}}^b\mathrm{dt}=\underset{t_0}{\overset{t_k}{\∫}}-C_b^{i_{b0}}{g}^b\mathrm{dt}+\underset{t_0}{\overset{t_k}{\∫}}{C}_b^{i_{b0}}(a_D^b+{\▿}^b)\mathrm{dt}=C_i^{i_{b0}}\underset{t_0}{\overset{t_k}{\∫}}{-g}^i\mathrm{dt}+\underset{t_0}{\overset{t_k}{\∫}}{C}_b^{i_{b0}}(a_D^b+{\▿}^b)\mathrm{dt}---\left(7\right)$

记 $V^i=\underset{t_0}{\overset{t_k}{\∫}}{-g}^i\mathrm{dt},$ $\mathrm{\δ}{V}^{i_{b0}}=\underset{t_0}{\overset{t_k}{\∫}}{C}_b^{i_{b0}}(a_D^b+{\▿}^b)\mathrm{dt},$ 由于

干扰为近似周期变化，有：

${\mathrm{\δV}}^{i_{b0}}=\underset{t_0}{\overset{t_k}{\∫}}{C}_b^{i_{b0}}(a_D^b+{\▿}^b)\mathrm{dt}\≈0---\left(8\right)$

将式(8)代入式(7)，有： ${\tilde{V}}^{i_{b0}}=C_i^{i_{b0}}{V}^i---\left(9\right)$

由于：

式中ω_ie为地球自转角速度，g为重力加速度。设Δt_k＝t_k-t₀，所以有：

由式(9)知，在t_k1、t_k2时刻(t₀＜t_k1＜t_k2)有：

${\tilde{V}}^{i_{b0}}\left(t_{k1}\right)=C_i^{i_{b0}}{V}^i\left(t_{k1}\right)---\left(12\right)$

${\tilde{V}}^{i_{b0}}\left(t_{k2}\right)=C_i^{i_{b0}}{V}^i\left(t_{k2}\right)---\left(13\right)$

可以证明：

${\tilde{V}}^{i_{b0}}\left(t_{k1}\right)\×{\tilde{V}}^{i_{b0}}\left(t_{k2}\right)=\left[C_i^{i_{b0}}{V}^i\left(t_{k1}\right)\right]\×\left[C_i^{i_{b0}}{V}^i\left(t_{k2}\right)\right]$

$\left(14\right)$

$=C_i^{i_{b0}}{V}^i\left(t_{k1}\right)\×V^i\left(t_{k2}\right)$

V_i表示地心惯性坐标系中的速度，

表示基座惯性系中的视速度，且有为求解矩阵

中的所有九个元素，需构造新的向量来增加方程数目。

在式

中，分别取t＝t_k1和t＝t_k2两个对准过程中的不同时刻，比如中间时刻和末了时刻，则有和

在利用矩阵构造算法可求得常值矩阵

$C_i^{i_{b0}}={\left[\begin{array}{c}{\left[V^i\left(t_{k1}\right)\right]}^T\\ {\left[V^i\left(t_{k2}\right)\right]}^T\\ {[V^i\left(t_{k1}\right)\×V^i\left(t_{k2}\right)]}^T\end{array}\right]}^{-1}\·\left[\begin{array}{c}{\left[{\tilde{V}}^{i_{b0}}\left(t_{k1}\right)\right]}^T\\ {\left[{\tilde{V}}^{i_{b0}}\left(t_{k2}\right)\right]}^T\\ {[{\tilde{V}}^{i_{b0}}\left(t_{k1}\right)\×{\tilde{V}}^{t_{b0}}\left(t_{k2}\right)]}^T\end{array}\right]---\left(15\right)$

由上式可以看出构造了Vⁱ(t_k1)×Vⁱ(t_k2)和

两个向量，分别是t_k1和t_k2时刻的叉乘向量，则要求Vⁱ(t_k1)和Vⁱ(t_k2)、

和

不能平行，否则义乘项无意义，所以在选择t_k1和t_k2的时候不要选太靠近的时间避免两个向量平行，一般选择的是整个对准过程的中间时刻和末了时刻。获得

后，代入式(1)，即可计算出

完成粗对准任务，姿态误差可以缩小到2度以内。

步骤三：粗对准结束后，由惯导输出的航向、纵摇和横摇信息，得到载体坐标系和半固定坐标系之间的转化关系；

惯性导航系统所得到的导航信息是基于载体坐标系的，而瞬时线运动的描述是在半固定坐标系中完成的，因此需要将其从载体坐标系投影到半固定坐标系，设粗对准结束之后得到的航向、纵摇和横摇分别γ,α,β，则半固定坐标系OX_dY_dZ_d与载体坐标系OX_bY_bZ_b之间的转动关系，可用下式表示：

$\left[\begin{array}{c}{x}_d\\ y_d\\ z_d\end{array}\right]=C_b^d\left[\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right]---\left(16\right)$

其中

为由载体坐标系转换到半固定坐标系的方向余弦矩阵，即：

$C_b^d=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& \sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}-\cos \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\γ}\sin \mathrm{\α}-\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ \sin \mathrm{\β}& -\cos \mathrm{\β}\sin \mathrm{\α}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(17\right)$

步骤四：将加速度计所测得的载体坐标系下的加速度信息投影到半固定坐标系的三个轴上，并对此信息进行积分，得到半固定坐标系下的速度信息；

步骤五：设计高通数字滤波器对信号进行处理，对半固定坐标系下的速度信息进行高通滤波，得到船舶瞬时线运动速度信号；

由步骤四所得到的速度信息中包含有瞬时线运动信息以及舒拉周期振荡分量（振荡周期是84.4min），而且船舶瞬时线运动相对于船舶的航行运动，属于高频运动，运动周期较短，一般在1.5s~10s，假设船舶瞬时线运动的周期为1.5s，频率为0.67Hz。选用船舶瞬时线运动频率的1/5（即0.13Hz）作为截止频率，设计相应的数字滤波器，对半固定坐标系下的速度信息进行滤波处理。

步骤六：以此差值作为卡尔曼滤波的量测量，列写卡尔曼滤波的状态方程和量测方程；

1)、状态方程

$\stackrel{\·}{X}\left(t\right)=A\·X\left(t\right)+B\·u\left(t\right)$

状态向量取为

状态向量中对应的变量分别为东向速度误差、北向速度误差、天向速度误差，纵摇失准角、横摇失准角、航向失准角、x轴加速度计零偏、y轴加速度计零偏、z轴加速度计零偏、x轴陀螺漂移、y轴陀螺漂移、z轴陀螺漂移。式中，a_x a_y a_z ω_x ω_y ω_z分别为加速度计和陀螺在载体坐标系下的噪声，是均值为0、方差为Q(t)、呈正态分布的白噪声。

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_e\\ \mathrm{\δ}{\stackrel{\·}{V}}_n\\ \mathrm{\δ}{\stackrel{\·}{V}}_u\\ {\stackrel{\·}{\mathrm{\φ}}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u\\ {\stackrel{\·}{\▿}}_x\\ {\stackrel{\·}{\▿}}_y\\ {\stackrel{\·}{\▿}}_u\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{cccccccccccc}{b}_{11}& b_{12}& b_{13}& 0& -f_u& f_n& C_{11}& C_{12}& C_{13}& 0& 0& 0\\ b_{21}& b_{22}& b_{23}& f_u& 0& -f_e& C_{21}& C_{22}& C_{23}& 0& 0& 0\\ b_{31}& b_{32}& b_{33}& -f_n& f_e& 0& C_{31}& C_{32}& C_{33}& 0& 0& 0\\ 0& b_{42}& b_{43}& 0& b_{45}& b_{46}& 0& 0& 0& C_{11}& C_{12}& C_{13}\\ b_{51}& 0& 0& b_{54}& 0& b_{56}& 0& 0& 0& C_{21}& C_{22}& C_{23}\\ b_{61}& 0& 0& b_{64}& b_{65}& 0& 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}\mathrm{\δ}{V}_e\\ \mathrm{\δ}{V}_n\\ \mathrm{\δ}{V}_u\\ {\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\▿}_x\\ {\▿}_y\\ {\▿}_z\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{cccccc}{C}_{11}& C_{12}& C_{13}& 0& 0& 0\\ C_{21}& C_{22}& C_{23}& 0& 0& 0\\ C_{31}& C_{32}& C_{33}& 0& 0& 0\\ 0& 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$ 其中：R为

地球半径，ω_ie为地球转速，V_n为北向速度，V_e为东向速度，V_u为天向速度，γ为航向角。

$b_{11}=\frac{V_n\tan \mathrm{\γ}}{R}$ $b_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e}{R}\tan \mathrm{\γ}$ $b_{13}=-({2\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R})$

$b_{21}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e}{R}\tan \mathrm{\γ})$ $b_{22}=-\frac{V_u}{R}$ $b_{23}=-\frac{V_n}{R}$

$b_{31}=2({\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R})$ $b_{32}=\frac{{2V}_n}{R}$ b₃₃＝-2V_eω_iesinγ

$b_{42}=\frac{1}{R}$ $b_{42}=-\frac{\tan \mathrm{\γ}}{R}$ $b_{45}={\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e\tan \mathrm{\γ}}{R}$

$b_{46}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R})$ $b_{51}=\frac{1}{R}$ $b_{54}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e\tan \mathrm{\γ}}{R})$

$b_{56}=-\frac{V_n}{R}$ $b_{61}=\frac{\tan \mathrm{\γ}}{R}$ $b_{64}={\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R}$ $b_{65}=\frac{V_n}{R}$

f_e为加速度计所测比力分量在地理坐标中东向的投影分量；

f_n为加速度计所测比力分量在地理坐标中北向的投影分量；

f_u为加速度计所测比力分量在地理坐标中天向的投影分量；

$\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$ 为载体坐标系到地理坐标系的转换矩阵

2)、量测方程

将半固定坐标系中的瞬时线速度为速度基准，以惯导解算出来的速度在半固定坐标系中的投影与速度基准之差作为量测量，系统量测方程可以写为：

Z(t)＝H(t)X(t)+v(t)

$H\left(t\right)=\left[\begin{array}{cccccccccccc}{T}_{11}& T_{12}& T_{13}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ T_{21}& T_{22}& T_{23}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ T_{31}& T_{32}& T_{33}& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

$\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$ 为从地理坐标系到半固定坐标系的转换矩阵。

其中，量测噪声v(t)，其方差为R(t)。

步骤七：将卡尔曼滤波的状态方程和量测方程离散化，完成对准。

将状态方程和量测方程离散化，

X(k+1)＝Φ(k,k-1)X(k)+Γ_k，k-1(t)W(k)            （18）

Z(k)＝H·X(k)+V(k)

式中，

Φ(k+1,k)＝I+A(t)Δt+0(Δt²)

${\mathrm{\Γ}}_{k+1,k}={\∫}_k^{k+1}\mathrm{\Φ}(k+1,k)G\left(\mathrm{\τ}\right)\mathrm{dt}$

$\stackrel{\‾}{W}\left(k\right)=\stackrel{\‾}{W}\left(t_k\right)$

$\stackrel{\‾}{V}\left(k\right)=\stackrel{\‾}{V}\left(t_k\right)$

应用标准卡尔曼滤波方程：

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k/k-1}{P}_{k-1}{\mathrm{\Φ}}_{k/k-1}^T+Q\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R)}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}^{*}{K}_k^T\end{array}\right.---\left(19\right).$

Claims (3)

1.一种适用于摇摆晃动基座的初始对准方法，其特征是：

步骤一：在摇摆加晃动干扰基座条件下，实时地同步采集三轴陀螺的角速率信息和三轴加速度计的比力信息；

步骤二：进行粗对准；

步骤三：粗对准结束后，由惯导输出的航向、纵摇和横摇信息，得到载体坐标系和半固定坐标系之间的转化关系；

步骤四：将载体坐标系下的加速度信息投影到半固定坐标系下的三个轴上，并进行积分，得到半固定坐标系下速度的相关信息；

步骤五：用高通数字滤波器对信号进行处理，得到相对于舒勒周期的高频信号，即船舶瞬时线运动速度信号；

步骤六：将已提取的船舶瞬时线运动速度信息作为速度基准，与惯导解算出的速度相减作差，以所得差值作为卡尔曼滤波的量测量，列写卡尔曼滤波的状态方程和量测方程；

步骤七：将卡尔曼滤波进行离散化，完成对准。

2.根据权利要求1所述的适用于摇摆晃动基座的初始对准方法，其特征是：所述粗对准是应用惯性凝固假设，将比力信息投影到基座惯性坐标系中，得到重力加速度相对于惯性空间随着地球旋转而引起的方向变化信息，将其积分，正负干扰加速度被积分掉，求解姿态矩阵

3.根据权利要求1或2所述的适用于摇摆晃动基座的初始对准方法，其特征是所述卡尔曼滤波的状态方程和量测方程为

1)、状态方程

$\stackrel{\·}{X}\left(t\right)=A\·X\left(t\right)+B\·u\left(t\right)$

状态向量取为状态向量中对应的变量分别为东向速度误差、北向速度误差、天向速度误差，纵摇失准角、横摇失准角、航向失准角、x轴加速度计零偏、y轴加速度计零偏、z轴加速度计零偏、x轴陀螺漂移、y轴陀螺漂移、z轴陀螺漂移；式中，a_x a_y a_z ω_x ω_y ω_z分别为加速度计和陀螺在载体坐标系下的噪声，是均值为0、方差为Q(t)、呈正态分布的白噪声；

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_e\\ \mathrm{\δ}{\stackrel{\·}{V}}_n\\ \mathrm{\δ}{\stackrel{\·}{V}}_u\\ {\stackrel{\·}{\mathrm{\φ}}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u\\ {\stackrel{\·}{\▿}}_x\\ {\stackrel{\·}{\▿}}_y\\ {\stackrel{\·}{\▿}}_u\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{cccccccccccc}{b}_{11}& b_{12}& b_{13}& 0& -f_u& f_n& C_{11}& C_{12}& C_{13}& 0& 0& 0\\ b_{21}& b_{22}& b_{23}& f_u& 0& -f_e& C_{21}& C_{22}& C_{23}& 0& 0& 0\\ b_{31}& b_{32}& b_{33}& -f_n& f_e& 0& C_{31}& C_{32}& C_{33}& 0& 0& 0\\ 0& b_{42}& b_{43}& 0& b_{45}& b_{46}& 0& 0& 0& C_{11}& C_{12}& C_{13}\\ b_{51}& 0& 0& b_{54}& 0& b_{56}& 0& 0& 0& C_{21}& C_{22}& C_{23}\\ b_{61}& 0& 0& b_{64}& b_{65}& 0& 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}\mathrm{\δ}{V}_e\\ \mathrm{\δ}{V}_n\\ \mathrm{\δ}{V}_u\\ {\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\▿}_x\\ {\▿}_y\\ {\▿}_z\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{cccccc}{C}_{11}& C_{12}& C_{13}& 0& 0& 0\\ C_{21}& C_{22}& C_{23}& 0& 0& 0\\ C_{31}& C_{32}& C_{33}& 0& 0& 0\\ 0& 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]\·\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\\ {\mathrm{\ω}}_x\\ {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z\\ 0\\ 0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$

其中：R为地球半径，ω_ie为地球转速，V_n为北向速度，V_e为东向速度，V_u为天向速度，γ为航向角；

$b_{11}=\frac{V_n\tan \mathrm{\γ}}{R}$ $b_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e}{R}\tan \mathrm{\γ}$ $b_{13}=-({2\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R})$

$b_{21}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e}{R}\tan \mathrm{\γ})$ $b_{22}=-\frac{V_u}{R}$ $b_{23}=-\frac{V_n}{R}$

$b_{31}=2({\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R})$ $b_{32}=\frac{{2V}_n}{R}$ b₃₃＝-2V_eω_iesinγ

$b_{42}=\frac{1}{R}$ $b_{42}=-\frac{\tan \mathrm{\γ}}{R}$ $b_{45}={\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e\tan \mathrm{\γ}}{R}$

$b_{46}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R})$ $b_{51}=\frac{1}{R}$ $b_{54}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{\γ}+\frac{V_e\tan \mathrm{\γ}}{R})$

$b_{56}=-\frac{V_n}{R}$ $b_{61}=\frac{\tan \mathrm{\γ}}{R}$ $b_{64}={\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{\γ}+\frac{V_e}{R}$ $b_{65}=\frac{V_n}{R}$

f_e为加速度计所测比力分量在地理坐标中东向的投影分量；

f_n为加速度计所测比力分量在地理坐标中北向的投影分量；

f_u为加速度计所测比力分量在地理坐标中天向的投影分量；

$\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$ 为载体坐标系到地理坐标系的转换矩阵；

2)、量测方程

将半固定坐标系中的瞬时线速度为速度基准，以惯导解算出来的速度在半固定坐标系中的投影与速度基准之差作为量测量，系统量测方程为：

Z(t)＝H(t)X(t)+v(t)

$H\left(t\right)=\left[\begin{array}{cccccccccccc}{T}_{11}& T_{12}& T_{13}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ T_{21}& T_{22}& T_{23}& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ T_{31}& T_{32}& T_{33}& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

$\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

为从地理坐标系到半固定坐标系的转换矩阵；

其中，量测噪声v(t)，其方差为R(t)。

Images