CN103398725A - 一种基于星敏感器的捷联惯导系统初始对准的方法 - Google Patents

Abstract

本发明公开了一种基于星敏感器的捷联惯导系统初始对准的方法，包括以下步骤：确定载体的初始位置；根据初始位置得到位置矩阵；获取惯性坐标系到地球坐标系的转换矩阵采集星敏感器输出确定初始对准矩阵捷联惯导系统解算姿态信息；估计出陀螺漂移。本发明通过采用以姿态误差角为观测量，利用卡尔曼滤波技术估计陀螺漂移，提高了对准精度，并且随着时间的增加精度有显著的提高。本发明利用了星敏感器提高高精度载体姿态的特点，误差小，速度快，基于星敏感器的初始对准技术不但可以在复杂环境下提供高精度的初始对准数据，同时可以作为一种对准信息源在其他对准过程中发挥自己应用的作用。

Description

一种基于星敏感器的捷联惯导系统初始对准的方法

技术领域

本发明属于捷联惯导系统初始对准技术领域，尤其涉及一种基于星敏感器的捷联惯导系统初始对准的方法。

背景技术

在各种应用场合中，惯导系统快速、准确的完成高精度对准是非常重要的。在捷联环境下进行初始对准的重要问题之一就是消除载体干扰运动引起的误差，而解决这个问题的理想方案是卡尔曼滤波技术；星敏感器是目前导航应用中最精确的姿态确定设备，它的精度比太阳敏感器高一个数量级，比地球敏感器高两个数量级。无论是地球轨道卫星还是深空探测器，大型空间结构还是小型卫星，高精度的姿态确定系统几乎都采用了星敏感器。

发明内容

本发明实施例的目的在于提供一种基于星敏感器的捷联惯导系统初始对准的方法，旨在解决捷联环境下进行初始对准存在的载体干扰运动引起的误差的问题。

本发明实施例是这样实现的，一种基于星敏感器的捷联惯导系统初始对准的方法，该基于星敏感器的捷联惯导系统初始对准的方法包括以下步骤：

确定载体的初始位置；

根据初始位置得到位置矩阵；

获取惯性坐标系到地球坐标系的转换矩阵

采集星敏感器输出

确定初始对准矩阵

捷联惯导系统解算姿态信息；

估计出陀螺漂移。

进一步，该基于星敏感器的捷联惯导系统初始对准的方法的具体步骤为：

步骤一，通过外部设备确定载体的初始位置参数；

步骤二，根据初始位置得到位置矩阵

步骤三，获取惯性坐标系到地球坐标系的转换矩阵

步骤四，采集星敏感器输出

即载体系相对于地心惯性系的姿态矩阵；

步骤五，确定初始对准矩阵

步骤六，捷联惯导系统解算姿态信息，得到姿态误差矩阵φ(t)；

步骤七，以步骤六得到的姿态误差矩阵φ(t)为观测量，当载体处于静基座时，不考虑天向陀螺漂移和加速度计零偏；当载体处于动基座时，加入天向陀螺漂移和加速度计零偏，建立系统误差模型；

步骤八，根据步骤七建立的误差模型，利用卡尔曼滤波技术估计出陀螺漂移ε，进而对系统进行补偿。

进一步，在步骤二中，位置矩阵

表示为：

其中：

为载体的当地地理纬度，λ为载体的当地地理经度。

进一步，在步骤三中，转换矩阵

表示为：

$C_i^e=\left[\begin{array}{ccc}\cos \left(\mathrm{GST}\right)& -\sin \left(\mathrm{GST}\right)& 0\\ \sin \left(\mathrm{GST}\right)& \cos \left(\mathrm{GST}\right)& 0\\ 0& 0& 1\end{array}\right]$

其中：GST为格林尼治恒星时角。

进一步，在步骤五中，确定初始对准矩阵

表示为：

$C_n^b=C_i^b{\left(C_e^n{C}_i^e\right)}^T.$

进一步，在步骤六中，误差矩阵φ(t)表示为：

$\mathrm{\φ}\left(t\right)=C_b^{n^{\′}}\·{\left(C_b^n\right)}^T$

其中：

为惯导解算的姿态。

进一步，在步骤七中，静基座情况下，不考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程：

使用一阶线性随即微分方程来描述捷联惯导系统的状态误差如下：

$\stackrel{\·}{X}\left(t\right)F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，δV_E、δV_N分别表示东向、北向的速度误差；δλ、分别表示经度误差、纬度误差；φ_x、φ_y、φ_z分别为X、Y、Z轴姿态误差角；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；

分别为X、Y轴加速度计的白噪声误差；

分别为X、Y、Z轴陀螺的白噪声误差；

用

（i,j),(i=1,2,3;j=1,2,3)表示中的元素；

计算系统噪声系数矩阵：

$G\left(t\right)=\left[\begin{array}{cccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& 0_{1\×3}& 0_{1\×7}\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0_{1\×3}& 0_{1\×7}\\ 0_{3\×1}& 0_{3\×1}& -C_b^{n^{\′}}& 0_{3\×7}\\ 0_{5\×1}& 0_{5\×1}& 0_{5\×3}& 0_{5\×7}\end{array}\right]$

系统的状态转移矩阵：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{5\×12}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

$F_2\left(t\right)=\left[\begin{array}{ccccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}(1,2)& 0& 0& 0\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& C_b^{n^{\′}}\left(\mathrm{1,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& C_b^{n^{\′}}\left(\mathrm{2,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{3,1}\right)& C_b^{n^{\′}}\left(\mathrm{3,2}\right)& C_b^{n^{\′}}\left(\mathrm{3,3}\right)\end{array}\right]$

ω_ie为地球自转角速度，

为当地地理纬度；

使用一阶线性随即微分方程来描述捷联惯导系统的量测方程如下：

Z(t)=H(t)X(t)+υ(t)

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；υ(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)=[0_4×4  I_3×3  0_5×5]

量测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\φ}}_x\left(t\right)\\ {\mathrm{\φ}}_y\left(t\right)\\ {\mathrm{\φ}}_z\left(t\right)\end{array}\right].$

进一步，在步骤七中，动基座情况下，考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程，只需对静基座模型进行改写：

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_z}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccc}{C}_b^{n^{\′}}& 0_{3\×3}& 0_{3\×8}\\ 0_{3\×3}& -C_b^{n^{\′}}& 0_{3\×8}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×8}\end{array}\right]$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{6\×14}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)

其中，

f_x、f_y、f_z为沿X、Y、Z轴加速度计的比力真值。

$F_2\left(t\right)\left[\begin{array}{cc}{C}_b^{n^{\′}}& 0_{3\×3}\\ 0_{2\×3}& 0_{2\×3}\\ 0_{3\×3}& -C_b^{n^{\′}}\end{array}\right]$

系统量测矩阵为：

H(t)=[0_5×5  I_3×3  0_6×6]。

本发明提供的基于星敏感器的捷联惯导系统初始对准的方法，通过采用以姿态误差角为观测量，利用卡尔曼滤波技术估计陀螺漂移，可大大提高对准精度，并且随着时间的增加精度有显著的提高。本发明利用了星敏感器提高高精度载体姿态的特点，误差小，速度快，基于星敏感器的初始对准技术不但可以在复杂环境下提供高精度的初始对准数据，同时可以作为一种对准信息源在其他对准过程中发挥自己应用的作用。此外，本发明操作方便，较好的解决了捷联环境下进行初始对准存在的载体干扰运动引起的误差的问题。

附图说明

图1是本发明实施例提供的基于星敏感器的捷联惯导系统初始对准的方法的流程图；

图2是本发明实施例提供的静基座条件下，仿真时间为600S陀螺漂移的估计值与真值比值的仿真曲线图；

图3是本发明实施例提供的静基座条件下，仿真时间为3600S时，陀螺漂移的估计值与真值比值的仿真曲线图；

图4是本发明实施例提供的动基座状态下仿真时间t=3600s，陀螺漂移的估计值与真值比值的仿真曲线对比图；

图5是本发明实施例提供的静基座条件状态下，仿真时间t=3600s，陀螺漂移的估计值与真值比值的仿真曲线对比图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

图1示出了本发明提供的基于星敏感器的捷联惯导系统初始对准的方法流程。为了便于说明，仅仅示出了与本发明相关的部分。

本发明的基于星敏感器的捷联惯导系统初始对准的方法，该基于星敏感器的捷联惯导系统初始对准的方法包括以下步骤：

确定载体的初始位置；

根据初始位置得到位置矩阵；

获取惯性坐标系到地球坐标系的转换矩阵

采集星敏感器输出

确定初始对准矩阵

捷联惯导系统解算姿态信息；

估计出陀螺漂移。

作为本发明实施例的一优化方案，该基于星敏感器的捷联惯导系统初始对准的方法的具体步骤为：

步骤一，通过外部设备确定载体的初始位置参数；

步骤二，根据初始位置得到位置矩阵

步骤三，获取惯性坐标系到地球坐标系的转换矩阵

步骤四，采集星敏感器输出

即载体系相对于地心惯性系的姿态矩阵；

步骤五，确定初始对准矩阵

步骤六，捷联惯导系统解算姿态信息，得到姿态误差矩阵φ(t)；

步骤七，以步骤六得到的姿态误差矩阵φ(t)为观测量，当载体处于静基座时，不考虑天向陀螺漂移和加速度计零偏；当载体处于动基座时，加入天向陀螺漂移和加速度计零偏，建立系统误差模型；

步骤八，根据步骤七建立的误差模型，利用卡尔曼滤波技术估计出陀螺漂移ε，进而对系统进行补偿。

作为本发明实施例的一优化方案，在步骤二中，位置矩阵

表示为：

其中：

为载体的当地地理纬度，λ为载体的当地地理经度。

作为本发明实施例的一优化方案，在步骤三中，转换矩阵表示为：

$C_i^e=\left[\begin{array}{ccc}\cos \left(\mathrm{GST}\right)& -\sin \left(\mathrm{GST}\right)& 0\\ \sin \left(\mathrm{GST}\right)& \cos \left(\mathrm{GST}\right)& 0\\ 0& 0& 1\end{array}\right]$

其中：GST为格林尼治恒星时角。

作为本发明实施例的一优化方案，在步骤五中，确定初始对准矩阵

表示为：

$C_n^b=C_i^b{\left(C_e^n{C}_i^e\right)}^T.$

作为本发明实施例的一优化方案，在步骤六中，误差矩阵φ(t)表示为：

$\mathrm{\φ}\left(t\right)=C_b^{n^{\′}}\·{\left(C_b^n\right)}^T$

其中：为惯导解算的姿态。

作为本发明实施例的一优化方案，在步骤七中，静基座情况下，不考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程：

使用一阶线性随即微分方程来描述捷联惯导系统的状态误差如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，δV_E、δV_N分别表示东向、北向的速度误差；δλ、分别表示经度误差、纬度误差；φ_x、φ_y、φ_z分别为X、Y、Z轴姿态误差角；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；

分别为X、Y轴加速度计的白噪声误差；分别为X、Y、Z轴陀螺的白噪声误差；

用

(i,j),(i=1,2,3;j=1,2,3)表示

中的元素；

计算系统噪声系数矩阵：

$G\left(t\right)=\left[\begin{array}{cccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& 0_{1\×3}& 0_{1\×7}\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0_{1\×3}& 0_{1\×7}\\ 0_{3\×1}& 0_{3\×1}& -C_b^{n^{\′}}& 0_{3\×7}\\ 0_{5\×1}& 0_{5\×1}& 0_{5\×3}& 0_{5\×7}\end{array}\right]$

系统的状态转移矩阵：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{5\×12}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

$F_2\left(t\right)=\left[\begin{array}{ccccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}(1,2)& 0& 0& 0\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& C_b^{n^{\′}}\left(\mathrm{1,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& C_b^{n^{\′}}\left(\mathrm{2,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{3,1}\right)& C_b^{n^{\′}}\left(\mathrm{3,2}\right)& C_b^{n^{\′}}\left(\mathrm{3,3}\right)\end{array}\right]$

ω_ie为地球自转角速度，

为当地地理纬度；

使用一阶线性随即微分方程来描述捷联惯导系统的量测方程如下：

Z(t)=H(t)X(t)+υ(t)

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；υ(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)=[0_4×4  I_3×3  0_5×5]

量测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\φ}}_x\left(t\right)\\ {\mathrm{\φ}}_y\left(t\right)\\ {\mathrm{\φ}}_z\left(t\right)\end{array}\right].$

作为本发明实施例的一优化方案，在步骤七中，动基座情况下，考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程，只需对静基座模型进行改写：

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_z}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccc}{C}_b^{n^{\′}}& 0_{3\×3}& 0_{3\×8}\\ 0_{3\×3}& -C_b^{n^{\′}}& 0_{3\×8}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×8}\end{array}\right]$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{6\×14}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

其中，

f_x、f_y、f_z为沿X、Y、Z轴加速度计的比力真值。

$F_2\left(t\right)\left[\begin{array}{cc}{C}_b^{n^{\′}}& 0_{3\×3}\\ 0_{2\×3}& 0_{2\×3}\\ 0_{3\×3}& -C_b^{n^{\′}}\end{array}\right]$

系统量测矩阵为：

H(t)=[0_5×5  I_3×3  0_6×6]。

下面结合附图及具体实施例对本发明的应用原理作进一步描述。

如图1所示，本发明实施例的基于星敏感器的捷联惯导系统初始对准的方法包括以下步骤：

S101：确定载体的初始位置；

S102：根据初始位置得到位置矩阵；

S103：获取惯性坐标系到地球坐标系的转换矩阵

S104：采集星敏感器输出

S105：确定初始对准矩阵

S106：捷联惯导系统解算姿态信息；

S107：估计出陀螺漂移。

本发明的具体步骤为：

步骤一，通过外部设备确定载体的初始位置参数；

步骤二，根据初始位置得到位置矩阵

其中：

为载体的当地地理纬度，λ为载体的当地地理经度；

步骤三，获取惯性坐标系到地球坐标系的转换矩阵

$C_i^e=\left[\begin{array}{ccc}\cos \left(\mathrm{GST}\right)& -\sin \left(\mathrm{GST}\right)& 0\\ \sin \left(\mathrm{GST}\right)& \cos \left(\mathrm{GST}\right)& 0\\ 0& 0& 1\end{array}\right]$

其中：GST为格林尼治恒星时角(Greenwich sidereal time,GST)；

步骤四，采集星敏感器输出

即载体系相对于地心惯性系的姿态矩阵；

步骤五，确定初始对准矩阵

$C_n^b=C_i^b{\left(C_e^n{C}_i^e\right)}^T;$

步骤六，捷联惯导系统解算姿态信息，得到姿态误差矩阵φ(t)；

$\mathrm{\φ}\left(t\right)=C_b^{n^{\′}}\·{\left(C_b^n\right)}^T$

其中：

为惯导解算的姿态；

步骤七，以步骤六得到的姿态误差矩阵φ(t)为观测量，当载体处于静基座时，不考虑天向陀螺漂移和加速度计零偏；当载体处于动基座时，加入天向陀螺漂移和加速度计零偏，建立系统误差模型。

步骤八，根据步骤七建立的误差模型，利用卡尔曼滤波技术估计出陀螺漂移ε，进而对系统进行补偿。

本发明的具体实施方式：

第一步，通过GPS装置或者是外部的高精度组合导航设备提供载体的初始位置参数。

第二步，根据初始位置得到位置矩阵

其中：

载体的当地地理纬度，λ为载体的当地地理经度；

第三步，获取惯性坐标系到地球坐标系的转换矩阵

$C_i^e=\left[\begin{array}{ccc}\cos \left(\mathrm{GST}\right)& -\sin \left(\mathrm{GST}\right)& 0\\ \sin \left(\mathrm{GST}\right)& \cos \left(\mathrm{GST}\right)& 0\\ 0& 0& 1\end{array}\right]$

其中：GST为格林尼治恒星时角(Greenwich sidereal time,GST)；

第四步，采集星敏感器输出

即载体系相对于地心惯性系的姿态矩阵；

第五步，确定初始对准矩阵

$C_n^b=C_i^b{\left(C_e^n{C}_i^e\right)}^T$

将

直接传递给捷联惯导系统作为初始对准的起始姿态，此过程体现的是传递效果；

第六步，捷联惯导系统解算姿态信息，得到姿态误差矩阵φ(t)

$\mathrm{\φ}\left(t\right)=C_b^{n^{\′}}\·{\left(C_b^n\right)}^T$

其中：

为惯导解算的姿态；

第七步，静基座情况下，不考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程：

使用一阶线性随即微分方程来描述捷联惯导系统的状态误差如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，δV_E、δV_N分别表示东向、北向的速度误差；δλ、

分别表示经度误差、纬度误差；φ_x、φ_y、φ_z分别为X、Y、Z轴姿态误差角；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；分别为X、Y轴加速度计的白噪声误差；分别为X、Y、Z轴陀螺的白噪声误差；

用

（i,j),(i=1,2,3;j=1,2,3)表示

中的元素；

计算系统噪声系数矩阵：

$G\left(t\right)=\left[\begin{array}{cccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& 0_{1\×3}& 0_{1\×7}\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0_{1\×3}& 0_{1\×7}\\ 0_{3\×1}& 0_{3\×1}& -C_b^{n^{\′}}& 0_{3\×7}\\ 0_{5\×1}& 0_{5\×1}& 0_{5\×3}& 0_{5\×7}\end{array}\right]$

系统的状态转移矩阵：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{5\×12}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

$F_2\left(t\right)=\left[\begin{array}{ccccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}(1,2)& 0& 0& 0\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& C_b^{n^{\′}}\left(\mathrm{1,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& C_b^{n^{\′}}\left(\mathrm{2,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{3,1}\right)& C_b^{n^{\′}}\left(\mathrm{3,2}\right)& C_b^{n^{\′}}\left(\mathrm{3,3}\right)\end{array}\right]$

ω_ie为地球自转角速度，当地地理纬度；

使用一阶线性随即微分方程来描述捷联惯导系统的量测方程如下：

Z(t)=H(t)X(t)+υ(t)

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；υ(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)=[0_4×4  I_3×3  0_5×5]

量测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\φ}}_x\left(t\right)\\ {\mathrm{\φ}}_y\left(t\right)\\ {\mathrm{\φ}}_z\left(t\right)\end{array}\right]$

第八步，动基座情况下，考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程，只需对静基座模型进行改写：

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_z}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccc}{C}_b^{n^{\′}}& 0_{3\×3}& 0_{3\×8}\\ 0_{3\×3}& -C_b^{n^{\′}}& 0_{3\×8}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×8}\end{array}\right]$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{6\×14}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

其中，

f_x、f_y、f_z为沿X、Y、Z轴加速度计的比力真值。

$F_2\left(t\right)\left[\begin{array}{cc}{C}_b^{n^{\′}}& 0_{3\×3}\\ 0_{2\×3}& 0_{2\×3}\\ 0_{3\×3}& -C_b^{n^{\′}}\end{array}\right]$

系统量测矩阵为：

H(t)=[0_5×5  I_3×3  0_6×6]

第九步，利用卡尔曼滤波估计出陀螺漂移，并进行误差补偿，提高对准精度。

本发明通过利用MATLAB进行仿真，仿真参数设定：星敏感器的误差为0～0.01°之间的随机变量，陀螺漂移为0.01°/h，加速度计零偏为1×10^-4×g₀，陀螺漂移白噪声误差为0.005°/h，加速度计白噪声误差为5×10^-5×g₀，运载体的初始位置为北纬45.7796°、东经126.6705°；地球半径R_e=6378393.0m；g₀=9.78049m/s²，

考虑天向陀螺漂移和加速度计零偏的情况下，将摇摆运动考虑其中，以便接近更真实的环境，

设载体的摇摆模型为：

$\mathrm{pitch}=\frac{\mathrm{\π}}{12}\·\cos (\frac{2\mathrm{\π}}{12}+\frac{\mathrm{\π}}{7})$

$\mathrm{roll}=\frac{\mathrm{\π}}{18}\·\cos (\frac{2\mathrm{\π}}{6}t+\frac{\mathrm{\π}}{4})$

$\mathrm{yaw}=\frac{\mathrm{\π}}{36}\·\cos (\frac{2\mathrm{\π}}{8}t+\frac{\mathrm{\π}}{3})+\frac{\mathrm{\π}}{6}$

其中：pitch、roll、yaw分别表示摇摆模型下的纵摇角、横摇角和艏摇角，相应的周期分别为12s、6s和8s，幅值分别为15°、10°和5°；

本发明采用以姿态误差角为观测量，利用卡尔曼滤波技术估计陀螺漂移，可大大提高对准精度，并且随着时间的增加精度有显著的提高。本发明利用了星敏感器提高高精度载体姿态的特点，误差小，速度快，基于星敏感器的初始对准技术不但可以在复杂环境下提供高精度的初始对准数据，同时可以作为一种对准信息源在其他对准过程中发挥自己应用的作用。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，该基于星敏感器的捷联惯导系统初始对准的方法包括以下步骤：

确定载体的初始位置；

根据初始位置得到位置矩阵；

获取惯性坐标系到地球坐标系的转换矩阵

采集星敏感器输出

确定初始对准矩阵

捷联惯导系统解算姿态信息；

估计出陀螺漂移。

2.如权利要求1所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，该基于星敏感器的捷联惯导系统初始对准的方法的具体步骤为：

步骤一，通过外部设备确定载体的初始位置参数；

步骤二，根据初始位置得到位置矩阵

步骤三，获取惯性坐标系到地球坐标系的转换矩阵

步骤四，采集星敏感器输出

即载体系相对于地心惯性系的姿态矩阵；

步骤五，确定初始对准矩阵

步骤六，捷联惯导系统解算姿态信息，得到姿态误差矩阵φ(t)；

步骤七，以步骤六得到的姿态误差矩阵φ(t)为观测量，当载体处于静基座时，不考虑天向陀螺漂移和加速度计零偏；当载体处于动基座时，加入天向陀螺漂移和加速度计零偏，建立系统误差模型；

步骤八，根据步骤七建立的误差模型，利用卡尔曼滤波技术估计出陀螺漂移ε，进而对系统进行补偿。

3.如权利要求2所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，在步骤二中，位置矩阵

表示为：

其中：

载体的当地地理纬度，λ为载体的当地地理经度。

4.如权利要求2所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，在步骤三中，转换矩阵

表示为：

$C_i^e=\left[\begin{array}{ccc}\cos \left(\mathrm{GST}\right)& -\sin \left(\mathrm{GST}\right)& 0\\ \sin \left(\mathrm{GST}\right)& \cos \left(\mathrm{GST}\right)& 0\\ 0& 0& 1\end{array}\right]$

其中：GST为格林尼治恒星时角。

5.如权利要求2所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，在步骤五中，确定初始对准矩阵

表示为：

$C_n^b=C_i^b{\left(C_e^n{C}_i^e\right)}^T.$

6.如权利要求2所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，在步骤六中，误差矩阵φ(t)表示为：

$\mathrm{\φ}\left(t\right)=C_b^{n^{\′}}\·{\left(C_b^n\right)}^T$

其中：

为惯导解算的姿态。

7.如权利要求2所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，在步骤七中，静基座情况下，不考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程：

使用一阶线性随即微分方程来描述捷联惯导系统的状态误差如下：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中X(t)为t时刻系统的状态向量；F(t)和G(t)分别为系统状态矩阵和噪声矩阵；W(t)为系统的噪声向量；

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中，δV^E、δV^N分别表示东向、北向的速度误差；δλ、

分别表示经度误差、纬度误差；φ_x、φ_y、φ_z分别为X、Y、Z轴姿态误差角；

分别为X、Y、Z轴加速度计的零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；

分别为X、Y轴加速度计的白噪声误差；

分别为X、Y、Z轴陀螺的白噪声误差；

用

(i,j),(i=1,2,3;j=1,2,3)表示

中的元素；

计算系统噪声系数矩阵：

$G\left(t\right)=\left[\begin{array}{cccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& 0_{1\×3}& 0_{1\×7}\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0_{1\×3}& 0_{1\×7}\\ 0_{3\×1}& 0_{3\×1}& -C_b^{n^{\′}}& 0_{3\×7}\\ 0_{5\×1}& 0_{5\×1}& 0_{5\×3}& 0_{5\×7}\end{array}\right]$

系统的状态转移矩阵：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{5\×12}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

$F_2\left(t\right)=\left[\begin{array}{ccccc}{C}_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}(1,2)& 0& 0& 0\\ C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{1,1}\right)& C_b^{n^{\′}}\left(\mathrm{1,2}\right)& C_b^{n^{\′}}\left(\mathrm{1,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{2,1}\right)& C_b^{n^{\′}}\left(\mathrm{2,2}\right)& C_b^{n^{\′}}\left(\mathrm{2,3}\right)\\ 0& 0& C_b^{n^{\′}}\left(\mathrm{3,1}\right)& C_b^{n^{\′}}\left(\mathrm{3,2}\right)& C_b^{n^{\′}}\left(\mathrm{3,3}\right)\end{array}\right]$

ω_ie为地球自转角速度，

为当地地理纬度；

使用一阶线性随即微分方程来描述捷联惯导系统的量测方程如下：

Z(t)=H(t)X(t)+υ(t)

其中，Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；υ(t)表示系统的测量噪声；

系统量测矩阵为：

H(t)=[0_4×4  I_3×3  0_5×5]

量测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\φ}}_x\left(t\right)\\ {\mathrm{\φ}}_y\left(t\right)\\ {\mathrm{\φ}}_z\left(t\right)\end{array}\right].$

8.如权利要求2所述的基于星敏感器的捷联惯导系统初始对准的方法，其特征在于，在步骤七中，动基座情况下，考虑天向陀螺漂移与加速度计零偏，推导卡尔曼滤波器的系统方程与观测方程，只需对静基座模型进行改写：

系统的状态向量为：

系统的白噪声向量为：

$W=\left[\begin{array}{cccccccccccccc}{w}_{{\▿}_x}& w_{{\▿}_x}& w_{{\▿}_z}& w_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

系统噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccc}{C}_b^{n^{\′}}& 0_{3\×3}& 0_{3\×8}\\ 0_{3\×3}& -C_b^{n^{\′}}& 0_{3\×8}\\ 0_{6\×3}& 0_{6\×3}& 0_{6\×8}\end{array}\right]$

系统的状态转移矩阵为：

$F\left(t\right)=\left[\begin{array}{c}{F}_s\left(t\right)\\ 0_{6\×14}\end{array}\right]$

其中，

F_s(t)=[F₁(t)  F₂(t)]

其中，

f_x、f_y、f_z为沿X、Y、Z轴加速度计的比力真值；

$F_2\left(t\right)\left[\begin{array}{cc}{C}_b^{n^{\′}}& 0_{3\×3}\\ 0_{2\×3}& 0_{2\×3}\\ 0_{3\×3}& -C_b^{n^{\′}}\end{array}\right]$

系统量测矩阵为：

H(t)=[0_5×5  I_3×3  0_6×6]。

Images