一种基于速度加比力匹配的传递对准时间延迟估计方法

摘要

本发明公开一种基于速度加比力匹配的传递对准时间延迟估计算法。其实现方法是：利用已对准的主惯导对子惯导进行粗对准，再利用子惯导相对主惯导的速度差、比力差作为滤波观测量，并将主惯导信息的时间延迟量扩充为滤波系统状态变量，结合惯导系统的误差模型，利用卡尔曼滤波算法估计出时间延迟以及子惯导相对于主惯导的平台误差角。本发明适用于主、子惯导均为平台式惯导系统时，延迟时间较短的速度加比力匹配传递对准。

说明书

技术领域

本发明涉及一种信息时间延迟的估计方法，特别是一种惯导系统传递对准中 对主惯导匹配信息存在的时间延迟的估计方法。

背景技术

传递对准是在动态条件下通过对载体主惯导（已对准）的输出信息与安装在 载体上的子惯导（未对准）的输出信息进行匹配，估计出子惯导的失准角，从而 完成子惯导初始对准的一种方法。对于平台惯导系统，由于传递对准滤波算法的 实现是在子惯导系统中进行的，这就需要将主惯导系统的一些相关信息传输到子 惯导系统，由于时间延迟的存在，导致子惯导接收到的导航信息跟主惯导传递时 的导航信息有一定误差，子惯导以接收到的信息作为基准进行传递对准，就会影 响对于平台误差角的估计精度。

在2005年2月出版的《中国惯性技术学报》第13卷第1期中，文献《传递 对准中测量延迟的补偿方法》采用将时间延迟扩展为状态变量的方法进行实时估 计，但是只针对速度加姿态传递对准进行了分析，由于时间延迟对姿态等角运动 信息的影响更大，因此采用速度加比力信息进行卡尔曼滤波，可以获得更好的时 间延迟估计效果。

发明内容

本发明的目的在于提供一种针对较小的信息延迟时间误差，估计速度更快、 精度更高的基于速度加比力匹配的传递对准时间延迟估计方法。

本发明的目的是通过一以下步骤实现的：

(1)子惯导预热，并利用已对准的平台式主惯导系统对子惯导系统进行一步 传递对准，完成子惯导系统的粗对准；

(2)主、子惯导进行导航解算，并采集主惯导速度、比力信息传输至子惯导 解算计算机；

(3)在子惯导计算机中，构造主、子惯导速度差、比力差进行标准离散卡尔 曼滤波的迭代解算；

其中，主、子惯导均为平台式惯导系统，所采用的子惯导误差模型为：

$\left(\begin{array}{c}\delta {\stackrel{·}{v}}^n=f^n\times {\phi }^n-(2{\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)\times \mathrm{\delta v}+{▿}^n\\ \delta {\stackrel{·}{\phi }}^n=-{\omega }_{\mathrm{in}}^n\times {\phi }^n+\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n+{ϵ}^n\\ \delta {\stackrel{·}{▿}}^n=0\\ \delta {\stackrel{·}{ϵ}}^n=0\\ \Delta \stackrel{·}{t}=0\end{array}\right)$

其中，n为当地水平坐标系，δvⁿ为子惯导速度误差在n系的投影；φⁿ为主惯导 与子惯导系统之间的平台误差角；fⁿ为子惯导比力输出在n系的投影；为地 球自转角速率在导航系n的投影，为导航系相对地球系的旋转角速率在n系 的投影；为子惯导加速度计零偏在n系的投影；为相对理想值的误差， 且ε^s为子惯导陀螺漂移；Δt表示延迟时间。

所选取的系统状态变量为：

$X={\left(\begin{array}{ccccccccccc}\delta v_x& \delta v_y& {\phi }_x& {\phi }_y& {\phi }_z& {▿}_x& {▿}_y& {ϵ}_x& {ϵ}_y& {ϵ}_z& \mathrm{\Delta t}\end{array}\right)}^T$

系统状态方程为：$\stackrel{·}{X}=\mathrm{AX}+\mathrm{BW}$

其中

$A=\left(\begin{array}{ccccc}{A}_{11}& A_{12}& I_{2\times 3}& 0_{2\times 3}& 0\\ A_{21}& A_{22}& 0_{3\times 3}& I_{3\times 3}& 0\\ 0_{2\times 2}& 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 3}& 0\\ 0_{4\times 3}& 0_{4\times 3}& 0_{4\times 3}& 0_{4\times 3}& 0\end{array}\right)$

$B=\left(\begin{array}{ccc}{I}_{2\times 2}& 0_{2\times 3}& 0_{2\times 6}\\ 0_{3\times 2}& I_{3\times 3}& 0_{3\times 6}\\ 0_{6\times 2}& 0_{6\times 3}& 0_{6\times 6}\end{array}\right)$

式中

$A_{12}=\left(\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\end{array}\right)$

$A_{22}=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{inu}}^n& {-\omega }_{\mathrm{inn}}^n\\ {-\omega }_{\mathrm{inu}}^n& 0& {\omega }_{\mathrm{ine}}^n\\ {\omega }_{\mathrm{inn}}^n& {-\omega }_{\mathrm{ine}}^n& 0\end{array}\right).$

系统噪声阵为：

W＝[w_ax w_ay w_εx w_εy w_εz 0 0 0 0 0 0]^T

其中，为纬度；R_e为地球半径；f_x、f_y、f_z分别为fⁿ在东向、北向、天向的 分量；w_ax、w_ay为加速度计零偏随机白噪声，w_εx、w_εy、w_εz为陀螺仪漂移随机 白噪声。

系统观测量为：

Z＝[δv_x δv_y δf_x δf_y]^T

观测方程为：

Z=HX+V

$H\left(\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-f}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right)$

其中，$\mathrm{Dv}\left(t\right)={\stackrel{·}{V}}_m^n\left(t\right),$$\mathrm{Da}\left(t\right)={\stackrel{·}{f}}_m^n\left(t\right),$且有${\stackrel{·}{V}}_m^n(t-\mathrm{\Delta t})={\stackrel{·}{V}}_m^n\left(t\right),$${\stackrel{·}{f}}_m^n(t-\mathrm{\Delta t})={\stackrel{·}{f}}_m^n\left(t\right).$

(4)滤波结束后，得到延迟时间估计值以及时间延迟补偿后的平台误差角。

本传递对准时间延迟估计方法具有良好的时间延迟估计效果，可较好地实时 跟踪时间延迟。步骤简单、实施方便，适用于对传递对准中主惯导信息的传输延 迟误差进行补偿。在估计得到主、子惯导之间信息的传输延迟时间后，可以有效 地提升主、子惯导平台误差角φⁿ的估计精度，实现子惯导快速、有效的传递对 准。

附图说明

图1是该种传递对准时间延迟估计方法的流程图。图2是船体做“S”形机 动下，用速度加比力匹配方案进行传递对准，考虑时间延迟补偿前和补偿后，仿 真得到的平台误差角估计曲线。图3是该算法对于延迟时间的实时估计曲线。

具体实施方式：

一种基于速度加比力匹配的传递对准时间延迟估计方法，该方法由以下步骤 实现：

(1)子惯导预热，并利用已对准的平台式主惯导系统对子惯导系统进行一步 传递对准，完成子惯导系统的粗对准；该步骤中还包括了对子惯导速度、位置等 其余导航信息的初始化，使子惯导系统可以开始独立解算并输出导航信息。

(2)主、子惯导进行导航解算，并采集主惯导速度、比力信息传输至子惯导 解算计算机；

(3)在子惯导计算机中，构造主、子惯导速度差、比力差进行标准离散卡尔 曼滤波的迭代解算；

该步骤主要包括4部分：1)惯导误差方程的建立;2)系统状态方程的建立;3) 观测方程的建立;4)离散卡尔曼滤波迭代解算。

设平台式惯导系统的平台要求模拟的导航坐标系n为地理坐标系，实际建立 的平台坐标系为n′。由于计算误差、施矩误差及信息源误差的影响，n′系相对n 系有偏差角φⁿ。

1)根据平台惯导系统的力学编排，建立惯导的误差模型。其误差方程为：

$\left(\begin{array}{c}\delta {\stackrel{·}{v}}^n=f^n\times {\phi }^n-(2{\omega }_{\mathrm{ie}}^n+{\omega }_{\mathrm{en}}^n)\times \mathrm{\delta v}+{▿}^n\\ \delta {\stackrel{·}{\phi }}^n=-{\omega }_{\mathrm{in}}^n\times {\phi }^n+\delta {\omega }_{\mathrm{ie}}^n+\delta {\omega }_{\mathrm{en}}^n+{ϵ}^n\\ \delta {\stackrel{·}{▿}}^n=0\\ \delta {\stackrel{·}{ϵ}}^n=0\\ \Delta \stackrel{·}{t}=0\end{array}\right)$

其中，δvⁿ为子惯导速度误差在n系的投影；φⁿ为主惯导与子惯导系统之间的平 台误差角；fⁿ为子惯导比力输出在n系的投影；为地球自转角速率在导航系n 的投影，为导航系相对地球系的旋转角速率在n系的投影；为子惯导加速 度计零偏在n系的投影；为相对理想值的误差，且ε^s为 子惯导陀螺漂移；Δt表示延迟时间，通常将主惯导信息延迟时间设为一个在 0～0.1s之间变化的随机常数。

2)建立系统的状态方程。结合上述惯导误差方程，忽略天向通道速度、信 息，并将延迟时间扩充为状态变量，得到系统状态量为：

$X={\left(\begin{array}{ccccccccccc}\delta v_x& \delta v_y& {\phi }_x& {\phi }_y& {\phi }_z& {▿}_x& {▿}_y& {ϵ}_x& {ϵ}_y& {ϵ}_z& \mathrm{\Delta t}\end{array}\right)}^T$

则系统状态方程为：$\stackrel{·}{X}=\mathrm{AX}+\mathrm{BW}$

其中

$A=\left(\begin{array}{ccccc}{A}_{11}& A_{12}& I_{2\times 3}& 0_{2\times 3}& 0\\ A_{21}& A_{22}& 0_{3\times 3}& I_{3\times 3}& 0\\ 0_{2\times 2}& 0_{2\times 3}& 0_{2\times 3}& 0_{2\times 3}& 0\\ 0_{4\times 3}& 0_{4\times 3}& 0_{4\times 3}& 0_{4\times 3}& 0\end{array}\right)$

$B=\left(\begin{array}{ccc}{I}_{2\times 2}& 0_{2\times 3}& 0_{2\times 6}\\ 0_{3\times 2}& I_{3\times 3}& 0_{3\times 6}\\ 0_{6\times 2}& 0_{6\times 3}& 0_{6\times 6}\end{array}\right)$

式中

$A_{12}=\left(\begin{array}{ccc}0& {-f}_z& f_y\\ f_z& 0& {-f}_x\end{array}\right)$

$A_{22}=\left(\begin{array}{ccc}0& {\omega }_{\mathrm{inu}}^n& {-\omega }_{\mathrm{inn}}^n\\ {-\omega }_{\mathrm{inu}}^n& 0& {\omega }_{\mathrm{ine}}^n\\ {\omega }_{\mathrm{inn}}^n& {-\omega }_{\mathrm{ine}}^n& 0\end{array}\right).$

系统噪声阵为：

W＝[w_ax w_ay w_εx w_εy w_εz 0 0 0 0 0 0]^T

其中，为纬度；R_e为地球半径；f_x、f_y、f_z分别为fⁿ在东向、北向、天向的 分量；w_ax、w_ay为加速度计零偏随机白噪声，w_εx、w_εy、w_εz为陀螺仪漂移随机 白噪声。

3)建立速度加比力匹配方法的系统观测方程。根据所建立的系统状态方程， 选择速度加比力作为系统观测量，则有：

Z＝[δv_x δv_y δf_x δf_y]^T

假设子惯导系统（SINS）在t时刻获得的主惯导的速度和比力是主惯导在 t-Δt时刻的数据。主子惯导间的速度差和比力差表示如下：

$\mathrm{\Delta V}=V_s^n\left(t\right)-V_m^n\left(t\right)$

$=V_s^n\left(t\right)-(V_m^n(t-\mathrm{\Delta t})+{\stackrel{·}{V}}_m^n(t-\mathrm{\Delta t})·\mathrm{\Delta t}$

同理，

$\mathrm{\Delta f}=f_s^n\left(t\right)-f_m^n\left(t\right)$

$=f_s^n\left(t\right)-(f_m^n(t-\mathrm{\Delta t})+{\stackrel{·}{f}}_m^n(t-\mathrm{\Delta t})·\mathrm{\Delta t})$

由于Δt选取的时间很短，默认为在延迟时间Δt内，速度的变化率和比力 的变化率在计算的周期内近似不变，即${\stackrel{·}{V}}_m^n(t-\mathrm{\Delta t})={\stackrel{·}{V}}_m^n\left(t\right),$${\stackrel{·}{f}}_m^n(t-\mathrm{\Delta t})={\stackrel{·}{f}}_m^n\left(t\right).$

令$\mathrm{Dv}\left(t\right)={\stackrel{·}{V}}_m^n\left(t\right),$$\mathrm{Da}\left(t\right)={\stackrel{·}{f}}_m^n\left(t\right),$则有：

$\mathrm{\Delta V}+\mathrm{Dv}\left(t\right)=V_s^n\left(t\right)-V_m^n(t-\mathrm{\Delta t})$

$\mathrm{\Delta f}+\mathrm{Da}\left(t\right)=f_s^n\left(t\right)-f_m^n(t-\mathrm{\Delta t})$

则可得速度加比力匹配的观测方程为：

Z=HX+V

$H=\left(\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(1\right)\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& -\mathrm{Dv}\left(2\right)\\ 0& 0& 0& {-f}_z& f_y& 1& 0& 0& 0& 0& -\mathrm{Da}\left(1\right)\\ 0& 0& f_z& 0& {-f}_x& 0& 1& 0& 0& 0& -\mathrm{Da}\left(2\right)\end{array}\right)$

4)依据系统的状态方程及观测方程，结合主、子惯导的速度与比力信息进 行离散卡尔曼滤波迭代解算。

(4)卡尔曼滤波迭代计算结束后，得到延迟时间的估计值以及时间延迟补偿 后的平台误差角。

本发明提供的基于速度加比力匹配的时间延迟估计方法具有以下优点：

一是本方法只需建立延迟时间和观测量的联系，将延迟时间扩充为状态量， 且仅影响观测方程，实施较为简单，计算量少；二是本方法可以在滤波过程中实 时的补偿时间延迟对于速度加比力匹配算法造成的影响；三是相比于现有的速度 加姿态匹配时间延迟补偿方法，本方法采用的速度加比力匹配信息均为线运动信 息，受时间延迟的作用较弱，因此能很好的适应动态环境，对时间延迟的测量以 及姿态失准角的估计有很好的效果，满足了舰船在实际航行条件下传递对准中对 主惯导时间延迟的实时估计并进行补偿的要求。

为了进一步说明所述方法的有益效果，在以下初始条件下对传递对准的估计 以及估计误差进行了仿真，仿真结果如图2、图3所示，并对其进行了分析比较。

初始条件：

1)假设主惯导无误差，杆臂效应误差已得到充分补偿。

2）载体初始位置：经度117°，纬度39°。载体初始姿态角（纵摇，横摇，航 向）分别为：0°,0°,45°；子惯导平台初始误差角为ψ_x=5′,ψ_y=5′,ψ_z=5′。

3）子惯导陀螺仪常值漂移均为0.01(°)/h，随机漂移噪声均为0.001(°)/h； 加速度计常值零偏均为1×10^-4g，随机零偏噪声均为1×10^-5g。

4）状态估计初始值均为0；初始方差阵P₀参数根据上述惯性器件误差进行设 置，保证卡尔曼滤估计的最优性。

P₀＝diag{(0.1m/s)² (0.1m/s)² (5′)²(5′)²(5′)² (1×10^-4g)²(1×10^-4g)²(1°×10^-3)² (1°×10^-3)² (1°×10^-3)² 0.05}

5）载体做“S”形机动，最初以10m/s的速度匀速直线运动，“S”形机动转 弯角速度为1(°)/s，机动历时100s。在仿真的过程中，为了考虑实际过程中存在 的传播延迟现象，利用随机产生函数将主惯导的数据(这里主要是主惯导的速度 和比力信息)加上一个延迟为0～0.1s之间均匀分布的延迟时间，延迟时间的均值为 0.05s，然后传给子惯导，与子惯导的数据进行匹配，从而完成传递对准的过程。

分析比较：

图2给出船舶在“S”形机动下利用速度加比力匹配传递对准算法对平台误 差角的误差估计曲线，蓝色实线为未对时间延迟进行估计的误差估计曲线，绿色 虚线为用本方法对时间延迟进行估计后的误差估计曲线。对比可知，当船舶做“S” 机动时，时间延迟对速度加比力匹配传递对准算法的影响很大，在做机动时不能 估计出平台误差角。而本方法可以有效地补偿时间延迟对于速度加比力传递对准 算法的影响，满足传递对准算法的高精度和快速性。从图3可以看出，本方法可 以实时的估计出延迟时间，为补偿时间延迟对速度加比力匹配传递对准算法的影 响奠定坚实的基础，由于延迟时间为0～0.1s，为了清晰地看到估计曲线，取前 20s对延迟时间的估计曲线。