CN103217159A - 一种sins/gps/偏振光组合导航系统建模及动基座初始对准方法 - Google Patents

Abstract

一种SINS/GPS/偏振光组合导航系统建模及动基座初始对准方法，涉及一种车辆、舰船和航空飞行器用动基座初始对准方法。具体包括以下步骤：(1)建立SINS误差方程，作为初始对准状态方程。(2)根据偏振光传感器输出的偏振方位角、GPS输出的速度和位置，建立基于偏振方位角误差、速度误差和位置误差的初始对准量测方程。(3)采用卡尔曼滤波估计姿态误差、速度误差和位置误差。(4)对SINS进行姿态、速度和位置的反馈校正。由SINS向用户输出载体的姿态、速度和位置信息。本发明具有精度高、计算量小、抗干扰能力强的优点，可用于提高车辆、舰船和航空飞行器动基座初始对准精度，并减少初始对准时间。

Description

一种SINS/GPS/偏振光组合导航系统建模及动基座初始对准方法

技术领域

本发明涉及一种SINS/GPS/偏振光组合导航系统建模及动基座初始对准方法，可用于提高地球大气层内运载体(车辆、舰船和飞行器)的动基座初始对准精度，缩短初始对准时间。 

背景技术

在进入导航状态前，导航系统需要进行初始对准，通过初始对准得知初始导航参数，包括姿态、速度和位置。在动基座条件下，载体在运动过程中存在晃动、振动等多种形式的干扰。这些干扰因素会降低导航系统初始对准精度，延长对准时间。因此，动基座初始对准方法一直是组合导航技术领域的难点和热点问题。 

目前，动基座初始对准方法主要包括两大类：一类是基于主/子惯导系统的传递对准方法；另一类是惯导系统与GPS、磁罗盘、星敏感器等导航设备进行组合初始对准方法。传递对准方法中，由于导弹载体存在结构挠性变形、主惯导与子惯导之间存在数据传输延迟等因素影响，初始对准精度及时间往往不能得到保证。而惯导系统与GPS、磁罗盘和星敏感器等导航设备进行组合初始对准方法，同样存在问题：其中，GPS在载体作机动时误差较大，且与惯导系统信号同步性不好；磁罗盘依赖于地磁场模型精度，特别是在地磁场异常的情况下，将出现较大误差，因此用于动基座初始对准效果并不理想；星敏感器可高精度估计载体姿态误差，但并不适用于在地球大气层内白昼时间段内的动基座初始对准。基于上述原因，SINS结合新型导航装置，设计更合适的信息匹配方式是动基座初始对准技术的发展趋势之一。 

偏振光导航是一种仿生导航方式。太阳光线入射大气层时，大气中的粒子会使入射光线散射，造成太阳光线的偏振现象，沙蚁等生物可利用天空中 的偏振光进行导航。根据仿生导航原理设计的偏振光传感器，可敏感到传感器观测方向的入射光最大偏振方向，输出偏振方位角。基于偏振方位角的偏振光导航具有自主性强、误差不随时间积累等优点，尤其适用于高精度的姿态确定。但偏振方位角对定位误差极不敏感，可达数十米。SINS的误差随时间积累，位置信息是发散的。因此偏振光传感器仅与SINS组合进行动基座初始对准会造成精度不高，甚至可能出现滤波结果发散的情况。 

综上，目前常用的动基座初始对准方法存在对准精度低、对准时间长等缺点，而偏振光传感器对位置误差极不敏感，仅与SINS组合进行动基座初始对准效果同样不好。 

发明内容

本发明要解决的技术问题是：克服现有方法的不足，提供一种精度高、用时短的SINS/GPS/偏振光组合导航系统动基座初始对准方法。 

本发明解决其技术问题所采用的技术方案为：一种SINS/GPS/偏振光组合导航系统建模及动基座初始对准方法，其特点在于包括下列步骤： 

(1)建立捷联惯导系统误差状态方程； 

(2)根据偏振光传感器输出的偏振方位角、GPS输出的速度和位置，建立系统量测方程，观测量包括偏振方位角误差、速度误差和位置误差； 

(3)采用卡尔曼滤波器估计平台失准角、速度误差和位置误差，并对SINS进行反馈校正。由SINS向用户输出载体的姿态、速度和位置信息。 

上述步骤(2)中的系统量测方程为： 

Z＝HX+V 

其中， $X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$ 为系统状态矢量，φ_E，φ_N，φ_U分别为东向失准角、北向失准角和天向失准角。ε_x、ε_y、ε_z分别为东向、北向、天向陀螺常值漂移。

分别为东向、北向、天向加速度计常值漂移。 $V={\left[\begin{array}{ccccccc}{v}_{\mathrm{\α}}& v_{V_E}& v_{V_N}& v_{V_U}& v_L& v_{\mathrm{\λ}}& v_h\end{array}\right]}^T$ 为量测噪声向量。H为系统量测矩阵，表示为： 

$H=\left[\begin{array}{c}{H}_1\\ H_2\\ H_3\end{array}\right]=\left[\begin{array}{cccc}{H}_{P1}& 0_{1\×3}& H_{P2}& 0_{1\×6}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×6}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×6}\end{array}\right]$

系统量测矩阵H中，H₁＝[H_P1  0_1×3  H_p2  0_1×6]为1×15维矩阵，是以偏振方位角误差为观测量的量测矩阵，H₁中 $H_{p1}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_E}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_N}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_U}\end{array}\right],$ $H_{p2}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}& 0\end{array}\right],$ H₂＝[0_3×3  I_3×3  0_3×3  0_3×6]为3×15维矩阵，是以速度误差为观测量的量测矩阵，H₃＝[0_3×3  0_3×3  I_3×3  0_3×6]为3×15维矩阵，是以位置误差为观测量的量测矩阵，I_3×3为3×3维单位矩阵。将H₁、H₂和H₃合并后，组成SINS/GPS/偏振光组合导航系统的量测矩阵。 

上述步骤(2)中，建立以偏振方位角误差为观测量的量测方程，具体步骤为： 

首先，建立以偏振方位角误差为观测量的非线性量测方程： 

$\mathrm{\δ\α}\left(X\right)={\mathrm{\α}}_{\mathrm{SINS}}-{\mathrm{\α}}_{\mathrm{POLAR}}=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\′}}^b{C}_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\′}}^b{C}_e^c{k}_{\mathrm{sun}}^e}\right)-\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\′}}^b(I-{\mathrm{\φ}}^{\×}+\mathrm{\δ}{\mathrm{\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\′}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}\right)+{\mathrm{\υ}}_1$

其中，α_SINS为SINS输出的姿态、位置信息计算的偏振方位角，α_POLAR为偏振光传感器输出的偏振方位角。υ₁为偏振光传感器的量测噪声。T₁＝[0  -1  0]，T₂＝[1  0  0]。

为载体坐标系至偏振光传感器坐标系的变换矩阵， 

为平台坐标系至载体坐标系的变换矩阵，

为地球固连坐标系至计算地理坐标系的变换矩阵。φ^×为平台失准角向量[φ_E  φ_N  φ_U]^T的反对称矩阵。δθ^×为位置误差向量[-δL δλcosL δλsinL]^T的反对称矩阵。

是由载体质心指向太阳的单位矢量在地球固连坐标系下的投影。 

其次，将δα(X)在系统状态变量零点，即X₀＝[0_1×15]^T处进行泰勒展开，得到以偏振方位角误差为观测量的线性化量测方程： 

Z₁＝H₁X+υ_α

其中，Z₁＝δα(X)，υ_α为量测噪声，包括量测方程线性化展开后省略的 高阶无穷小项以及偏振光传感器的量测噪声。H₁＝[H_p1  0_1×3  H_p2  0_1×6]。  $H_{p1}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_E}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_N}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_U}\end{array}\right],$ $H_{p2}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}& 0\end{array}\right].$ 与系统量测矩阵H的第一行对应。 

以偏振方位角误差为观测量的量测方程中，由载体质心指向太阳方向的单位矢量在地球固连坐标系下的投影的计算方法为： 

$k_{\mathrm{sun}}^e=\left[\begin{array}{ccc}\sin \left(L\right)\cos \left(\mathrm{\λ}\right)& -\sin \left(\mathrm{\λ}\right)& -\cos \left(L\right)\cos \left(\mathrm{\λ}\right)\\ \sin \left(L\right)\sin \left(\mathrm{\λ}\right)& -\cos \left(\mathrm{\λ}\right)& \cos \left(L\right)\sin \left(\mathrm{\λ}\right)\\ -\cos \left(L\right)& 0& -\sin \left(L\right)\end{array}\right]{\left[\begin{array}{ccc}\cos (-h_s)\cos \left(A_s\right)& \cos (-h_s)\sin \left(A_s\right)& \sin \left({-h}_s\right)\end{array}\right]}^T$

其中，L为地理纬度，λ为经度。h_s为地平坐标系下的太阳高度角，h_s∈[-π/2，π/2]。A_s为地平坐标系下的太阳方位角，A_s∈[-π，π]。 

在估计出导航系统状态变量后，对SINS进行反馈校正，由SINS向用户输出载体的姿态、速度和位置信息，并进入下一次导航计算。其中，偏振光传感器需要时间信息及位置信息计算偏振方位角，所需时间信息可由GPS实时提供，所需位置信息由经过反馈校正后的SINS位置信息提供。 

本发明与现有技术相比的优点在于：本发明将偏振光传感器与SINS、GPS相结合，其中偏振光传感器输出的偏振方位角误差不随时间积累，且具有高度自主性，具备高精度姿态校正能力；GPS提供速度、位置信息，弥补了偏振方位角信息对位置误差不敏感及SINS速度、位置误差随时间积累并发散的缺点，从而提高载体动基座初始对准的估计精度，缩短对准时间。 

附图说明

图1为本发明的设计流程图。 

图2为本发明的原理框图。 

图4为太阳高度角、太阳方位角算法流程图。 

图3为卡尔曼滤波算法流程图。 

具体实施方式

如图1、2、3所示，本发明的具体方法如下： 

(1)SINS/GPS/偏振光组合导航系统的状态方程建立，如式1所示。 

系统状态方程： 

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(1\right)$

其中，F为系统状态矩阵，X为系统状态矢量，G为系统噪声变换矩阵，W 

为系统噪声矢量： 

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

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

(2)建立以偏振方位角误差为观测量的方程 

根据瑞利散射理论，设由载体质心指向太阳方向的矢量为k_sun，偏振光传感器的观测方向的矢量为k_ob，则与偏振光传感器观测方向平行的入射光的最大偏振方向矢量k_pol垂直于k_sun与k_ob形成的平面。瑞利散射理论在偏振光传感器坐标系(q系)中可表示为： 

$k_{\mathrm{pol}}^q=k_{\mathrm{ob}}^q\×k_{\mathrm{sun}}^q---\left(2\right)$

根据坐标变换原理，由偏振光传感器指向太阳方向的矢量

满足： 

$k_{\mathrm{sun}}^q=C_b^q{C}_n^b{C}_e^n{k}_{\mathrm{sun}}^e---\left(\text{3}\right)$

其中，

是由载体质心指向太阳方向的矢量在偏振光传感器坐标系下的投影。

为载体坐标系至偏振光传感器坐标系的变换矩阵，

为真实地理坐标系至载体坐标系的变换矩阵，

为地球固连坐标系至真实地理坐标系的变换矩阵，

为载体质心指向太阳方向的单位矢量在地球固连坐标系下的投影。 

由式2和式3可知，确定与偏振光传感器观测方向平行的入射光的最大偏振方向矢量在偏振光传感器坐标系下的投影

需要求得

和

为简化分析过程，假设和

均为单位向量。而

的求解首先需要求解太阳高度角h_s、太阳方位角A_s。下面分别阐述计算方法： 

a.太阳高度角h_s、太阳方位角A_s的计算方法： 

根据天文学可知，在已知载体所处地理位置及当地时间的情况下，可计算出太阳高度角h_s、太阳方位角A_s。在SINS/GPS/偏振光组合导航系统中，偏振光传感器所需的时间信息由GPS提供，位置信息由经过校正后的SINS位置信息提供。太阳高度角h_s、太阳方位角A_s。计算方法为：该算法流程如图3所示。 

①计算积日t 

式中，符号

表示实数向下取整。YEAR为年份，MONTH为月份，DATE为日期。C的取值方法是：如果年份是闰年，且月份在3至12月，则C取31.8。如果年份不是闰年，且月份是1月，C取30.6，如果年份不是闰年，且月份是2至12月，C取32.8。HOUR、MINIUTE表示当地时间的小时和分钟。±λ_degree为经度，单位是度。东经取负号，西经取正号。 

②计算日角θ 

θ＝2πt/365.2422    (2) 

式中，t为积日。 

③计算真太阳时与平太阳时的时差Eq： 

Eq＝0.0028-1.9857×sin(θ)+9.9059×sin(2θ)-7.0924×cos(θ)-0.6882×cos(2θ)    (6) 

式中，θ为日角。 

④计算太阳赤纬角δ： 

日地中心的连线与赤道面间的夹角称为太阳赤纬角。太阳赤纬角的计算方法为： 

δ＝0.3723+23.2567×sin(θ)+0.1149×sin(2θ)-0.1712×sin(3θ)    (7) 

-0.758×cos(θ)+0.3656×cos(2θ)+0.0201×cos(3θ) 

式中，θ为日角。 

⑤计算太阳时角Ω： 

太阳时角的计算方法分为两步： 

首先计算真太阳时S_T

S_T＝HOUR+(MINIUTE-(120°-λ_degree)×4)/60+Eq/60    (3) 

其中，HOUR、MINIUTE表示当地时间的小时和分钟。λ_degree为经度，单位为度。Eq为真太阳时与平太阳时的时差。 

然后计算太阳时角Ω： 

Ω＝(S_T-12)×15    (9) 

⑥计算太阳高度角h_s： 

h_s＝arcsin(sinLsinδ+cosLcosδcosΩ)    (10) 

式中，L为地理纬度，δ为太阳赤纬角，Ω为太阳时角。 

⑦计算太阳方位角A_s： 

$A_s=\±\arccos \left(\frac{{\sinh }_s\sin L-\sin \mathrm{\δ}}{{\cosh }_s\cos L}\right)---\left(4\right)$

当S_T在0时到12时之间，A_s取负号，当S_T在12时到次日零时之间，A_s取正号。 

b.

的计算方法： 

太阳高度角h_s和太阳方位角A_s是在地平坐标系下表示的。地平坐标系原点O位于载体质心，X轴指向正南方，Y轴指向正西方，Z轴指向天底方向。由于地平坐标系不满足X-Y-Z右手定则，不便于进行坐标转换，因此定义第二地平坐标系：原点O位于载体质心，X轴指向为正南方，Y轴指向为正西方，Z轴指向为天底方向。则由载体质心指向太阳的单位矢量在第二地平坐标系下的投影为： 

$k_{\mathrm{sun}}^d={\left[\begin{array}{ccc}\cos (-h_s)\cos \left(A_s\right)& \cos \left({-h}_s\right)\sin \left(A_s\right)& \sin \left({-h}_s\right)\end{array}\right]}^T$

将

投影到地球固连坐标系的方法为：设第二地平坐标系为OX_dY_dZ_d，首先，绕Y_d轴顺时针旋转π/2-L，形成的坐标系为OX_d1Y_d1Z_d1。然后，绕X_d1轴逆时 针旋转π，形成的坐标系为OX_d2Y_d2Z_d2。最后，绕Z_d2轴逆时针旋转λ，形成地球固连坐标系。通过上述坐标变换方法，

和

的关系可表示为： 

$k_{\mathrm{sun}}^e=\left[\begin{array}{ccc}\cos \left(\mathrm{\λ}\right)& -\sin \left(\mathrm{\λ}\right)& 0\\ \sin \left(\mathrm{\λ}\right)& \cos \left(\mathrm{\λ}\right)& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \left(\mathrm{\π}\right)& -\sin \left(\mathrm{\π}\right)\\ 0& \sin \left(\mathrm{\π}\right)& \cos \left(\mathrm{\π}\right)\end{array}\right]\left[\begin{array}{ccc}\cos (\mathrm{\π}/2-L)& 0& -\sin (\mathrm{\π}/2-L)\\ 0& 1& 0\\ \sin (\mathrm{\π}/2-L)& 0& \cos (\mathrm{\π}/2-L)\end{array}\right]k_{\mathrm{sun}}^d$ (13) 

$=\left[\begin{array}{ccc}\sin \left(L\right)\cos \left(\mathrm{\λ}\right)& -\sin \left(\mathrm{\λ}\right)& -\cos \left(L\right)\cos \left(\mathrm{\λ}\right)\\ \sin \left(L\right)\sin \left(\mathrm{\λ}\right)& -\cos \left(\mathrm{\λ}\right)& \cos \left(L\right)\sin \left(\mathrm{\λ}\right)\\ -\cos \left(L\right)& 0& -\sin \left(L\right)\end{array}\right]{\left[\begin{array}{ccc}\cos \left({-h}_s\right)\cos \left(A_s\right)& \cos (-h_s)\sin \stackrel{}{\left(A_s\right)}& \sin (-h_s)\end{array}\right]}^T$

其中，L为地理纬度，λ为经度。 

C.

的计算方法： 

偏振光传感器的观测方向单位矢量定义为偏振光传感器坐标系中沿Z轴方向单位矢量，在偏振光传感器坐标系中可表示为： 

$k_{\mathrm{ob}}^q={\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T---\left(14\right)$

在计算出

和

后，将式13、式14共同代入式2，可得： 

式中，T₁＝[0  -1  0]，T₂＝[1  0  0]。因此，偏振方位角可用偏振光传感器坐标系下的入射光最大偏振方向矢量表示： 

${\mathrm{\α}}_{\mathrm{POLAR}}=\arctan \left(\frac{T_2{C}_b^q{C}_n^b{C}_e^n{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_n^b{C}_e^n{k}_{\mathrm{sun}}^e}\right)---\left(16\right)$

在式16中，

均是根据载体姿态和位置的真值计算出的变换矩阵，而通过SINS输出的姿态和位置只可计算出平台坐标系至载体坐标系的姿态变换矩阵以及地球固连坐标系至计算地理坐标系的变换矩阵

与平台失准角的关系为： 

$C_n^b=C_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×})---\left(17\right)$

其中，φ^×为平台失准角向量[φ_H  φ_N  φ_U]^T的反对称矩阵，φ_E，φ_N，φ_U分别为东向失准角、北向失准角和天向失准角。 

与位置误差的关系为： 

$C_e^n=(I+{\mathrm{\δ\θ}}^{\×})C_e^c---\left(18\right)$

其中，δθ^×为位置误差向量[-δL δλcosL δλsinL]^T的反对称矩阵。δL为纬度误差，δL为经度误差，L为地理纬度。 

将式17、式18共同代入式16，可得： 

${\mathrm{\α}}_{\mathrm{POLAR}}=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×})(I+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×})(I+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}\right)---\left(19\right)$

将式19展开，并忽略式中的高阶小量，可得 

${\mathrm{\α}}_{\mathrm{POLAR}}=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}\right)---\left(20\right)$

根据SINS输出的姿态、位置信息计算的偏振方位角α_SINS为： 

${\mathrm{\α}}_{\mathrm{SINS}}=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}\right)---\left(21\right)$

因此，可建立以偏振方位角误差为观测量的量测方程： 

$\mathrm{\δ\α}\left(X\right)={\mathrm{\α}}_{\mathrm{SINS}}-{\mathrm{\α}}_{\mathrm{POLAR}}$

$=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}\right)-\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}\right)+{\mathrm{\υ}}_1---\left(22\right)$

其中，υ₁为偏振光传感器的量测噪声。式22为系统状态变量X的非线性方程，为便于进行卡尔曼滤波，对其进行线性化。将式22在系统状态变量的零点处，即X₀＝[0_1×15]^T处进行泰勒展开，可得： 

$\mathrm{\δ\α}\left(X\right)=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}\right)-\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e}\right)$ (23) 

$-(\frac{\∂\mathrm{\δ\α}}{\∂{\mathrm{\φ}}_E}{\mathrm{\φ}}_E+\frac{\∂\mathrm{\δ\α}}{{\∂\mathrm{\φ}}_N}{\mathrm{\φ}}_N+\frac{\∂\mathrm{\δ\α}}{{\∂\mathrm{\φ}}_U}{\mathrm{\φ}}_U+\frac{\∂\mathrm{\δ\α}}{\∂\mathrm{\δL}}\mathrm{\δL}+\frac{\∂\mathrm{\δ\α}}{\∂\mathrm{\δ\λ}}\mathrm{\δ\λ})+{\mathrm{\υ}}_{\mathrm{\α}}$

化简后可得到以偏振方位角误差为观测量的线性化量测方程： 

Z₁＝H₁X+υ_α    (24) 

其中，Z₁＝δ_α(X)，υ_α为包括量测方程线性化展开后省略的高阶无穷小项以及偏振光传感器的量测噪声。 

$H_1=\left[\begin{array}{ccccccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_E}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_N}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_U}& 0_{1\×3}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}& 0_{1\×7}\end{array}\right]$

为便于与速度误差和位置误差量测方程合并，令 

$H_{p1}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_E}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_N}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_U}\end{array}\right]$

$H_{p2}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}& 0\end{array}\right]$

则H₁＝[H_P1 0_1×3H_P2 0_1×6]。H₁中的非零元素为： 

$\frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_E}=((q_2{b}_3{c}_2{k}_{\mathrm{sun}}^e-q_2{b}_2{c}_3{k}_{\mathrm{sun}}^e)q_1{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e-(q_1{b}_3{c}_2{k}_{\mathrm{sun}}^e-q_1{b}_2{c}_3{k}_{\mathrm{sun}}^e)q_2{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e)/M$

$\frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_N}=((q_1{b}_3{c}_1{k}_{\mathrm{sun}}^e-q_1{b}_1{c}_3{k}_{\mathrm{sun}}^e)q_2{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e-(q_2{b}_3{c}_1{k}_{\mathrm{sun}}^e-q_2{b}_1{c}_3{k}_{\mathrm{sun}}^e)q_1{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e)/M$

$\frac{\∂\mathrm{\δ\α}\left(X\right)}{{\∂\mathrm{\φ}}_U}=((q_1{b}_2{c}_3{k}_{\mathrm{sun}}^e-q_1{b}_3{c}_2{k}_{\mathrm{sun}}^e)q_2{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e-(q_2{b}_2{c}_3{k}_{\mathrm{sun}}^e-q_2{b}_3{c}_2{k}_{\mathrm{sun}}^e)q_1{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e)/M$

$\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}=((q_1{b}_2{c}_3{k}_{\mathrm{sun}}^e-q_1{b}_3{c}_2{k}_{\mathrm{sun}}^e)q_2{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e-(q_2{b}_2{c}_3{k}_{\mathrm{sun}}^e-q_2{b}_3{c}_2{k}_{\mathrm{sun}}^e)q_1{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e)/M$

$\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}=((q_2{b}_3{c}_1{k}_{\mathrm{sun}}^e-q_2{b}_1{c}_3{k}_{\mathrm{sun}}^e)q_1{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e-{(q_1{b}_3{c}_1{k}_{\mathrm{sun}}^e-q_1{b}_1{c}_3{k}_{\mathrm{sun}}^e)}_{}{q}_2{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e)\cos L/M$

$-((q_2{b}_2{c}_1{k}_{\mathrm{sun}}^e-q_2{b}_1{c}_2{k}_{\mathrm{sun}}^e){q_1}^{}{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e-(q_1{b}_2{c}_1{k}_{\mathrm{sun}}^e-q_1{b}_1{c}_2{k}_{\mathrm{sun}}^e)q_2{C}_{n^{\text{'}}}^b{C}_e^c{k}_{\mathrm{sun}}^e)\sin L/M$

在以上非零元素中， $M={\left(q_2{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e\right)}^2+{\left(q_1{C}_{n^{\text{'}}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e\right)}^2.$

为载体坐标系至偏振光传感器坐标系的变换矩阵，

为平台坐标系至载体坐标系的变换矩阵，

为地球固连坐标系至计算地理坐标系的变换矩阵。 

是由载体质心指向太阳方向的单位矢量在地球固连坐标系下的投影。φ^×为平台失准角向量[φ_E  φ_N  φ_U]^T的反对称矩阵。δθ^×为位置误差向量[-δL δλcosL δλsinL]^T的反对称矩阵。q_i，i＝1，2，3为

的第i行。c_i，i＝1，2，3为

的第i行。b_i，i＝1，2，3为

的第i列。 

(3)SINS/GPS/偏振光组合导航系统的量测方程建立。 

建立以速度误差为观测量的量测方程： 

Z₂＝H₂X+V₂    (25) 

其中，Z₂＝[δV_E  δV_N  δV_U]^T为量测向量，H₂＝[0_3×3  I_3×3  0_3×3  0_3×6]为速度误 差量测矩阵。δV_E、δV_N、δV_U分别为东向、北向、天向速度误差。 $V_2={\left[\begin{array}{ccc}{\mathrm{\υ}}_{V_E}& {\mathrm{\υ}}_{V_N}& {\mathrm{\υ}}_{V_U}\end{array}\right]}^T$ 为速度量测噪声向量。 

建立以位置误差为观测量的量测方程： 

Z₃＝H₃X+V₃    (26) 

其中，Z₃＝[δL  δλ  δh]^T为量测向量，H₃＝[0_3×3  0_3×3  I_3×3  0_3×6]为位置误差量测矩阵。δL为纬度误差，δλ为经度误差，δh为高度误差。V₃＝[υ_L  υ_λ  υ_h]^T为位置量测噪声向量。 

将式24、式25、式26合并，得到SINS/GPS/偏振光组合导航系统量测方程： 

Z＝HX+V    (27) 

其中：Z为系统观测矢量，H为系统量测矩阵，V为量测噪声矢量： 

Z＝[δα  δV_E  δV_N  δV_u  δL  δλ  δh]^T

$H=\left[\begin{array}{cccc}{H}_{P1}& 0_{1\×3}& H_{P2}& 0_{1\×6}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×6}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×6}\end{array}\right]$

$V={\left[\begin{array}{ccccccc}{\mathrm{\υ}}_{\mathrm{\α}}& {\mathrm{\υ}}_{V_E}& {\mathrm{\υ}}_{V_N}& {\mathrm{\υ}}_{V_U}& {\mathrm{\υ}}_L& {\mathrm{\υ}}_{\mathrm{\λ}}& {\mathrm{\υ}}_h\end{array}\right]}^T$

对系统状态方程式1和系统量测方程式27进行离散化，可得 

X(k)＝Φ_k，k-1X_k-1+Γ_k-1W_k-1    (28) 

Z_k＝H_k+V_k    (29) 

式中，W_k，V_k为均值为零的白噪声序列，且W_k，V_k互不相关。系统噪声序列W_k的方差阵为E[W_kW_k ^T]＝Q_k，量测噪声序列的协方差阵为E[V_kV_k ^T]＝R_k。 

(4)卡尔曼滤波器的基本编排：该算法流程如图4所示。 

状态一步预测方程： 

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}---\left(30\right)$

状态估计计算方程： 

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})---\left(31\right)$

滤波增益方程： 

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(32\right)$

一步预测均方误差阵方程： 

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(33\right)$

估计均方误差阵方程： 

$P_k=(I-K_k{H}_k)P_{k/k-1}(I-K_k{H}_k{)}^T+K_k{R}_k{K}_k^T---\left(34\right)$

(5)在卡尔曼滤波器估计出平台失准角、速度误差和位置误差后，采用反馈校正的方式进行动基座初始对准。 

设卡尔曼滤波器估计出的系统状态变量为： 

$\hat{X}={\left[\begin{array}{ccccccccccccccc}{\widehat{\mathrm{\φ}}}_E& {\widehat{\mathrm{\φ}}}_N& {\widehat{\mathrm{\φ}}}_U& {\mathrm{\δ}\hat{V}}_E& {\mathrm{\δ}\hat{V}}_N& {\mathrm{\δ}\hat{V}}_U& \mathrm{\δ}\hat{L}& \mathrm{\δ}\widehat{\mathrm{\λ}}& \mathrm{\δ}\hat{h}& {\widehat{\mathrm{\ϵ}}}_x& {\widehat{\mathrm{\ϵ}}}_y& {\widehat{\mathrm{\ϵ}}}_z& {\widehat{\▿}}_x& {\widehat{\▿}}_y& {\widehat{\▿}}_z\end{array}\right]}^T,$ 则姿态校正方法、速度校正方法及位置校正方法如下所述： 

a.姿态校正方法 

首先计算平台坐标系至真实地理坐标系的转换矩阵： 

$C_{n^{\text{'}}}^n=\left[\begin{array}{ccc}1& -{\widehat{\mathrm{\φ}}}_U& {\widehat{\mathrm{\φ}}}_N\\ {\widehat{\mathrm{\φ}}}_U& 1& {\widehat{\mathrm{\φ}}}_E\\ -{\widehat{\mathrm{\φ}}}_N& {\widehat{\mathrm{\φ}}}_E& 1\end{array}\right]$

则校正后的姿态矩阵

可表示为： 

${\stackrel{\‾}{C}}_b^n=C_{n^{\text{'}}}^n{C}_b^{n^{\text{'}}}$

其中，

为根据SINS输出的姿态计算出的载体坐标系至平台坐标系的变换矩阵。将校正后的姿态矩阵

代入下一次捷联解算过程，即完成姿态校正。 

b.速度校正方法 

设校正后的东向、北向、天向速度为V_adE、V_adN、V_adU，则速度校正可表示为： 

$\left[\begin{array}{c}{V}_{\mathrm{adE}}\\ V_{\mathrm{adN}}\\ V_{\mathrm{adU}}\end{array}\right]=\left[\begin{array}{c}{V}_{\mathrm{SINS}\_E}\\ V_{\mathrm{SINS}\_N}\\ V_{\mathrm{SINS}\_U}\end{array}\right]-\left[\begin{array}{c}\mathrm{\δ}{\hat{V}}_E\\ \mathrm{\δ}{\hat{V}}_N\\ \mathrm{\δ}{\hat{V}}_U\end{array}\right]$

其中，V_{SINS_E}为SINS输出的东向速度，V_{SINS_N}为SINS输出的北向速度，V_{SINS_U}为SINS输出的天向速度。 

c.位置校正方法 

设校正后的纬度、经度和高度分别为L_ad、λ_ad和h_ad，则位置校正可表示为： 

$\left[\begin{array}{c}{L}_{\mathrm{ad}}\\ {\mathrm{\λ}}_{\mathrm{ad}}\\ h_{\mathrm{ad}}\end{array}\right]=\left[\begin{array}{c}{L}_{\mathrm{SINS}}\\ {\mathrm{\λ}}_{\mathrm{SINS}}\\ h_{\mathrm{SINS}}\end{array}\right]-\left[\begin{array}{c}\mathrm{\δ}\hat{L}\\ \mathrm{\δ}\widehat{\mathrm{\λ}}\\ \mathrm{\δ}\hat{h}\end{array}\right]$

(6)在导航系统进行反馈校正后，进入下一次导航计算过程，偏振光传感器所需的时间信息可由GPS实时提供，所需的位置信息由经过反馈校正后的SINS位置信息提供。 

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。 

Claims (4)

1.一种SINS/GPS/偏振光组合导航系统建模及动基座初始对准方法，其特征在于包括下列步骤：

(1)建立捷联惯导系统误差状态方程；

(2)根据偏振光传感器输出的偏振方位角、GPS输出的速度和位置，建立系统量测方程，观测量包括偏振方位角误差、速度误差和位置误差；

(3)采用卡尔曼滤波器估计平台失准角、速度误差和位置误差，并对SINS进行反馈校正。由SINS向用户输出载体的姿态、速度和位置信息。

2.根据权利要求1所述的SINS/GPS/偏振光组合导航系统的动基座初始对准方法，其特征在于所述步骤(2)中的系统量测方程为：

Z＝HX+V

其中， $X={\left[\begin{array}{ccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$ 为系统状态矢量，φ_E，φ_N，φ_U分别为东向失准角、北向失准角和天向失准角。ε_x、ε_y、ε_z分别为东向、北向、天向陀螺常值漂移。分别为东向、北向、天向加速度计常值漂移。 $V={\left[\begin{array}{ccccccc}{v}_{\mathrm{\α}}& v_{V_E}& v_{V_N}& v_{V_U}& v_L& v_{\mathrm{\λ}}& v_h\end{array}\right]}^T$ 为量测噪声向量。H为系统量测矩阵，具体表示为：

$H=\left[\begin{array}{c}{H}_1\\ H_2\\ H_3\end{array}\right]=\left[\begin{array}{cccc}{H}_{P1}& 0_{1\×3}& H_{P2}& 0_{1\×6}\\ 0_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×6}\\ 0_{3\×3}& 0_{3\×3}& I_{3\×3}& 0_{3\×6}\end{array}\right]$

系统量测矩阵H中，H₁＝[H_P1  0_1×3  H_p2  0_1×6]为1×15维矩阵，是以偏振方位角误差为观测量的量测矩阵。H₁中 $H_{p1}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_E}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_N}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_U}\end{array}\right],$ $H_{p2}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}& 0\end{array}\right].$ H₂＝[0_3×3  I_3×3  0_3×3  0_3×6]为3×15维矩阵，是以速度误差为观测量的量测矩阵，H₃＝[0_3×3  0_3×3  I_3×3  0_3×6]为3×15维矩阵，是以位置误差为观测量的量测矩阵，I_3×3为3×3维单位矩阵。将H₁、H₂和H₃合并后，组成SINS/GPS/偏振光组合导航系统的量测矩阵。

3.根据权利要求2所述的SINS/GPS/偏振光组合导航系统的动基座初始对准方法，其特征在于：系统量测方程中，建立以偏振方位角误差为观测量的量测方程，具体方法为：

首先，建立以偏振方位角误差δα为观测量的非线性量测方程：

$\mathrm{\δ\α}\left(X\right)={\mathrm{\α}}_{\mathrm{SINS}}-{\mathrm{\α}}_{\mathrm{POLAR}}=\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\′}}^b{C}_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\′}}^b{C}_e^c{k}_{\mathrm{sun}}^e}\right)-\arctan \left(\frac{T_2{C}_b^q{C}_{n^{\′}}^b(I-{\mathrm{\φ}}^{\×}+\mathrm{\δ}{\mathrm{\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}{T_1{C}_b^q{C}_{n^{\′}}^b(I-{\mathrm{\φ}}^{\×}+{\mathrm{\δ\θ}}^{\×})C_e^c{k}_{\mathrm{sun}}^e}\right)+{\mathrm{\υ}}_1$

其中，α_SINS为根据SINS输出的姿态、位置信息计算的偏振方位角，α_POLAR为偏振光传感器输出的偏振方位角。υ₁为偏振光传感器的量测噪声。T₁＝[0  -1  0]，T₂＝[1  0  0]。

为载体坐标系至偏振光传感器坐标系的变换矩阵，

为平台坐标系至载体坐标系的变换矩阵，

为地球固连坐标系至计算地理坐标系的变换矩阵。φ^×为平台失准角向量[φE  φ_N  φ_U]^T的反对称矩阵。δθ^×为位置误差向量[-δL δλcosLδλsinL]^T的反对称矩阵。

是由载体质心指向太阳的单位矢量在地球固连坐标系下的投影。

其次，将δα(X)在系统状态变量零点X₀＝[0_1×15]^T处进行泰勒展开，得到以偏振方位角误差为观测量的线性化量测方程：

Z₁＝H₁X+υ_α

其中，Z₁＝δα(X)，υ_α为量测噪声，包括量测方程线性化展开后省略的高阶无穷小项以及偏振光传感器的量测噪声。H₁＝[H_p1  0_1×3  H_p2  0_1×6]。 $H_{p1}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_E}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_N}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂{\mathrm{\φ}}_U}\end{array}\right],$ $H_{p2}=\left[\begin{array}{ccc}\frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δL}}& \frac{\∂\mathrm{\δ\α}\left(X\right)}{\∂\mathrm{\δ\λ}}& 0\end{array}\right].$ 与系统量测矩阵H的第一行对应。

4.根据权利要求3所述的建立以偏振方位角误差为观测量的量测方程，其特征在于：由载体质心指向太阳方向的单位矢量在地球固连坐标系下的投影

的计算方法为：

$k_{\mathrm{sun}}^e=\left[\begin{array}{ccc}\sin \left(L\right)\cos \left(\mathrm{\λ}\right)& -\sin \left(\mathrm{\λ}\right)& -\cos \left(L\right)\cos \left(\mathrm{\λ}\right)\\ \sin \left(L\right)\sin \left(\mathrm{\λ}\right)& -\cos \left(\mathrm{\λ}\right)& \cos \left(L\right)\sin \left(\mathrm{\λ}\right)\\ -\cos \left(L\right)& 0& -\sin \left(L\right)\end{array}\right]{\left[\begin{array}{ccc}\cos (-h_s)\cos \left(A_s\right)& \cos (-h_s)\sin \left(A_s\right)& \sin (-h_s)\end{array}\right]}^T$

其中，L为地理纬度，λ为经度。h_s为地平坐标系下的太阳高度角，h_s∈[-π/2，π/2]。A_s为地平坐标系下的太阳方位角，A_s∈[-π，π]。

Images