[发明专利]基于合作目标卫星甚短弧观测的仅测角光学组合导航方法

权利要求书

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，若结束则停止导航过程。