一种基于惯性系的最优两位置对准方法

摘要

本发明公开了一种基于惯性系的最优两位置对准方法。包括以下几个步骤：以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量，建立状态方程；以惯性系下SINS结算的速度和GPS测得的惯性系下的速度的差值作为量测量，建立量测方程；将状态方程和量测方程构成两位置对准的卡尔曼滤波模型；根据得到的卡尔曼滤波模型，对状态量进行估计；在设定的对准时间，分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°；再利用卡尔曼滤波器估计出的失准角精确估计值

说明书

技术领域

本发明属于一种捷联惯导系统初始对准方法，尤其涉及针对两位置的一种基于惯性系的 最优两位置对准方法。

背景技术

捷联惯导系统(strapdown inertial navigation system,SINS)以其特有的优点，广泛 应用在飞机、舰船、导弹等装备上。初始对准是SINS关键技术之一，对准的精度直接影响到 其工作精度。目前，对于舰船SINS而言，在没有外观测信息的情况下，其对准方法以解析 式对准和罗经对准为主；在有外观测信息的情况下，以卡尔曼滤波对准为主；并且这些方法 都是基于地理坐标系。

随着初始对准技术的发展，惯性坐标系对准(包括粗对准和精对准)以其特点成为初始 对准的一个新的研究方向。在理论研究方面，认为惯性系对准可以有效的隔离载体角速度和 线运动干扰的影响；研究表明，将重力加速度放在惯性系内分析，并结合外观测信息进行卡 尔曼滤波对准，其航向的对准精度要优于在地理系下的对准精度。

发明内容

本发明的目的是提供一种具有高初始对准精度的，基于惯性系的最优两位置对准方法。

本发明是通过以下技术方案实现的：

一种基于惯性系的最优两位置对准方法，包括以下几个步骤：

步骤一：以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量，建立状态方程；以惯 性系下SINS结算的速度和GPS测得的惯性系下的速度的差值作为量测量，建立量测方程； 将状态方程和量测方程构成两位置对准的卡尔曼滤波模型；

步骤二：根据得到的卡尔曼滤波模型，对状态量进行估计；

步骤三：在设定的对准时间，分别将IMU绕纵摇轴、横摇轴和航向轴旋转180°，使SINS 系统为完全可观测；

步骤四：再利用卡尔曼滤波器估计出的失准角精确估计值对当前时刻载体系到计算惯性 系的转换矩阵进行修正，得到载体系到惯性系的转换矩阵

步骤五：利用得到的转换矩阵结合惯性系到导航系的转换矩阵求解载体系到惯性系的 转换矩阵实现基于惯性系的两位置对准。

本发明一种基于惯性系的最优两位置对准方法，还可以包括：

状态方程为：

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

其中X(t)为t时刻捷联惯导系统的状态变量，A(t)和B(t)分别为捷联惯导系统的状态矩 阵和噪声驱动矩阵，W(t)为捷联惯导系统噪声序列，

以速度误差、失准角、陀螺常值漂移和加速度零偏为状态变量：

其中，失准角为：

为惯性坐标系相对于理想坐标系的失准角，为t时刻载体坐标系到惯性坐标系的真 实方向余弦阵，为导航解算得到的姿态阵，为Gauss白噪声，陀螺漂移ε^b包括陀螺常 值漂移和陀螺随机漂移

速度误差为：

式中δVⁱ为计算速度与GPS测得速度的差值，即δVⁱ＝V^i′-Vⁱ，gⁱ为地球重力加速度在惯 性系的投影，为Gauss白噪声，加速度计偏差▽^b由加速度计零位误差和随机偏差构 成；

以惯性系下SINS结算的速度和GPS测得的惯性系下的速度的差值作为量测量，建立量 测方程：

$Z\left(t\right)=\left(\begin{array}{c}{V}_x^{i^{\prime }}-V_x^i\\ V_y^{i^{\prime }}-V_y^i\\ V_z^{i^{\prime }}-V_z^i\end{array}\right)=H\left(t\right)X\left(t\right)+{\eta }_w\left(t\right)$

式中Z(t)为t时刻的观测值，η_w(t)为观测噪声序列，H(t)为观测阵， H(t)＝[I3_×3 0_3×3 0_3×3 0_3×3]。

有益效果：

惯性系下最优两位置对准是在第一次滤波达到稳定后，通过将IMU绕航向轴旋转来实现 的，本发明可以使原来不完全可观测的系统变得完全可观测，从而有效的提高了初始对准的 精度。

附图说明

图1为本发明惯性系下卡尔曼滤波对准的流程图。

图2为本发明惯性系下固定位置和两位置对准的横摇误差角的仿真曲线。

图3为本发明惯性系下固定位置和两位置对准的纵摇误差角的仿真曲线。

图4为本发明惯性系下固定位置和两位置对准的航向误差角的仿真曲线。

具体实施方式

下面将结合附图对本发明做进一步详细说明。

如图1所示，本发明包括以下几个步骤

步骤1：以速度误差、失准角、陀螺常值漂移和加速度零偏为状态量，建立状态方程；以 惯性系下SINS解算的速度和GPS测得的惯性系下的速度的差值作为量测量，建立量测方程； 状态方程和量测方程构成两位置对准的卡尔曼滤波模型；

(1)失准角方程

式中为惯性坐标系相对于理想坐标系的失准角；为t时刻载体坐标系到惯性坐标系的真 实方向余弦阵；为导航解算得到的姿态阵；假设为Gauss白噪声，陀螺漂移ε^b包括陀 螺常值漂移和陀螺随机漂移

(2)速度误差方程

式中δVⁱ为计算速度与GPS测得速度的差值，即δVⁱ＝V^i′-Vⁱ；gⁱ为地球重力加速度在惯性系 的投影；假设为Gauss白噪声，加速度计偏差▽^b由加速度计零位误差和随机偏差构 成。

式(1)、(2)便构成惯性系下捷联惯导系统静基座的误差方程。

(3)状态方程

根据惯性系下SINS静基座的失准角方程和速度误差方程，便可建立对应的状态空间模型。

状态方程的形式为：

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

式中X(t)为t时刻系统的状态变量；A(t)和B(t)分别为系统的状态矩阵和噪声驱动矩阵； W(t)为系统噪声序列。

根据误差模型，将陀螺常值漂移和加速度计常值偏差均扩充到状态变量中，构建状 态变量：

状态矩阵A(t)为：

$A\left(t\right)=\left(\begin{array}{cc}{A}_1{\left(t\right)}_{6\times 6}& A_2{\left(t\right)}_{6\times 6}\\ 0_{6\times 6}& 0_{6\times 6}\end{array}\right)$

其中

$\left(\begin{array}{c}{A}_1\left(t\right)=\left(\begin{array}{cc}{0}_{3\times 3}& A_{12}\left(t\right)\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right)\\ A_2\left(t\right)=\begin{array}{}\end{array}\left(\begin{array}{cccccc}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}\\ {-C}_{11}& {-C}_{12}& {-C}_{13}& 0& 0& 0\\ {-C}_{21}& -C{22}_{}& {-C}_{23}& 0& 0& 0\\ {-C}_{31}& -C{31}_{}& {-C}_{33}& 0& 0& 0\end{array}\right)\end{array}\right)$

A₂(t)中C_ks为中对应的各元素，A₁(t)中,$A_{12}\left(t\right)=\left(\begin{array}{ccc}0& f_z^i& {-f}_y^i\\ {-f}_z^i& 0& f_x^i\\ f_y^i& {-f}_x^i& 0\end{array}\right).$

噪声驱动阵B(t)为：

$B\left(t\right)=\left(\begin{array}{cc}{B}_1{\left(t\right)}_{6\times 6}& 0_{6\times 6}\\ 0_{6\times 6}& 0_{6\times 6}\end{array}\right)$

其中

$B_1\left(t\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}\end{array}\right)$

噪声序列W(t)为：

$W\left(t\right)={\left(\begin{array}{cccc}{ϵ}_w^b& {▿}_w^b& 0_{1\times 3}& 0_{1\times 3}\end{array}\right)}^T$

其中为载体坐标系的陀螺随机漂移，为Gauss白噪声过程；为载体坐标系的加速度计随 机偏差，为Gauss白噪声过程。

(4)量测方程

取惯性系下计算地速V^i′和理想地速Vⁱ的差值作为观测量，其中V^i′为惯性系下SINS解算 的载体速度，Vⁱ为GPS测得的载体在惯性系下的速度，可得惯性系下卡尔曼滤波的观测方程 为：

$Z\left(t\right)=\left(\begin{array}{c}{V}_x^{i^{\prime }}-V_x^i\\ V_y^{i^{\prime }}-V_y^i\\ V_z^{i^{\prime }}-V_z^i\end{array}\right)=H\left(t\right)X\left(t\right)+{\eta }_w\left(t\right)---\left(4\right)$

式中Z(t)为t时刻的观测值；η_w(t)为观测噪声序列；H(t)为观测阵，其具体表达式为：

H(t)＝[I_3×3 0_3×3 0_3×3 0_3×3]。

步骤2：利用步骤1得到的卡尔曼滤波模型，根据卡尔曼滤波基本方程对状态量进行估计；

(1)对连续的滤波模型进行离散化处理，可得离散卡尔曼滤波器的状态方程和量测方程：

$\left(\begin{array}{c}{X}_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}\right)$

式中X_k为k时刻状态量的估计值；Φ_k,k-1表示k-1时刻至k时刻的一步转移矩阵；Γ_k-1表 示k-1时刻的系统噪声驱动阵；W_k-1表示k-1时刻的系统激励噪声序列；H_k表示k时刻的量 测阵；为k时刻的量测噪声序列。

(2)设k时刻的量测值为Z_k，根据卡尔曼滤波基本方程，状态X_k的估计可按下述过 程求解：

状态一步预测：${\hat{X}}_{k,k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$

状态估计：${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k(Z_k-H_k{\hat{X}}_{k,k-1})$

滤波增益：$K_k=P_{k.k-1}{H}_k^T{\left(H_k{P}_{k,k-1}{H_k^T+P}_k\right)}^{-1}$

一步预测均方差：$P_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+Q_{k-1}$

估计均方误差：$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^{-1}+K_k{R}_k{K}_k^T$

式中K_k为k时刻的增益矩阵；P_k,k-1表示k-1时刻至k时刻的一步预测估计方差阵；P_k-1表 示k-1时刻的状态估计方差；Q_k为系统噪声序列的方差阵，非负定；R_k为量测噪声序列的 方差阵，正定。

步骤3：在设定的对准时间达到中间某一时段时(第一次达到稳定后)，分别将IMU绕纵 摇轴、横摇轴和航向轴旋转180°，检验系统的可观测性；

惯性系下PWCS(piece-wise constant system)可观测性定理：如果其中1≤j≤r，则

Rank(U)＝Rank(U_S)

其中，A_j为第j个时间段的状态矩阵，U为TOM(total observability matrix)，U_S为 SOM(stripped observability matrix)。

由于则对U_j进行矩阵的行初等变换，可以得到简 化的U_j如下：

${\bar{U}}_j=\left(\begin{array}{cccc}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& -(g^i\times )& 0_{3\times 3}& C_b^i\\ 0_{3\times 3}& 0_{3\times 3}& {-C}_b^i& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right)---\left(5\right)$

对于两位置对准，取惯性系误差模型中(A_j,H)对，定义U(α₁,α₂)为TOM，α₁，α₂分别表 示第一个位置和第二个位置的欧拉角。同理，根据定理可以利用简化的代替 TOMU(α₁,α₂)进行两位置对准可观测性分析，这里

${\bar{U}}_S({\alpha }_1,{\alpha }_2)=\left(\begin{array}{c}{\bar{U}}_1\left({\alpha }_1\right)\\ {\bar{U}}_2\left({\alpha }_2\right)\end{array}\right)---\left(6\right)$

式中，当欧拉角为α_j时，可由式(5)确定。

对准过程中，滤波器第一次达到稳定后分别将IMU绕纵摇轴、横摇轴和航向轴旋转180° 有如下结论：

$\left(\begin{array}{cc}\mathrm{Rank}\left(U({\theta }_1,{\theta }_2)\right)=12,& {\theta }_2\ne 2\mathrm{k\pi }+{\theta }_1\\ \mathrm{Rank}\left(U({\phi }_1,{\phi }_2)\right)=12,& {\phi }_2\ne 2\mathrm{k\pi }+{\phi }_1\\ \mathrm{Rank}\left(U({\psi }_1,{\psi }_2)\right)=12,& {\psi }_2\ne 2\mathrm{k\pi }+{\psi }_1\end{array}\right)---\left(7\right)$

式中，θ,φ,ψ分别表示纵摇角、横摇角和航向角。

可见，而对于两位置对准，无论绕三个轴中哪个轴旋转，系统的12个状态量都可以变得 完全可观测；显然，两位置对准提高了系统的可观测性。

步骤4：再利用卡尔曼滤波模型进行估计，利用得到的失准角的精确估计值对进行修 正，得到较精确的

用卡尔曼滤波器估计出的失准角为在小角度情况下可得用失准角表示的误差矩阵：

式中为失准角的估计值在惯性系x、y、z三轴上的投影。

对进行修正，即：

式中为精对结束时刻载体系b至计算惯性系i′的转换矩阵，通过陀螺仪的输出更新计 算得到。

步骤5：利用以上步骤4得到的结合求解捷联矩阵的初始值实现基于惯性系 的两位置对准。

本发明的有益效果通过如下方法得以验证：

通过Matlab仿真，陀螺和加速度计的输出信号由SINS模拟器产生。仿真条件设置如下：

陀螺和加速度计的参数如下：

陀螺的常值漂移为：ε_bj＝0.01°/h，其中(j＝x,y,z)；

陀螺的角随机游走系数为：其中(j＝x,y,z)；

加速度计的常值偏置为：▽_bj＝1×10^-4g，其中(j＝x,y,z)；

加速度计测量白噪声为：其中(j＝x,y,z)。

(2)卡尔曼滤波器中P、Q、R阵初始取值如下：

P(0)＝diag{(0.1)²，(0.1)²，(0.1)²，(0.0067°)²，(0.0067°)²，(0.0087°)²，(0.01°/h)²，(0.01°/h)²， (0.01°/h)²，(10^-4g)²，(10^-4g)²，(10^-4g)²}

Q(0)＝diag{(10^-5g)²，(10^-5g)²，(10^-5g)²，(0.001°/h)²，(0.001°/h)²，(0.001°/h)²，0，0，0，0，0，0}

R(0)＝diag{(0.01)²，(0.01)²，(0.01)²}

(3)设置粗对准结束后水平失准角为0.1°，方位失准角为0.5°；初始的经度126°， 纬度45°。

(4)仿真时间1200s，对准进行到500s时旋转对应的轴180°，旋转时间10s。

仿真结果：从图2至图4可知，固定位置对准结束时横摇误差角为0.0071°、纵摇误差 角为-0.0073°、航向误差角为-0.077°；最优两位置对准结束时横摇误差角为0.00039°、 纵摇误差角为-0.0002°、航向误差角为-0.0013°。可见对准的精度得到了明显的提高：横 摇角和纵摇角的对准精度分别提高了18倍和35倍，航向角的对准精度提高了60倍。