CN114840994B - 一种极大型空间柔性系统在轨自主构建与状态感知方法 - Google Patents

Abstract

本发明公开了一种极大型空间柔性系统在轨自主构建与状态感知方法，首先采用双金字塔绳系系统构建极大型空间网状天线，给出系统结构及构建策略；然后采用拉格朗日法进行系统的动力学建模，最后考虑系绳绳长约束给出非线性约束下的扩展卡尔曼滤波算法，实现最少传感器布局下极大型空间网状天线的状态精确感知。本发明突破了现有大型空间天线尺寸的极限，可形成百米级，甚至千米级的空间网状天线技术能力，具有创新性，成本较低，更适合工程实际应用。

Description

一种极大型空间柔性系统在轨自主构建与状态感知方法

技术领域

本发明属于天线技术领域，具体涉及一种空间柔性系统在轨自主构建与状态感知方法。

背景技术

星载天线被广泛应用于电子侦察、空间通信、导航制导及深空探测等领域，是卫星系统的“眼睛”和“耳朵”，起着决定性作用。为满足多功能、多波段、大容量、高功率的需求，星载天线正朝着大口径、高精度、轻质量、高收纳比的方向发展。然而，由于现有火箭整流罩尺寸与发射费用的限制，要求星载天线轻且收拢体积小，故大口径星载天线必须做成可展开模式，即发射时收拢于火箭整流罩内，入轨后自动展开到位。因其广阔的军事及民用前景，使得空间可展开天线结构一直是空间结构研究的重点。

大型空间可展开天线是20世纪60年代随航天科技发展需求而诞生的一类新型空间展开结构系统，它以高强度比、高刚度比、高几何稳定性、超低热胀系绳等特点的宇航材料为主体，并包含低副可动机构、驱动元件和主动或被动控制器等。此类天线在地面发射时为收拢状态，固定于卫星上；当卫星发射并进入轨道以后，由地面控制中心发射指令，按照其空间轨道上的设计要求，逐步完成展开动作，最终通过锁定并保持为工作状态。需要指出的是，大型是指反射器口径大于等于4米，通常将20米以上的大型空间可展开天线称为20米以上的大型空间可展开天线称为超大型可展开天线，将50米以上称为极大型可展开天线。

大型可展开天线分为反射面天线、阵列天线和微电子机械天线。反射面天线是各种应用卫星上使用最多的一类天线形式，可作为超高频、微波乃至毫米波波段的通信卫星天线，形成赋形区域波束、点波束、多波束及扫描波束。根据反射面的结构形式可分为：刚性反射面天线、充气反射面天线、网状反射面天线及薄膜反射面天线。目前，在轨运行的星载大型可展开天线主要为网状可展开天线。网状可展开天线主要由前后索网、调整索、支撑框架以及金属丝网组成。其中，前后索网通过调整索相连构成索网系统，金属丝网铺设在前索网上构成电磁反射面系统。支撑结构系统是一个可展开结构，由若干平行四边形单元组成，对角杆采用可伸缩的套杆机构组成，天线展开时，电机驱动穿过平行四边形的对角杆的绳索使环形桁架展开到位。前索网、后索网以及竖向拉索在结构内力的作用下达到平衡位置，形成天线所需的抛物面。这类天线为柔性受力系统，因而索网的张力点、张力水平对保形能力、形面精度、展开的平稳性与可靠性均具有显著影响。现有网状反射面质量轻、易折叠、收纳率高、易于实现大口径、便于组合各种可展开的支撑结构，但存在结构较复杂，形面精度、可靠性及重复精度较低等局限性。根据当前的技术发展，这类天线的口径在理论上最大可做到50m,难以满足未来空间探索对星载天线大口径的需求。

发明内容

为了克服现有技术的不足，本发明提供了一种极大型空间柔性系统在轨自主构建与状态感知方法，首先采用双金字塔绳系系统构建极大型空间网状天线，给出系统结构及构建策略；然后采用拉格朗日法进行系统的动力学建模，最后考虑系绳绳长约束给出非线性约束下的扩展卡尔曼滤波算法，实现最少传感器布局下极大型空间网状天线的状态精确感知。本发明突破了现有大型空间天线尺寸的极限，可形成百米级，甚至千米级的空间网状天线技术能力，具有创新性，成本较低，更适合工程实际应用。

本发明解决其技术问题所采用的技术方案包括如下步骤：

步骤1：构建极大型空间网状天线在轨自主构建策略；

极大型空间网状天线系统由五颗卫星构成双金字塔绳系卫星编队系统；其中，三颗副星构成三角形基础面子系统用于支撑网状天线金属网面，顶端锚星和底端锚星位于三角形基础面两侧，三颗副星两两之间、顶端锚星与三颗副星之间、底端锚星与三颗副星之间都分别通过系绳连接，共9根系绳；顶端锚星为系统的馈源星，编号为1；底端锚星为系统的主星，编号为5；三颗副星的编号分别为2、3、4；

具体的网状天线系统在轨自主构建策略步骤如下：

步骤1-1：将极大型空间网状天线系统一次性总装发射入轨；

步骤1-2：网状天线系统自主调整系统姿态，稳定至地球重力梯度方向，系统的主星依次主动弹射出馈源星与中间三角形基础面子系统，在系绳拉伸和地球重力梯度力作用下，馈源星与中间三角形基础面子系统运动至期望位置；

步骤1-3：中间基础面子系统自主绕网状天线系统主轴以指定角速度起旋，利用自旋离心力效应展开中间基础面从而展开天线金属网面；

步骤1-4：网状天线系统主动调整姿态，利用主星和馈源星主动机动，实现天线对目标指向与变轨；

步骤2：基于拉格朗日法进行网状天线系统的动力学建模；

步骤2-1：进行如下假设；

1)相对于系绳长度，卫星尺寸忽略不计，将卫星视为质点；

2)系绳质量相对于卫星质量忽略不计，系绳只受拉力不受压力，只考虑其中的弹性势能；

3)天线金属网面的质量相对于卫星质量忽略不计；

4)网状天线系统作用在开普勒圆轨道上，旋转角速度为ω₀；除控制力外，网状天线系统只受到地球的万有引力，忽略太阳光压、电磁力外力；

步骤2-2：定义地心惯性坐标系o-xyz，其原点o为地心，ox轴位于赤道面内并指向春分点方向，oz轴指向地球北极，oy轴由右手螺旋法则确定；网状天线系统中的物理量定义如下：

1)顶端锚星的质量为m₁，底端锚星的质量为m₅，三颗副星的质量分别为m₂、m₃、m₄；

2)系绳长度分别为l_nom,12、l_nom,13、l_nom,14、l_nom,23、l_nom,24、l_nom,34、l_nom,25、l_nom,35、l_nom,45；其中下标的数字为卫星的编号；

步骤2-3：网状天线系统的动能包括五颗卫星质点的动能，表示为：

式中：r_i＝[x_i y_i z_i]^T为第i个卫星在地心惯性坐标系中的位置矢量；

网状天线系统的势能包括五个卫星所具有的重力势能和系绳中的弹性势能，其中卫星的重力势能表示为：

其中，μ为引力常数，对于地球μ＝3.986012×10⁵km³/s²；

由于系绳具有受拉力不受压力的特性，根据胡克定律得到网状天线系统中系绳的弹性势能表示为：

其中，E为系绳弹性模量；A为系绳的横截面面积；系绳变形长度分别为l₁₂，l₁₃，l₁₄，l₂₃，l₂₄，l₃₄，l₂₅，l₃₅，l₄₅，系绳无形变长度分别为l_norm,12，l_norm,13，l_norm,14，l_norm,23，l_norm,24，l_norm,34，l_norm,25，l_norm,35，l_norm,45，当变形后系绳长度大于系绳原长时系数e_i等于1，否则等于0；系数e_i代表式(3)中的e₁₂，e₁₃，e₁₄，e₂₃，e₂₄，e₃₄，e₂₅，e₃₅，e₄₅；

其中各系绳变形后的长度具体表示为：

步骤2-4：将系统的动能T与势能V＝V_G+V_E带入拉格朗日方程后得到：

其中，Q_x1,Q_y1,Q_z1,Q_x2,Q_y2,Q_z2,Q_x3,Q_y3,Q_z3,Q_x4,Q_y4,Q_z4,Q_x5,Q_y5,Q_z5为相应的广义力；

将上式整理并化简得到：

其中：q＝[r₁ r₂ r₃ r₄ r₅]^T为系统的广义坐标；Q_q＝[Q_r1 Q_r2 Q_r3 Q_r4 Q_r5]^T表示广义坐标对应的广义力；M表示质量矩阵，K(q)表示参数矩阵；

为把式(5)转化为一阶表达式，令X₁＝q，则令X＝[X₁X₂]^T则:

令并加入过程噪声W，则得：

步骤3：完成系统最小传感器布局，进行可观性证明；

步骤3-1：在顶端锚星与底端锚星安装GPS传感器，三颗副星上未安装GPS传感器，因此，系统的观测方程为：

式中：V₁为观测噪声，是均值为0，方差为R₁的高斯白噪声；

由于系绳的绳长先验已知，将9根系绳绳长整理为：

L^m＝L(X)+V₂ (9)

式中：L^m为包含系绳绳长的列向量；V₂为为均值为0，方差为R₂的高斯白噪声；L(X)为系绳绳长约束矩阵且L(X)＝[l₁₂ l₁₃ l₁₄ l₂₃ l₂₄ l₃₄ l₂₅ l₃₅ l₄₅]^T；

基于伪测量法将系统的绳长加入系统的状态方程可得：

Z＝h(X)+V (10)

式中：h(X)＝[X^m L^m]^T，V＝[V₁ V₂]^T为系统观测噪声；

得到系统的状态方程和观测方程如下：

步骤3-2：基于Lie导数可观性秩判据分析系统的对系统的可观性进行证明；

考虑如下无限平滑的非线性系统

式中是状态矢量，是控制输入矢量， 是测量矢量，其分量为y_k＝h_k(x),k＝1,…,m；l表示控制输入数量，m表示测量量的个数；

如果过程函数f是线性输入，则可以写成一系列独立函数之和，其中每一个独立函数对应一个控制输入矢量分量；则式(12)写成：

式中，f₀(.)是零控制输入分量对应的过程函数；f₁(.)...f_l(.)分别表示对应于输入u_l的系统状态方程函数；

则系统的可观性矩阵由李导数求导的行向量组成为：

根据以上系统的可观性矩阵的定义，求得系统的可观性矩阵为：

其中，A_3×3＝Θ(r_t,x-r_1,x,rt_t,11,zt,z1,y_t,y)，B_3×3＝Θ(r_t,x-r_2,x,rt_t,22,zt,z2,y_t,y)，以及Θ函数由式(15)确定：

式中，分别表示Θ函数的中间变量；

步骤4：设计非线性约束卡尔曼滤波算法实现系统状态的高精度感知；

步骤4-1：将系统的状态空间模型线性化离散后得到：

步骤4-2：采用扩展卡尔曼滤波进行初步状态估计；

状态预测：X_k+1＝X_k+ΔT₁f(X_k,Q_q)

协方差预测：

增益计算：

状态更新：

协方差更新：

其中：X_k、X_k+1分别表示k时刻与k+1的状态，ΔT表示采样时间，Q_q表示系统的广义力，P_k(-)表示预测的协方差，Φ_k表示k时刻的系统状态方程雅克比矩阵，P_k(+)表示k时刻的协方差，Γ_k-1表示系统噪声协方差，表示滤波增益，H_k表示测量方程雅克比矩阵，Υ_k表示测量噪声协方差，表示表示k时刻的更新状态，表示k时刻的预测状态，Z_k表示k时刻的测量，h_k表示k时刻的测量方程，表示k-1时刻的更新状态；

步骤4-3：考虑系绳约束，将系绳约束方程线性化处理，考虑系绳绳长，将式(4)表示为矩阵形式为：

g(X)＝l (17)将式(17)进行线性化处理后得到：

DX＝d (18)其中，d＝l-g(X)+g′(X)^TX，g′(X)为一阶导数；

步骤4-4：将无约束的状态估计投影到约束空间；

采用估计投影法将无约束扩展卡尔曼滤波算法得到的估计结果简化称为投影到约束空间S＝{X|DX＝d}，得到约束状态估计结果约束估计为：

其中W是一个正定的加权矩阵；

基于拉格朗日乘子法得：

即得到了非线性约束下系统状态的高精度估计。

本发明的有益效果如下：

(1)本发明采用空间绳系编队技术构建极大型空间网状天线，突破了现有大型空间天线尺寸的极限，可形成极大型空间网状天线技术能力；

(2)本发明利用编队自旋离心力效应展开天线金属网面，利用系绳的物理约束实现天线金属网面的基本保型，基于系绳的主动收放实现天线网面在轨型面高精度保持，具有创新性，成本较低，更适合工程实际应用。

附图说明

图1为本发明空间网状天线系统示意图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

本发明采用空间双金字塔绳系系统，利用系统中间基础面支撑极大型网状天线金属网面，基于系绳的主动收放实现天线网面在轨型面高精度保持，仅使用两个GPS传感器实现了系统状态的高精度感知。

一种极大型空间柔性系统在轨自主构建与状态感知方法，包括如下步骤：

步骤1：构建极大级空间网状天线在轨自主构建策略；

极大公里级空间网状天线系统由五颗卫星构成双金字塔绳系卫星编队系统；其中，三颗副星构成三角形基础面子系统用于支撑网状天线金属网面，顶端锚星和底端锚星位于三角形基础面两侧，三颗副星两两之间、顶端锚星与三颗副星之间、底端锚星与三颗副星之间都分别通过系绳连接，共9根系绳；顶端锚星为系统的馈源星，编号为1；底端锚星为系统的主星，编号为5；三颗副星的编号分别为2、3、4；利用编队绕系统主轴自旋离心力效应展开双金字塔绳系系统的中间基础面从而展开天线金属网面。利用系绳的物理约束实现天线金属网面的基本保型。克服热交变冲击等空间环境效应导致的空间天线结构变形，基于系绳的主动收放实现天线网面在轨型面高精度保持。

具体的网状天线系统在轨自主构建策略步骤如下：

步骤1-1：将极大级空间网状天线系统一次性总装发射入轨；

步骤1-2：网状天线系统自主调整系统姿态，稳定至地球重力梯度方向，系统的主星依次主动弹射出馈源星与中间三角形基础面子系统(三颗卫星通过系绳相连为封闭的三角形平面用以支撑极大型网状天线金属网面)，，在系绳拉伸和地球重力梯度力作用下，馈源星与中间三角形基础面子系统运动至期望位置；

步骤1-3：中间基础面子系统自主绕网状天线系统主轴以指定角速度起旋，利用自旋离心力效应展开中间基础面从而展开天线金属网面；

步骤1-4：网状天线系统主动调整姿态，利用主星和馈源星主动机动，实现天线对目标指向与变轨；

步骤2：基于拉格朗日法进行网状天线系统的动力学建模；

步骤2-1：本发明所提出的极大型空间网状天线系统由九根系绳和五颗卫星相联而成的空间多体封闭系统。建立该系统的动力学模型时，为了简化并保证模型的准确程度，进行如下假设；

5)相对于系绳长度，卫星尺寸忽略不计，将卫星视为质点；

6)系绳质量相对于卫星质量忽略不计，系绳只受拉力不受压力，只考虑其中的弹性势能；

7)天线金属网面的质量相对于卫星质量忽略不计；

8)网状天线系统作用在开普勒圆轨道上，旋转角速度为ω₀；除控制力外，网状天线系统只受到地球的万有引力，忽略太阳光压、电磁力外力；

步骤2-2：定义地心惯性坐标系o-xyz，其原点o为地心，ox轴位于赤道面内并指向春分点方向，oz轴指向地球北极，oy轴由右手螺旋法则确定；网状天线系统中的物理量定义如下：

3)顶端锚星的质量为m₁，底端锚星的质量为m₅，三颗副星的质量分别为m₂、m₃、m₄；

4)系绳长度分别为l_nom,12、l_nom,13、l_nom,14、l_nom,23、l_nom,24、l_nom,34、l_nom,25、l_nom,35、l_nom,45；其中下标的数字为卫星的编号；

步骤2-3：网状天线系统的动能包括五颗卫星质点的动能，表示为：

式中：r_i＝[x_i y_i z_i]^T为第i个卫星在地心惯性坐标系中的位置矢量；

网状天线系统的势能包括五个卫星所具有的重力势能和系绳中的弹性势能，其中卫星的重力势能表示为：

其中，μ为引力常数，对于地球μ＝3.986012×10⁵km³/s²；

由于系绳具有受拉力不受压力的特性，根据胡克定律得到网状天线系统中系绳的弹性势能表示为：

其中，E为系绳弹性模量；A为系绳的横截面面积；系绳变形长度分别为l₁₂，l₁₃，l₁₄，l₂₃，l₂₄，l₃₄，l₂₅，l₃₅，l₄₅，当变形后系绳长度大于系绳原长时系数e_i等于1，否则等于0；系数e_i代表式(3)中的e₁₂，e₁₃，e₁₄，e₂₃，e₂₄，e₃₄，e₂₅，e₃₅，e₄₅；

其中各系绳变形后的长度具体表示为：

步骤2-4：将系统的动能T与势能V＝V_G+V_E带入拉格朗日方程后得到：

将上式整理并化简得到：

其中：q＝[r₁ r₂ r₃ r₄ r₅]^T为系统的广义坐标；表示广义坐标对应的广义力；

为把式(5)转化为一阶表达式，令X₁＝q，则令X＝[X₁X₂]^T则:

令并加入过程噪声W，则得：

步骤3：完成系统最小传感器布局，进行可观性证明；

步骤3-1：在顶端锚星与底端锚星安装GPS传感器，三颗副星上未安装GPS传感器，因此，系统的观测方程为：

式中：V₁为观测噪声，是均值为0，方差为R₁的高斯白噪声；

由于系绳的绳长先验已知，将9根系绳绳长整理为：

L^m＝L(X)+V₂ (9)

式中：L^m为包含系绳绳长的列向量；V₂为为均值为0，方差为R₂的高斯白噪声；

基于伪测量法将系统的绳长加入系统的状态方程可得：

Z＝h(X)+V (10)

式中：h(X)＝[X^m L^m]^T，V＝[V₁ V₂]^T为系统观测噪声；

得到系统的状态方程和观测方程如下：

步骤3-2：基于Lie导数可观性秩判据分析系统的对系统的可观性进行证明；

考虑如下无限平滑的非线性系统

式中是状态矢量，是控制输入矢量， 是测量矢量，其分量为y_k＝h_k(x),k＝1,…,m；

如果过程函数f是线性输入，则可以写成一系列独立函数之和，其中每一个独立函数对应一个控制输入矢量分量；则式(12)写成：

式中，f₀(.)是零控制输入分量对应的过程函数；

则系统的可观性矩阵由李导数求导的行向量组成为：

如果上式定义的非线性系统可观性矩阵O是满秩的，则系统在本发明提出的传感器布局下是局部弱可观。即中间基础面的三颗卫星在未安装GPS传感器的条件下卫星的状态依然可以被精确感知。

根据以上系统的可观性矩阵的定义，求得系统的可观性矩阵为：

其中，A_3×3＝Θ(r_t,x-r_1,x,rt_t,11,zt,z1,y_t,y)，B_3×3＝Θ(r_t,x-r_2,x,rt_t,22,zt,z2,y_t,y)，以及Θ函数由式(15)确定：

经计算，可观性矩阵Ξ是满秩的。因此，当仅有两个GPS传感器时，中间基础面三颗星的状态可以精确感知。

步骤4：设计非线性约束卡尔曼滤波算法实现系统状态的高精度感知；

步骤4-1：将系统的状态空间模型线性化离散后得到：

步骤4-2：采用扩展卡尔曼滤波进行初步状态估计；

状态预测：X_k+1＝X_k+ΔT₁f(X_k,Q_q)

协方差预测：

增益计算：

状态更新：

协方差更新：

步骤4-3：考虑系绳约束，将系绳约束方程线性化处理，考虑系绳绳长，将式(4)表示为矩阵形式为：

g(X)＝l (17)将式(17)进行线性化处理后得到：

DX＝d (18)其中，d＝l-g(X)+g′(X)^TX，g′(X)为一阶导数；

步骤4-4：将无约束的状态估计投影到约束空间；

采用估计投影法将无约束扩展卡尔曼滤波算法得到的估计结果简化称为投影到约束空间S＝{X|DX＝d}，得到约束状态估计结果约束估计为：

其中W是一个正定的加权矩阵；

基于拉格朗日乘子法得：

即得到了非线性约束下系统状态的高精度估计。

Claims (1)

1.一种极大型空间柔性系统在轨自主构建与状态感知方法，其特征在于，包括如下步骤：

步骤1：构建极大型空间网状天线在轨自主构建策略；

极大型空间网状天线系统由五颗卫星构成双金字塔绳系卫星编队系统；其中，三颗副星构成三角形基础面子系统用于支撑网状天线金属网面，顶端锚星和底端锚星位于三角形基础面两侧，三颗副星两两之间、顶端锚星与三颗副星之间、底端锚星与三颗副星之间都分别通过系绳连接，共9根系绳；顶端锚星为系统的馈源星，编号为1；底端锚星为系统的主星，编号为5；三颗副星的编号分别为2、3、4；

具体的网状天线系统在轨自主构建策略步骤如下：

步骤1-1：将极大型空间网状天线系统一次性总装发射入轨；

步骤1-2：网状天线系统自主调整系统姿态，稳定至地球重力梯度方向，系统的主星依次主动弹射出馈源星与中间三角形基础面子系统，在系绳拉伸和地球重力梯度力作用下，馈源星与中间三角形基础面子系统运动至期望位置；

步骤1-3：中间基础面子系统自主绕网状天线系统主轴以指定角速度起旋，利用自旋离心力效应展开中间基础面从而展开天线金属网面；

步骤1-4：网状天线系统主动调整姿态，利用主星和馈源星主动机动，实现天线对目标指向与变轨；

步骤2：基于拉格朗日法进行网状天线系统的动力学建模；

步骤2-1：进行如下假设；

1)相对于系绳长度，卫星尺寸忽略不计，将卫星视为质点；

2)系绳质量相对于卫星质量忽略不计，系绳只受拉力不受压力，只考虑其中的弹性势能；

3)天线金属网面的质量相对于卫星质量忽略不计；

4)网状天线系统作用在开普勒圆轨道上，旋转角速度为ω₀；除控制力外，网状天线系统只受到地球的万有引力，忽略太阳光压、电磁力外力；

步骤2-2：定义地心惯性坐标系o-xyz，其原点o为地心，ox轴位于赤道面内并指向春分点方向，oz轴指向地球北极，oy轴由右手螺旋法则确定；网状天线系统中的物理量定义如下：

1)顶端锚星的质量为m₁，底端锚星的质量为m₅，三颗副星的质量分别为m₂、m₃、m₄；

2)系绳长度分别为l_nom，12、l_nom，13、l_nom，14、l_nom，23、l_nom，24、l_nom，34、l_nom，25、l_nom，35、l_nom，45；其中下标的数字为卫星的编号；

步骤2-3：网状天线系统的动能包括五颗卫星质点的动能，表示为：

式中：r_i＝[x_i y_i z_i]^T为第i个卫星在地心惯性坐标系中的位置矢量；

网状天线系统的势能包括五个卫星所具有的重力势能和系绳中的弹性势能，其中卫星的重力势能表示为：

其中，μ为引力常数，对于地球μ＝3.986012×10⁵km³/s²；

由于系绳具有受拉力不受压力的特性，根据胡克定律得到网状天线系统中系绳的弹性势能表示为：

其中，E为系绳弹性模量；A为系绳的横截面面积；系绳变形长度分别为l₁₂，l₁₃，l₁₄，l₂₃，l₂₄，l₃₄，l₂₅，l₃₅，l₄₅，系绳无形变长度分别为l_norm，12，l_norm，13，l_norm，14，l_norm，23，l_norm，24，l_norm，34，l_norm，25，l_norm，35，l_norm，45，当变形后系绳长度大于系绳原长时系数e_i等于1，否则等于0；系数e_i代表式(3)中的e₁₂，e₁₃，e₁₄，e₂₃，e₂₄，e₃₄，e₂₅，e₃₅，e₄₅；

其中各系绳变形后的长度具体表示为：

步骤2-4：将系统的动能T与势能V＝V_G+V_E带入拉格朗日方程后得到：

其中，为相应的广义力；

将上式整理并化简得到：

其中：q＝[r₁ r₂ r₃ r₄ r₅]^T为系统的广义坐标；表示广义坐标对应的广义力；M表示质量矩阵，K(q)表示参数矩阵；

为把式(5)转化为一阶表达式，令X₁＝q，则令X＝[X₁ X₂]^T则：

令并加入过程噪声W，则得：

步骤3：完成系统最小传感器布局，进行可观性证明；

步骤3-1：在顶端锚星与底端锚星安装GPS传感器，三颗副星上未安装GPS传感器，因此，系统的观测方程为：

式中：V₁为观测噪声，是均值为0，方差为R₁的高斯白噪声；

由于系绳的绳长先验已知，将9根系绳绳长整理为：

L^m＝L(X)+V₂ (9)

式中：L^m为包含系绳绳长的列向量；V₂为为均值为0，方差为R₂的高斯白噪声；L(X)为系绳绳长约束矩阵且L(X)＝[l₁₂ l₁₃ l₁₄ l₂₃ l₂₄ l₃₄ l₂₅ l₃₅ l₄₅]^T；

基于伪测量法将系统的绳长加入系统的状态方程可得：

Z＝h(X)+V (10)

式中：h(X)＝[X^m L^m]^T，V＝[V₁ V₂]^T为系统观测噪声；

得到系统的状态方程和观测方程如下：

步骤3-2：基于Lie导数可观性秩判据分析系统的对系统的可观性进行证明；

考虑如下无限平滑的非线性系统

式中是状态矢量，是控制输入矢量， 是测量矢量，其分量为y_k＝h_k(x)，k＝1，…，m；l表示控制输入数量，m表示测量量的个数；

如果过程函数f是线性输入，则可以写成一系列独立函数之和，其中每一个独立函数对应一个控制输入矢量分量；则式(12)写成：

式中，f₀(.)是零控制输入分量对应的过程函数；f₁(.)...f_l(.)分别表示对应于输入u_l的系统状态方程函数；

则系统的可观性矩阵由李导数求导的行向量组成为：

根据以上系统的可观性矩阵的定义，求得系统的可观性矩阵为：

其中，A_3×3＝Θ(r_t，x-r_1，x，rt_t，11，zt，z1，y_t，y)，B_3×3＝Θ(r_t，x-r_2，x，rt_t，22，zt，z2，y_t，y)，以及Θ函数由式(15)确定：

式中，分别表示Θ函数的中间变量；

步骤4：设计非线性约束卡尔曼滤波算法实现系统状态的高精度感知；

步骤4-1：将系统的状态空间模型线性化离散后得到：

步骤4-2：采用扩展卡尔曼滤波进行初步状态估计；

状态预测：X_k+1＝X_k+ΔT₁f(X_k，Q_q)

协方差预测：

增益计算：

状态更新：

协方差更新：

其中：X_k、X_k+1分别表示k时刻与k+1的状态，ΔT表示采样时间，Q_q表示系统的广义力，P_k(-)表示预测的协方差，Φ_k表示k时刻的系统状态方程雅克比矩阵，P_k(+)表示k时刻的协方差，Γ_k-1表示系统噪声协方差，表示滤波增益，H_k表示测量方程雅克比矩阵，Υ_k表示测量噪声协方差，表示表示k时刻的更新状态，表示k时刻的预测状态，Z_k表示k时刻的测量，h_k表示k时刻的测量方程，表示k-1时刻的更新状态；

步骤4-3：考虑系绳约束，将系绳约束方程线性化处理，考虑系绳绳长，将式(4)表示为矩阵形式为：

g(X)＝l (17)

将式(17)进行线性化处理后得到：

DX＝d (18)

其中，d＝l-g(X)+g′(X)^TX，g′(X)为一阶导数；

步骤4-4：将无约束的状态估计投影到约束空间；

采用估计投影法将无约束扩展卡尔曼滤波算法得到的估计结果简化称为投影到约束空间S＝{X|DX＝d}，得到约束状态估计结果约束估计为：

其中W是一个正定的加权矩阵；

基于拉格朗日乘子法得：

即得到了非线性约束下系统状态的高精度估计。