CN1995916A - 一种基于星敏感器标定的深综合组合导航方法 - Google Patents

Abstract

一种基于星敏感器标定的深综合组合导航方法，是一种基于星敏感器标定的惯性/天文/卫星组合导航方法，综合惯性、天文、卫星各导航系统的优点和缺点，可用于各种高精度的飞行器导航。它利用星敏感器观测不同天体得到的星对角距可以修正星敏感器的安装误差，得到高精度姿态信息；用GPS给出的星历数据和惯导给出的位置与速度计算相应于惯导位置的伪距和伪距率，并与GPS测量的伪距和伪距率相比较作为量测值，通过组合卡尔曼滤波器估计惯性导航系统和GPS的误差量，然后对两个系统进行反馈校正。由此即可达到高水平的综合(深综合)，提高组合导航精度，组合导航系统精度优于任何一个单一导航系统精度。

Description

一种基于星敏感器标定的深综合组合导航方法

技术领域

本发明涉及惯性/天文/卫星三组合导航方法，特别是一种基于星敏感器标定的深综合组合导航方法，可用于各种高精度的飞行器导航。

背景技术

现有各种单一的导航系统均有其优点和缺点。惯性导航系统是一种完全自主的定位导航系统，它可以连续实时地提供位置、速度和姿态信息，其短时精度很高，但惯性系统的误差随着使用时间而不断积累，因此纯惯性制导系统难以满足远程高精度导航的要求。卫星导航系统能够全天候、实时提供载体的三维位置和速度信息，且误差不随时间积累，可完成高精度导航与定位。以GPS(全球定位系统)、GLONASS(全球导航卫星系统)为代表的全球卫星定位系统在国外已获得广泛应用。但它的不足之处在于不能提供高精度的姿态信息，导航信息不连续，且易于受到干扰。天文导航技术也是一种自主定位导航技术，利用星敏感器等天体敏感器器件可以提供载体精确的姿态信息，且误差不随时间积累，但它受星光见度的制约，通常不能单独完成导航定位功能。上述各种单一的导航方式各有优缺点，为充分发挥它们各自的优势，互相取长补短，组成组合导航系统是实现精确制导的重要发展方向。

高精度和高可靠性是组合导航系统的基本衡量指标，惯性导航系统自主性好，可靠性高，虽然惯性导航系统位置误差是随工作时间而积累的，但其在初始条件正确给定的条件下，短时间精度是很高的，并且可以提供连续实时的全参数(位置、速度、姿态)导航信息，因而在组合导航和制导系统中，往往以惯性导航系统作为主导航系统，而将其他导航定位误差不随时间积累的导航系统，作为辅助导航系统，利用卡尔曼滤波，进行组合导航信息处理，利用辅助信息观测量对组合系统的状态变量进行估计，以获得高精度的导航信息。通常组合导航系统采用简单的组合方式，采用惯性导航系统的误差方程作为组合导航系统的状态方程，以卫星导航和天文导航输出的位置、速度和姿态信息作为观测量，利用卡尔曼滤波，进行组合导航信息处理修正惯性导航误差，这种组合方式，最大的优点是容易实现，提高了系统的导航精度，但是组合导航精度取决于观测量的精度，位置、速度精度与GPS精度相当，姿态精度与星敏感器确定的姿态精度相当，而无法实现各导航系统的最优组合，达到组合导航系统精度优于任何一个单一导航系统精度的结果，而且模型随时间增长而造成的误差积累和一些系统误差的突变都会造成误差传递，往往会导致组合滤波器的不稳定，因此必须进一步加以补偿辅助导航系统的误差，才可获得更高的导航精度。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种基于星敏感器标定的深综合组合导航方法，可实现直接利用观测信息进行组合导航，提高组合导航精度。

本发明的技术解决方案为：一种基于星敏感器标定的深综合组合导航方法，综合惯性、天文、卫星各导航系统的优点和缺点，充分发挥它们各自的优势，互相取长补短，采用惯性/天文/卫星组合导航模式，进行组合滤波提高导航系统精度。过程包括以下三个步骤：

(1)对惯性导航系统的姿态误差角、速度误差、位置误差和GPS的测距误差进行建模，将它们作为状态方程的状态变量，以惯性导航误差方程和GPS误差方程作为组合导航系统的状态方程，建立惯性/天文/卫星组合导航系统状态方程；

(2)利用惯性导航系统解算的速度、位置信息与接收机得到的伪距、伪距率求得伪距、伪距率观测方程；利用星对角距识别法修正后的星敏感器得到组合导航系统姿态量测方程，以此建立惯性/天文/卫星组合导航系统量测方程；

(3)结合组合导航系统状态方程和得到的导航状态量测方程，通过组合卡尔曼滤波估计惯性导航系统和GPS的误差量，然后对导航系统进行反馈校正，修正惯性导航系统和GPS的误差，由此完成基于星敏感器标定的惯性/天文/卫星深综合组合导航，进而达到组合导航系统精度优于任何一个单一导航系统精度的结果。

本发明的原理是：组合导航系统是利用各导航子系统的不同特点。进行相互组合得到优势互补的功效。惯性导航系统可以连续、实时地提供位置、速度和姿态信息，其短时精度很高，但惯性导航系统的误差随着使用时间的增长而不断积累。天文导航系统也是一种自主定位导航系统，利用天空中的恒星作为导航信息源，可以提供精确的载体姿态信息，且误差不随时间积累，但它易受气候条件的限制，通常不能单独完成导航定位功能。卫星导航系统能够全天候、实时地提供载体的三维位置和速度信息，且误差不随时间积累，但不能提供连续的导航信息，也不能提供高精度的载体姿态信息。鉴于各导航系统的特点，一般常采用其它导航设备与惯性导航进行组合，组成以惯性导航为主体的组合导航系统。通常简单的惯性/天文/卫星组合导航系统采用浅组合模式，各导航子系统独立工作，综合表现为天文、卫星子系统辅助惯性导航系统，但并不校正天文、卫星系统本身的误差。深综合模式是将组合导航系统中各子系统误差均加以估计和补偿的一种高精度组合方式，直接采用天文和卫星导航系统量测得到的信息作为观测量，计算并估计各导航子系统的误差加以补偿，更好的实现了导航子系统相互辅助的作用，更好的提高了导航系统的精度。

本发明与现有技术相比的优点在于：(1)天文导航系统利用星对角距识别法进行星敏感器安装参数误差修正，实现星敏感器的自标定技术，得到高精度姿态信息作为惯性/天文/卫星组合导航系统姿态观测量，补偿辅助导航系统的误差，获得更高的惯性/天文/卫星组合导航精度，抑制了模型随时间增长而造成的误差积累和一些系统误差的突变都会造成误差传递；(2)紧密综合的伪距，伪距率的综合，GPS接收机和惯性导航系统相互辅助，将惯性导航系统误差和GPS的测距误差建入惯性/天文/卫星组合导航系统状态方程，通过滤波估计可以修正惯性导航系统和GPS的误差，得到优于惯性导航系统和GPS的导航系统精度；(3)惯性/天文/卫星深综合组合导航系统，综合考虑了各导航系统的误差，并对其进行修正，实现惯性/天文/卫星组合导航系统的最优组合，可达到组合导航系统精度优于任何一个单一导航系统精度的导航结果。

附图说明

图1为本发明的一种基于星敏感器标定的深综合组合导航方法结构原理图；

图2为本发明的一种基于星敏感器标定的深综合组合导航方法流程图。

具体实施方式

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

如图1所示，惯性导航系统利用捷联解算得到载体的速度、位置和姿态信息；卫星导航系统利用惯性导航系统解算的速度、位置信息和GPS接收机接收到的星历求得相对惯性导航系统给出位置处的伪距、伪距率，与接收机得到的伪距、伪距率求差得到伪距、伪距率观测信息；天文导航系统利用星对角距识别法进行星敏感器安装参数误差修正，再通过天文导航的星图识别算法和姿态确定算法得到载体姿态信息，通过组合卡尔曼滤波估计惯性导航系统和GPS的误差量，然后对两个系统进行反馈校正，可以修正惯性导航系统和GPS的误差。基于星敏感器标定的深综合惯性/天文/卫星组合导航系统综合惯性、天文、卫星各导航系统的优点和缺点，充分发挥它们各自的优势，互相取长补短，提高了导航系统精度。

如图2所示，惯性/天文/卫星组合导航系统流程，惯性、天文、卫星各导航系统利用各自导航敏感器件采集数据，惯性导航系统采用捷联惯性导航系统，利用陀螺和加速度计采集角速率和加速度信息；天文导航系统利用星敏感器获得的恒星在星敏感器感光面上的位置和星敏感器焦距信息，得到恒星观测矢量；卫星导航系统采用GPS，利用GPS接收机得到卫星的伪距、伪距率和星历信息，对惯性导航系统误差和GPS的测距误差进行建模，把它们扩充为状态，建立组合导航系统状态方程。惯性导航系统利用捷联解算得到载体的速度、位置和姿态信息；卫星导航系统利用惯性导航系统解算的速度、位置信息和GPS接收机接收到的星历求得相对惯性导航系统给出位置处的伪距、伪距率，与接收机得到的伪距、伪距率求差得到伪距、伪距率量测方程；天文导航系统利用星对角距识别法进行星敏感器安装参数误差修正，再通过天文导航的星图识别算法和姿态确定算法得到载体姿态信息，得到组合导航系统高精度的姿态观测信息，建立组合导航系统姿态量测方程。利用得到的组合导航系统状态方程和组合导航系统量测方程，通过组合卡尔曼滤波估计惯性导航系统和GPS的误差量，然后对两个系统进行反馈校正，可以修正惯性导航系统和GPS的误差，由此完成基于星敏感器的深综合组合导航。

1.建立惯性/天文/卫星组合导航系统状态方程

以地理系(东北天)为导航解算的基本坐标系，考虑飞行高度h，并假设地球为旋转椭球体，惯导误差状态方程如下。

(1)指北惯性导航系统姿态误差角方程为：

${\stackrel{\·}{\mathrm{\φ}}}_x=-\frac{\mathrm{\δ}{v}_y}{R_M+h}+({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_y-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_z+{\mathrm{\ϵ}}_x$

${\stackrel{\·}{\mathrm{\φ}}}_y=\frac{\mathrm{\δ}{v}_x}{R_N+h}-{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{L\δL}-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\φ}}_x-\frac{v_y}{R_M+h}{\mathrm{\φ}}_z+{\mathrm{\ϵ}}_y---\left(1\right)$

${\stackrel{\·}{\mathrm{\φ}}}_z=\frac{\mathrm{\δ}{v}_x}{R_N+h}\mathrm{tgL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\φ}}_x+\frac{v_N}{R_M+h}{\mathrm{\φ}}_y+{\mathrm{\ϵ}}_z$

式中，R_M为当地子午面内主曲率半径；R_N为与子午面垂直平面上的主曲率半径；ω_ie为地球自转角速率，L、λ和h代表地理纬度、经度和高度，变量[φ_x φ_y φ_z]为三个数学平台误差角；[δv_x δv_y δv_z]为三个速度误差；[δL δλ δh]为纬度误差、精度误差和高度误差；[ε_x ε_y ε_z]为三个陀螺仪常值漂移；[v_x v_y v_z]为三个方向速度；R_M＝R_e(1-2E+3Esin²L)，R_N＝R_e(1+Esin²L)，R_e＝6378137m，E＝1/298.257。

(2)速度误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_x=f_y{\mathrm{\φ}}_z-f_z{\mathrm{\φ}}_y+(\frac{v_y}{R_M+h}\mathrm{tgL}-\frac{v_z}{R_M+h})\mathrm{\δ}{v}_x+(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h})\mathrm{\δ}{v}_y+(2{\mathrm{\ω}}_{\mathrm{ie}}{v}_y\cos L\\ +\frac{v_x{v}_y}{R_N+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}{v}_z\sin L){\mathrm{\δ}}_L-(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_x+{\▿}_x\\ \mathrm{\δ}{\stackrel{\·}{v}}_y=f_z{\mathrm{\δ}}_x-f_x{\mathrm{\φ}}_z-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_x}{R_N+h}\mathrm{tgL}){\mathrm{\δv}}_x-\frac{v_z}{R_M+h}{\mathrm{\δv}}_y-\frac{v_y}{R_M+h}{\mathrm{\δv}}_z---\left(2\right)\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}{\sec }^{2}L)v_x\mathrm{\δL}+{\▿}_y\\ \mathrm{\δ}{\stackrel{\·}{v}}_z=f_x{\mathrm{\δ}}_y-f_y{\mathrm{\φ}}_x-2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_x}{R_N+h}){\mathrm{\δv}}_x+2\frac{v_y}{R_M+h}{\mathrm{\δv}}_z-2{\mathrm{\ω}}_{\mathrm{ie}}{v}_x\sin \mathrm{L\δL}+{\▿}_z\end{array}\right.$

上述速度误差方程可通过对惯性导航系统的速度解算方程的各变量求微分得到，[_x_y_z]为三个加速度计常值偏置，[f_x f_y f_z]为三个比力。

(3)位置误差方程：

(4)惯性导航系统误差状态方程：

将上述平台失准角误差、速度误差和位置误差方程联立起来，得到系统状态方程为：

状态变量为：

X＝[φ_x φ_y φ_z δv_x δv_y δv_z δL δλ δh ε_x ε_y ε_z _x _y _z]^T(5)

F为系统转移矩阵。

$F=\left[\begin{array}{cc}{F}_N& F_S\\ O_{6\×6}& F_M\end{array}\right]---\left(6\right)$

式中，F_N为对应的9维基本导航参数系统阵，与惯性导航系统的误差方程相对应。

F_S和F_M分别为：

$F_S=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right];F_M=\left[0_{6\×15}\right]$

C_b ⁿ为姿态阵，，γ，θ为航向角，俯仰角和横滚角。

噪声驱动阵G为：

$G=\left[\begin{array}{ccc}{C}_b^n& 0_{3\×3}& 0_{3\×9}\\ 0_{3\×3}& C_b^n& 0_{3\×9}\\ 0_{9\×3}& 0_{9\×3}& I_{9\×9}\end{array}\right]---\left(7\right)$

G阵和W阵可以简写为：

$G=\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]$

系统噪声矢量为：

${w=\left[\begin{array}{ccccccccccccccc}{w}_{{\mathrm{\ϵ}}_x}& w_{{\mathrm{\ϵ}}_y}& w_{{\mathrm{\ϵ}}_z}& w_{{\▿}_x}& w_{{\▿}_y}& w_{{\▿}_z}& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}^T---\left(8\right)$

式(4)中变量[w_εx w_εy w_εz]和[w_x w_y w_z]分别为陀螺仪和加速度计的随机误差。

第二部分是卫星导航系统误差方程，卫星导航系统采用GPS，GPS误差状态方程通常取两个误差状态量：一个是等效时钟误差相应的距离误差δt_u，另一个是等效时钟频率误差相应的距离率误差δt_ru，即

X_G(t)＝[δt_u δt_ru]

其微分方程为：

式中β_tru为相关系数，W_tu和W_tru为测量噪声。

GPS的误差状态方程可写为：

式中：

$F_G\left(t\right)=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β}}_{\mathrm{tru}}\end{array}\right],$ $G_G\left(t\right)=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],$ W_G(t)＝[w_tu-w_tru]

综合惯性导航系统的误差状态方程和GPS系统的误差状态方程，可以得到用伪距、伪距率组合的惯性/天文/卫星组合导航系统状态方程：

2.建立惯性/天文/卫星组合导航系统量测方程

(1)伪距差量测方程

相对惯性导航系统给出的位置处的伪距为：

${\mathrm{\ρ}}_{\mathrm{Ij}}={[{(x_1-x_{\mathrm{sj}})}^2+{(y_1-y_{\mathrm{sj}})}^2+{(z_I-z_{\mathrm{sj}})}^2]}^{\frac{1}{2}}---\left(8\right)$

x_I，y_I，z_I为惯性导航系统输出位置，x_sj，y_sj，z_sj为GPS第j颗卫星位置，相对真实位置x，y，z把上式展开成泰勒级数，取到一次项得：

${\mathrm{\ρ}}_{\mathrm{Ij}}={[{(x-x_{\mathrm{sj}})}^2+{(y-y_{\mathrm{sj}})}^2+{(z-z_{\mathrm{sj}})}^2]}^{\frac{1}{2}}+\frac{{\∂\mathrm{\ρ}}_{\mathrm{Ij}}}{\∂x}\mathrm{\δx}+\frac{\∂{\mathrm{\ρ}}_{\mathrm{IJ}}}{\∂y}\mathrm{\δy}+\frac{\∂{\mathrm{\ρ}}_{\mathrm{IJ}}}{\mathrm{\δz}}\mathrm{\δz}---\left(11\right)$

式中

设 $r_j={[{(x-x_{\mathrm{sj}})}^2+{(y-y_{\mathrm{sj}})}^2+{(z-z_{\mathrm{sj}})}^2]}^{\frac{1}{2}}$

$\frac{{\∂\mathrm{\ρ}}_{\mathrm{Ij}}}{\∂x}=\frac{(x-x_{\mathrm{sj}})}{{[{(x-x_{\mathrm{sj}})}^2+{(y-y_{\mathrm{sj}})}^2+{(z-z_{\mathrm{sj}})}^2]}^{\frac{1}{2}}}=\frac{x-x_{\mathrm{sj}}}{r_j}{e}_{\mathrm{jI}}$

$\frac{{\∂\mathrm{\ρ}}_{\mathrm{Ij}}}{\∂y}=\frac{y-y_{\mathrm{sj}}}{r_j}=e_{j2}$

$\frac{\∂{\mathrm{\ρ}}_{\mathrm{Ij}}}{\∂z}=\frac{z-z_{\mathrm{sj}}}{r_j}=e_{j3}$

式(11)改写为：

ρ_Ij＝r_j+e_j1δx+e_j2δy+e_j3δz    (12)

同时，GPS接收机测得的伪距为：

ρ_Gj＝r_j+δt_u+v_ρj    (13)

其中δt_u＝b_k-δt^j，b_k为接收机钟差，δt^j为第j颗卫星的钟差。

则伪距差量测方程可以写成：

δ_ρj＝ρ_Ij-ρ_Gj＝e_j1δx+e_j2δy+e_j3δz-δt_u-v_ρj    (14)

因为GPS接收机至少要选取4颗卫星来解算载体位置和钟差，所以取j＝1，2，3，4得：

$\mathrm{\δ\ρ}=\left[\begin{array}{cccc}{e}_{11}& e_{12}& e_{13}& -1\\ e_{21}& e_{22}& e_{23}& -1\\ e_{31}& e_{32}& e_{33}& -1\\ e_{41}& e_{42}& e_{43}& -1\end{array}\right]\left[\begin{array}{c}\mathrm{\δx}\\ \mathrm{\δy}\\ \mathrm{\δz}\\ {\mathrm{\δt}}_u\end{array}\right]-\left[\begin{array}{c}{v}_{\mathrm{\ρ}1}\\ v_{\mathrm{\ρ}2}\\ v_{\mathrm{\ρ}3}\\ v_{\mathrm{\ρ}4}\end{array}\right]---\left(15\right)$

惯性导航系统以地理系为导航坐标系，则伪距量测方程可以用上式构成。但是，一般讨论的惯性导航系统是用经度、纬度和高度定位的，因此要把δx，δy，δz用δL，δλ，δh表示，本文中使用经纬度定位，所以也要对上面的伪距量测方程进行变换。

由大地坐标系和地理系之间的转换公式(e为第一偏心率)：

$\left\{\begin{array}{c}x=(R_N+h)\cos L\cos \mathrm{\λ}\\ y=(R_N+h)\cos L\sin \mathrm{\λ}\\ z=[R_N(1-e^2)+h]\sin L\end{array}\right.$

可以得到：

$\left\{\begin{array}{c}\mathrm{\δx}=\mathrm{\δ}h\cos L\cos \mathrm{\λ}-(R_N+h)\mathrm{\δ}L\sin L\cos \mathrm{\λ}-(R_N+h)\mathrm{\δ\λ}\cos L\sin \mathrm{\λ}\\ \mathrm{\δy}=\mathrm{\δ}h\cos L\cos \mathrm{\λ}-(R_N+h)\mathrm{\δ}L\sin L\sin \mathrm{\λ}+(R_N+h)\mathrm{\δ\λ}\cos L\cos \mathrm{\λ}\\ \mathrm{\δz}=\mathrm{\δ}h\sin L+[R_N(1-e^2)+h]\mathrm{\δ}L\cos L\end{array}\right.---\left(16\right)$

将式(16)代入式(15)则可得出伪距量测方程为：

Z_ρ(t)＝H_ρ(t)X(t)-V_ρ(t)    (17)

式中：

H_ρ＝[O_4×6MH_ρ1MO_4×6MH_ρ2]_4×17

$H_{\mathrm{\ρ}1}=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\\ a_{41}& a_{42}& a_{43}\end{array}\right];$ $H_{\mathrm{\ρ}2}=\left[\begin{array}{cc}-1& 0\\ -1& 0\\ -1& 0\\ -1& 0\end{array}\right]$

$\left\{\begin{array}{c}{a}_{j1}=(R_N+h)[-e_{j1}\sin L\cos \mathrm{\λ}-e_{j2}\sin L\sin \mathrm{\λ}]+[R_N(1-e^2)+h]e_{j3}\cos L\\ a_{j2}=(R_N+h)[e_{j2}\cos L\cos \mathrm{\λ}-e_{j1}\cos L\sin \mathrm{\λ}]\\ a_{j3}=e_{j1}\cos L\cos \mathrm{\λ}+e_{j2}\cos L\sin \mathrm{\λ}+e_{j3}\sin L\end{array}\right.$

(2)伪距率量测方程

对应于惯性导航系统给出的位置处，载体与卫星间的伪距变化率为

惯性导航给出的位置坐标可以看成真值与误差之和，于是有：

则式(18)可写成：

由GPS接收机测得的伪距变化率为：

将惯性导航和GPS的伪距率求差，可得伪距率量测方程：

取接收4颗卫星，即j＝1，2，3，4，则有：

以地理系为导航坐标系，则伪距率量测方程可以用上式构成，把

用地理系中的速度δv_e，δv_n，δv_u来表示。这可由坐标转换矩阵C_t ^e来实现。即

式中

$C_t^e=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& -\sin L\cos \mathrm{\λ}& \cos L\cos \mathrm{\λ}\\ \cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\sin \mathrm{\λ}\\ 0& \cos L& \sin L\end{array}\right]$

则式(24)变为

把式(25)代入式(23)，则可以得出伪距率量测方程为：

式中

$\left\{\begin{array}{c}{b}_{j1}=-e_{j1}\sin \mathrm{\λ}+e_{j2}\cos \mathrm{\λ}\\ b_{j2}=-e_{j1}\sin L\cos \mathrm{\λ}-e_{j2}\sin L\sin \mathrm{\λ}+e_{j3}\cos L\\ b_{j3}=e_{j1}\cos L\cos \mathrm{\λ}+e_{j2}\cos L\sin \mathrm{\λ}+e_{j3}\sin L\end{array}\right.$

将伪距差量测方程(17)与伪距率量测方程(26)合并为组合导航系统的量测方程，其表达式为：

由此得到进行深综合的组合导航系统状态方程(10)和伪距、伪距率量测方程(27)。

由于系统安装等因素不可避免地造成星敏感器焦距f和光轴位置(x₀，y₀)存在一定的误差。这些误差对系统影响很大，甚至可能导致不能进行正确的星图识别。只有通过系统的参数校正来降低。下面主要分析这些参数的校正方法。将星的位置计算偏差忽略，得第i颗星和第j颗星在星敏感器坐标系中的对角距为：

${\hat{w}}_i^T{\hat{w}}_j=\frac{N}{D_1{D}_2}=G_{\mathrm{ij}}({\hat{x}}_0,{\hat{y}}_0,\hat{f})---\left(28\right)$

其中： 为第i颗星和第j颗星在星敏感器坐标系下的星光矢量；

$N=(x_i-{\hat{x}}_0)(x_j-{\hat{x}}_0)+(y_i-{\hat{y}}_0)(y_j-{\hat{y}}_0)+{\hat{f}}^2;$ $D_1=\sqrt{{(x_i-{\hat{x}}_0)}^2+{(y_i-{\hat{y}}_0)}^2+{\hat{f}}^2};$

$D_2=\sqrt{{(x_j-{\hat{x}}_0)}^2+{(y_j-{\hat{y}}_0)}^2+{\hat{f}}^2}.$

而第i颗星和第j颗星在星敏感器视场中与星历表中对应惯性参考矢量为 $v_i=\left[\begin{array}{c}\cos {\mathrm{\α}}_i\cos {\mathrm{\β}}_i\\ \sin {\mathrm{\α}}_i\cos {\mathrm{\β}}_i\\ \sin {\mathrm{\β}}_i\end{array}\right],$ $v_j=\left[\begin{array}{c}\cos {\mathrm{\α}}_j\cos {\mathrm{\β}}_j\\ \sin {\mathrm{\α}}_j\cos {\mathrm{\β}}_j\\ \sin {\mathrm{\β}}_j\end{array}\right],$ 其中α，β为恒星的赤经和赤纬，可从星历表中查得。由于两颗恒星在星敏感器坐标系下的星对角距相等，有：w_iw_j＝v_iv_j。

由于安装误差是很小的，故在估计值(x₀，y₀，f)处线性化以上方程得：

$v_i{v}_j={\hat{w}}_i^T{\hat{w}}_j=G_{\mathrm{ij}}({\hat{x}}_0,{\hat{y}}_0,\hat{f})-|\frac{{\∂G}_{\mathrm{ij}}}{\∂x_0},\frac{\∂G_{\mathrm{ij}}}{\∂y_0},\frac{\∂G_{\mathrm{ij}}}{\∂g}|\left|\begin{array}{c}\mathrm{\δ}{x}_0\\ \mathrm{\δ}{y}_0\\ \mathrm{\δf}\end{array}\right|---\left(29\right)$

$R_{\mathrm{ij}}=v_i{v}_j-G_{\mathrm{ij}}({\hat{x}}_0,{\hat{y}}_0,\hat{f})=-|\frac{\∂G_{\mathrm{ij}}}{\∂x_0},\frac{\∂G_{\mathrm{ij}}}{\∂y_0},\frac{\∂G_{\mathrm{ij}}}{\∂f}|\left|\begin{array}{c}\mathrm{\δ}{x}_0\\ \mathrm{\δ}{y}_0\\ \mathrm{\δf}\end{array}\right|---\left(30\right)$

这样对于i＝1，...n-1，即可以得到下式：

R＝AδZ    (31)

其中：

$A=-\left|\begin{array}{ccc}\frac{\∂G_{12}}{\∂x_0}& \frac{\∂G_{12}}{\∂y_0}& \frac{\∂G_{12}}{\∂f}\\ \frac{\∂G_{13}}{\∂x_0}& \frac{\∂G_{13}}{\∂y_0}& \frac{\∂G_{13}}{\∂f}\\ M& M& M\\ \frac{\∂G_{n-1,n}}{\∂x_0}& \frac{\∂G_{n-1,n}}{\∂y_0}& \frac{\∂G_{n-1,n}}{\∂f}\end{array}\right|,R={\left[R_{12,}{R}_{13}{\mathrm{LR}}_{23}L{R}_{n-1,n}\right]}^T$

δZ＝[δx₀ δy₀ δf]^T

由最小二乘法可得：

δZ＝[A^TA]^-1A^TR    (32)

由此可得星敏感器的参数误差，加以标定修正后将提高星敏感器的测量精度，即提高了通过星敏感器测得的载体姿态精度，也即提高了姿态量测精度。

星敏感器输出量为观测到的惯性平台失准角，观测量取平台失准角误差φ_x，φ_y和φ_z，组合导航系统高精度姿态量测方程如下：

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

式中，H＝[I_3×3 O_3×3 O_3×9]； $V=\left[\begin{array}{c}V{X}_s\\ V{Y}_s\\ V{Z}_s\end{array}\right],$ VX_s，VY_s，VZ_s是星敏感器误差转换成平台失准角的白噪声。

自此，可以得到惯性/天文/卫星深综合导航系统的状态方程和量测方程。

3.实现惯性/天文/卫星组合导航

利用标定好的星敏感器获得恒星在星敏感器感光面上的位置和星敏感器焦距信息，得到恒星观测矢量，再通过天文导航的星图识别算法和姿态确定算法得到载体姿态信息，作为组合导航系统高精度的姿态观测信息；利用GPS接收机得到卫星的伪距、伪距率和星历信息，通过组合卡尔曼滤波估计惯性导航系统和GPS的误差量，然后对两个系统进行反馈校正，可以修正惯性导航系统和GPS的误差，得到优于惯性导航系统和GPS的导航系统精度。由此完成基于星敏感器标定的惯性/天文/卫星深综合组合导航，实现各导航系统的最优组合，补偿辅助导航系统的误差，便可获得更高的导航精度，抑制了模型随时间增长而造成的误差积累和一些系统误差的突变都会造成误差传递，达到组合导航系统精度优于任何一个单一导航系统精度的结果。

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

Claims (2)

1、一种基于星敏感器标定的深综合组合导航方法，其特征在于：包括以下步骤：

(1)对惯性导航系统的姿态误差角、速度误差、位置误差和GPS的测距误差进行建模，将它们作为状态方程的状态变量，以惯性导航误差方程和GPS误差方程作为组合导航系统的状态方程，建立惯性/天文/卫星组合导航系统状态方程；

(2)利用惯性导航系统解算的速度、位置信息与接收机得到的伪距、伪距率求得伪距、伪距率观测方程；利用星对角距识别法修正后的星敏感器得到组合导航系统姿态量测方程，以此建立惯性/天文/卫星组合导航系统量测方程；

(3)结合组合导航系统状态方程和得到的导航状态量测方程，通过组合卡尔曼滤波估计惯性导航系统和GPS的误差量，然后对导航系统进行反馈校正，修正惯性导航系统和GPS的误差，由此完成基于星敏感器标定的惯性/天文/卫星深综合组合导航，进而达到组合导航系统精度优于任何一个单一导航系统精度的结果。

2、根据权利要求1所述的一种基于星敏感器标定的深综合组合导航方法，其特征在于：步骤(2)中所述的组合导航系统姿态量测方程，采用星对角距识别法进行星敏感器安装参数误差修正，得到高精度姿态信息作为惯性/天文/卫星组合导航系统姿态观测量，补偿辅助导航系统的误差。