一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法

摘要

本发明提出一种基于全面最优校正的SINS/CNS组合导航系统及其导航方法，属于组合导航技术领域。该组合导航系统包括天文导航子系统、惯性导航子系统和信息融合子系统；该导航方法包括：基于星光折射的解析天文定位、导航系统状态方程建立、导航系统量测方程的建立和基于卡尔曼滤波的组合导航系统信息融合。本发明利用星光折射间接敏感地平的基本原理和大视场星敏感器可以同时观测多颗恒星的特点，将星光折射间接敏感地平方法应用于不满足轨道动力学模型的飞行器，解决了天文导航系统高精度自主地平的问题。且本发明充分利用天文导航系统的位置和姿态信息，对SINS误差进行全面最优校正，显著地提高了组合导航精度。

主权项

一种基于全面最优校正的SINS/CNS组合导航方法，该方法的实施是利用基于全面最优校正的SINS/CNS组合导航系统，该系统包括天文导航子系统、惯性导航子系统和信息融合子系统；天文导航子系统和惯性导航子系统为信息融合子系统提供位置、姿态信息，信息融合子系统为惯性导航子系统提供估计误差；所述天文导航子系统，包括大视场星敏感器、大气折射模型、数字滤波器、天文定位单元和天文定姿单元；大视场星敏感器将观测到的星光折射角提供给大气折射模型；大气折射模型根据星光折射角计算折射恒星的视高度，并将其提供给数字滤波器；数字滤波器对视高度信息进行降噪处理，并将处理后的信息发送给天文定位单元；天文定位单元利用基于星光折射间接敏感地平的解析天文定位方法得到位置信息和地平信息；天文定姿单元利用大视场星敏感器提供的惯性姿态信息和天文定位单元提供的地平信息确定姿态信息；该大视场星敏感器是采用数学仿真同时观测多颗恒星，直接输出飞行器的惯性姿态信息和折射恒星的星光折射角；该大气折射模型是国际参考大气CIRA1986，利用该大气折射模型根据星光折射角计算出折射恒星的视高度；该数字滤波器是二阶数字低通滤波器；该天文定位单元是利用视高度信息计算飞行器的位置；该天文定姿单元是直接利用惯性姿态信息和地平信息计算飞行器相对于导航坐标系的姿态信息；所述惯性导航子系统，包括惯性测量单元和SINS解算单元；SINS解算单元利用惯性测量单元的输入解算出飞行器的位置和姿态信息；并利用信息融合子系统提供的估计误差对SINS导航误差进行校正；该惯性测量单元是由三个加速度计和三个陀螺仪组成，测量飞行器的加速度和角速度；该SINS解算单元是SINS解算过程，利用飞行器的加速度和加速度信息计算飞行器的位置、速度和姿态导航信息；所述信息融合子系统，包括失准角计算单元和卡尔曼滤波器；失准角计算单元利用天文导航子系统和惯性导航子系统提供的姿态信息求得平台失准角，并提供给卡尔曼滤波器；卡尔曼滤波器以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量进行卡尔曼滤波，得到平台失准角、位置误差和陀螺仪漂移误差的估计值；该失准角计算单元是根据惯性导航子系统和天文导航子系统姿态输出的误差角计算系统的失准角；该卡尔曼滤波器采用标准卡尔曼滤波算法，以SINS误差方程为状态方程，以位置误差和平台失准角作为观测量，对平台失准角、位置误差和陀螺仪漂移误差进行估计；其特征在于：该方法具体包括以下步骤：步骤一：天文导航信息和惯性导航信息的计算a、天文导航信息的计算利用大视场星敏感器观测多颗导航恒星和折射恒星，得到星光折射角；大气折射模型利用星光折射角计算出视高度，利用数字滤波器对视高度进行预处理，得到的处理结果用于天文定位单元；天文定位单元利用基于星光折射的解析天文定位方法实现天文定位，具体过程如下：根据星光折射的几何原理，得到：$\cos \alpha =\sqrt{1-{\left(\frac{R_e+h_a}{r_s}\right)}^2}---\left(1\right)$其中：α为折射后的星光矢量与飞行器位置矢量之间的夹角；R_e为地球半径；h_a为视高度；r_s为地心距，即飞行器到地心的距离；当观测到n颗折射恒星时，根据夹角α的定义得：$\left\{\begin{array}{c}{\overrightarrow{u}}_1·\overrightarrow{r}=\cos {\alpha }_1\\ {\overrightarrow{u}}_2·\overrightarrow{r}=\cos {\alpha }_2\\ .\\ .\\ .\\ {\overrightarrow{u}}_n·\overrightarrow{r}=\cos {\alpha }_n\end{array}\right.---\left(2\right)$其中：(i＝1,2,...,n)为第i颗折射恒星折射后的星光矢量，由大视场星敏感器测得；地心单位矢量，即由地心指向飞行器的单位矢量；α_i为和的夹角；将公式(1)代入方程组(2)，得$\left\{\begin{array}{c}{\overrightarrow{u}}_1·\overrightarrow{r}=\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ {\overrightarrow{u}}_2·\overrightarrow{r}=\sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ .\\ .\\ .\\ {\overrightarrow{u}}_n·\overrightarrow{r}=\sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right.---\left(3\right)$其中：h_ai(i＝1,2,...,n)为第i颗折射恒星的视高度；方程组(3)中，地球半径R_e为已知量，折射后的星光矢量由大视场星敏感器获得，视高度h_ai利用大视场星敏感器、大气折射模型和数字滤波器得到，因此该方程组实际上含有r_x,r_y,r_z和r_s四个未知数；这样，根据的约束条件，如果同时观测三颗以上的折射恒星，就可以确定地心距r_s和地心单位矢量进而求得飞行器的三维位置信息即经度、纬度和高度；当大视场星敏感器观测到n≥3颗折射恒星时，方程组(3)写成：$U·\overrightarrow{r}=Z---\left(4\right)$其中：$U=\left[\begin{array}{ccc}{u}_{x1}& u_{y1}& u_{z1}\\ u_{x2}& u_{y2}& u_{z2}\\ .& .& .\\ .& .& .\\ .& .& .\\ u_{\mathrm{xn}}& u_{\mathrm{yn}}& u_{\mathrm{zn}}\end{array}\right],Z\left(r_s\right)=\left[\begin{array}{c}\sqrt{1-{[(R_e+h_{a1})/r_s]}^2}\\ \sqrt{1-{[(R_e+h_{a2})/r_s]}^2}\\ .\\ .\\ .\\ \sqrt{1-{[(R_e+h_{\mathrm{an}})/r_s]}^2}\end{array}\right]$利用最小二乘法求解公式(4)，得到地心单位矢量的表达式为：$\overrightarrow{r}=B·Z\left(r_s\right)---\left(5\right)$式中，B＝(U^TU)^‑1U^T为矩阵U的广义逆矩阵；由于所以根据公式(5)得到地心距r_s的一元方程为$F\left(r_s\right)=Z{\left(r_s\right)}^T{B}^T\mathrm{BZ}\left(r_s\right)-1={\overrightarrow{r}}^T\overrightarrow{r}-1=0---\left(6\right)$采用牛顿迭代法解算方程(6)，具体迭代步骤如下：(1)选取一个初始的地心距r_s(0)；(2)利用迭代公式计算出下一时刻的地心距；迭代公式为：$r_s(k+1)=r_s\left(k\right)-\frac{F\left[r_s\left(k\right)\right]}{A}---\left(7\right)$其中：r_s(k)、r_s(k+1)分别为第k次和第k+1次的地心距估计值；A为F(r_s)对r_s的微分，即：$A=\frac{\mathrm{dF}\left(r_s\right)}{d{r}_s}=2Z^T{B}^{T}B\frac{\partial Z}{\partial r_s}$其中：为Z对地心距r_s的偏微分，即：$\frac{\partial Z}{\partial r_s}=\left[\begin{array}{c}{M}_1\\ M_2\\ .\\ .\\ .\\ M_n\end{array}\right]=\left[\begin{array}{c}\frac{{(h_{a1}+R_e)}^2}{r_s^3\sqrt{1-{((h_{a1}+R_e)/r_s)}^2}}\\ \frac{{(h_{a2}+R_e)}^2}{r_s^3\sqrt{1-{((h_{a2}+R_e)/r_s)}^2}}\\ .\\ .\\ .\\ \frac{{(h_{\mathrm{an}}+R_e)}^2}{r_s^3\sqrt{1-{((h_{\mathrm{an}}+R_e)/r_s)}^2}}\end{array}\right]$(3)若|r_s(k+1)‑r_s(k)|＜τ，τ为给定的小量，则迭代结束，且r_s(k+1)为地心距r_s的数值解；否则，以r_s(k+1)作为新的初始条件返回第(2)步重新进行计算；然后，根据牛顿迭代法解算出地心距r_s的数值解，并将其数值解代入公式(5)，即得到地心单位矢量根据地心距的定义，由地心距r_s确定飞行器的高度h为：h＝r_s‑R_e           (8)根据地心单位矢量的定义，表示为：$\overrightarrow{r}={[r_x,r_y,r_z]}^T={\left[\begin{array}{ccc}\cos {\delta }_d\cos {\alpha }_d& \cos {\delta }_d\sin {\alpha }_d& \sin {\delta }_d\end{array}\right]}^T---\left(9\right)$因此，根据公式(9)确定飞行器的赤经α_d、赤纬δ_d为α_d＝arctan(r_y/r_x)，δ_d＝arcsin(r_z)       (10)其中：α_d∈(0～2π)，δ_d∈(‑π/2～π/2)；将惯性系下的坐标赤经、赤纬(α_d,δ_d)转变为地理系下的经、纬度坐标(λ,L)，即：λ＝α_d‑t_G，L＝δ_d            (11)其中：(λ,L)为飞行器的经、纬度；t_G为春分点的格林时角，由时间基准得到；此外，根据地心单位矢量的定义，还得到地平信息$C_i^n=\left[\begin{array}{ccc}-\frac{r_y}{\sqrt{1-r_z^2}}& \frac{r_x}{\sqrt{1-r_z^2}}& 0\\ -\frac{r_x{r}_z}{\sqrt{1-r_z^2}}& -\frac{r_y{r}_z}{\sqrt{1-r_z^2}}& \sqrt{1-r_z^2}\\ r_x& r_y& r_z\end{array}\right]---\left(12\right)$这样，天文定位单元输出天文位置信息和地平信息；天文姿态信息由天文定姿单元求得，天文定姿单元利用惯性姿态信息和地平信息，直接计算出天文姿态信息；b、惯性导航信息的计算惯性位置信息和惯性姿态信息由SINS解算单元求出，惯性导航子系统利用惯性测量单元测量飞行器的加速度信息和角速度信息；SINS解算单元根据加速度信息和角速度信息解算出飞行器的位置信息和姿态信息；步骤二：组合导航系统状态方程的建立选择东北天地理坐标系作为导航坐标系，组合导航系统的状态方程为SINS的误差方程，表示为：$\stackrel{·}{X}=\mathrm{FX}+\mathrm{GW}---\left(13\right)$其中：状态向量X＝[φ_x,φ_y,φ_z,δv_x,δv_y,δv_z,δL,δλ,δh,ε_bx,ε_by,ε_bz,▽_bx,▽_by,▽_bz]^T，包括平台失准角φ_x,φ_y,φ_z、速度误差δv_x,δv_y,δv_z、位置误差δL,δλ,δh、陀螺仪漂移误差ε_bx,ε_by,ε_bz和加速度计零偏误差▽_bx,▽_by,▽_bz；F为状态转移矩阵：$F=\left[\begin{array}{cc}{F}_N& F_S\\ 0_{6\times 9}& 0_{6\times 6}\end{array}\right],F_S=\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\end{array}\right]$F_N是平台失准角误差、速度误差和位置误差对应的状态转移矩阵；G为噪声驱动矩阵：$G=\left[\begin{array}{cc}-C_b^n& 0_{3\times 3}\\ 0_{3\times 3}& C_b^n\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right];$W＝[ω_gx,ω_gy,ω_gz,ω_dx,ω_dy,ω_dz]为系统噪声向量，包括陀螺仪随机误差ω_gx,ω_gy,ω_gz和加速度计随机误差ω_dx,ω_dy,ω_dz；步骤三：组合导航系统量测方程的建立选取平台失准角误差和位置误差作为系统观测量，建立量测方程；a、平台失准角误差的量测方程建立平台失准角由天文导航子系统和惯性导航子系统输出的姿态信息求得，令表示天文导航子系统和惯性导航子系统的姿态误差角，其定义为：δθ＝θ_s‑θ_cδγ＝γ_s‑γ_c其中：是天文导航子系统输出的姿态信息，是惯性导航子系统输出的姿态信息；失准角计算单元利用姿态误差角求得平台失准角φ_x,φ_y,φ_z为：根据公式(14)、(15)，根据天文导航子系统和惯性导航子系统的姿态输出求的平台失准角，进而得平台失准角对应的观测方程为：Z₁＝H₁X+V₁          (16)其中：Z₁＝[φ_x φ_y φ_z]^T为平台失准角的观测量；H₁＝[I_3×3 0_3×12]为平台失准角对应的观测矩阵；V₁为观测噪声；b、位置误差的量测方程建立将天文导航子系统与惯性导航子系统位置输出的差值作为位置误差的观测量，则位置误差对应的观测方程为：Z₂＝H₂X+V₂        (17)其中：Z₂＝[L_s‑L_c λ_s‑λ_c]^T为位置误差的观测量；L_c,λ_c为天文导航子系统输出的位置信息，L_s,λ_s为惯性导航子系统输出的位置信息；H₂＝[0_2×6 I_2×2 0_2×7]为位置误差对应的观测矩阵；V₂为CNS的定位误差；步骤四：基于卡尔曼滤波的组合导航系统信息融合卡尔曼滤波器利用捷联惯导的误差方程作为状态方程，将天文导航子系统和惯性导航子系统位置输出的差值和失准角计算单元输出的平台失准角作为观测值，利用卡尔曼滤波算法对导航误差进行实时估计，并将估计误差发送到SINS解算单元，对导航误差进行校正，提高导航精度。