CN110567461A - 一种考虑无陀螺仪的非合作航天器姿态和参数估计方法 - Google Patents

Abstract

本发明公开了一种考虑无陀螺仪的非合作航天器姿态和参数估计方法，包括如下步骤：考虑依据光学敏感器建立的目标非合作航天器相对坐标系与其本体坐标系不重合的问题，建立目标非合作航天器相对坐标系下的空间快速翻滚的非合作航天器的姿态运动学与动力学模型；选取目标非合作航天器姿态四元数矢量部分、三轴旋转角速度和转动惯量比的误差作为滤波变量，推导线性化系统的离散状态方程和观测方程；设计乘性拓展卡尔曼滤波器，并改进设计五步预测，以提高滤波估计精度和跟随快速性，解决了光学敏感器采样频率过低导致滤波误差增大的问题。

Description

一种考虑无陀螺仪的非合作航天器姿态和参数估计方法

技术领域

本发明属于航天器导航领域，特别涉及一种考虑无陀螺仪的非合作航天器姿态和参数估计方法，能够辨识空间非合作航天器转动惯量比并实现高精度、实时的相对姿态运动估计。

背景技术

航天器姿态确定是在拥有星敏感器和陀螺的情况下对本体进行定姿，而目前面临的多是失效航天器维护和失控航天器抓捕等任务，所针对的研究对象是空间非合作无控航天器。这类航天器往往在空间失控快速翻滚，因而需要在目标外形特征未知、无应答、无标识的情况下获取该非合作航天器的运动状态和动力学参数。具体而言，为实施空间非合作航天器的抓捕和控制等任务，需要其姿态和角速度以及惯量比等运动状态和参数进行估计和辨识。

针对航天器定姿的同时进行惯量比辨识的问题，中国专利CN 102620886B利用一种两步在轨辨识系统参数的方法，首先建立EKF滤波器得到转动惯量比，然后对组合航天器施加控制力矩，得到转动惯量的最小二乘估计。该方法属于组合体定姿，目标航天器和主动航天器组合后，主动航天器上的陀螺仪提供角速度测量信息。但该方法不适用于没有陀螺仪测量信息的相对导航，其施加控制力矩求解的做法也不适用于对空间非合作无控翻滚航天器进行定姿。

中国专利CN 107607737 A提出一种考虑无陀螺角速度测量的导航技术，该技术基于星光矢量，通过匹配两拍摄时刻星图对应关系，估算姿态变化矩阵。但是该方法只能对本体进行定姿，不适用于非合作航天器的相对导航。

发明内容

为了解决上述现有技术的缺陷，本发明提出了一种考虑无陀螺仪的非合作航天器姿态和参数估计方法。本发明在仅采用光学成像系统作为敏感器采集目标航天器姿态信息的前提下，提出一种基于无陀螺仪信息的滤波估计算法，能够实现对空间非合作航天器姿态、角速度的高精度估计和转动惯量比辨识。

根据本发明的一方面，提供了一种考虑无陀螺仪的非合作航天器姿态和参数估计方法，包括如下步骤：

S1：考虑依据光学敏感器建立的目标非合作航天器相对坐标系与其本体坐标系不重合的问题，建立目标非合作航天器相对坐标系下的空间快速翻滚的非合作航天器的姿态运动学与动力学模型；

S2：基于步骤S1中建立的非合作航天器的姿态运动学与动力学模型，选取目标非合作航天器姿态四元数矢量部分、三轴旋转角速度和转动惯量比的误差作为滤波变量，推导线性化系统的离散状态方程和观测方程；

S3：基于步骤S2中推导的线性化系统的离散状态方程和观测方程，设计乘性拓展卡尔曼滤波器，并改进设计五步预测，以提高滤波估计精度和跟随快速性。

进一步，步骤S1中，依据光学敏感器建立目标非合作航天器相对坐标系具体如下：

当同时观测到目标非合作航天器上的3个非共线的特征点P₁，P₂，P₃时，定义目标非合作航天器相对坐标系的原点o_T为P₁，矢量的方向为x_T轴方向，x_T轴的单位矢量为

定义z_T轴为矢量和矢量所在平面的法向量，其单位矢量为

根据右手定则确定y_T轴的单位矢量r_y为

r_y＝r_z×r_x (3)

则坐标系o_Tx_Ty_Tz_T即为目标非合作航天器相对坐标系；

非合作航天器的姿态动力学方程为：

其中，J是目标非合作航天器在其相对坐标系下的转动惯量矩阵，为3×3的对称矩阵：

其中，对角元素J_xx，J_yy，J_zz为主轴转动惯量，非对角元素J_xy，J_xz，J_yz为惯量积；ω＝[ω_x ω_y ω_z]^T为目标非合作航天器的三轴旋转角速度；为目标非合作航天器的三轴旋转角加速度；τ为外部干扰力矩，建模为高斯白噪声；ω^×是斜反对称矩阵，其具体形式如下：

非合作航天器的姿态运动学方程采用四元数描述，为非合作航天器提供全局无奇异的运动姿态表示，

定义目标非合作航天器相对坐标系下的非合作航天器姿态四元数为 则非合作航天器的姿态运动学的四元数表述为：

式中，矩阵Ξ(q)定义如下：

进一步，步骤S2具体过程如下：

基于步骤S1中建立的非合作航天器的姿态动力学方程(4)和姿态运动学的四元数表述方程(7)，定义姿态误差四元数为

式中，为四元数乘法；上标“^”代表估计值；四元数的逆为 满足：

定义各变量误差如下：其中，δq_v为目标非合作航天器相对坐标系下定义的姿态误差四元数的矢量部分；δω为误差三轴旋转角速度；δJ′为误差转动惯量比；

定义状态变量：

其中，

对姿态误差四元数的矢量部分的动力学方程线性化得到：

对非合作航天器的姿态动力学方程线性化得：

其中，为偏微分符号；均为雅克比矩阵；

则线性化系统的离散状态方程和观测方程为

x_k+1＝Φ_kx_k+ε_k (14)

y_k＝Hx_k+υ_k (15)

式中，下标“k”表示k时刻的取值；x_k为k时刻的状态变量；ε_k为过程噪声；υ_k为观测噪声；y_k为观测值；Φ_k为状态转移矩阵；H为观测矩阵，其中，过程噪声ε_k和观测噪声υ_k为均值为零的不相关白噪声，

Φ_k＝e^AΔT≈I_11×11+AΔT (16)

式中，ΔT为离散时间间隔；I表示单位矩阵；

观测方程根据光学敏感器的输出给出，由于观测量仅为姿态误差四元数，与三轴旋转角速度和转动惯量比无关，即 得到观测矩阵H＝[I_3×3 0_3×3 0_3×5]。

进一步，步骤S3中，乘性拓展卡尔曼滤波器的建立分为状态一步预测、滤波观测量计算、滤波更新和姿态校正四个部分：

1)状态一步预测

由非合作航天器的姿态动力学方程式(4)和非合作航天器的姿态运动学方程式(7)得到和计算姿态四元数的预估计值：

其中，为k时刻三轴旋转角加速度的估计值；为k时刻姿态四元数矢量部分的估计值；为k时刻姿态四元数矢量部分对时间导数的估计值；

2)滤波观测量计算

根据光学敏感器观测四元数和预测四元数运算计算姿态误差四元数

3)滤波更新

依照Kalman方程进行递推计算：

P_k|k-1＝ΦP_k-1Φ^T+Q_k-1 (20)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (21)

x_k＝x_k-1+K_k(δq_v-Hx_k-1) (23)

其中，P_k|k-1为k时刻最优预测估值误差协方差阵；P_k-1为k-1时刻最优滤波值误差协方差阵；K_k为k时刻增益矩阵；Q_k-1为k-1时刻系统的噪声方差矩阵；R_k为量测方差矩阵；

4)姿态校正

更新角速度：δω为状态变量x_k中的误差三轴旋转角速度；并保持姿态误差四元数归一性：

循环步骤1)-4)，输出目标非合作航天器的相对状态四元数、三轴旋转角速度和转动惯量比矩阵；

乘性拓展卡尔曼滤波系统采用姿态误差四元数的矢量部分用于滤波更新，而完整四元数用于全局非奇异位姿递推，因而利用式(24)在每步滤波更新中修正姿态四元数进行姿态递推，

考虑到非合作航天器搭载光学成像敏感器的频率不能满足姿态估计的高精度和快速跟踪的要求，在离散时间间隔ΔT内划分五个小时间隔，进行五次预测，一次测量更新，即，以五段折线拟合航天器姿态非线性方程在ΔT内的数值解。

本发明的有益效果：

1)本发明基于乘性拓展卡尔曼滤波的方法采用误差四元数矢量部分作为状态变量，不仅避免状态奇异，而且可以提升滤波收敛速度；此外，能够对带惯量积的转动惯量矩阵进行完整快速的辨识，获取目标系统参数的同时进一步提升姿态估计精度。

2)实际非合作航天器缺少陀螺仪提供的角速度观测信息，本发明仅采用光学成像敏感器采集目标非合作航天器姿态信息，设备简单，适合在实际相对导航中应用。

3)与常规搭建的滤波预测不同，本发明采用五步预测环节，克服了光学敏感器采样频率过低导致滤波误差增大的问题，可以减小非线性函数递推产生的估计误差，提高估计精度。

附图说明

图1为本发明的考虑无陀螺仪的非合作航天器姿态和参数估计方法的步骤流程图；

图2为本发明的姿态确定Kalman滤波流程图；

图3为本发明的Kalman滤波更新模块框图；

图4为本发明的多步预测误差效果展示图；

图5为本发明的目标非合作航天器相对姿态的误差图；

图6为本发明的目标非合作航天器相对角速度的误差图；

图7为本发明的目标非合作航天器转动惯量比的误差图。

具体实施方式

下面结合附图对本发明作进一步描述。

如图1所示，本发明的考虑无陀螺仪的非合作航天器姿态和参数估计的方法，包括：首先，考虑目标坐标系与其本体坐标系不重合的问题，建立相对目标非合作航天器坐标系下空间快速翻滚的非合作航天器的姿态运动学与动力学模型；然后，选取相对姿态四元数矢量部分、三轴旋转角速度和转动惯量比的误差作为滤波变量，推导线性化系统的离散状态方程和观测方程；最后，设计乘性拓展卡尔曼滤波器，并改进设计五步预测，以提高滤波估计精度和跟随快速性，解决了光学敏感器采样频率过低导致滤波误差增大的问题。

本发明的具体实现步骤如下：

S1：考虑依据光学敏感器建立的目标非合作航天器坐标系与其本体坐标系不重合的问题，建立目标非合作航天器相对坐标系下的空间快速翻滚的非合作航天器的姿态运动学与动力学模型。

本发明建立的目标航天器相对坐标系与传统航天器定姿中所建立的坐标系有一个显著不同的地方：传统航天器通常对自身进行定姿，本体信息是确定的，三坐标轴与转动惯量主轴重合；而本方法针对空间非合作航天器，对目标非合作航天器缺乏先验信息，与目标也不进行通信，无法获取相关姿态信息，所以依赖光学敏感器(例如相机)建立的目标相对坐标系往往与目标本体系不重合，导致转动惯量矩阵出现惯量。依据光学敏感器建立目标相对坐标系如下：

当同时观测到非合作航天器上的3个非共线的特征点P₁，P₂，P₃时，定义相对坐标系的原点o_T为P₁，矢量方向为x_T轴的方向，其单位矢量为

定义z_T轴为矢量和矢量所在平面的法向量，其单位矢量为

根据右手定则可以确定y_T轴单位矢量为r_y

r_y＝r_z×r_x (3)

则坐标系o_Tx_Ty_Tz_T即为目标相对坐标系。

考虑非合作航天器的姿态动力学方程为：

其中，J是非合作航天器在相对坐标系下的转动惯量矩阵，为3×3的对称矩阵：

在本实例中，设定其中，对角元素J_xx，J_yy，J_zz为主轴转动惯量，非对角元素J_xy，J_xz，J_yz为惯量积；ω为非合作航天器的三轴旋转角速度，初值

ω＝[ω_x ω_y ω_z]^T＝[0.2 0.05 0.05]^Trad/s；为非合作航天器的三轴旋转角加速度；τ为外部干扰力矩，包括重力梯度力矩，磁力矩以及气动扭矩，太阳辐射力矩等，可建模为高斯白噪声；ω^×是斜反对称矩阵，其具体形式如下：

非合作航天器的姿态运动学方程采用四元数来描述，其可以为非合作航天器提供全局无奇异的运动姿态表示。目标相对坐标系下的三轴旋转角速度为ω，并定义目标相对坐标系下的非合作航天器姿态四元数为 其初值设为[0.9943 0.0991 0.0274 0.0274]^T，则非合作航天器的四元数运动学方程为

式中，矩阵Ξ(q)定义如下：

S2：选取相对姿态四元数矢量部分、三轴旋转角速度和转动惯量比的误差作为滤波变量，推导线性化系统的离散状态方程和观测方程.

乘性拓展卡尔曼滤波比经典拓展卡尔曼滤波在四元数描述的姿态估计中更有优势，四元数的归一化约束导致其四个参数有一个是冗余的，故在基于EKF的姿态估计中，使用全部四个参数建立状态方程会导致状态不可观，表现在滤波过程中状态误差协方差阵产生奇异：所以选择乘性拓展卡尔曼滤波，估计无约束的姿误差态四元数矢量部分并在滤波更新预测周期内保持四元数的归一化约束，为航天器提供全局非奇异姿态描述。由式(4)继续推导得：

其中，

不失一般性，假设J_xx为转动惯量矩阵中最大的元素，对航天器动力学方程两边同除以J_xx＝1000：

其中

式(4-3)中等号右边第二项同样可以建模为高斯白噪声，因此在姿态滤波估计不需要辨识惯量的真实值，只需利用转动惯量之间的相对比率，同时这并不影响航天器的姿态确定与估计。

根据以上推导，定义姿态误差四元数为：

式中为四元数乘法，上标“∧”代表估计值，四元数的逆为 满足：

定义各变量误差如下：式中，δq_v为目标相对坐标系下定义的姿态误差四元数的矢量部分；δω为误差三轴旋转角速度；δJ′为误差转动惯量比。选择各变量的误差变量作为滤波器的状态，相比直接使用变量在滤波收敛速度上有较大优势。

定义状态变量：

其中，

根据小角近似条件对姿态误差四元数的矢量部分的动力学方程线性化得：

对非合作航天器的姿态动力学方程线性化得

其中，为偏微分符号；均为雅克比矩阵。

则线性化系统的离散状态方程和观测方程为

x_k+1＝Φ_kx_k+ε_k (14)

y_k＝Hδx_k+υ_k (15)

式中，下标“k”表示k时刻的取值；x_k为k时刻的状态变量；ε_k为过程噪声；υ_k为观测噪声；y_k为观测值；Φ_k为状态转移矩阵；H为观测矩阵，其中，过程噪声ε_k和观测噪声υ_k为均值为零的不相关白噪声。

Φ_k＝e^AΔT≈I_11×11+AΔT (16)

式中，ΔT为离散时间间隔，即光学敏感器的采样间隔，在本实例中取1s。观测方程根据光学敏感器的输出给出。由于观测量仅为姿态误差四元数，与三轴旋转角速度和转动惯量比无关，即 得到观测矩阵为：H＝[I_3×3 0_3×3 0_3×5]。

S3：基于步骤S2中推导的线性化系统的离散状态方程和观测方程，设计乘性拓展卡尔曼滤波器，并改进设计五步预测，以提高滤波估计精度和跟随快速性。

乘性拓展卡尔曼滤波器的建立分成预测、计算、更新和校正四个部分详细描述如下：

1)状态一步预测

由式(4)和式(7)得到和计算姿态四元数的预估计值：

其中，为k时刻三轴旋转角加速度的估计值；为k时刻姿态四元数矢量部分对时间导数的估计值；为k时刻姿态四元数矢量部分的估计值。

2)滤波观测量计算

根据光学敏感器观测四元数和预测四元数计算可得姿态误差四元数：

3)滤波更新

依照Kalman方程进行递推计算：

P_k|k-1＝ΦP_k-1Φ^T+Q_k-1 (20)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (21)

x_k＝x_k-1+K_k(δq_v-Hx_k-1) (23)

其中，P_k|k-1为k时刻最优预测估值误差协方差阵；P_k-1为k-1时刻最优滤波值误差协方差阵；K_k为k时刻增益矩阵；Q_k-1为k-1时刻系统的噪声方差矩阵；R_k为量测方差矩阵；

4)姿态校正

更新角速度：δω为状态变量x_k中的误差三轴旋转角速度；并保持姿态误差四元数归一性：

循环上述步骤1)-4)，输出目标航天器的相对状态四元数、三轴旋转角速度和转动惯量比矩阵。滤波流程和更新框图如图2和图3所示。乘性拓展卡尔曼滤波系统采用误差四元数的矢量部分用于滤波更新，而完整四元数用于全局非奇异位姿递推。因而利用式(24)在每步滤波更新中修正姿态四元数进行姿态递推，这是乘性拓展卡尔曼最显著的特征。

此外，本发明的滤波预测环节是区别于传统拓展卡尔曼的五步预测。滤波过程中的一步预测本质上是根据式(4)与式(7)，然后由牛顿梯度下降法进行预测：考虑非合作航天器搭载光学成像敏感器的频率为1Hz，即，1s内仅对滤波结果校正一次，不能满足姿态估计的高精度和快速跟踪的要求。当采样时间达到1s时，角速度和估计姿态均偏离真实值较多。所以在ΔT＝1s内划分成五个小时间间隔Δt＝0.2s，进行五次预测，一次测量更新，即以五段折线拟合航天器姿态非线性方程在这1s内的数值解，由此提高滤波估计精度和跟随快速性，解决了光学敏感器采样频率过低导致滤波误差增大的问题。本发明的多步预测误差效果如图4所示。

利用上面给出的乘性拓展卡尔曼滤波器，可以实现无陀螺获取空间非合作航天器的转动惯量参数并实现高精度、实时的运动特性估计。最后为了验证本发明的有效性，在MATLAB/Simulink平台环境下提供一个实际算例来阐述本发明，但此算例并不对本发明构成不当限定。仿真结果如下：

表1各状态变量的滤波精度和收敛时间结果

目标航天器相对姿态的误差如图5所示；目标航天器相对角速度的误差如图6所示；目标航天器转动惯量比的误差如图7所示。

上述仿真结果充分说明，在无陀螺仪信息的情况下，本发明设计的非合作航天器姿态和参数估计方法可以满足无控翻滚航天器运动估计的精度和实时性要求，实现高精度的运动目标相对姿态估计。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。以上所述仅为本发明的具体实施算例，并不用以限制本发明。其他在本发明的技术和原则之内，所做的任何改进，替换等，均应属于本发明的保护范围之内。

Claims (4)

1.一种考虑无陀螺仪的非合作航天器姿态和参数估计方法，其特征在于，包括如下步骤：

S1：考虑依据光学敏感器建立的目标非合作航天器相对坐标系与其本体坐标系不重合的问题，建立目标非合作航天器相对坐标系下的空间快速翻滚的非合作航天器的姿态运动学与动力学模型；

S2：基于步骤S1中建立的非合作航天器的姿态运动学与动力学模型，选取目标非合作航天器姿态四元数矢量部分、三轴旋转角速度和转动惯量比的误差作为滤波变量，推导线性化系统的离散状态方程和观测方程；

S3：基于步骤S2中推导的线性化系统的离散状态方程和观测方程，设计乘性拓展卡尔曼滤波器，并改进设计五步预测，以提高滤波估计精度和跟随快速性。

2.根据权利要求1所述的方法，其特征在于,步骤S1中，依据光学敏感器建立目标非合作航天器相对坐标系具体如下：

当同时观测到目标非合作航天器上的3个非共线的特征点P₁，P₂，P₃时，定义目标非合作航天器相对坐标系的原点o_T为P₁，矢量的方向为x_T轴方向，x_T轴的单位矢量为

定义z_T轴为矢量和矢量所在平面的法向量，其单位矢量为

根据右手定则确定y_T轴的单位矢量r_y为

r_y＝r_z×r_x (3)

则坐标系o_Tx_Ty_Tz_T即为目标非合作航天器相对坐标系；

非合作航天器的姿态动力学方程为：

其中，J是目标非合作航天器在其相对坐标系下的转动惯量矩阵，为3×3的对称矩阵：

其中，对角元素J_xx，J_yy，J_zz为主轴转动惯量，非对角元素J_xy，J_xy，J_yz为惯量积；ω＝[ω_xω_y ω_z]^T为目标非合作航天器的三轴旋转角速度；为目标非合作航天器的三轴旋转角加速度；τ为外部干扰力矩，建模为高斯白噪声；ω^×是斜反对称矩阵，其具体形式如下：

非合作航天器的姿态运动学方程采用四元数描述，为非合作航天器提供全局无奇异的运动姿态表示，

定义目标非合作航天器相对坐标系下的非合作航天器姿态四元数为 则非合作航天器的姿态运动学的四元数表述为：

式中，矩阵Ξ(q)定义如下：

其中，I为单位矩阵。

3.根据权利要求2所述的方法，其特征在于，步骤S2具体过程如下：

基于步骤S1中建立的非合作航天器的姿态动力学方程(4)和姿态运动学的四元数表述方程(7)，定义姿态误差四元数为

式中，为四元数乘法；上标“^”代表估计值；四元数的逆为 满足：

定义各变量误差如下：其中，δq_v为目标非合作航天器相对坐标系下定义的姿态误差四元数的矢量部分；δω为误差三轴旋转角速度；δJ′为误差转动惯量比；

定义状态变量：

其中，

对姿态误差四元数的矢量部分的动力学方程线性化得到：

对非合作航天器的姿态动力学方程线性化得：

其中，为偏微分符号；均为雅克比矩阵；

则线性化系统的离散状态方程和观测方程为

x_k+1＝Φ_kx_k+ε_k (14)

y_k＝Hx_k+υ_k (15)

式中，下标“k”表示k时刻的取值；x_k为k时刻的状态变量；ε_k为过程噪声；υ_k为观测噪声；y_k为观测值；Φ_k为状态转移矩阵；H为观测矩阵，其中，过程噪声ε_k和观测噪声υ_k为均值为零的不相关白噪声，

Φ_k＝e^AΔT≈I_11×11+AΔT (16)

式中，ΔT为离散时间间隔；I表示单位矩阵；

观测方程根据光学敏感器的输出给出,由于观测量仅为姿态误差四元数，与三轴旋转角速度和转动惯量比无关，即 得到观测矩阵H＝[I_3×3 0_3×3 0_3×5]。

4.根据权利要求3所述的方法，其特征在于：步骤S3中，乘性拓展卡尔曼滤波器的建立分为状态一步预测、滤波观测量计算、滤波更新和姿态校正四个部分：

1)状态一步预测

由非合作航天器的姿态动力学方程式(4)和非合作航天器的姿态运动学方程式(7)得到和计算姿态四元数的预估计值：

其中，为k时刻三轴旋转角加速度的估计值；为k时刻姿态四元数矢量部分的估计值；为k时刻姿态四元数矢量部分对时间导数的估计值；

2)滤波观测量计算

根据光学敏感器观测四元数和预测四元数运算计算姿态误差四元数

3)滤波更新

依照Kalman方程进行递推计算：

P_k|k-1＝ΦP_k-1Φ^T+Q_k-1 (20)

K_k＝P_k|k-1H^T(HP_k|k-1H^T+R_k)^-1 (21)

x_k＝x_k-1+K_k(δq_v-Hx_k-1) (23)

其中，P_k|k-1为k时刻最优预测估值误差协方差阵；P_k-1为k-1时刻最优滤波值误差协方差阵；K_k为k时刻增益矩阵；Q_k-1为k-1时刻系统的噪声方差矩阵；R_k为量测方差矩阵；

4)姿态校正

更新角速度：δω为状态变量x_k中的误差三轴旋转角速度；并保持姿态误差四元数归一性：

循环步骤1)-4)，输出目标非合作航天器的相对状态四元数、三轴旋转角速度和转动惯量比矩阵；

乘性拓展卡尔曼滤波系统采用姿态误差四元数的矢量部分用于滤波更新，而完整四元数用于全局非奇异位姿递推，因而利用式(24)在每步滤波更新中修正姿态四元数进行姿态递推，

考虑到非合作航天器搭载光学成像敏感器的频率不能满足姿态估计的高精度和快速跟踪的要求，在离散时间间隔ΔT内划分五个小时间隔，进行五次预测，一次测量更新，即，以五段折线拟合航天器姿态非线性方程在ΔT内的数值解。