CN104374402A - 一种位置未知条件下的星敏感器/加速度计粗对准方法 - Google Patents

Abstract

本发明公开了一种位置未知条件下的星敏感器/加速度计粗对准方法。利用星敏感器提供的相对于惯性空间的姿态转换矩阵将加速度计输出信息投影转换至惯性系，根据重力加速度在惯性系投影为常值这一信息特性，设计Butterworth低通滤波器并对投影结果进行提取，进而根据提取信息解算得到载体姿态角的粗略估算结果，完成粗对准过程。本发明方法使得粗对准过程不受惯性组件测量误差影响，不需要已知位置信息，适用于载体摇摆和升沉等运动，扩大了粗对准方法的使用范围，增强了捷联惯导系统粗对准方法的适用性。

Description

一种位置未知条件下的星敏感器/加速度计粗对准方法

技术领域

本发明属于捷联惯导系统粗对准方法，尤其涉及一种位置未知条件下的星敏感器/加速度计粗对准方法。 

背景技术

捷联惯性导航系统作为一种不对外发射信号、不受外界干扰的全自主导航系统，目前已经广泛应用于舰船等军用设备上。然而，捷联惯导系统在进入导航工作状态之前，需要明确载体系与导航系之间的转换关系，即初始对准过程，以便完成导航的任务。初始对准按对准的阶段又分为粗对准和精对准，粗对准是为后续的对准提供基准，所以要求速度快，精度可以低一些。 

由万德钧和房建成编撰的《惯性导航初始对准》一书中，介绍了传统的解析式粗对准方法，该方法中将加速度计输出的重力加速度信息作为水平对准的基准，进而获取水平姿态角的粗对准结果。再利用罗经效应，将地球角速率向量Ω沿导航系北向的投影结果作为方位对准的基准，进而获取方位姿态角的粗对准结果。这种方法尽管普遍应用，但是该方法的缺点是：(1)上述对准方法需要已知运载体位置信息；(2)对准过程中，没有考虑惯性器件测量误差对对准精度的影响；(3)上述对准方法仅适用于载体静止状态，没有考虑船舶在系泊或者机动时所引起的运动加速度对对准精度的影响。 

针对上述问题，《西北工业大学学报》2005年第05期由秦永元、严恭敏等人撰写的《摇摆基座上基于g信息的捷联惯导粗对准研究》提出了载体系泊状态下的粗对准方法。该方法将姿态矩阵分散成4个矩阵求取，建立基座惯性坐标系使舰体相对坐标系的姿态矩阵初值成为单位阵，从而使姿态更新解算成为可能。然而，这种方法不仅没有考虑惯性器件误差的影响，也无法消除载体升沉等运动对对准精度的影响；于2011年6月在IEEE Transactions on Instrumentation and Measurement期刊中第1930-1941页由Silson,P.M.G.撰写的Coarse Alignment of a ship’s Strapdown Inertial Attitude Reference System Using Velocity Loci一文中提出了基于GPS速度的粗对准方法，通过GPS速度及其微分项来消除加速度计测量值中除重力加速度分量以外的速度信息，进而避免载体系泊状态下的被动运动对粗对准精度的影响。然而，该方法需要GPS实时提供速度信息，对GPS的依赖严重制约了其战时可用性。此外，该对准方法需要已知载体位置信息，限制了该方法的适用范围。 

发明内容

本发明的目的是提供不需要载体位置信息并且能够减小惯性器件误差的，一种位置未知 条件下的星敏感器/加速度计粗对准方法。 

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

一种位置未知条件下的星敏感器/加速度计粗对准方法，包括以下几个步骤： 

步骤一：采集安装于载体表面的加速度计的输出在载体坐标系投影f^b，采集星敏感器输出的载体坐标系b到惯性坐标系i的姿态转换矩阵其中，f表示加速度计测量输出，b表示载体坐标系； 

步骤二：根据星敏感器输出的姿态转换矩阵将加速度计的输出在载体坐标系投影f^b转换为在惯性坐标系投影

步骤三：根据加速度计的输出在惯性坐标系投影fⁱ的信息特性，设计Butterworth低通滤波器，将加速度计的输出在惯性坐标系投影fⁱ通过Butterworth低通滤波器，得到滤波后的信息δfⁱ＝gⁱ，gⁱ为惯性坐标系下重力加速度； 

步骤四：根据滤波后的信息δfⁱ，得到惯性坐标系旋转至导航坐标系的旋转角度α和β，确定惯性坐标系到导航坐标系的姿态转换矩阵

步骤五：根据惯性坐标系到导航坐标系的姿态转换矩阵和星敏感器输出的载体坐标系b到惯性坐标系i的姿态转换矩阵确定载体坐标系到导航坐标系的转换矩阵

步骤六：根据载体坐标系到导航坐标系的转换矩阵得到载体姿态角的粗略估计，完成粗对准过程。 

本发明一种位置未知条件下的星敏感器/加速度计粗对准方法，还可以包括： 

1、加速度计的输出在载体坐标系投影f^b为： 

f^b＝g^b+a^b+Δ^b

其中，g表示重力加速度，a表示载体系泊状态下由升沉、振荡等引起的载体周期振荡加速度，Δ表示加速度计零位偏置。 

2、加速度计在惯性坐标系投影惯性坐标系下重力加速度gⁱ的信息特性为常量，惯性坐标系下加速度aⁱ为周期振荡形式，惯性坐标系下零位偏置Δⁱ为周期振荡形式。 

3、Butterworth低通滤波器为： 

$H\left(z\right)=\frac{(e^{-A+B}-{3e}^{-A+B}\cos B+1)z^{-1}+[e^{-A}-e^{-A+B}(\sin B-\cos B)]z^{-2}}{1-{2e}^{-A+B}\cos {\mathrm{Bz}}^{-1}+e^{-A}{z}^{-2}}$

其中，滤波器的截止频率为aHz，采样频率为b₁Hz，z为z变换的变量；e为数学自然常数，e≈2.71828。 

4、惯性坐标系旋转到导航坐标系的旋转角度α和β为： 

$\left\{\begin{array}{c}\mathrm{\α}=-\arctan \left(\frac{{\mathrm{\δf}}_x^i}{{\mathrm{\δf}}_y^i}\right)\\ \mathrm{\β}=\arctan \left(\frac{{\mathrm{\δf}}_z^i\sin \mathrm{\α}}{{\mathrm{\δf}}_x^i}\right)\end{array}\right.$

其中，分别表示滤波后的信息δfⁱ在惯性坐标系系x、y和z轴的投影。 

惯性坐标系到导航坐标系的姿态转换矩阵为： 

$C_i^n=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin \mathrm{\β}& \cos \mathrm{\β}\\ 0& -\cos \mathrm{\β}& \sin \mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\α}& \sin \mathrm{\α}& 0\\ -\sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right].$

5、载体姿态角的粗略估计为： 

$\left\{\begin{array}{c}{\mathrm{\φ}}_x=\arcsin c_{32}\\ {\mathrm{\φ}}_y=\arctan ({-c}_{31}/c_{33})\\ {\mathrm{\φ}}_z=\arctan (-c_{12}/c_{22})\end{array}\right.$

其中，φ_x、φ_y、φ_z分别表示载体纵摇角、横摇角和航向角的估算结果，c_ij i,j＝1,2,3表示矩阵第i行、第j列矩阵元素。 

本发明的有益效果： 

本发明利用星敏感器输出的载体系相对惯性系的高精度姿态矩阵将加速度计测量输出投影转换至惯性系，利用Butterworth低通滤波器提取出重力加速度在惯性系的常值投影，进而根据滤波后信息解算出载体姿态角的粗略估算结果。本发明不需外界提供位置信息、不受惯性组件测量误差和载体摇摆、升沉等运动的影响，使得本发明的粗对准效果更好，适用范围更广，适用性更强。 

附图说明

图1为一种位置未知条件下的星敏感器/加速度计粗对准方法的流程图； 

图2为惯性坐标系oxⁱyⁱzⁱ和导航坐标系oxⁿyⁿzⁿ转换关系示意图； 

图3为根据gⁱ信息特性设计的Butterworth低通滤波器幅频响应曲线； 

图4-1为仿真得到本发明的纵摇误差曲线，图4-2为仿真得到本发明的横摇误差曲线，图4-3为仿真得到本发明的航向误差曲线； 

图5-1为仿真得到使用传统的解析式粗对准方法解算的纵摇误差曲线，图5-2为仿真得到使用传统的解析式粗对准方法解算的横摇误差曲线，图5-3为仿真得到使用传统的解析式粗对准方法解算的航向误差曲线； 

图6为对粗对准仿真结果统计表。 

具体实施方式

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

如图1所示，本发明提供一种位置未知条件下的星敏感器/加速度计粗对准方法，具体包括如下步骤： 

步骤1：采集安装于载体表面的加速度计的测量输出f^b。根据加速度计建模结果可知，该测量信息包含的具体信息形式如下，即 

f^b＝g^b+a^b+Δ^b     (1) 

其中，b表示载体坐标系，上角标表示在b系投影，f表示加速度计测量输出，g表示重力加速度，a表示载体系泊状态下由升沉、振荡等引起的载体周期振荡加速度，Δ表示加速度计零位偏置； 

步骤2：采集星敏感器的输出信息其中，表示载体坐标系到惯性坐标系的姿态转换矩阵； 

步骤3：利用星敏感器的输出信息将沿载体系的加速度计输出投影转换至惯性系，投影至惯性系的信息具体形式如下： 

$f^i=C_b^i{f}^b=g^i+a^i+{\mathrm{\Δ}}^i---\left(2\right)$

其中，i表示惯性坐标系，上角标表示在i系投影； 

步骤4：根据gⁱ、aⁱ和Δⁱ的信息特性分析可知，gⁱ的信息特性为常量，aⁱ为周期振荡形式，Δⁱ为周期振荡形式。因为gⁱ表达式为： 

$g^i={\left(C_i^n\right)}^T{g}^n=\left[\begin{array}{c}-g\cos \mathrm{\β}\sin \mathrm{\α}\\ g\cos \mathrm{\β}\cos \mathrm{\α}\\ -g\sin \mathrm{\β}\end{array}\right]---\left(3\right)$

其中，由图2可知，α和β表示惯性坐标系旋转两至惯性坐标系的旋转角度，具体旋转过程为，惯性坐标系oxⁱyⁱzⁱ绕ozⁱ旋转α角得到坐标系ox′y′z′，坐标系ox′y′z′绕ox'旋转90°-β得到导航坐标系oxⁿyⁿzⁿ；表示由惯性系到导航坐标系的姿态转换矩；gⁿ＝[0 0 -g]^T表示重力加速度信息在导航坐标系的投影，g表示重力加速度，其值与地理纬度有关；因为载体在系泊状态下位置不变化，则g、α和β均不发生变化，因此重力加速度在惯性系的投影gⁱ为常量；a^b表示系泊状态下由升沉、纵荡、横荡等引起的载体周期振荡加速度，则投影至n系后的aⁿ仍为振荡形式，因为载体系泊状态下为常值，所以aⁱ为周期振荡形式；和Δ^b为常值，由海浪等外界因素引起舰船的摇摆晃动使得载体系到导航系的姿态转换矩阵呈振荡变化形式，因此，Δⁱ为周期振荡变化形式。进而根据gⁱ、aⁱ和Δⁱ的信息特性分析设计Butterworth低通滤波器。 

虽然滤波器的阶次越高，滤波效果越好，但与此同时导航信息的实时性变差，因此本发明拟采用二阶滤波器。假设滤波器的截止频率为aHz，采样频率为b₁Hz，所设计的数字低通滤波器为(用z^-1表示)： 

$H\left(z\right)=\frac{(e^{-A+B}-{3e}^{-A+B}\cos B+1)z^{-1}+[e^{-A}-e^{-A+B}(\sin B-\cos B)]z^{-2}}{1-{2e}^{-A+B}\cos {\mathrm{Bz}}^{-1}+e^{-A}{z}^{-2}}---\left(4\right)$

其中，z为z变换的变量；e为数学自然常数，e≈2.7182；8 

利用Butterworth低通滤波器对fⁱ进行信息提取，即 

δfⁱ＝Hfⁱ     (5) 

其中，H表示Butterworth低通滤波器；δfⁱ表示滤波后信息，理论上只包含gⁱ信息。 

步骤5：根据步骤4中的Butterworth低通滤波器滤波后的信息δfⁱ可以确定α和β的表达式为 

$\left\{\begin{array}{c}\mathrm{\α}=-\arctan \left(\frac{{\mathrm{\δf}}_x^i}{{\mathrm{\δf}}_y^i}\right)\\ \mathrm{\β}=\arctan \left(\frac{{\mathrm{\δf}}_z^i\sin \mathrm{\α}}{{\mathrm{\δf}}_x^i}\right)\end{array}\right.---\left(6\right)$

其中，分别表示δfⁱ在x、y和z轴的投影； 

步骤6：根据惯性坐标系oxⁱyⁱzⁱ到导航坐标系oxⁿyⁿzⁿ的转换关系确定矩阵， 

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

其中，表示惯性坐标系到导航坐标系的姿态转换矩阵； 

步骤7：将与星敏感器矩阵结合，就可以得到载体系到导航系的转换矩阵即 

$C_b^n=C_i^n{C}_b^i---\left(8\right)$

步骤8：根据得到载体姿态角的粗略估算结果，即 

$\left\{\begin{array}{c}{\mathrm{\φ}}_x=\arcsin c_{32}\\ {\mathrm{\φ}}_y=\arctan ({-c}_{31}/c_{33})\\ {\mathrm{\φ}}_z=\arctan (-c_{12}/c_{22})\end{array}\right.---\left(9\right)$

从而完成粗对准过程；其中，φ_x、φ_y、φ_z分别表示载体纵摇角、横摇角和航向角的估算结果；c_ij(i,j＝1,2,3)表示矩阵第i行、第j列矩阵元素； 

对本发明的有益效果进行验证如下： 

在Matlab仿真条件下，对该方法进行仿真实验： 

(1)首先根据gⁱ、aⁱ和Δⁱ信息特性设计Butterworth低通滤波器参数。因为滤波器拟提取常值信息，剔除所有非常值信息，因此滤波器的截止频率设定值越低，滤波效果越理想，结合舰船系泊状态下的运动特性，这里选择低通滤波器的截止频率为a＝0.001Hz，采用频率为b＝1Hz；虽然滤波器的阶次越高，滤波效果越好，但与此同时导航信息的实时性变差，因此本发明采用二阶滤波器。因此本发明所设计的滤波器为： 

$H\left(z\right)=\frac{1.97\×{10}^{-5}{z}^{-1}+1.96\×{10}^{-5}{z}^{-2}}{1-1.9911143z^{-1}+0.9911536z^{-2}}---\left(10\right)$

仿真结果如图3所示，从图中可以看出，设计的低通滤波器通带截止频率约为0.001Hz， 阻带下限截止频率约为0.004Hz，该滤波器能够满足设计要求。 

(2)模拟载体运动状态，对本发明方法进行仿真： 

假设载体系泊状态，受风浪影响下，载体姿态作周期变化，具体形式为 

其中，i＝x,y,z，分别表示纵摇角φ_x0、横摇角φ_y0和方位角φ_z0；姿态基准分别为φ_x′₀＝0°、φ_y′₀＝0°、φ_z′₀＝30°；摇摆幅值分别为A_x＝8°、A_y＝6°、A_z＝5°；摇摆周期分别为T_x＝5s、T_y＝5s、T_z＝2s；初始相位分别为

横荡、纵荡和垂荡引起的载体运动线速度为 

其中，i＝x,y,z； $A_{D_x}=0.02m,A_{D_y}=0.03m,A_{D_z}=0.2m;T_{D_x}=8s,T_{D_y}=7s,T_{D_z}=5s,$ 并且 为[0,2π]上服从均匀分布的随机相位。 

高频振动引起的载体运动线速度为 

其中，i＝x,y,z； $A_{{\mathrm{DH}}_x}=4.1g,A_{{\mathrm{DH}}_y}=3.9g,A_{{\mathrm{DH}}_z}=4g;f_{{\mathrm{DH}}_x}=260\mathrm{Hz},f_{{\mathrm{DH}}_y}=270\mathrm{Hz},$ 为[0,2π]上服从均匀分布的随机相位。 

载体初始位置：北纬45.7796°，东经126.6705°； 

赤道半径：R＝6378393.0m； 

由万有引力可得到地球表面重力加速度：g＝9.78049m/s²； 

地球自转角速度：Ω＝7.2921158*e^-5rad/s； 

常数：π＝3.1415926535； 

三轴加速度计零位偏置：10^-5g₀，g₀表示赤道重力加速度，g₀＝9.78049； 

三轴陀螺常值漂移：0.01°/h； 

上述仿真条件下，利用本发明和解析式粗对准两种方法进行仿真，仿真次数均为50次。仿真结果曲线及统计结果分别如图4-1～图4-3、图5-1～图5-3和图6所示。 

通过仿真曲线和统计结果可以看出，50次粗对准结果中，无论是姿态对准误差带宽、还是姿态对准误差的均值和方差，专利中提出的星敏感器/加速度计粗对准的结果都好于解析式粗对准。解析式对准水平对准精度为1.5°，方位对准精度为3°；专利方法的水平对准精度为0.17°，方位对准精度为0.8°。 

上述粗对准方法能有效的计算出船舶姿态的粗略值，姿态误差可视为小角度，在此粗对准基础上可对舰船姿态矩阵做精确估计，即捷联惯导系统的精对准。该方法可以实现的前提条件是该方法中将f^b投影转换至惯性系后重力加速度为常值这一特性。该方法的优点为：不需要位置信息、不受惯性器件测量误差影响、适用于载体摇摆和升沉等运动等，使得本发明的粗对准效果更好，适用范围更广，适用性更强。 

Claims (6)

1.一种位置未知条件下的星敏感器/加速度计粗对准方法，其特征在于，包括以下几个步骤：

步骤一：采集安装于载体表面的加速度计的输出在载体坐标系投影f^b，采集星敏感器输出的载体坐标系b到惯性坐标系i的姿态转换矩阵其中，f表示加速度计测量输出，b表示载体坐标系；

步骤二：根据星敏感器输出的姿态转换矩阵将加速度计的输出在载体坐标系投影f^b转换为在惯性坐标系投影

步骤三：根据加速度计的输出在惯性坐标系投影fⁱ的信息特性，设计Butterworth低通滤波器，将加速度计的输出在惯性坐标系投影fⁱ通过Butterworth低通滤波器，得到滤波后的信息δfⁱ＝gⁱ，gⁱ为惯性坐标系下重力加速度；

步骤四：根据滤波后的信息δfⁱ，得到惯性坐标系旋转至导航坐标系的旋转角度α和β，确定惯性坐标系到导航坐标系的姿态转换矩阵

步骤五：根据惯性坐标系到导航坐标系的姿态转换矩阵和星敏感器输出的载体坐标系b到惯性坐标系i的姿态转换矩阵确定载体坐标系到导航坐标系的转换矩阵

步骤六：根据载体坐标系到导航坐标系的转换矩阵得到载体姿态角的粗略估计，完成粗对准过程。

2.根据权利要求1所述的一种位置未知条件下的星敏感器/加速度计粗对准方法，其特征在于：所述的加速度计的输出在载体坐标系投影f^b为：

f^b＝g^b+a^b+Δ^b

其中，g表示重力加速度，a表示载体系泊状态下由升沉、振荡等引起的载体周期振荡加速度，Δ表示加速度计零位偏置。

3.根据权利要求1所述的一种位置未知条件下的星敏感器/加速度计粗对准方法，其特征在于：所述的加速度计在惯性坐标系投影惯性坐标系下重力加速度gⁱ的信息特性为常量，惯性坐标系下加速度aⁱ为周期振荡形式，惯性坐标系下零位偏置Δⁱ为周期振荡形式。

4.根据权利要求1所述的一种位置未知条件下的星敏感器/加速度计粗对准方法，其特征在于：所述的Butterworth低通滤波器为：

$H\left(z\right)=\frac{(e^{-A+B}-{3e}^{-A+B}\cos B+1)z^{-1}+[e^{-A}-e^{-A+B}(\sin B-\cos B)]z^{-2}}{1-{2e}^{-A+B}\cos {\mathrm{Bz}}^{-1}+e^{-A}{z}^{-2}}$

其中，滤波器的截止频率为aHz，采样频率为b₁Hz，z为z变换的变量；e为数学自然常数，e≈2.71828。

5.根据权利要求1所述的一种位置未知条件下的星敏感器/加速度计粗对准方法，其特征在于：所述的惯性坐标系旋转到导航坐标系的旋转角度α和β为：

$\left\{\begin{array}{c}\mathrm{\α}=-\arctan \left(\frac{{\mathrm{\δf}}_x^i}{{\mathrm{\δf}}_y^i}\right)\\ \mathrm{\β}=\arctan \left(\frac{{\mathrm{\δf}}_z^i\sin \mathrm{\α}}{{\mathrm{\δf}}_x^i}\right)\end{array}\right.$

其中，分别表示滤波后的信息δfⁱ在惯性坐标系系x、y和z轴的投影。

惯性坐标系到导航坐标系的姿态转换矩阵为：

$C_i^n=\left[\begin{array}{ccc}1& 0& 0\\ 0& \sin \mathrm{\β}& \cos \mathrm{\β}\\ 0& -\cos \mathrm{\β}& \sin \mathrm{\β}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\α}& \sin \mathrm{\α}& 0\\ -\sin \mathrm{\α}& \cos \mathrm{\α}& 0\\ 0& 0& 1\end{array}\right].$

6.根据权利要求1所述的一种位置未知条件下的星敏感器/加速度计粗对准方法，其特征在于：所述的载体姿态角的粗略估计为：

$\left\{\begin{array}{c}{\mathrm{\φ}}_x=\arcsin c_{32}\\ {\mathrm{\φ}}_y=\arctan (-c_{31}/c_{33})\\ {\mathrm{\φ}}_z=\arctan (-c_{12}/c_{22})\end{array}\right.$

其中，φ_x、φ_y、φ_z分别表示载体纵摇角、横摇角和航向角的估算结果，c_ij i,j＝1,2,3表示矩阵第i行、第j列矩阵元素。