CN116105730A - 基于合作目标卫星甚短弧观测的仅测角光学组合导航方法 - Google Patents

Abstract

本发明公开了一种基于合作目标卫星甚短弧观测的仅测角光学组合导航方法。主要步骤为：导航数据输入；建立惯导系统位置、速度、姿态更新模型；判断星敏感器有无数据输出；星敏感器量测数据处理；建立组合导航系统状态方程和量测方程；组合导航系统方程离散化与卡尔曼滤波；修正惯导系统的位置、速度、姿态信息；判断导航过程是否结束，若未结束，则重复上述步骤。本发明方法针对甚短弧观测的特点，改进C‑W相对运动方程，仅需对一颗合作目标卫星进行一分钟以内甚短弧段的观测，即可完成对惯导系统位置、速度、姿态全导航参数的修正，精度高，观测成本低，解算效率高，具有良好的实时性，适用于近地空间的高动态环境。

Description

基于合作目标卫星甚短弧观测的仅测角光学组合导航方法

技术领域

本发明涉及相对导航和组合导航技术领域，具体涉及一种基于合作目标卫星甚短弧观测的仅测角光学组合导航方法。

背景技术

随着航天技术的飞速发展，越来越多的任务要求载体具有自主运行能力，这对减轻地面测控负担、降低载体运营成本、提高载体生存能力、拓展载体应用范围等方面具有重要意义与价值。自主导航技术作为载体自主运行的前提和基础，是实现载体自主运行的关键技术之一，也成为制约空间智能自主控制技术发展的瓶颈。

载体自主导航是利用载体自身携带的测量设备，结合载体运动学方程和状态估计方法确定载体位置、速度、姿态等参数的过程。目前，常用来测量空间物体之间相对状态信息的有源主动式传感器(例如激光雷达等)，由于功耗、质量和体积都比较大，且作用范围相对较小(一般小于一百千米)，使得它们难以应用在微小卫星平台上；而无源被动式传感器(例如星敏感器等)对载体的总质量和功耗设计影响很小，寿命长，隐蔽性高，实时性好，且作用范围广，可用于拍摄其视野范围内的空间目标，并根据测得的视线矢量信息执行自主导航任务。

由于单目光学敏感器只能获得方位信息，缺乏距离信息，故基于光学敏感器观测的导航定位问题可转化为仅测角光学自主导航问题。传统的仅测角光学自主导航方法利用载体携带的光学敏感器在轨获取地理标志物或星历已知的导航目标源(如恒星、行星等)的光学图像，通过图像处理从中提取导航目标源的方向信息(如星光角距、视线矢量等)，经导航算法获得载体在参考坐标系中的位置、速度等导航参数。随着通信技术的发展，一箭多星技术不断完善，近地空间人造物体数量增长迅猛，这些物体以低轨星座卫星为主，分布在高度500到2000千米的轨道。相比于恒星目标，低轨星座卫星与载体之间的相对距离有限，其位置信息可通过星历数据计算，为精确定位提供了可行性，能够作为自主光学导航过程中优秀的信息源。

载体在观测低轨星座卫星的过程中，利用对应时刻相对运动视线角信息及目标星座卫星运动信息求解载体自身运动状态，此时问题可转化为仅测角光学自主相对导航问题。仅测角相对导航技术指通过单个相机(例如星敏感器)在一段时间内测量载体和目标卫星之间的视线矢量，以推导出它们之间的相对运动状态信息。然而，相对导航方法受实际工况的影响较大，在不同工况下具有不同的简化形式，对于在近地轨道运行的载体和目标卫星，需要克服相对运动速度快(10km/s量级)，观测时间短(1分钟以内)，观测弧段为甚短弧(1°以内)，观测连续性差(相邻两颗目标卫星的观测间隔为1min以上)等因素的影响，因此，有必要设计开发新型基于合作目标的近地载体高精度仅测角光学自主相对导航技术，以适应近地空间的高动态环境。

单一的仅测角相对导航算法存在星间距离不可观测、视场角小、数据更新率低、观测不连续、缺乏姿态信息(相对距离很远时目标卫星在相平面上可看作质点)等问题。将载体上既有的惯性测量单元(Inertial Measurement Unit,IMU)引入仅测角相对导航系统，不仅具有输出频率高，导航数据连续等优势，而且能够结合星历信息解算得到载体与合作目标卫星之间的潜在距离信息，大大改善仅测角导航算法的可观测性，相对导航算法输出的高精度位置、速度信息也可以修正惯导的积累误差。除此之外，基于星敏感器所拍摄的背景星图，经过解算可以获得高精度的载体姿态信息，进而对惯导的陀螺漂移(姿态)误差进行修正，从而能够克服单一测量系统存在的缺陷，具有功耗小、输出频率高、导航参数全、延迟低、隐蔽性好和精度高等优势。

发明内容

本发明的目的在于采用多种导航方式相结合，提高导航精度，本发明提出了一种基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，包括以下步骤：

步骤1，导航数据输入；

输入到惯性导航系统的导航数据主要为惯性元件加速度计和陀螺仪的输出数据；

步骤2，建立惯性导航系统姿态、速度、位置更新模型；

根据惯性导航系统姿态、速度、位置更新模型，分别得到惯性导航系统的姿态ψ、速度V和位置P；

步骤3，判断星敏感器有无数据输出；

以惯性导航系统的时间步长为基准，在每个时间步长开始时刻，首先判断此时星敏感器有无数据输出，若无数据输出，则直接输出步骤2中惯性导航系统更新后的姿态、速度和位置信息；若有数据输出，则执行步骤4-7；

步骤4，星敏感器量测数据处理；

S41，得到敏感器坐标系中的单位方向矢量；

S42，得到惯性坐标系和载体坐标系之间的姿态转换矩阵；

S43，得到目标卫星在载体坐标系下的观测角；

S44，得到目标卫星和载体潜在距离信息；

S45，将目标卫星单位方向矢量从载体坐标系转换到目标卫星轨道坐标系；

步骤5，建立组合导航系统状态方程和量测方程；

S51，建立组合导航系统的状态方程：

其中，X为组合导航系统状态变量，

为组合导航系统状态变量的一阶导，F是组合导航系统的状态转移矩阵，G为噪声驱动阵，W为系统噪声矩阵；

52，建立组合导航系统的量测方程：

选择惯性导航系统的平台误差角、北东地速度误差和经纬高位置误差作为观测量,使用组合导航系统中的星光导航系统和相对导航系统得到载体的姿态角和位置、速度，将星光导航系统和相对导航系统得到的导航信息与惯性导航系统得到的导航信息做差后，便可得到姿态角误差、速度误差和位置误差，再通过转换即可得到平台误差角、北东地速度误差和经纬高位置误差。

组合导航系统的量测方程为：

在建立组合导航系统量测方程的过程中，需要以C-W方程为基础建立目标卫星与载体的相对运动方程；但是，C-W方程要求目标卫星与载体之间的距离较小，当相对距离大于100km时，C-W方程的误差会迅速增大，因此需要改进C-W方程。

记目标卫星和载体在惯性坐标系下的绝对位置矢量分别为r_t和r_c，在二体问题的假设下，有：

其中，μ为地心引力常数，值为398600.44×10⁹m³/s²,

和

分别为矢量r_t和r_c的二阶导。

载体相对于目标卫星的相对位置矢量为ρ＝r_c-r_t，|ρ|为相对位置矢量ρ的模值，则有：

将式(44)转换到目标卫星轨道坐标系下，有：

其中，ω_t为目标卫星轨道角速度矢量，对于近圆轨道，ω_t的值可由平均角速度n表示，将式(45)写成分量形式，有：

其中，

ρ＝[x y z]。

对于近圆轨道，有

式(46)可简化为：

当目标卫星与载体之间的相对距离远小于其轨道半径，即|r_c|≈|r_t|,|ρ|＜＜R_e时，可对重力场进行一阶近似，有：

进而有：

μ/|r_t|²-μ|r_t|/|r_c|³＝μ/|r_t|²(1-|r_t|³/|r_c|³)≈3μx/|r_t|³  (49)

在载体无轨道机动且飞行轨迹为圆轨道的假设下，根据惯性导航系统输出的信息求得轨道角速度n′，有：

其中，|r_c|为地心与载体之间的距离。

当星敏感器光轴方向沿地心指向载体方向时，光轴方向与载体轨道坐标系的

轴重合，此时地心、载体与目标卫星可近似认为在一条直线上，即有：

x≈|ρ|≈|r_t-r_c|  (75)

根据式(49)有：

其中，k为比例系数，其表达式为：

此时，式(47)简化为：

对应的状态矩阵A'为：

改进后的状态方程仍然保留了线性齐次的良好性质，依靠比例系数k对相对运动模型进行修正，以使其保持良好的精度；

相对导航系统的量测方程为：

步骤6，组合导航系统方程离散化与卡尔曼滤波；

对通过式(32)和式(73)建立的组合导航系统的状态方程和量测方程进行离散化；离散化后的方程再通过卡尔曼滤波算法进行滤波，得到惯性导航系统的导航误差的最优估计值δφ,δV,δP；

步骤7，修正惯性导航系统的位置、速度、姿态信息；

步骤2中惯性导航系统得到的导航信息为ψ,V,P，利用步骤6中得到的导航误差的最优估计值δφ,δV,δP，对步骤2中惯性导航系统得到的导航信息进行修正，得到组合导航系统输出的导航信息：

其中，P_组合、V_组合和ψ_组合分别为组合导航系统输出的位置、速度和姿态信息；

步骤8，判断导航过程是否结束；

若未结束则返回步骤1，若结束则停止导航过程。

与现有技术相比，本发明具有以下有益效果：

1.仅需对一颗合作目标卫星进行极短时间的观测(一分钟以内，观测弧段为甚短弧)即可完成对惯导系统位置、速度、姿态全导航参数的修正，精度高，观测成本低，解算效率高，具有良好的实时性，适用于近地空间的高动态环境。

2.针对甚短弧观测的特点，对传统的仅适用于近距离(100km以内)的C-W相对运动方程进行了改进，改进后的方程仍然保留了线性齐次的良好性质，当载体与目标卫星距离很远时，依靠比例系数对相对运动模型进行修正，使其能保持良好的精度，用于远距离相对导航。

附图说明

图1是本发明基于合作目标卫星甚短弧观测的仅测角光学组合导航方法的流程图；

图2是地球坐标系、地理坐标系和载体坐标系示意图；

图3是捷联惯性导航系统基本原理图；

图4是捷联惯性导航系统进行导航的流程图；

图5是星敏感器测量原理图；

图6是地球坐标系与惯性坐标系之间的转换关系示意图；

图7是轨道坐标系与相对运动示意图；

图8是星敏感器观测示意图；

图9是C-W方程模型和改进模型的相对位置滤波效果对比示意图；

图10是C-W方程模型和改进模型的相对速度滤波效果对比示意图；

图11是组合导航系统位置误差示意图；

图12是组合导航系统速度误差示意图；

图13是组合导航系统姿态误差示意图。

具体实施方式

以下将参考附图详细说明本发明的示例性实施例、特征和方面。附图中相同的附图标记表示功能相同或相似的元件。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。

根据本发明所采用的技术方案，下面结合附图和具体实施例对实施方式进行进一步说明。

如图1所示，基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，包括以下步骤：

步骤1，导航数据输入。

输入到惯性导航系统的导航数据主要为惯性元件加速度计和陀螺仪的输出数据。对于捷联式惯性导航系统，惯性元件直接固连在载体上，三轴陀螺仪和加速度计的输入轴安装时要保持严格正交，与载体坐标系一致。加速度计的输出为

表示载体坐标系(b系)相对于惯性坐标系(i系)的比力在载体坐标系的投影，陀螺仪的输出为

表示载体坐标系(b系)相对于惯性坐标系(i系)的转动角速度在载体坐标系的投影。

惯性导航系统输出的导航数据为导航过程中，载体的位置P、速度V和姿态ψ。在惯性导航系统启动时，需要输入初始导航数据，主要包括载体的位置、速度和姿态的初始值。选取北-东-地地理坐标系(g系)作为导航坐标系(n系)，位置P为经度λ、纬度L和高度h的数据；速度V为地理坐标系内的北向速度V_N、东向速度V_E和地向速度V_D；选取欧拉角作为姿态角的表示方式，姿态可定义为载体坐标系相对于导航坐标系的三个欧拉角，即偏航角、俯仰角和滚转角数据，则从导航系变换到载体系，旋转顺序为先绕z轴旋转偏航角

接着绕y轴旋转俯仰角(Pitch)θ，最后绕x轴旋转滚转角(Roll)γ，规定顺时针为负，逆时针为正。地球坐标系(e系)、地理坐标系和载体坐标系之间的关系如图2所示。

步骤2，建立惯性导航系统姿态、速度、位置更新模型。

捷联惯性导航系统的基本原理图如图3所示，IMU起到惯性传感器信号输入的作用，所有的信号处理都在计算机内实现。捷联惯性导航系统的核心是由计算机实现的惯性平台，惯性平台利用捷联陀螺仪测量的载体角速度解算矩阵，从姿态矩阵中提取载体的姿态和航向信息；并利用姿态矩阵把加速度计的输出从载体坐标系变换到导航坐标系。

根据牛顿力学运动定律和刚体定点转动的欧拉定理，在地理坐标系下建立捷联惯性导航系统中的姿态、速度、位置更新模型具体如下：

S21，建立姿态更新模型：

姿态和航向的实时获取是捷联惯性导航系统的关键技术之一，通过姿态矩阵给出载体的姿态并为导航提供最基本的数据。在姿态更新模型中，采用四元数法得到姿态并进行姿态更新。

姿态矩阵和四元数之间的转换关系为：

其中，

表示载体坐标系到导航坐标系的姿态四元数，q₀,q₁,q₂,q₃为

的四个分量，Q_z,Q_y,Q_x为三次姿态旋转所对应的四元数，γ,θ,

为三个欧拉角。

其中，

表示载体坐标系到导航坐标系的姿态旋转矩阵。在本实施例中，C表示姿态旋转矩阵，下标表示当前坐标系，上标表示目标坐标系。

根据姿态角的定义，可推导导航坐标系到载体坐标系的姿态旋转矩阵为

用T_ij(i＝1,2,3；j＝1,2,3)表示

中的元素，则俯仰角、滚转角和偏航角与矩阵

中的元素对应关系如式(4)所示：

建立姿态运动学微分方程：

其中，

为姿态角速度

组成的反对称矩阵，T符号代表矩阵的转置，姿态角速度根据

得到，

为载体坐标系到导航坐标系的姿态旋转矩阵

的一阶导。

根据公式(5)可求得

转换为

后再使用公式(4)即可求出俯仰角θ、滚转角γ和偏航角

利用四元数矩阵

根据

和

的关系，式(5)可表达为四元数微分方程：

其中，

为四元数矩阵

的一阶导；

该四元数微分方程可用毕卡逼近方法求解，有：

其中，Q(q_t+Δt)表示下一时刻的

姿态四元数，Q(q_t)表示当前时刻的

姿态四元数，Δt为捷联惯性导航系统的时间步长，t为当前导航时间；Δθ＝[Δθ_x Δθ_y Δθ_z]为单个时间步长内的姿态变化量，其表达式为：

其中，

为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影，

表示导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影，其表达式分别为：

其中，

是

的模值；L为纬度，h为高度，V_N、V_E分别为地理坐标系内的北向速度和东向速度；R_M和R_N分别为子午圈和卯酉圈的主曲率半径，若地球的长半径为R_e，扁率为f，则R_M和R_N表达式分别为：

R_M＝R_e(1-2f+3f sin²L) (11)

R_N＝R_e(1+f sin²L)  (12)

求解式(7)得到Q(q_t+Δt)的近似解，即姿态更新模型为：

其中，

I为四阶单位矩阵，[Δθ]的表达式为：

S22，建立速度更新模型为：

其中，g为地球重力加速度，

为速度V的一阶导。

S23，建立位置更新模型为：

其中，h₀,L₀,λ₀分别表示载体高度、纬度和经度的初始值，t₀为导航初始时间。

根据姿态更新模型、速度更新模型和位置更新模型分别得到更新后的姿态、速度和位置后，惯性导航系统的一次更新流程完成，其整体流程图见图4。

步骤3，判断星敏感器有无数据输出。

由于星敏感器的数据更新率低，而惯性导航系统的数据更新率高，故时间步长应以惯性导航系统为基准，在每个时间步长开始时刻，首先判断此时星敏感器有无数据输出，若无数据输出，则直接输出步骤2中惯性导航系统更新后的姿态、速度和位置信息；若有数据输出，则执行步骤4-7，建立组合导航系统，通过卡尔曼滤波修正惯性导航系统更新后的姿态、速度和位置，最后输出经过修正的姿态、速度和位置。组合导航系统包括星光导航系统、相对导航系统和惯性导航系统。

步骤4，星敏感器量测数据处理。

星敏感器的测量原理如图5所示。

S41，得到敏感器坐标系中的单位方向矢量。

星敏感器通过CCD元件拍摄星空，通过星图匹配、星光识别、目标提取等，获得拍摄到的恒星和目标卫星在星敏感器坐标系中的位置；O_s-x_sy_sz_s为星敏感器坐标系(s系)，O-uvw为CCD成像平面坐标系，f为光学透镜的焦距，OO_s为光轴方向，O_sz_s轴、Ow轴均与光轴方向一致，(u_k,v_k)为目标在CCD阵面上的像点位置信息。根据该像点位置，可以得到该目标在星敏感器坐标系中的单位方向矢量为：

其中，s_k为第k个目标在星敏感器坐标系的单位方向矢量。

S42，得到惯性坐标系和载体坐标系之间的姿态转换矩阵。

当星敏感器坐标系与载体坐标系重合时，即不考虑传感器安装误差的情况下，通过星敏感器即可获得目标相对于载体坐标系的单位方向矢量s₁,s₂,···s_n,s_n+1，其中s_k＝[x_sk y_sk z_sk]^T(k＝1,2,…n+1)，本实施例中，第1至n个目标为恒星，第n+1个目标为目标卫星，s₁,s₂,···s_n为恒星在载体坐标系的单位方向矢量，s_n+1为目标卫星在载体坐标系的单位方向矢量。通过导航星历可以得到恒星和目标卫星在惯性坐标系中的坐标h₁,h₂,···h_n,h_n+1，其中h_k＝[x_hk y_hk z_hk]^T(k＝1,2,…n+1)。则s_k和h_k的关系可表示为：

其中，

表示星敏感器坐标系到惯性坐标系的姿态旋转矩阵；

表示载体坐标系到惯性坐标系的姿态旋转矩阵；

恒星用于求解载体的姿态信息，目标卫星通过求解观测角和潜在距离信息用于相对导航滤波。

对于恒星目标，记矩阵S和H分别为：

则由式(18)可得：

其中，

表示从惯性坐标系到载体坐标系的姿态旋转矩阵；

当观测恒星数目n≥3时，姿态转换矩阵

可采用最小二乘法计算得到：

即通过星敏感器获得恒星在载体坐标系的单位方向矢量，通过导航星历获得恒星在惯性坐标系中的坐标，结合公式(21)就可得到从惯性坐标系到载体坐标系的姿态旋转矩阵

S43，得到目标卫星在载体坐标系下的观测角。

对于目标卫星，基于单位方向矢量可求得其在载体坐标系下的观测角，即高度角α和方位角β分别为：

其中，x_s(n+1)、y_s(n+1)和z_s(n+1)为第n+1个目标相对于载体坐标系的单位方向矢量s_n+1的三个分量，本实施例中第n+1个目标为目标卫星。

S44，得到潜在距离信息。

虽然星敏感器缺少距离信息，但由于目标卫星的星历已知，基于惯性导航系统可获得自身的位置信息，因而可得到目标卫星和载体二者之间的距离，得到潜在距离信息|ρ|，具体方式如下：

记t时刻目标卫星在惯性坐标系下的星历位置为h_it，从惯性导航系统得到位置，位置包括载体自身的经度λ_t、纬度L_t、高度h_t，根据经纬高和地球坐标系之间的转换关系，如图2所示，可得到t时刻载体在地球坐标系中的坐标r_et，坐标r_et＝[x_et y_et z_et]^T的三个分量使用式(23)得到：

其中，R_N为卯酉圈的主曲率半径，f为地球的扁率。

根据地球坐标系与惯性坐标系之间的转换关系，如图6所示，可知地球坐标系到惯性坐标系的姿态旋转矩阵

为：

由此可得t时刻载体在惯性坐标系下的位置r_it为：

由此得到潜在距离信息|ρ|为：

|ρ|＝|h_it-r_it|  (26)

S45，将目标卫星单位方向矢量从载体坐标系转换到目标卫星轨道坐标系。

考虑到观测角信息的参考基准为载体坐标系，而在进行相对导航滤波时，应以目标卫星轨道坐标系(o系)为基准，得到载体相对于目标卫星的相对运动信息，再结合目标卫星的星历信息，才能对应得到载体的绝对运动信息，故需要将单位方向矢量

从载体坐标系转换到目标卫星轨道坐标系。具体为：

首先将

从载体坐标系转换到惯性坐标系：

其中，

表示目标卫星单位方向矢量在载体坐标系下的坐标，

表示目标卫星单位方向矢量在惯性坐标系下的坐标，

表示载体坐标系到惯性坐标系的姿态旋转矩阵；

根据目标卫星的星历位置h_it和速度l_it信息，根据图7中轨道坐标系

的定义，其

轴沿地心指向目标卫星质心；

轴在轨道平面内与

轴垂直，指向目标卫星速度方向；

轴根据右手定则确定，与轨道平面的法线平行。则有：

则从惯性坐标系到目标卫星轨道坐标系的姿态矩阵

为：

由此可将

从惯性坐标系转换到目标卫星轨道坐标系下：

根据公式(22)得到目标卫星在载体坐标系下的观测角，使用公式(27)-(30)，将观测角转换到目标卫星轨道坐标系下。

步骤5，建立组合导航系统状态方程和量测方程。

S51，建立组合导航系统的状态方程：

因为步骤1中，加速度计的输入

和陀螺仪的输入

在实际测量过程中均存在噪声，因此需要对步骤2中惯性导航系统更新后的姿态、速度和位置输出结果进行误差分析，并建立组合导航系统的状态方程。

根据加速度计和陀螺仪的误差特性，将其建模为一阶马尔科夫过程：

其中，ε和κ分别表示陀螺仪和加速度计误差，

和

分别表示陀螺仪和加速度计误差的一阶导，T_r和T_a分别为陀螺仪和加速度计的一阶马尔科夫过程相关时间，ω_ε和w_κ为陀螺仪高斯白噪声和加速度计高斯白噪声。

根据对惯性导航系统平台误差角、速度误差和位置误差方程的推导，可以得到地理坐标系下组合导航系统的状态方程为：

其中，X为组合导航系统状态变量，

为组合导航系统状态变量的一阶导，F是组合导航系统的状态转移矩阵，G为噪声驱动阵，W为系统噪声矩阵；

下面将对式(32)中的变量进行详细说明：

F是组合导航系统的状态转移矩阵，可写为：

上式中，F_N为对应的9维基本导航参数系统矩阵，其非零元素为：

F_N(4,2)＝f_D,F_N(4,3)＝-f_E

F_N(5,1)＝-f_D,F_N(5,3)＝f_N

F_N(6,1)＝f_E,F_N(6,2)＝-f_N

F_N(9,6)＝-1

其中，f_N,f_E,f_D为比力沿北东地地理坐标系的三个分量，可根据加速度计输出

得到。

F_s和F_m分别为：

diag()表示对角线元素为括号内所列出元素的对角矩阵；

状态变量X为：

X＝[φ_N φ_E φ_D δV_N δV_E δV_D δL δλ δh ε_N ε_E ε_D κ_N κ_E κ_D]^T       (35)

其中，[φ_N φ_E φ_D]^T为平台误差角；[δV_N δV_E δV_D]^T为北东地速度误差；[δL δλ δh]^T为经纬高位置误差；[ε_N ε_E ε_D]^T为陀螺仪误差；[κ_N κ_E κ_D]^T为加速度计误差。

噪声驱动阵G为：

系统噪声矩阵W为：

其中，

和

分别为陀螺仪高斯白噪声和加速度计高斯白噪声的三个分量。

S52，建立组合导航系统的量测方程：

本发明选择惯性导航系统的平台误差角、北东地速度误差和经纬高位置误差作为观测量。使用组合导航系统中的星光导航系统和相对导航系统得到载体的姿态角和位置、速度，且精度较高，可近似认为是标准值，将星光导航系统和相对导航系统得到的导航信息与惯性导航系统得到的导航信息做差后，便可得到姿态角误差、速度误差和位置误差，再通过转换即可得到平台误差角、北东地速度误差和经纬高位置误差。

对于星光导航系统，根据惯性导航系统得到的经纬高信息可得到地球坐标系和导航坐标系之间的转换矩阵：

由式(21)和(24)得到

和

可求得导航坐标系相对于载体坐标系的姿态旋转矩阵

与步骤S21相似，步骤S21中根据式(5)得到

后使用式(4)求出姿态角。此处根据式(39)得到

后也根据式(4)即可求出姿态角，记为

由于量测方程的状态量为平台误差角，因此需要将姿态误差角转换为平台误差角，才能作为信息融合时的观测量。

令

表示滚转角、俯仰角和偏航角姿态误差，则有：

其中，γ,θ,

表示惯性导航系统输出的姿态角，

表示星光导航系统输出的姿态角。

姿态误差角和平台误差角之间的转换关系为：

根据式(41)可得到状态变量X中的平台误差角[φ_N φ_E φ_D]^T。

将转换后的平台误差角作为信息融合的观测量，建立平台误差角量测方程为：

其中，H_φ＝[I_3×3 0_3×12]，V_φ表示系统的姿态角量测噪声。

对于相对导航系统，星敏感器输出的观测角信息和潜在距离信息需要基于相对运动方程进行卡尔曼滤波后，才能得到精确的载体位置、速度信息。

在步骤4中，已经将观测角信息换算到目标卫星轨道坐标系下，下面以目标卫星轨道坐标系为参考系建立相对运动方程。考虑到近地载体飞行速度快，观测弧段为甚短弧，观测时间短，故可忽略摄动力作用的影响，仅考虑地球中心引力场的作用，将问题简化为二体问题，且由于目标卫星的运行轨道为近圆轨道，目标卫星与载体均无轨道机动，满足C-W方程的简化条件，故以C-W方程为基础建立相对运动方程。

记目标卫星和载体在惯性坐标系下的绝对位置矢量分别为r_t和r_c，在二体问题的假设下，有：

其中，μ为地心引力常数，值为398600.44×10⁹m³/s²,

和

分别为矢量r_t和r_c的二阶导，|r_t|和|r_c|分别为r_t和r_c的模值。

载体相对于目标卫星的相对位置矢量为ρ＝r_c-r_t，|ρ|为相对位置矢量ρ的模值，则有：

将式(44)转换到目标卫星轨道坐标系下，有：

其中，ω_t为目标卫星轨道角速度矢量，对于近圆轨道，ω_t的值可由平均角速度n表示，将式(45)写成分量形式，有：

其中，

ρ＝[x y z]。

对于近圆轨道，有

式(46)可简化为：

当目标卫星与载体之间的相对距离远小于其轨道半径，即|r_c|≈|r_t|,|ρ|＜＜R_e时，可对重力场进行一阶近似，有：

进而有：

μ/|r_t|²-μ|r_t|/|r_c|³＝μ/|r_t|²(1-|r_t|³/|r_c|³)≈3μx/|r_t|³  (49)

式(47)可进一步简化为：

上式即为C-W方程，令载体在目标卫星轨道坐标系中的相对状态向量Ψ为：

则可将C-W方程写为状态方程形式：

其中，

以星敏感器的采样时间t_s为单位对式(52)进行离散化，有：

Ψ_k＝Φ_k,k-1Ψ_k-1+w_k-1  (54)

其中，Ψ_k为离散化后第k个时间步长载体在目标卫星轨道坐标系中的相对状态向量，Ψ_k-1为离散化后第k-1个时间步长载体在目标卫星轨道坐标系中的相对状态向量，w_k-1为离散化后第k-1个时间步长的过程噪声，Φ_k,k-1为从第k-1个时间步长到第k个时间步长的状态转移矩阵，其表达式为：

相对导航系统的量测方程为：

由于观测模型的非线性，使用扩展卡尔曼滤波方法进行状态估计，对量测方程进行离散化和线性化处理，得到量测方程：

Z_k＝H_kΨ_k+V_k (57)

其中，

表示第k个时间步长的量测矩阵，V_k表示第k个时间步长星敏感器的量测噪声，Z_k表示第k个时间步长的量测量。

建立扩展卡尔曼滤波过程如下：

预测状态变量：

Ψ_k,k-1＝Φ_k,k-1Ψ_k-1 (58)

预测协方差矩阵：

计算滤波增益矩阵：

更新状态估计：

Ψ_k＝Ψ_k,k-1+K_k(Z_k-H_kΨ_k,k-1) (61)

更新协方差矩阵：

P_k＝(I-K_kH_k)P_k,k-1 (62)

其中，Q_k-1表示第k-1个时间步长的过程噪声方差矩阵，R_k表示第k个时间步长的观测噪声方差矩阵，I为单位矩阵。

经过卡尔曼滤波后可输出精确的相对位置

和相对速度

需要将其转换为惯性导航系统可用的位置、速度量测信息。

将相对位置

和相对速度

分别转换到惯性坐标系下，记为

和

对于相对位置，根据式(29)，有：

其中，

表示从目标卫星轨道坐标系到惯性坐标系的姿态旋转矩阵；

对于相对速度，根据矢量绝对导数与相对导数的关系，有：

其中，ω_t表示t时刻目标卫星轨道角速度矢量，其表达式为：

其中，h_it表示t时刻目标卫星的星历位置，|h_it|为h_it的模值，l_it表示t时刻目标卫星的星历速度。

则载体在惯性坐标系下的位置r_i和速度v_i为：

将位置和速度转换到地球坐标系下，有：

其中，r_e和v_e分别为地球坐标系下的位置和速度，

为从惯性坐标系到地球坐标系的姿态旋转矩阵；

对于位置矢量，惯性导航系统误差状态方程以经纬高位置误差作为状态量，故需要将r_e＝[x_e y_e z_e]^T转换为经度λ、纬度L和高度h，可采用迭代法完成，得到相对导航系统输出的位置。

对于速度矢量，相对导航系统根据式(38)得到的

进一步可得：

其中，v_n为相对导航系统输出的在导航坐标系下的速度，

为地球坐标系和导航坐标系之间的转换矩阵，将转换后的北东地速度误差和经纬高位置误差作为信息融合的观测量，分别建立速度和位置误差量测方程。

令δ_V＝[δV_N δV_E δV_D]^T表示北向速度、东向速度和地向速度误差，则有：

其中，V_N,V_E,V_D表示惯性导航系统输出的速度在导航坐标系下的分量，

表示相对导航系统输出的速度在导航坐标系下的分量。

根据式(69)可得到状态变量X中的北东地速度误差[δV_N δV_E δV_D]^T。

速度量测方程为：

其中，H_V＝[0_3×3 I_3×3 0_3×9]，V_V表示系统的速度量测噪声。

令δ_P＝[δL δλ δh]^T表示纬度、经度和高度误差，则有：

其中，L,λ,h表示惯性导航系统输出的位置，

表示相对导航系统输出的位置。

根据式(71)，可得到状态变量X中的经纬高位置误差[δL δλ δh]^T。

位置量测方程为：

其中，H_P＝[0_3×6 I_3×3 0_3×6]，V_P表示系统的位置量测噪声。

将平台误差角量测方程、速度量测方程和位置量测方程合并，得到组合导航系统的量测方程：

至此完成组合导航系统状态方程和量测方程的建立。

本发明对相对导航系统进行了改进：

对于相对导航系统，当相对距离较大(大于100km)时，C-W方程的误差会迅速增大，原因在于式(48)的简化过程中，忽略了与相对距离|ρ|有关的小量，而当相对距离变大时，此项误差的影响便不能再忽略。针对此问题，本发明提出一种甚短弧观测条件下的C-W方程改进方法，使其在相对距离很大时仍能有较好的模型精度，这是本发明的重要发明点之一。

考虑到星敏感器的观测视场角较小，可假设为10°×10°的圆形视场。为简化分析过程，假设载体无轨道机动，飞行轨迹为大圆航线，即可近似为圆轨道，且星敏感器光轴沿地心指向载体方向，星敏感器观测示意图如图8所示。

式(46)为精确的相对运动方程，未进行任何简化，基于目标卫星近圆轨道假设，式(46)可简化为式(47)，在载体无轨道机动且飞行轨迹为圆轨道的假设下，可根据惯性导航系统输出的信息求得轨道角速度n′，有：

其中，|r_c|为地心与载体之间的距离。

当星敏感器光轴方向沿地心指向载体方向时，光轴方向与载体轨道坐标系的

轴重合，且由于星敏感器的观测视场很小，此时地心、载体与目标卫星可近似认为在一条直线上，有：

对式(49)的简化过程进行改进，

x≈|ρ|≈|r_t-r_c|  (75)

有：

其中，k为比例系数，其表达式为：

此时，式(47)可简化为：

对应的状态矩阵A'为：

可以看到，改进后的状态方程仍然保留了线性齐次的良好性质，当载体与目标卫星的距离很近时，有A'≈A，此时C-W方程具有较好的精度，当载体与目标卫星距离很远时，依靠比例系数k对相对运动模型进行修正，以使其保持良好的精度，后续的求解过程与式(54)-(62)相同。对于星敏感器光轴方向偏移的情况，只需根据偏移的角度对应修改比例系数k即可。

步骤6，组合导航系统方程离散化与卡尔曼滤波。

在步骤5中，通过式(32)和式(73)建立了组合导航系统的状态方程和量测方程组合为：

由于式(80)中方程仍为连续形式，需要离散化为：

其中，X_k为离散化后第k个时间步长载体在目标卫星轨道坐标系中的相对状态向量，X_k-1为离散化后第k-1个时间步长载体在目标卫星轨道坐标系中的相对状态向量，W_k-1为离散化后第k-1个时间步长的过程噪声，Θ_k,k-1为离散化后从第k-1个时间步长到第k个时间步长的状态转移矩阵，Γ_k-1为离散化后第k-1个时间步长的噪声驱动阵，Z_k为第k个时间步长的量测量，H_k为第k个时间步长的量测矩阵，V_k表示第k个时间步长的量测噪声，其中，Θ_k,k-1和Γ_k-1的计算方式为：

其中，F_k-1表示第k-1个时间步长的状态转移矩阵，G_k表示第k个时间步长的噪声驱动阵,I表示单位矩阵，Δt为捷联惯性导航系统的时间步长。

离散化后的方程通过卡尔曼滤波算法进行滤波，得到姿态、速度和位置相应的误差最优估计值δφ,δV,δP。卡尔曼滤波算法为现有技术，滤波算法可参考式(58)-(62)。

步骤7，修正惯性导航系统的位置、速度、姿态信息。

惯性导航系统输出的导航信息为ψ,V,P，利用步骤6中得到的导航误差的最优估计值δφ,δV,δP对步骤2中惯性导航系统得到的导航信息进行修正，得到组合导航系统输出的导航信息：

其中，P_组合、V_组合和ψ_组合分别为组合导航系统输出的位置、速度和姿态信息。

步骤8，判断导航过程是否结束。

若未结束则返回步骤1，若结束则停止导航过程。

实施例

为验证本发明方法的性能，设计一组飞行轨迹进行仿真实验。载体的飞行高度为100km，在飞行过程中依次观测到4颗目标卫星，其轨道高度依次为500km，1000km，1500km，2000km，星敏感器视场角为10°×10°的圆形视场，每颗卫星的平均观测时间为20s。

以2000km轨道高度的目标卫星为例，图9和图10分别展示了C-W方程模型和本发明所提出的改进模型对相对位置和速度的滤波效果对比。可以看到，在相对距离非常大时，C-W方程模型的误差会迅速增大，难以稳定收敛，而改进模型可以快速稳定的收敛，显著提高了模型精度和滤波稳定性。

图11、图12和图13分别展示了本发明的组合导航方法对惯性导航系统位置、速度和姿态误差的修正效果，可以看到，本发明方法有效消除了惯性导航系统的积累误差，显著提高了导航精度。

以上所述的实施例仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (5)

1.一种基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，其特征在于，包括以下步骤：

步骤1，导航数据输入；

输入到惯性导航系统的导航数据主要为惯性元件加速度计和陀螺仪的输出数据；

步骤2，建立惯性导航系统姿态、速度、位置更新模型；

根据惯性导航系统姿态、速度、位置更新模型，分别得到惯性导航系统的姿态ψ、速度V和位置P；

步骤3，判断星敏感器有无数据输出；

以惯性导航系统的时间步长为基准，在每个时间步长开始时刻，首先判断此时星敏感器有无数据输出，若无数据输出，则直接输出步骤2中惯性导航系统更新后的姿态、速度和位置信息；若有数据输出，则执行步骤4-7；

步骤4，星敏感器量测数据处理；

S41，得到敏感器坐标系中的单位方向矢量；

S42，得到惯性坐标系和载体坐标系之间的姿态转换矩阵；

S43，得到目标卫星在载体坐标系下的观测角；

S44，得到目标卫星和载体潜在距离信息；

S45，将目标卫星单位方向矢量从载体坐标系转换到目标卫星轨道坐标系；

步骤5，建立组合导航系统状态方程和量测方程；

S51，建立组合导航系统的状态方程：

其中，X为组合导航系统状态变量，

为组合导航系统状态变量的一阶导，F是组合导航系统的状态转移矩阵，G为噪声驱动阵，W为系统噪声矩阵；

52，建立组合导航系统的量测方程：

选择惯性导航系统的平台误差角、北东地速度误差和经纬高位置误差作为观测量,使用组合导航系统中的星光导航系统和相对导航系统得到载体的姿态角和位置、速度，将星光导航系统和相对导航系统得到的导航信息与惯性导航系统得到的导航信息做差后，便可得到姿态角误差、速度误差和位置误差，再通过转换即可得到平台误差角、北东地速度误差和经纬高位置误差；

组合导航系统的量测方程为：

在建立组合导航系统量测方程的过程中，需要以C-W方程为基础建立目标卫星与载体的相对运动方程；但是，C-W方程要求目标卫星与载体之间的距离较小，当相对距离大于100km时，C-W方程的误差会迅速增大，因此需要改进C-W方程；

记目标卫星和载体在惯性坐标系下的绝对位置矢量分别为r_t和r_c，在二体问题的假设下，有：

其中，μ为地心引力常数，值为398600.44×10⁹m³/s²,

和

分别为矢量r_t和r_c的二阶导；

载体相对于目标卫星的相对位置矢量为ρ＝r_c-r_t，|ρ|为相对位置矢量ρ的模值，则有：

将式(44)转换到目标卫星轨道坐标系下，有：

其中，ω_t为目标卫星轨道角速度矢量，对于近圆轨道，ω_t的值可由平均角速度n表示，将式(45)写成分量形式，有：

其中，

ρ＝[x y z]；

对于近圆轨道，有

式(46)可简化为：

当目标卫星与载体之间的相对距离远小于其轨道半径，即|r_c|≈|r_t|,|ρ|＜＜R_e时，可对重力场进行一阶近似，有：

进而有：

μ/|r_t|²-μ|r_t|/|r_c|³＝μ/|r_t|²(1-|r_t|³/|r_c|³)≈3μx/|r_t|³  (49)

在载体无轨道机动且飞行轨迹为圆轨道的假设下，根据惯性导航系统输出的信息求得轨道角速度n’，有：

其中，|r_c|为地心与载体之间的距离；

当星敏感器光轴方向沿地心指向载体方向时，光轴方向与载体轨道坐标系的

轴重合，此时地心、载体与目标卫星可近似认为在一条直线上，即有：

x≈|ρ|≈|r_t-r_c|  (75)

根据式(49)有：

其中，k为比例系数，其表达式为：

此时，式(47)简化为：

对应的状态矩阵A'为：

改进后的状态方程仍然保留了线性齐次的良好性质，依靠比例系数k对相对运动模型进行修正，以使其保持良好的精度；

相对导航系统的量测方程为：

步骤6，组合导航系统方程离散化与卡尔曼滤波；

对通过式(32)和式(73)建立的组合导航系统的状态方程和量测方程进行离散化；离散化后的方程再通过卡尔曼滤波算法进行滤波，得到惯性导航系统的导航误差的最优估计值δφ,δV,δP；

步骤7，修正惯性导航系统的位置、速度、姿态信息；

步骤2中惯性导航系统得到的导航信息为ψ,V,P，利用步骤6中得到的导航误差的最优估计值δφ,δV,δP，对步骤2中惯性导航系统得到的导航信息进行修正，得到组合导航系统输出的导航信息：

其中，P_组合、V_组合和ψ_组合分别为组合导航系统输出的位置、速度和姿态信息；

步骤8，判断导航过程是否结束；

若未结束则返回步骤1，若结束则停止导航过程。

2.根据权利要求1所述的基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，其特征在于，所述步骤2中建立惯性导航系统姿态、速度、位置更新模型具体为：

S21，建立姿态更新模型；

姿态矩阵和四元数之间的转换关系为：

其中，

表示载体坐标系到导航坐标系的姿态四元数，γ为滚转角，θ为俯仰角，

为偏航角；

其中，

表示载体坐标系到导航坐标系的姿态旋转矩阵；

导航坐标系到载体坐标系的姿态旋转矩阵为

用T_ij(i＝1,2,3；j＝1,2,3)表示

中的元素，俯仰角、滚转角和偏航角与矩阵

中的元素对应关系为：

建立姿态四元数运动学微分方程为：

其中，

为四元数矩阵

的一阶导；

建立姿态更新模型为：

其中，Q(q_t+Δt)表示下一时刻的

姿态四元数，Q(q_t)表示当前时刻的

姿态四元数，Δt为捷联惯性导航系统的时间步长，t为当前导航时间；

I为四阶单位矩阵，Δθ＝[Δθ_x Δθ_y Δθ_z]，为单个时间步长内的姿态变化量，[Δθ]的表达式为：

S22，速度更新模型为：

其中，g为地球重力加速度，

为速度V的一阶导，

表示载体坐标系到导航坐标系的姿态旋转矩阵，

为加速度计的输出，表示载体坐标系相对于惯性坐标系的比力在载体坐标系的投影，

为地球坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影，

表示导航坐标系相对于地球坐标系的旋转角速度在导航坐标系下的投影；

S23，位置更新模型为：

其中，h₀,L₀,λ₀分别表示载体高度、纬度和经度的初始值，t₀为导航初始时间；V_N为地理坐标系内的北向速度、V_E为地理坐标系内的东向速度和V_D为地理坐标系内的地向速度；R_M和R_N分别为子午圈和卯酉圈的主曲率半径。

3.根据权利要求1所述的基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，其特征在于，所述步骤7中，修正惯性导航系统的位置、速度、姿态信息，具体为：

惯性导航系统输出的导航信息为ψ,V,P，利用步骤6中得到的导航误差的最优估计值δφ,δV,δP对步骤2中惯性导航系统得到的导航信息进行修正，得到组合导航系统输出的导航信息：

其中，P_组合、V_组合和ψ_组合分别为组合导航系统输出的位置、速度和姿态信息。

4.根据权利要求1所述的基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，其特征在于，所述步骤4星敏感器量测数据处理具体为：

S41，得到敏感器坐标系中的单位方向矢量；

目标在星敏感器坐标系中的单位方向矢量为：

其中，s_k为第k个目标在星敏感器坐标系的单位方向矢量；

S42，得到惯性坐标系和载体坐标系之间的姿态转换矩阵；

通过导航星历可以得到恒星和目标卫星在惯性坐标系中的坐标h₁,h₂,···h_n,h_n+1，其中h_k＝[x_hk y_hk z_hk]^T(k＝1,2,…n+1)；则s_k和h_k的关系可表示为：

其中，

表示星敏感器坐标系到惯性坐标系的姿态旋转矩阵；

表示载体坐标系到惯性坐标系的姿态旋转矩阵；

对于恒星目标，记矩阵S和H分别为：

则由式(18)可得：

其中，

表示从惯性坐标系到载体坐标系的姿态旋转矩阵；

当观测恒星数目n≥3时，姿态转换矩阵

采用最小二乘法计算得到：

即通过星敏感器获得恒星在载体坐标系的单位方向矢量，通过导航星历获得恒星在惯性坐标系中的坐标，结合公式(21)就可得到从惯性坐标系到载体坐标系的姿态旋转矩阵

S43，得到目标卫星在载体坐标系下的观测角；

对于目标卫星，基于单位方向矢量可求得其在载体坐标系下的观测角，即高度角α和方位角β分别为：

其中，x_s(n+1)、y_s(n+1)和z_s(n+1)为第n+1个目标相对于载体坐标系的单位方向矢量s_n+1的三个分量，本实施例中第n+1个目标为目标卫星；

S44，得到潜在距离信息；

由于目标卫星的星历已知，基于惯性导航系统可获得自身的位置信息，因而可得到目标卫星和载体二者之间的距离，得到潜在距离信息|ρ|，具体方式如下：

记t时刻目标卫星的星历位置为h_it，从惯性导航系统得到载体位置，载体位置包括载体自身的经度λ_t、纬度L_t、高度h_t，可得到t时刻载体在地球坐标系中的坐标r_et，坐标r_et的三个分量使用式(23)得到：

其中，R_N为卯酉圈的主曲率半径，f为地球的扁率；

根据地球坐标系与惯性坐标系之间的转换关系，可知地球坐标系到惯性坐标系的姿态旋转矩阵

为：

由此可得t时刻载体在惯性坐标系下的位置为：

由此得到潜在距离信息|ρ|为：

|ρ|＝|h_it-r_it| (26)；

S45，将目标卫星单位方向矢量从载体坐标系转换到目标卫星轨道坐标系首先将

从载体坐标系转换到惯性坐标系：

其中，

表示目标卫星单位方向矢量在载体坐标系下的坐标，

表示目标卫星单位方向矢量在惯性坐标系下的坐标，

表示载体坐标系到惯性坐标系的姿态旋转矩阵；

根据目标卫星的星历位置h_it和速度l_it信息，结合轨道坐标系

的定义，有：

则从惯性坐标系到目标卫星轨道坐标系的姿态矩阵

为：

由此可将

从惯性坐标系转换到目标卫星轨道坐标系下：

根据公式(22)得到目标卫星在载体坐标系下的观测角，使用公式(27)-(30)，将观测角转换到目标卫星轨道坐标系下。

5.根据权利要求1所述的基于合作目标卫星甚短弧观测的仅测角光学组合导航方法，其特征在于，所述步骤5中建立组合导航系统状态方程和量测方程，具体为：

S51，建立组合导航系统的状态方程；

根据加速度计和陀螺仪的误差特性，建模为一阶马尔科夫过程：

其中，ε和κ分别表示陀螺仪和加速度计误差，

和

分别表示陀螺仪和加速度计误差的一阶导，T_r和T_a分别为陀螺仪和加速度计的一阶马尔科夫过程相关时间，ω_ε和w_κ为陀螺仪高斯白噪声和加速度计高斯白噪声；

F是组合导航系统的状态转移矩阵，可写为：

上式中，F_s和F_m分别为：

diag()表示对角线元素为括号内所列出元素的对角矩阵；

状态变量X为：

X＝[φ_N φ_E φ_D δV_N δV_E δV_D δL δλ δh ε_N ε_E ε_D κ_N κ_E κ_D]^T(35)

其中，[φ_N φ_E φ_D]^T为平台误差角；[δV_N δV_E δV_D]^T为北东地速度误差；[δL δλ δh]^T为经纬高位置误差；[ε_N ε_E ε_D]^T为陀螺仪误差；[κ_N κ_E κ_D]^T为加速度计误差；

F_N为对应的9维基本导航参数系统矩阵，其非零元素为：

F_N(4,2)＝f_D,F_N(4,3)＝-f_E

F_N(5,1)＝-f_D,F_N(5,3)＝f_N

F_N(6,1)＝f_E,F_N(6,2)＝-f_N

F_N(9,6)＝-1

其中，f_N,f_E,f_D为比力沿北东地地理坐标系的三个分量，可根据加速度计输出

得到；

噪声驱动阵G为：

系统噪声矩阵W为：

其中，

和

分别为陀螺仪高斯白噪声和加速度计高斯白噪声的三个分量；

S52，建立组合导航系统的量测方程；

根据惯性导航系统得到的经纬高信息得到地球坐标系和导航坐标系之间的转换矩阵为：

导航坐标系相对于载体坐标系的姿态旋转矩阵

为：

姿态误差角和平台误差角之间的转换关系为：

将转换后的平台误差角作为信息融合的观测量，建立平台误差角量测方程为：

其中，H_φ＝[I_3×3 0_3×12]，V_φ表示系统的姿态角量测噪声；

将相对位置

和相对速度

分别转换到惯性坐标系下；

对于相对位置，有：

其中，

表示从目标卫星轨道坐标系到惯性坐标系的姿态旋转矩阵；

对于相对速度，根据矢量绝对导数与相对导数的关系，有：

其中，ω_t表示t时刻目标卫星轨道角速度矢量，其表达式为：

其中，h_it表示t时刻目标卫星的星历位置，|h_it|为h_it的模值，l_it表示t时刻目标卫星的星历速度；

则载体在惯性坐标系下的位置r_i和速度v_i为：

将位置和速度转换到地球坐标系下，有：

其中，r_e和v_e分别为地球坐标系下的位置和速度，

为从惯性坐标系到地球坐标系的姿态旋转矩阵；

对于位置矢量，惯性导航系统误差状态方程以经纬高位置误差作为状态量，故需要将r_e＝[x_e y_e z_e]^T转换为经度λ、纬度L和高度h，可采用迭代法完成，得到相对导航系统输出的位置；

对于速度矢量，相对导航系统根据式(38)得到的

进一步可得：

其中，v_n为相对导航系统输出的在导航坐标系下的速度，

为地球坐标系和导航坐标系之间的转换矩阵，将转换后的北东地速度误差和经纬高位置误差作为信息融合的观测量，分别建立速度和位置误差量测方程；

速度量测方程为：

其中，H_V＝[0_3×3I_3×3 0_3×9]，V_V表示系统的速度量测噪声；

位置量测方程为：

其中，H_P＝[0_3×6I_3×3 0_3×6]，V_P表示系统的位置量测噪声。

Images