CN103557871B - 一种浮空飞行器捷联惯导空中初始对准方法 - Google Patents

Abstract

本发明公开了一种浮空飞行器捷联惯导空中初始对准方法，属于惯性技术领域。本发明采用GPS辅助的惯性凝固解析式粗对准方法实现浮空飞行器惯导系统的空中粗对准，粗略的获取惯导姿态信息，而位置和速度信息直接由GPS获取：在粗对准基础上，考虑大方位失准角情况，建立以包括粗对准失准角误差的卡尔曼滤波状态方程，和以导航解算与GPS所测速度差为量测量的卡尔曼滤波量测方程，通过卡尔曼滤波，估计出失准角。本发明采用GPS辅助的惯性凝固解析式粗对准方法能实时跟踪载体姿态，屏蔽角运动与线运动干扰，大大缩短了粗对准时间；本发明提供的方法适用于载体平动加转动的运动情况，具有很好的普适性；不需要增加昂贵的设备，更具有可行性。

Description

一种浮空飞行器捷联惯导空中初始对准方法

技术领域

本发明属于惯性技术领域，特别涉及一种浮空飞行器在空中机动过程中同时具有平动和绕天向转动条件下捷联惯导的空中初始对准方法。

背景技术

浮空飞行器必须具有几天甚至几个月在空中执行任务的能力。为了节约能源和减少不必要的设备老化损耗，其捷联惯性导航系统必须具备空中开机后正常工作的功能。这就要求捷联惯导系统必须能够在浮空器机动的动态过程中实现初始对准，提供可用的导航初值信息的能力，这是实现后续的导航定位和姿态测量的前提。浮空飞行器漂浮在高空中，受到风力等因素的影响，相对于地面同时具有平动和绕天向转动的机动特点。

浮空飞行器导航系统的空中初始对准分为粗对准和精对准两个阶段。粗对准的主要任务就是在载体姿态未知的情形下，根据惯性传感器及外部信息传感器的测量信息初步确定载体姿态角和航向角，为下一步精对准提供初始条件。在运载体运动的环境下，地球角速率相对很小，传统解析式粗对准不可用，因此载体运动条件下要获取姿态和方位信息就必须引入外界辅助措施以测量载体的运动，在算法中对运动量进行补偿，降低或消除运动的影响。

捷联惯导系统的精对准在完成粗对准的基础上进行。因为粗对准精度较低，当方位失准角较大时，常用的线性模型和滤波方法已经不再适用，需要研究适用于大失准角对准的误差模型和滤波算法。为此，众多学者研究了能适用于大角度误差的非线性模型及相应的非线性滤波算法，同时基于大方位误差模型的非线性滤波方法在动基座和飞行对准中的应用越来越广泛。

文献1：夏家和等.捷联惯导动态粗对准算法研究与仿真[J].系统仿真学报，2010，22(4)：967-969，主要讲述在外部信息里程计的辅助下实现舰载航姿系统的粗对准，仿真结果表明该粗对准方法在实际工程中的可行性。而浮空飞行器在高空运动，非常利于获取GPS信息，从而获取初始的位置和速度信息。借助于GPS信息的辅助，寻找合适的初始姿态角和航向角获取方法，是浮空飞行器粗对准方法的研究重点。

文献2：严恭敏.捷联惯导系统动基座初始对准及其它相关问题研究[D].西安：西北工业大学，2008，围绕大失准角下的精对准进行了详细的研究，理论分析及仿真结果表明方法的可行性。

文献3：徐博等.舰船捷联航姿系统自主粗对准仿真与实验研究[J].兵工学报，2008，29(12)：1465-1473，主要围绕载体晃动基座下的对准展开研究，文献4：陈良，等.捷联惯导空中粗对准方法[J].国防科技大学学报，2012，34(2):130－133，主要针对飞行器进近着陆情况下采用基于四元数的解析粗对准法实现载体粗对准，该方法对飞行器机动特性有所约束且受侧滑速度的影响较大。文献5：刘百奇等.一种基于可观测度分析的SINS/GPS空中对准新方法[J].系统仿真学报，2008,20（16）：4302-4348，研究了GPS辅助的空中对准方法，该方法以系统参数的可观测度作为反馈值，研究了在匀速直航情况下采用各种机动方式来提高系统可观测度，上述几种方法均没有考虑载体平动的同时绕天向轴存在转动的运动特点下的空中对准。

因此，为满足浮空飞行器捷联惯导空中开机后正常工作的需要，考虑浮空飞行器行进的同时绕天向存在角运动的情况即平动加转动的运动特性，选取一种既简单精度又高的浮空飞行器空中对准的方法，对浮空飞行器的工程应用具有很强的实际意义。

发明内容

本发明的目的在于提供一种既简单精度又高的浮空飞行器捷联惯导空中初始对准方法，该方法在外界信息GPS的辅助下，实现浮空飞行器在平动加转动运动条件下的空中对准。

本发明的具体实现步骤如下：

A、采用GPS辅助的惯性凝固解析式粗对准方法实现浮空飞行器惯导系统的空中粗对准，粗略的获取惯导姿态信息，而位置和速度信息直接由GPS获取：

第一步：采用惯性凝固解析式粗对准方法，即将t时刻的捷联姿态矩阵Cbn(t)拆写成如下三个矩阵相乘形式：

$C_b^n\left(t\right)=C_{i_{n0}}^n\left(t\right)C_{i_{b0}}^{i_{n0}}{C}_b^{i_{b0}}\left(t\right)$

其中：n表示导航坐标系，b表示载体坐标系，i_n0、i_b0分别表示与粗对准时刻t₀的导航坐标系n(t₀)、载体坐标系b(t₀)重合的惯性坐标系。导航坐标系选为东北天地理坐标系。

第二步：依据GPS实时测得的载体所在点的经度和纬度，可由粗对准经历的时间t实时确定；第三步：利用陀螺输出的角运动信息，根据捷联惯导姿态更新算法可获取t时刻的

第四步：表示两个惯性坐标系之间在t₀时刻的变换矩阵，是一个常值阵。利用GPS实时测得的载体速度信息，针对惯导比力方程构造两组速度差，根据矩阵构造算法即可求得常值矩阵

B、在粗对准基础上，考虑大方位失准角情况，建立以包括粗对准失准角误差的卡尔曼滤波状态方程，和以导航解算与GPS所测速度差为量测量的卡尔曼滤波量测方程，通过卡尔曼滤波，估计出失准角；具体实现步骤如下：第一步：建立卡尔曼滤波状态方程：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}^n=[I-\left(C_n^{n^{\′}}{)}^T\right]C_b^{n^{\′}}{f}^b+{\left(C_n^{n^{\′}}\right)}^T{C}_b^{n^{\′}}{\▿}^b-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δ}{V}^n\\ \stackrel{\·}{\mathrm{\α}}=C_{\mathrm{\ω}}^{-1}(I-C_n^{n^{\′}}){\mathrm{\ω}}_{\mathrm{in}}^n+C_n^{n^{\′}}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n-C_b^{n^{\′}}{\mathrm{\ϵ}}^b\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\\ {\stackrel{\·}{\▿}}^b=0\end{array}\right.$

其中：n'为惯导模拟的数学平台，δVⁿ为惯导解算速度与GPS实测速度的速度差，为所述速度差δVⁿ的一阶微分，f^b为惯导系统测得的比力加速度，为地球自转角速度，为导航坐标系相对地球载体线速度引起的角速度，为惯导模拟的数学平台与理想导航坐标系间的转换矩阵，为导航解算的姿态更新矩阵，为计算参数误差，α为导航解算数学平台与理想导航坐标系间的失准角，为失准角α的一阶微分，为欧拉平台误差角与计算平台坐标系角运动之间的转换关系矩阵，▽^b为惯导加速度计输出误差，为加速度计输出误差▽^b的一阶微分，ε^b为惯导陀螺输出误差，为陀螺输出误差ε^b的一阶微分。

第二步：建立卡尔曼滤波量测方程：

Z＝HX+v

其中： $X={\left[\begin{array}{ccccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& \mathrm{\δ}{V}_x^n& \mathrm{\δ}{V}_y^n& \mathrm{\δ}{V}_z^n& {\mathrm{\α}}_x& {\mathrm{\α}}_y& {\mathrm{\α}}_z& {\▿}_x^b& {\▿}_y^b& {\▿}_z^b& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]}^T$ 是卡尔曼滤波器的状态矢量，δL为惯导解算与GPS实测的纬度差，δλ为惯导解算与GPS实测的经度差，δh为惯导解算与GPS实测的高度差，为导航坐标系x、y、z方向的速度误差分量，α_x、α_y、α_z为导航坐标系x、y、z方向的失准角误差分量，为载体坐标系x、y、z方向的加速度计输出误差分量，为载体坐标系x、y、z方向的陀螺输出误差分量。量测量量测矩阵为H＝[0_3×3|I_3×3|0_3×9]为，v为量测噪声阵。

第三步：采用UKF滤波（无迹卡尔曼滤波）算法进行状态估计。

本发明具有如下优点：

（1）GPS辅助的惯性凝固解析式粗对准方法能实时跟踪载体姿态，屏蔽角运动与线运动干扰，大大缩短了粗对准时间；

（2）外界GPS信息的辅助以及状态方程的建立使得该粗对准方法适用于载体平动加转动的运动情况，具有很好的普适性；

（3）不需要增加昂贵的设备，更具有可行性。

附图说明

图1a为本发明的初始对准方法总体流程图，图1b为本发明中精对准流程图；

图2为仿真模拟的浮空飞行器惯导真实姿态角变化曲线；

图3为GPS辅助的惯性凝固解析式粗对准法下姿态失准角角估计曲线；

图4解析式粗对准法的50次蒙特-卡洛仿真结果图；

图5为大方位失准角下（15°）姿态失准角估计曲线。

具体实施方式

下面结合附图和实施例对本发明做更详细的描述。

本发明提供一种浮空飞行器捷联惯导空中初始对准方法，如图1a所示。利用某平流层验证浮空器进行了试飞实验，试验持续10小时。为了分析方便，将浮空器飞行的中间时间段提取出来。图2为浮空器平稳飞行过程中的姿态变化曲线。该运动背景下的空中初始对准方法首先进行粗对准，然后进行精对准具体如下：

第一部分，采用GPS辅助的惯性凝固解析式粗对准方法实现浮空飞行器惯导系统的空中粗对准，粗略的获取惯导姿态信息，而位置和速度信息直接由GPS获取，具体实现步骤如下：

第一步：采用惯性凝固解析式粗对准方法，即将t时刻的捷联姿态矩阵拆写成如下三个矩阵相乘形式：

$C_b^n\left(t\right)=C_{i_{n0}}^n\left(t\right)C_{i_{b0}}^{i_{n0}}{C}_b^{i_{b0}}\left(t\right)$

式中：n表示导航坐标系，b表示载体坐标系，i_n0、i_b0分别表示与粗对准时刻t₀的导航坐标系n(t₀)、载体坐标系b(t₀)重合的惯性坐标系。导航坐标系选为东北天地理坐标系。第二步：记λ₀和L₀分别为粗对准起始t₀时刻捷联惯导的经度和纬度，λ_t和L_t分别为对准t时刻捷联惯导的经度和纬度，则可表示为：

$C_{i_{n0}}^n\left(t\right)=\begin{array}{c}\left[\begin{array}{ccc}-\sin (\mathrm{\Δ}{\mathrm{\λ}}_t+{\mathrm{\ω}}_{\mathrm{ie}}^n(t-t_0))& \cos (\mathrm{\Δ}{\mathrm{\λ}}_t+{\mathrm{\ω}}_{\mathrm{ie}}^n(t-t_0))& 0\\ -\sin L_t\cos (\mathrm{\Δ}{\mathrm{\λ}}_t+{\mathrm{\ω}}_{\mathrm{ie}}^n(t-t_0))& -\sin L_t\sin (\mathrm{\Δ}{\mathrm{\λ}}_t+{\mathrm{\ω}}_{\mathrm{ie}}^n(t-t_0))& \cos L_t\\ \cos L_t\cos (\mathrm{\Δ}{\mathrm{\λ}}_t+{\mathrm{\ω}}_{\mathrm{ie}}^n(t-t_0))& \cos L_t\sin (\mathrm{\Δ}{\mathrm{\λ}}_t+{\mathrm{\ω}}_{\mathrm{ie}}^n(t-t_0))& \sin L_t\end{array}\right]\end{array}$

其中，Δλ_t为t时刻经度相对变化量，Δλ_t＝λ_t-λ₀。依据GPS测得的实时纬度L_t与经度相对变化量便可求得对准t时刻的 为地球自转角速率。

第三步：利用陀螺输出的角运动信息，记运算符（V×）表示由矢量V构成的反对称矩阵，根据捷联惯导姿态更新算法，即，

${\stackrel{\·}{C}}_b^{i_{b_0}}\left(t\right)=C_b^{i_{b_0}}\left(t\right)[{\mathrm{\ω}}_{\mathrm{ib}}^b\left(t\right)\×]$

获得t时刻的且在t₀时刻满足I为单位阵，为t时刻陀螺输出的角速率。

第四步：表示初始对准时刻两个惯性坐标系i_n0、i_b0之间的变换矩阵，是一个常值阵。惯导比力方程如下所示：

$f^n\left(t\right)={\stackrel{\·}{V}}^n\left(t\right)+[{\mathrm{\ω}}_{\mathrm{en}}^n\left(t\right)+{2\mathrm{\ω}}_{\mathrm{ie}}^n\left(t\right)]\×V^n\left(t\right)-g$

式中，fⁿ(t)为加速度计测得的在导航系的比力信息，Vⁿ(t)为载体在导航坐标系内的速度信息，有GPS实时测得，为载体在导航坐标系的加速度信息，由GPS速度差分获得，g为重力加速度信息。

将上式两边同时左乘且分别从t₀时刻到t时刻同时积分，可得：

$\mathrm{\Δ}{v}^{i_{b0}}\left(t\right)=C_{i_{n0}}^{i_{b0}}\mathrm{\Δ}{u}^{i_{n0}}\left(t\right)$

其中

$\mathrm{\Δ}{v}^{i_{b0}}\left(t\right)={\∫}_{t_0}^t{C}_b^{i_{b0}}\left(s\right)f^b\left(s\right)\mathrm{ds},\mathrm{\Δ}{u}^{i_{n0}}\left(t\right)={\∫}_{t0}^t{C}_n^{i_{n0}}\left(s\right)\{{\stackrel{\·}{V}}^n\left(s\right)+[{\mathrm{\ω}}_{\mathrm{en}}^n\left(s\right)+2{\mathrm{\ω}}_{\mathrm{ie}}^n\left(s\right)]\×V^n\left(s\right)+g\}\mathrm{ds}$

f^b(t)为加速度计测得的在载体坐标系中的比力信息，分别取t＝t_l和t＝t_m两个对准过程中的不同时刻，构造两组速度差和根据矩阵构造算法即可求得常值矩阵即，

$C_{i_{b_0}}^{i_{n0}}={\left[\begin{array}{c}{\left[\mathrm{\Δ}{u}^{i_{n0}}\left(t_l\right)\right]}^T\\ [\mathrm{\Δ}{u}^{i_{n0}}\left(t_l\right)\×\mathrm{\Δ}{u}^{i_{n0}}\left(t_m\right){]}^T\\ [\mathrm{\Δ}{u}^{i_{n0}}\left(t_l\right)\×\mathrm{\Δ}{u}^{i_{n0}}\left(t_m\right)\×\mathrm{\Δ}{u}^{i_{n0}}\left(t_l\right){]}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left[\mathrm{\Δ}{v}^{i_{b0}}\left(t_l\right)\right]}^T\\ {[\mathrm{\Δ}{v}^{i_{b0}}\left(t_l\right)\×\mathrm{\Δ}{v}^{i_{b0}}\left(t_m\right)]}^T\\ {[\mathrm{\Δ}{v}^{i_{b0}}\left(t_l\right)\×\mathrm{\Δ}{v}^{i_{b0}}\left(t_m\right)\×\mathrm{\Δ}{v}^{i_{b0}}\left(t_l\right)]}^T\end{array}\right]$

第二部分，在粗对准基础上，考虑大方位失准角情况，建立以包括粗对准失准角误差的卡尔曼滤波状态方程，和以导航解算与GPS所测速度差为量测量的卡尔曼滤波量测方程，通过卡尔曼滤波，估计出失准角。粗对准结束后，导航解算模拟的数学平台与理想导航坐标系之间存在转动误差，分别记为α_z、α_x、α_y。当α_z为大失准角时，传统的线性误差模型不再适用，如图1b所示，估计失准角的精对准过程包括如下步骤：

第一步：建立卡尔曼滤波状态方程；

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}^n=[I-\left(C_n^{n^{\′}}{)}^T\right]C_b^{n^{\′}}{f}^b+{\left(C_n^{n^{\′}}\right)}^T{C}_b^{n^{\′}}{\▿}^b-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δ}{V}^n\\ \stackrel{\·}{\mathrm{\α}}=C_{\mathrm{\ω}}^{-1}(I-C_n^{n^{\′}}){\mathrm{\ω}}_{\mathrm{in}}^n+C_n^{n^{\′}}\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{in}}^n-C_b^{n^{\′}}{\mathrm{\ϵ}}^b\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\\ {\stackrel{\·}{\▿}}^b=0\end{array}\right.$

式中： $C_n^{n^{\′}}=\left[\begin{array}{ccc}\mathrm{cso}{\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& -{\mathrm{\α}}_y\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& {\mathrm{\α}}_x\\ {\mathrm{\α}}_y\cos {\mathrm{\α}}_z+{\mathrm{\α}}_x\sin {\mathrm{\α}}_z& {\mathrm{\α}}_y\sin {\mathrm{\α}}_z-{\mathrm{\α}}_x\cos {\mathrm{\α}}_z& 1\end{array}\right],C_{\mathrm{\ω}}^{-1}=\left[\begin{array}{ccc}1& 0& {\mathrm{\α}}_y\\ 0& 1& -{\mathrm{\α}}_x\\ -{\mathrm{\α}}_y& 0& 1\end{array}\right]$

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T,{\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-\frac{V_y^n}{R+h}& \stackrel{\·}{\mathrm{\λ}}\cos L& \stackrel{\·}{\mathrm{\λ}}\sin L\end{array}\right]}^T$

${\mathrm{\δ\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{c}0\\ -{\mathrm{\ω}}_{\mathrm{ie}}^n\sin \mathrm{L\δL}\\ {\mathrm{\ω}}_{\mathrm{ie}}^n\cos \mathrm{L\δL}\end{array}\right],{\mathrm{\δ\ω}}_{\mathrm{en}}^n=\left[\begin{array}{c}-\mathrm{\δ}{V}_y^n/(R+h)\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}\cos L-\stackrel{\·}{\mathrm{\λ}}\sin \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}\sin L+\stackrel{\·}{\mathrm{\λ}}\cos \mathrm{L\δL}\end{array}\right]$

$\stackrel{\·}{\mathrm{\λ}}=\frac{V_x^n\sec L}{R+h},\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\mathrm{\δ}{V}_x^n\sec L}{R+h}+\frac{V_x^n\sec L\tan L}{R+h}\mathrm{\δL}$

其中，R为地球半径，L、λ、h分别表示惯导纬度、经度和高度。

第二步：建立卡尔曼滤波量测方程；

该方法取系统状态向量为X：

$X={\left[\begin{array}{ccccccccccccccc}\mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& \mathrm{\δ}{V}_x^n& \mathrm{\δ}{V}_y^n& \mathrm{\δ}{V}_z^n& {\mathrm{\α}}_x& {\mathrm{\α}}_y& {\mathrm{\α}}_z& {\▿}_x^b& {\▿}_y^b& {\▿}_z^b& {\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]}^T$

量测量 $Z=\left[\begin{array}{ccc}{\mathrm{\δV}}_x^n& {\mathrm{\δV}}_y^n& {\mathrm{\δV}}_z^n\end{array}\right],H=\left[0_{3\×3}\right|I_{3\×3}\left|0_{3\×9}\right]$ 为量测矩阵，v为量测噪声阵。对建立的系统状态变量进行估计，需要对系统状态方程与量测方程进行离散化，因此将系统状态方程和量测方程用如下公式描述：

$\left\{\begin{array}{c}{x}_k=f\left(x_{k-1}\right)+g\left(x_{k-1}\right)w_{k-1}\\ z_k=H_k{x}_k+v_k\end{array}\right.$

式中，x_k、z_k分别表示为k时刻的状态向量和量测向量，f(·)、g(·)均为表示系统状态方程的非线性函数，w_k、v_k为k时刻的状态和量测噪声向量，系统模型为简单加性噪声模型。

第三步：采用UKF滤波（无迹卡尔曼滤波）算法进行状态估计：

记L为状态向量X的维数，对卡尔曼滤波器进行迭代，第k步的量测值为z_k，则x_k的卡尔曼滤波估计值按下述方程求解:

设计2L+1个Sigma对称采样点组成χ_k-1向量：

${\mathrm{\χ}}_{k-1}=\left[\begin{array}{ccc}{\hat{x}}_{k-1}& [{\hat{x}}_{k-1}{]}_L+\mathrm{\γ}\sqrt{P_{k-1}}& {\left[{\hat{x}}_{k-1}\right]}_L\end{array}-\mathrm{\γ}\sqrt{P_{k-1}}\right],$ P_k-1为k-1时刻的协方差阵。

由状态方程获取χ_k-1向量的一步预测

${\mathrm{\χ}}_{i,k|k-1}^{*}=f\left({\mathrm{\χ}}_{i,k-1}\right)$

获取一步预测的均值

${\hat{x}}_{k|k-1}={\mathrm{\Σ}}_{i=0}^{2L}{W}_i^{\left(m\right)}{\mathrm{\χ}}_{i,k|k-1}^{*}$

获取一步预测均方差阵P_k|k-1：

$P_{k|k-1}={\mathrm{\Σ}}_{i=0}^{2L}{W}_i^{\left(c\right)}({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){({\mathrm{\χ}}_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+g\left({\hat{x}}_{k-1}\right)Q_{k-1}g{\left({\hat{x}}_{k-1}\right)}^T,$ Q_k-1为k-1时刻的系统噪声序列方差矩阵。

获取状态预测和量测预测之间协方差阵和量测一步预测方差阵

$\begin{array}{cc}{P}_{{\hat{x}}_k{\hat{z}}_k}=P_{k|k-1}{H}_k^T,& P_{{\hat{z}}_k{\hat{z}}_k}=H_k{P}_{k|k-1}{H}_k^T+R_k,\end{array}$ R_k为k时刻的量测噪声序列方差矩阵。

获取卡尔曼滤波器的增益K_k：

$K_k=P_{{\hat{x}}_k{\hat{z}}_k}{P_{{\hat{z}}_k{\hat{z}}_k}}^{-1}$

获取系统状态的估计值

${\hat{x}}_k={\hat{x}}_{k|k-1}+K_k(z_k-H_k{\hat{x}}_{k|k-1})$

获取滤波器估计误差的方差P_k：

$P_k=P_{k|k-1}-K_k{P}_{{\hat{z}}_k{\hat{z}}_k}{K}_k^T$

其中，[x_k]_L表示每个列向量均为x_k的一个含L列的矩阵，f为系统状态方程，为均值比力系数。

其它参数计算如下：

$\left\{\begin{array}{c}\mathrm{\λ}={\mathrm{\α}}^2(L+\mathrm{\κ})-L\\ \mathrm{\γ}=\sqrt{L+\mathrm{\λ}}\\ \begin{array}{}\end{array}\begin{array}{cc}{W}_0^{\left(m\right)}=\mathrm{\λ}/(L+\mathrm{\λ}),& W_0^{\left(c\right)}=\mathrm{\λ}/(L+\mathrm{\λ})+(1-{\mathrm{\α}}^2+\mathrm{\β})\end{array}\\ W_i^{\left(m\right)}=W_i^{\left(c\right)}=1/\left[2(L+\mathrm{\λ})\right],(i=1,...,2L)\end{array}\right.$

式中，α用于确定Sigma点在其均值附近的分布情况（往往取为一个较小的正值，如1e-4≤α≤1），κ是一比例因子（一般在状态估计时设为0，在参数估计时设为3-L），β是另一个比例因子（用于合并状态分布的先验知识，对于高斯分布其最优值为2），表示矩阵P的平方根矩阵（如满足矩阵方程AA^T＝P的下三角cholesky分解矩阵A）。

通过上述方法，给定相应的初始条件通过滤波算法可以得到浮空飞行器惯导系统空中对准后的位置、速度和姿态信息，为后续的实时导航提供初始条件。

对本发明通过如下仿真进行验证：

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

仿真条件：

1)浮空飞行器处于20km高空，以15m/s的速度向前行进；

2)以5°/s的恒定角速率绕天向运动；

3)初始位置为经度116.343803°、纬度39.977584°；

4)初始姿态角分别为航向角45°、俯仰角0.5°、横滚角0.5°；

5)粗对准时间设为1分钟；

6)陀螺的随机常值漂移为0.5°/h，陀螺随机游走系数为加速度计常值偏置误

差为200ug，加速度计白噪声为50ug；

7)GPS的速度测量误差为0.01m/s，不考虑其他误差项；

8)赤道半径6378393m，地球曲率1.0/298.257，地球自转角速度（rad/s）7.292115147e-5。

在以上仿真条件下，GPS辅助的惯性凝固解析式粗对准结果如图3所示；图4为GPS辅助的惯性凝固解析式粗对准法的50次蒙特-卡洛仿真结果图；粗对准完成后，方位失准角为15°下的精对准结果如图5所示。

仿真结果表明，GPS辅助的惯性凝固解析式粗对准法能实时跟踪载体姿态，可大大缩短粗对准时间，且50次蒙特-卡洛仿真结果表明水平失准角变化范围较小，而方位失准角变化较大其范围为0～20°，图4结果表明该方法采用的精对准方法可有效解决大方位失准角问题。

Claims (3)

1.一种浮空飞行器捷联惯导空中初始对准方法，其特征在于：采用GPS辅助的惯性凝固解析式粗对准方法实现浮空飞行器惯导系统的空中粗对准，获取惯导姿态信息，而位置和速度信息直接由GPS获取；在粗对准基础上，考虑大方位失准角情况，建立以包括粗对准失准角误差的卡尔曼滤波状态方程，和以导航解算与GPS所测速度差为量测量的卡尔曼滤波量测方程，通过卡尔曼滤波，估计出失准角，进行精对准；

所述粗对准方法具体如下：

第一步：采用惯性凝固解析式粗对准方法，即将t时刻的捷联姿态矩阵拆写成如下三个矩阵相乘形式：

其中：n表示导航坐标系，b表示载体坐标系，i_n0、i_b0分别表示与粗对准时刻t₀的导航坐标系n(t₀)、载体坐标系b(t₀)重合的惯性坐标系；导航坐标系选为东北天地理坐标系；

第二步：依据GPS实时测得的载体所在点的经度和纬度，由粗对准经历的时间t实时确定；

第三步：利用陀螺输出的角运动信息，根据捷联惯导姿态更新算法可获取t时刻的

第四步：表示两个惯性坐标系之间在t₀时刻的变换矩阵，是一个常值阵；

所述的变换矩阵的计算过程如下：

惯导比力方程如下所示：

式中，fⁿ(t)为加速度计测得的在导航系的比力信息，Vⁿ(t)为载体在导航坐标系内的速度信息，由GPS实时测得，为载体在导航坐标系的加速度信息，由GPS速度差分获得，g为重力加速度信息；

将上式两边同时左乘且分别从t₀时刻到t时刻同时积分，得：

其中

f^b(t)为加速度计测得的在载体坐标系中的比力信息，分别取t＝t_l和t＝t_m两个对准过程中的不同时刻，构造两组速度差和根据矩阵构造算法即求得常值矩阵即，

所述的精对准具体包括如下步骤：

步骤一：建立卡尔曼滤波状态方程：

其中：n'为惯导模拟的数学平台，δVⁿ为惯导解算速度与GPS实测速度的速度差，为所述速度差δVⁿ的一阶微分，f^b为惯导系统测得的比力加速度，为地球自转角速度，为导航坐标系相对地球载体线速度引起的角速度，为惯导模拟的数学平台与理想导航坐标系间的转换矩阵，为导航解算的姿态更新矩阵，为计算参数误差，α为导航解算数学平台与理想导航坐标系间的失准角，为失准角α的一阶微分，为欧拉平台误差角与计算平台坐标系角运动之间的转换关系矩阵，为惯导加速度计输出误差，为加速度计输出误差的一阶微分，ε^b为惯导陀螺输出误差，为陀螺输出误差ε^b的一阶微分；

步骤二：建立卡尔曼滤波量测方程：

Z＝HX+v

其中：是卡尔曼滤波器的状态矢量，δL为惯导解算与GPS实测的纬度差，δλ为惯导解算与GPS实测的经度差，δh为惯导解算与GPS实测的高度差，为导航坐标系x、y、z方向的速度误差分量，α_x、α_y、α_z为导航坐标系x、y、z方向的失准角误差分量，为载体坐标系x、y、z方向的加速度计输出误差分量，为载体坐标系x、y、z方向的陀螺输出误差分量；量测量量测矩阵为H＝[0_3×3|I_3×3|0_3×9]为，v为量测噪声阵；

步骤三：采用UKF滤波算法进行状态估计。

2.根据权利要求1所述的一种浮空飞行器捷联惯导空中初始对准方法，其特征在于：第二步中的如下：

记λ₀和L₀分别为粗对准起始t₀时刻捷联惯导的经度和纬度，λ_t和L_t分别为对准t时刻捷联惯导的经度和纬度，则表示为：

其中，Δλ_t为t时刻经度相对变化量，Δλ_t＝λ_t-λ₀；依据GPS测得的实时纬度L_t与经度相对变化量便可求得对准t时刻的为地球自转角速率。

3.根据权利要求1所述的一种浮空飞行器捷联惯导空中初始对准方法，其特征在于：第三步中，利用陀螺输出的角运动信息，记运算符(V×)表示由矢量V构成的反对称矩阵，根据捷联惯导姿态更新算法，即，

获得t时刻的且在t₀时刻满足I为单位阵，为t时刻陀螺输出的角速率。