CN112525203A - 一种基于角度约束辅助测量的航天器自主天文导航方法 - Google Patents

Abstract

本发明涉及一种基于角度约束辅助测量的航天器自主天文导航方法，首先建立基于太阳和八大行星的轨道动力学模型作为航天器的状态模型；其次根据导航天体至航天器的单位矢量与导航天体之间的单位矢量获取方位角量测量，并建立量测量的角度约束模型，在对系统非线性不等式角度约束建模的基础上利用序列二次规划(SQP)非线性规划方法辅助减小航天器自主导航系统的量测误差，之后建立方位角量测模型作为航天器的量测模型；根据航天器的状态模型和量测模型都呈现非线性特性，且系统噪声为非高斯噪声的特点，采用UKF滤波方法，估计航天器位置、速度，获得相对于导航天体的航天器位置和速度。

Description

一种基于角度约束辅助测量的航天器自主天文导航方法

技术领域

本发明属于航天导航技术领域，可在线估计航天器的位置和速度，适用于航天器的转移段和捕获段；具体涉及在航天器进行天文测量时，基于航天器和导航天体之间的角度约束信息，对量测量进行优化的自主天文导航方法，是一种非常适用于航天器的导航方法。

背景技术

21世纪初，世界主要航天集团都提出了未来的深空探测计划。我国的首个火星探测器“天问一号”已于2020年7月23日发射升空，并成功进入预定轨道。深空探测器距离地球较远，飞行速度较快，飞行距离远，时间长，环境未知性较强，传统上依靠地面测控的深空探测器导航与控制方法在实时性、成本和资源上受到种种限制，存在很多不足，很难满足深空探测一些特殊任务对高精度导航与控制的需要，因而对自主导航的要求更为迫切。自主天文导航已成为深空探测的关键技术。自主天文导航系统需要在同一时刻观测至少两个导航天体，通过两个天体的位置信息确定探测器的位置和速度。导航系统测量误差是制约深空探测自主天文导航精度的重要影响因素，因此，有效抑制敏感器的测量误差是深空探测器高精度自主天文导航的关键。

传统量测量误差修正方法主要有两种，一种是敏感器标定方法，可以去除敏感器测量中的常值误差，另一种是滤波方法，减小天文光学敏感器测量时的随机误差。但是这两种方法都没有利用航天器自主天文导航系统的内在系统约束，导航估计结果并不是最优的。

发明内容

本发明要解决的技术问题是：减小自主天文导航测量误差对导航精度的影响，弥补现有测量手段难以自主减小随机测量误差的不足，提供一种基于角度约束辅助测量的航天器自主天文导航方法，有效利用航天器自主天文导航系统满足的内在系统约束，为航天器提供一种高精度测量信息的自主天文导航方法。

本发明解决其技术问题所采用的技术方案为：一种基于角度约束辅助测量的航天器自主天文导航方法，首先建立基于太阳和八大行星的轨道动力学模型作为天文导航系统的状态模型；其次根据导航天体至航天器的单位矢量与导航天体之间的单位矢量获取方位角量测量，并建立量测量的角度约束模型，在对系统非线性不等式角度约束建模的基础上利用序列二次规划(SQP)非线性规划方法辅助减小航天器自主导航系统的量测误差，之后建立方位角量测模型作为天文导航系统的量测模型；根据航天器的状态模型和量测模型都呈现非线性特性，且系统噪声为非高斯噪声的特点，采用UKF滤波方法，估计航天器位置、速度，获得相对于导航天体的航天器位置和速度，提高航天器自主天文导航精度。

在实际航空任务中，航天器自主天文导航系统满足一定的内在系统约束。这些约束是实际复杂物理系统内在的固有特性，对这些内在约束建模将提高系统估计性能。已有航天器自主天文导航方法都尚未考虑导航系统量测量所满足的多种约束条件，更未对约束条件建模优化、导致系统模型中信息相互孤立，并未完全利用所含信息。因此需要对航天器自主天文导航系统量测量的角度约束条件进行建模、并针对约束模型，利用约束模型规划算法，辅助优化测量信息，进而可实现航天器高精度自主天文导航。

具体包括以下步骤：

1、建立航天器天文导航系统的状态模型

考虑太阳和火星、地球等八大行星对航天器的引力作用，建立基于太阳和八大行星引力轨道动力学模型，选取目标天体为中心的惯性坐标系，可得航天器在目标天体为中心的惯性坐标系中的状态模型：

式中，x,y,z为航天器在目标天体质心惯性坐标系中三轴位置，v_x,v_y,v_z为航天器在目标天体质心惯性坐标系中三轴速度，

为航天器在目标天体质心惯性坐标系中三轴位置的微分，

为航天器在目标天体质心惯性坐标系中三轴速度的微分，μ_s、μ_m和μ_ic分别为太阳、目标天体和第i_c颗行星的引力常数；r_ps为日心到航天器的距离；r_pm为目标天体质心到航天器的距离；r_ms为日心到目标天体质心的距离；

为第i_c颗行星到航天器的距离；r_mi为第i_c颗行星质心到火心的距离；(x_s,y_s,z_s)，

分别为太阳、第i_c颗行星在目标天体质心惯性坐标系中的三轴位置坐标，可根据时间由行星星历表获得，w_x,w_y,w_z分别为状态模型中航天器三轴的状态模型误差；i_c表示太阳和八大行星中从内至外的第i_c颗行星，如目标天体为火星，则i_c＝1,2,3...,N(i_c≠4)，N＝8，由于i_c＝4表示第4颗行星(火星)，因此不包含在求和表达式中。

式(1)中的各变量都是与时间t有关的变量，航天器天文导航系统的状态模型可以简写为：

式中，X(t)＝[x,y,z,v_x,v_y,v_z]^T为状态模型的状态向量，x,y,z,v_x,v_y,v_z分别为航天器在目标天体质心为中心的惯性坐标系中三轴的位置和速度，

为X(t)的微分，h(X(t),t为状态模型的系统非线性连续状态转移函数，w(t)＝[0 0 0 w_x w_y w_z]^T为航天器天文导航系统状态模型误差；

2、方位角量测量的获取

以导航天体与航天器之间的方位角作为航天器天文导航系统的量测量Z＝[θ_12S,θ_23S,θ_31S]^T，可根据导航天体至航天器的单位矢量与导航天体之间的单位矢量计算获得。航天器和第i颗导航天体之间的方位角θ_ijSi,j∈{1,2,3}(i≠j)表达式为：

θ_ijS＝arccos[L_iS·L_ij]+e_ijS (3)

式中，e_ijS是θ_ijS的测量噪声，L_iS是导航天体到航天器的单位矢量，L_ij是导航天体之间的单位矢量，可表示为：

式中，r_i、r_j和r_S分别为导航天体和航天器在以目标天体质心为中心的惯性坐标系中的位置矢量。

3、建立量测量角度约束模型

在同一时刻，三个导航天体到航天器的矢量方向不共线(如图2所示)，即

并且i≠j,我们可以通过以下方式确定与其相关的六个角度。

式中，i,j∈{1,2,3},i≠j。

为导航天体与目标天体间的方位角估计值，e_i为误差，真正的方位角φ_iS满足

φ_ij是准确的导航天体之间的方位角，φ_ij∈[0,2π)。定义角度θ_ijS＝(φ_iS-φ_ij)，θ_ijS∈(-π,π]。从几何上讲，我们可以把θ_ijS解释为从连接导航天体i和目标S的线段到从连接导航天体i和j的线段顺时针旋转光线获得的角度。因此，很明显θ_ijS＝-θ_iSj。

由于θ_ijS和θ_jiS的定义意味着这两个量的符号相反，根据正弦定理，可以得出以下方程组：

Asin(θ_12S)+Bsin(θ_21S)＝0

Bsin(θ_23S)+Csin(θ_32S)＝0

Asin(θ_13S)+Csin(θ_31S)＝0 (6)

消除A、B、C可得：

sin(θ_12S)sin(θ_23S)sin(θ_31S)+sin(θ_21S)sin(θ_32S)sin(θ_13S)＝0 (7)

公式(7)是三角形系统的角度之间的关系。把公式(5)代入公式(7)，可得测量误差的约束方程(8)：

式中，

是误差。约束条件(8)的优点是只包含测量的角度值作为参数。

航天器天文测角导航测量误差的角度约束，可以采用基于约束最优问题的最优化方法对测量误差进行进一步限制，从而减小测量误差对导航精度的影响。本文采用天文测角角度测量误差平方和f(e₁,e₂,e₃,…)＝e₁ ²+e₂ ²+e₃ ²+…作为目标函数，以测量误差的三角约束作为约束方程，建立最优化模型。

从式(9)(10)可以看出，基于角度约束的测量误差最优化问题是一个非线性规划问题。

4、利用序列二次规划(Sequential Quadratic Programming，SQP)非线性优化方法优化量测量

将量测量所满足的角度约束(如式(9)(10)所示)展开为拉格朗日二次逼近函数：

式中：λ_i为第i个约束的拉格朗日乘子。

线性化非线性约束，可得二次规划子问题，其目标函数为：

式中：d为全变量搜索方向，H_s为拉格朗日函数Hessian矩阵的正定拟牛顿近似，其可以采用BFGS方法更新，即：

式中：

δ_s＝x_s+1-x_s。形成如下迭代方程：

x_s+1＝x_s+α_sd_s (13)

式中：d_s为x_s到x_s+1的向量，步长参数α_s通过合适的线性搜索方式确定，使指标函数值得到足够的精度。

5、建立航天器天文导航系统的量测模型

天文导航系统量测模型选取航天器和三颗导航天体的方位角模型作为量测模型：

式中，θ_12S、θ_23S和θ_31S为航天器和三颗导航天体之间的方位角，L_iS是导航天体到航天器的单位矢量，L_ij是导航天体之间的单位矢量，e_12S,e_23S,e_31S分别为θ_12S,θ_23S,θ_3S1的测量噪声；

设天文导航系统量测量Z＝[θ_12S,θ_23S,θ_31S]^T，天文导航系统量测噪声

v_θ12S,v_θ23S,v_θ31S分别为测量θ_12S,θ_23S,θ_31S的测量误差，由于各变量都是与时间t有关的变量，则可建立天文导航系统量测模型的表达式为：

Z(t)＝f[X(t),t]+v(t) (15)

式中，f[X(t),t]为天文导航系统非线性连续量测函数；

6、对天文导航系统进行UKF滤波

根据天文导航系统状态模型(2)、天文导航系统量测模型(15)、星敏感器获得的量测量(3)，进行天文导航系统UKF滤波，获得在目标天体惯性坐标系中表示航天器位置、速度的估计状态量

和估计均方误差阵P_k；

7、计算目标天体质心为中心的惯性坐标系中航天器位置、速度估计状态向量

输出在目标天体为中心的惯性坐标系中的估计状态量

和估计均方误差阵P_k，估计状态量

表示在目标天体为中心的惯性坐标系中航天器的位置、速度信息，输出的估计均方误差阵P_k表示了滤波估计的性能，并将这些导航信息分别返回天文导航系中，用于k+1时刻的位置、速度导航信息，k＝1,2,...。

本发明的原理是：首先建立基于太阳和八大行星的轨道动力学模型作为天文导航系统的状态模型；其次根据导航天体至航天器的单位矢量与导航天体之间的单位矢量获取方位角量测量，并建立量测量的角度约束模型，在对系统非线性不等式角度约束建模的基础上利用序列二次规划(SQP)非线性规划方法辅助减小航天器自主导航系统的量测误差，之后建立方位角量测模型作为天文导航系统的量测模型；根据航天器的状态模型和量测模型都呈现非线性特性，且系统噪声为非高斯噪声的特点，采用UKF滤波方法，估计航天器位置、速度，获得相对于目标天体的航天器位置和速度，提高航天器自主天文导航的精度。

本发明与现有技术相比的优点在于：本发明对航天器自主天文导航系统量测量的角度约束条件进行建模、并在对系统非线性不等式角度约束建模的基础上，利用SQP非线性规划方法，对航天器自主天文导航系统的非线性不等式约束进行非线性规划，合理有效地利用航天器自主天文导航系统满足的内在系统约束，直接辅助减小航天器自主导航系统的量测误差。

附图说明

图1为本发明方法的流程图；

图2为本发明天文测角角度约束示意图。

具体实施方式

如图1所示，前述技术方案中所涉及的目标天体可以为火星、金星、木星、土星等太阳系内的天体，以下以火星作为实施例，说明本发明的具体实施过程：

1、建立航天器天文导航系统的状态模型

考虑太阳和火星、地球等八大行星对航天器的引力作用，建立基于太阳和八大行星的引力轨道动力学模型，选取火心惯性坐标系，可得航天器在火心惯性坐标系中的状态模型：

式中，x,y,z为航天器在火心惯性坐标系中三轴位置，v_x,v_y,v_z为航天器在火心惯性坐标系中三轴速度，

为航天器在火心惯性坐标系中三轴位置的微分，

为航天器在火心惯性坐标系中三轴速度的微分，μ_s、μ_m和

分别为太阳、火星和第i_c颗行星的引力常数；r_ps为日心到航天器的距离；r_pm为火心到航天器的距离；r_ms为日心到火心的距离；

为第i_c颗行星到航天器的距离；r_mi为第i_c颗行星质心到火心的距离；(x_s,y_s,z_s)，

分别为太阳、第i_c颗行星在火心惯性坐标系中的三轴位置坐标，可根据时间由行星星历表获得，w_x,w_y,w_z分别为状态模型中航天器三轴的状态模型误差；i_c表示太阳和八大行星中从内至外的第i_c颗行星，如目标天体为火星，则i_c＝1,2,3...,N(i_c≠4)，N＝8，由于i_c＝4表示第4颗行星(火星)，因此不包含在求和表达式中。

式(1)中的各变量都是与时间t有关的变量，航天器天文导航系统的状态模型可以简写为：

式中，X(t)＝[x,y,z,v_x,v_y,v_z]^T为状态模型的状态向量，x,y,z,v_x,v_y,v_z分别为航天器在火心惯性坐标系中三轴的位置和速度，

为X(t)的微分，h(X(t)，t)为状态模型的系统非线性连续状态转移函数，w(t)＝[0 0 0 w_x w_y w_z]^T为航天器天文导航系统状态模型误差；

2、方位角量测量的获取

以火星、火卫一、火卫二与航天器之间的方位角作为航天器天文测角导航系统的量测量Z＝[θ_12S,θ_23S,θ_31S]^T，可根据火星、火卫一、火卫二至航天器的单位矢量与火星、火卫一、火卫二之间的单位矢量计算获得。航天器和火星、火卫一、火卫二之间的方位角表达式为：

式中：θ_12S、θ_23S、θ_31S分别为火星、火卫一、火卫二与航天器之间的方位角，e_12S,e_23S,e_31S是火星、火卫一、火卫二与航天器之间的方位角的测量误差，L_pr、L_dr和L_mr分别为火卫一、火卫二和火星的天体矢量方向，L_pm、L_md、L_dp分别为火卫一、火卫二和火星之间的矢量方向，可表示为：

式中：r_p、r_d和r_m分别为火卫一、火卫二和火星在火心惯性坐标系中的位置矢量。

3、建立量测量角度约束模型

在同一时刻，火星、火卫一、火卫二到航天器的矢量方向不共线(如图2所示，即

并且i≠j，通过以下方式确定与其相关的六个角度。

式中，i,j∈{1,2,3},i≠j。

为火星、火卫一、火卫二与航天器之间的方位角估计值，e_i为误差，真正的方位角φ_iS满足

φ_ij是准确的火星、火卫一、火卫二之间的方位角，φ_ij∈[0,2π)。定义角度θ_ijS＝(φ_iS-φ_ij)，θ_ijS∈(-π,π]。从几何上讲，可以把θ_ijS解释为从连接导航天体i和目标S的线段到从连接导航天体i和j的线段顺时针旋转光线获得的角度。因此，很明显θ_ijS＝-θ_iSj。

由于θ_ijS和θ_jiS的定义意味着这两个量的符号相反，根据正弦定理，可以得出以下方程组：

Asin(θ_12S)+Bsin(θ_21S)＝0

Bsin(θ_23S)+Csin(θ_32S)＝0

Asin(θ_13S)+Csin(θ_31S)＝0 (6)

消除A、B、C可得：

sin(θ_12S)sin(θ_23S)sin(θ_31S)+sin(θ_21S)sin(θ_32S)sin(θ_13S)＝0 (7)

公式(7)是三角形系统的角度之间的关系。把公式(5)代入公式(7)，可得测量误差的约束方程(8)：

式中，

是误差。约束条件(8)的优点是只包含测量的角度值作为参数。

航天器天文测角导航测量误差的角度约束，可以采用基于约束最优问题的最优化方法对测量误差进行进一步限制，从而减小测量误差对导航精度的影响。本文采用天文测角角度测量误差平方和f(e₁,e₂,e₃,…)＝e₁ ²+e₂ ²+e₃ ²+…作为目标函数，以测量误差的三角约束作为约束方程，建立最优化模型。

从式(9)(10)可以看出，基于角度约束的测量误差最优化问题是一个非线性规划问题。

4、利用序列二次规划(Sequential Quadratic Programming，SQP)非线性优化方法优化量测量

将量测量所满足的角度约束(如式(9)(10)所示)展开为拉格朗日二次逼近函数：

式中：λ_i为第i个约束的拉格朗日乘子。

线性化非线性约束，可得二次规划子问题，其目标函数为：

式中：d为全变量搜索方向，H_s为拉格朗日函数Hessian矩阵的正定拟牛顿近似，其可以采用BFGS方法更新，即：

式中：

δ_s＝x_s+1-x_s。形成如下迭代方程：

x_s+1＝x_s+α_sd_s (13)

式中：d_s为x_s到x_s+1的向量，步长参数α_s通过合适的线性搜索方式确定，使指标函数值得到足够的精度。

5、建立航天器天文导航系统的量测模型

以火星、火卫一、火卫二与航天器之间的方位角作为天文测角导航系统的量测量，可由敏感器获得的像元像线坐标计算得到，则火星、火卫一、火卫二与航天器之间的方位角的量测模型可以表示为：

式中：θ_12S、θ_23S、θ_31S分别为火星、火卫一、火卫二与探测器之间的方位角，L_pr、L_dr和L_mr分别为火卫一、火卫二和火星的天体矢量方向，L_pm、L_md、L_dp分别为火卫一、火卫二和火星之间的矢量方向，e_12S,e_23S,e_31S是火星、火卫一、火卫二与探测器之间的方位角的测量误差。

式中：r_p、r_d和r_m分别为火卫一、火卫二和火星在火心惯性坐标系中的位置矢量，||r_p-r||、||r_m-r||、||r_d-r||分别为探测器与火卫一、火星和火卫二之间的距离。

令Z＝[θ_12S,θ_23S,θ_31S]^T，量测噪声E＝[e_12S ^T,e_23S ^T,e_31S ^T]^T，则以火星、火卫一、火卫二与探测器之间的方位角作为量测量的量测模型可表示为：

Z(t)＝H[X(t),t]+E(t) (16)

式中：H(·)为天文测角自主导航系统以火星、火卫一、火卫二与探测器之间的方位角为量测量的量测方程。

6、对天文导航系统进行UKF滤波

根据天文导航系统状态模型(2)、天文导航系统量测模型(16)、星敏感器获得的量测量(3)，进行航天器天文导航系统UKF滤波，获得在火心惯性坐标系中表示航天器位置、速度的估计状态量

和估计均方误差阵P_k；

7、计算火心惯性坐标系中航天器位置、速度估计状态向量

输出在火心惯性坐标系中的估计状态量

和估计均方误差阵P_k，估计状态量

表示在火心惯性坐标系中航天器的位置、速度信息，输出的估计均方误差阵P_k表示了滤波估计的性能，并将这些导航信息分别返回天文导航系中，用于k+1时刻的位置、速度导航信息，k＝1,2,...。

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

Claims (4)

1.一种基于角度约束辅助测量的航天器自主天文导航方法，其特征在于：

第一步，建立基于太阳和八大行星的轨道动力学模型作为航天器的状态模型；

第二步，根据导航天体至航天器的单位矢量与导航天体之间的单位矢量获取方位角量测量，并建立量测量的角度约束模型，在对系统非线性不等式角度约束建模的基础上利用序列二次规划(SQP)非线性规划方法辅助减小航天器自主导航系统的量测误差，建立方位角量测模型作为航天器的量测模型；

第三步，根据航天器的状态模型和量测模型均呈现非线性特性，且系统噪声为非高斯噪声的特点，采用UKF滤波方法，估计航天器位置、速度，获得相对于导航天体的航天器位置和速度，提高航天器自主天文导航系统的精度。

2.根据权利要求1所述的基于角度约束辅助测量的航天器自主天文导航方法，其特征在于：所述第一步骤中，建立基于太阳和八大行星的轨道动力学模型作为天文导航系统的状态模型具体如下：

基于太阳和火星、地球等八大行星对航天器的引力作用，建立基于太阳和八大行星引力轨道动力学模型，选取目标天体为中心的惯性坐标系，得到航天器在目标天体为中心的惯性坐标系中的状态模型：

式中，x,y,z为航天器在目标天体质心惯性坐标系中三轴位置，v_x,v_y,v_z为航天器在目标天体质心惯性坐标系中三轴速度，

为航天器在目标天体质心惯性坐标系中三轴位置的微分，

为航天器在目标天体质心惯性坐标系中三轴速度的微分，μ_s、μ_m和μ_ic分别为太阳、目标天体和第i_c颗行星的引力常数；r_ps为日心到航天器的距离；r_pm为目标天体质心到航天器的距离；r_ms为日心到目标天体质心的距离；

为第i_c颗行星到航天器的距离；r_mi为第i_c颗行星质心到火心的距离；(x_s,y_s,z_s)，

分别为太阳、第i_c颗行星在目标天体质心惯性坐标系中的三轴位置坐标，根据时间由行星星历表获得，w_x,w_y,w_z分别为状态模型中航天器三轴的状态模型误差；i_c表示太阳和八大行星中从内至外的第i_c颗行星；

式(1)中的各变量都是与时间t有关的变量，航天器天文导航系统的状态模型简写为：

式中，X(t)＝[x,y,z,v_x,v_y,v_z]^T为状态模型的状态向量，x,y,z,v_x,v_y,v_z分别为航天器在目标天体质心为中心的惯性坐标系中三轴的位置和速度，

为X(t)的微分，h(X(t),t为状态模型的系统非线性连续状态转移函数，w(t)＝[0 0 0 w_x w_y w_z]^T为航天器天文导航系统状态模型误差。

3.根据权利要求1所述的基于角度约束辅助测量的航天器自主天文导航方法，其特征在于：所述第二步具体实现如下：

(1)方位角量测量的获取为：

以导航天体与航天器之间的方位角作为航天器天文导航系统的量测量Z＝[θ_12S,θ_23S,θ_31S]^T，根据导航天体至航天器的单位矢量与导航天体之间的单位矢量计算获得，航天器和第i颗导航天体之间的方位角θ_ijSi,j∈{1,2,3}(i≠j)表达式为：

θ_ijS＝arccos[L_iS·L_ij]+e_ijS (3)

式中，e_ijS是θ_ijS的测量噪声，L_iS是导航天体到航天器的单位矢量，L_ij是导航天体之间的单位矢量，表示为：

式中，r_i、r_j和r_S分别为导航天体和航天器在以目标天体质心为中心的惯性坐标系中的位置矢量；

(2)建立量测量角度约束模型

在同一时刻，三个导航天体到航天器的矢量方向不共线，即：

并且i≠j,通过以下方式确定与其相关的六个角度；

式中，i,j∈{1,2,3},i≠j，

为导航天体与目标天体间的方位角估计值，e_i为误差，真正的方位角φ_iS满足

φ_ij是准确的导航天体之间的方位角，φ_ij∈[0,2π)；定义角度θ_ijS＝(φ_iS-φ_ij)，θ_ijS∈(-π,π]；θ_ijS为从连接导航天体i和目标S的线段到从连接导航天体i和j的线段顺时针旋转光线获得的角度，θ_ijS＝-θ_iSj；

根据正弦定理，得出以下方程组：

A sin(θ_12S)+B sin(θ_21S)＝0

B sin(θ_23S)+C sin(θ_32S)＝0

A sin(θ_13S)+C sin(θ_31S)＝0 (6)

消除A、B、C得：

sin(θ_12S)sin(θ_23S)sin(θ_31S)+sin(θ_21S)sin(θ_32S)sin(θ_13S)＝0 (7)

公式(7)是三角形系统的角度之间的关系；

把公式(5)代入公式(7)，得到测量误差的约束方程(8)：

式中，

是误差，约束条件(8)只包含测量的角度值作为参数；

航天器天文导航系统测量误差的角度约束，采用基于约束最优问题的最优化方法对测量误差进行进一步限制，减小测量误差对导航精度的影响；采用天文测角角度测量误差平方和f(e₁,e₂,e₃,…)＝e₁ ²+e₂ ²+e₃ ²+…作为目标函数，以测量误差的三角约束作为约束方程，建立最优化模型：

(3)利用序列二次规划(Sequential Quadratic Programming，SQP)非线性优化方法优化量测量；

将量测量所满足的角度约束如式(9)所示展开为拉格朗日二次逼近函数：

式中：λ_i为第i个约束的拉格朗日乘子；

线性化非线性约束，得到二次规划子问题，其目标函数为：

式中：d为全变量搜索方向，H_s为拉格朗日函数Hessian矩阵的正定拟牛顿近似，采用BFGS方法更新，即：

式中：

δ_s＝x_s+1-x_s，形成如下迭代方程：

x_s+1＝x_s+α_sd_s (13)

式中：d_s为x_s到x_s+1的向量，步长参数α_s通过合适的线性搜索方式确定，使指标函数值得到足够的精度；

(4)建立航天器天文导航系统的量测模型

天文导航系统的量测模型选取航天器和三颗导航天体的方位角模型作为量测模型：

式中，θ_12S、θ_23S和θ_31S为航天器和三颗导航天体之间的方位角，L_iS是导航天体到航天器的单位矢量，L_ij是导航天体之间的单位矢量，e_12S,e_23S,e_31S分别为θ_12S,θ_23S,

的测量噪声；

设天文导航系统量测量Z＝[θ_12S,θ_23S,θ_31S]^T，天文导航系统量测噪声

分别为测量θ_12S,θ_23S,θ_31S的测量误差，由于各变量都是与时间t有关的变量，则建立天文导航系统量测模型的表达式为：

Z(t)＝f[X(t),t]+v(t) (15)

式中，f[X(t),t]为天文导航系统非线性连续量测函数。

4.根据权利要求1所述的基于角度约束辅助测量的航天器自主天文导航方法，其特征在于：所述第三步具体实现如下：

(1)根据天文导航系统的状态模型、天文导航系统的量测模型、星敏感器获得的量测量，进行天文导航系统UKF滤波，获得在目标天体惯性坐标系中表示航天器位置、速度的估计状态量

和估计均方误差阵P_k；

(2)计算目标天体质心为中心的惯性坐标系中航天器位置、速度估计状态向量输出在目标天体为中心的惯性坐标系中的估计状态量

和估计均方误差阵P_k，估计状态量

表示在目标天体为中心的惯性坐标系中航天器的位置、速度信息，输出的估计均方误差阵P_k表示了滤波估计的性能，并将这些导航信息分别返回天文导航系中，用于k+1时刻的位置、速度导航信息，k＝1,2,...。

Images