1 引言（Introduction）

空间绳系系统（space tethered system，STS）是指借助柔性系绳将2个或2个以上的空间卫星/机器人连接在一起的刚柔耦合群体系统。作为一种新型航天器，STS在灵活性、生命周期和功能拓展性等方面优势显著^[1-2]，成为近年来国内外航天领域的研究热点。其在空间垃圾清除^[3]、空间观测^[4]以及太空电梯^[5]等方面具有广阔的应用前景。

STS动力学建模的难点在于柔性系绳的建模，常用的有杆模型^[6]、珠点模型^[7]和连续体模型^[8]等。研究者利用以上3种模型开展了大量研究。黄攀峰等^[9-12]提出了一种空间绳系机器人，其可看作由柔性系绳将抓捕器与空间平台相连组成的二体STS，可广泛应用于空间在轨维护、在轨加注和空间碎片清除等任务中，并对系统的动力学建模和稳定控制等方面展开了深入研究。Yousefian等^[13]针对二体STS的摆动抑制问题，采用扩展卡尔曼滤波算法估计系绳的实时摆角用于控制器的状态反馈。Chung^[14]针对NASA（美国国家航空航天局）的宇宙结构进化亚毫米深空探测器工程提出了三体STS，并设计了相对状态感知器用于实时反馈受控系统的相对位姿状态。Misra^[15]、Li^[16]、史格非^[17]、王振坤^[18]等针对空间天梯系统存在单个或多个攀爬器时系统的动力学特性和摆动控制进行了研究。Netzer等^[19]针对直链式多体STS设计了降阶观测器用于估计相邻系绳单元之间的曲率角。方国涛等^[20]针对双金字塔STS在状态保持阶段的状态估计问题，提出了一种最少传感器布局下的状态估计方法。

精确的运动状态测量与估计是空间系统导航、控制与应用的前提。目前有关STS的研究主要集中于系统的动力学分析和稳定控制方面，而关于STS状态估计的研究相对较少。柔性系绳连接的存在，造就了不同于传统空间编队系统的全新状态估计问题。一方面，柔性系绳连接能够提供系统运动状态之间有价值的耦合约束关系，如何充分利用系绳－卫星之间的耦合约束关系，增强系统状态感知效能，优化系统传感器布局以降低成本^[21]，是STS状态感知亟待解决的难题，而现有研究^{[13-14, 19]}均未考虑柔性系绳连接约束。另一方面，STS是一个多维、大尺度、刚柔耦合的能量耗散系统，系统的应用涉及系绳的伸展、收回等复杂过程，并且由于卫星之间存在柔性系绳约束，因此STS的轨道运动与系统姿态运动存在耦合关系^[22-23]，这为系统的高精度状态估计带来挑战。

针对含约束的系统状态估计，国内外学者已开展了大量的研究。目前处理含有等式约束信息的状态估计的方法主要有伪测量法^[24]、投影法^[25]以及降维法^[26]等。其中，伪测量法将系统的状态约束视为“精确”测量值，然后将该测量值与同时刻的量测方程放在一起进行扩维，得到一个新的观测方程，从而将约束系统转变为一个无约束动态方程，最后应用卡尔曼滤波进行状态估计。该方法因简单有效已被广泛应用于目标跟踪、经济模型等领域。但扩维后的新噪声协方差为奇异矩阵，在进行滤波时会引起数值发散问题^[27-28]。

本文针对一类直链式$ N $体STS的状态估计问题，将系绳绳长作为先验信息，基于伪测量法在仅使用2个GPS传感器的条件下实现了系统的状态估计。需要指出的是，本文研究的直链式$ N $体STS可涵盖一类采用珠点模型的直链式空间二体\!/\!多体STS或者含有多个攀爬器的太空天梯系统等。

2 系统模型（System model） 2.1 系统描述

本文研究的直链式$ N $（$ N\geqslant 3 $）体STS如图 1所示，系统由$ N $个卫星通过系绳相连，系统两端的卫星质量分别为$ m_{1} $和$ m_{N} $并安装有GPS传感器，其他卫星均未安装GPS传感器。为对$ N $体STS进行动力学建模，定义地心惯性坐标系$ O $-$ XYZ $，其原点$ O $固连于地心，$ OX $轴位于赤道面内并指向春分点方向，$ OZ $轴指向地球北极，$ OY $轴由右手螺旋法则确定。为简化研究，对系统做以下假设：

(1) 相对于系绳长度，卫星大小可忽略，因此所有卫星均假设为质点；

(2) 空间系绳假设为无质量刚性杆；

(3) 系统运行于圆形开普勒轨道，除了地球引力，其他外界干扰均忽略。

2.2 基于Udwadia-Kalaba方法的STS动力学建模

STS是多参数耦合的约束系统，一般的系统建模方法有牛顿法、拉格朗日法和凯恩法等。其中，牛顿法推导简单，但未知数和方程数目多，当刚体个数较多时，解耦过程困难；其他方法均需要在建模过程中引入中间变量，计算复杂。为此，本文基于Udwadia-Kalaba方法建立直链式$ N $体STS的通用动力学模型。该方法将系统的约束关系融入动力学方程，在不出现中间变量的条件下，得出系统在约束状态下的动力学方程，并给出系统约束力的解析表达式^[29-30]。

基于Udwadia-Kalaba方法的系统动力学建模一般遵循以下3个步骤：

步骤1：通过牛顿－欧拉法或拉格朗日法建立无约束多体系统方程

本文采用拉格朗日法对无约束直链式$ N $体STS进行动力学建模，其动能可表示为

$ \begin{align} E_{\rm k} =\sum\limits_{i=1}^N {\frac{1}{2}m_{i} | {\dot{{\mathit{\boldsymbol{r}}}}_{i}} |^{2}} \end{align} $

(1)

式中，$ m_{i} $为第$ i $颗卫星的质量，$ \mathit{\boldsymbol{r}}_{i} =[{r_{i, x}} \; \; {r_{i, y}} \; \; {r_{i, z}} ]^{\rm T}\in \mathbb{R}^{3} $是第$ i $颗卫星作为质点在绝对坐标系下的坐标，$ | \cdot | $表示求模计算。

同样，系统的势能可表示为

$ \begin{align} E_{\rm p} =-\sum _{i=1}^N {GM_{\rm E} \frac{m_{i}} {| {{\mathit{\boldsymbol{r}}}_{i}} |}} \end{align} $

(2)

式中，$ G $为地球引力参数，$ M_{\rm E} $为地球质量。

将式(1)(2) 代入第二类拉格朗日方程，可得到系统的无约束动力学方程为

$ \begin{align} m_{i} \mathit{\boldsymbol{\ddot{r}}}_{i} +\frac{\mu m_{i} \mathit{\boldsymbol{r}}_{i}} {| {\mathit{\boldsymbol{r}}_{i}} |^{3}}=\mathit{\boldsymbol{Q}}_{ri}, \quad i=1, 2, \cdots, N \end{align} $

(3)

式中，$ \mathit{\boldsymbol{Q}}_{ri} \in \mathbb{R}^{3} $为系统的广义力，由外界推力等非保守力组成。

由式(3) 可得到无约束力作用时的系统加速度：

$ \begin{align} {\mathit{\boldsymbol{a}}} \triangleq \mathit{\boldsymbol{\ddot{q}}} =\mathit{\boldsymbol{M}}^{-1}{\mathit{\boldsymbol{Q}}}^{\rm (u)}({{\mathit{\boldsymbol{q}}}, {\mathit{\boldsymbol{u}}}}) \end{align} $

(4)

式中，$ \mathit{\boldsymbol{M}}={\rm{diag}}({m_{1}, m_{1}, m_{1}, \cdots, m_{N}, m_{N}, m_{N}})\in \mathbb{R}^{3 N} $为系统质量矩阵，$ \mathit{\boldsymbol{q}}=[{\mathit{\boldsymbol{r}}_{1}^{\rm T}, \cdots, \mathit{\boldsymbol{r}}_{N}^{\rm T}}]\in \mathbb{R}^{3 N} $为坐标向量，$ {\mathit{\boldsymbol{Q}}}^{\rm (u)}({{\mathit{\boldsymbol{q}}}, {\mathit{\boldsymbol{u}}}}) $为无约束系统广义力。

步骤2：对无约束系统施加约束

对于直链式$ N $体STS，存在$ N-1 $个系绳绳长约束，可表示为向量形式：

$ \begin{align} \mathit{\boldsymbol{L}}=[{l_{1}, l_{2}, \cdots, l_{N-1}}]^{\rm T} \end{align} $

(5)

其中，$ l_{i} =| {\mathit{\boldsymbol{r}}_{i} -\mathit{\boldsymbol{r}}_{i+1}} | $，$ i=1, 2, \cdots, N-1 $。

将式(5) 对时间连续2次求导，可得到约束的2阶Pfaffian标准微分形式如下：

$ \begin{align} \mathit{\boldsymbol{A\ddot{q}}}=\mathit{\boldsymbol{b}} \end{align} $

(6)

式中，

$ \begin{align*} \mathit{\boldsymbol{A}} & =\begin{bmatrix} {\mathit{\boldsymbol{r}}_{1} -\mathit{\boldsymbol{r}}_{2}} & {\mathit{\boldsymbol{r}}_{2} -\mathit{\boldsymbol{r}}_{1}} & \cdots & {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{0}}_{1\times 3}} \\ \vdots & \vdots & \ddots & \vdots & \vdots \\ {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{0}}_{1\times 3}} & \cdots & {\mathit{\boldsymbol{r}}_{N-1} -\mathit{\boldsymbol{r}}_{N}} & {\mathit{\boldsymbol{r}}_{N} -\mathit{\boldsymbol{r}}_{N-1}} \end{bmatrix} \\ \mathit{\boldsymbol{b}} & =\begin{bmatrix} {\dot{l}_{t1}^{2} +l_{t1} \ddot{l}_{t1} -| {\mathit{\boldsymbol{\dot{r}}}_{1} -\mathit{\boldsymbol{\dot{r}}}_{2}} |^{2}} \\ \vdots \\ {\dot{l}_{t(N-1)}^{2} +l_{t(N-1)} \ddot{l}_{t(N-1)} -| {\mathit{\boldsymbol{\dot{r}}}_{N-1} -\mathit{\boldsymbol{\dot{r}}}_{N}} |^{2}} \end{bmatrix} \end{align*} $

其中，$ l_{t1}, \cdots, l_{t(N-1)} $表示系绳的实时长度，$ \dot{l}_{t1}, $ $ \cdots, $ $ \dot{l}_{t(N-1)} $表示系绳绳长的收放速度，$ \ddot{l}_{t1}, \cdots, \ddot{l}_{t(N-1)} $表示系绳绳长收放加速度。注意，系绳的实时长度、速度及加速度可由系统的系绳收放装置提供。

步骤3：约束力求解

在已知广义力$ {\mathit{\boldsymbol{Q}}}^{\rm (u)}({{\mathit{\boldsymbol{q}}}, {\mathit{\boldsymbol{u}}}}) $与约束力$ {\mathit{\boldsymbol{Q}}}^{\rm (c)}({{\mathit{\boldsymbol{q}}}}) $的共同作用下，系统动力学方程为

$ \begin{align} \mathit{\boldsymbol{M}}\mathit{\boldsymbol{\ddot{q}}}={\mathit{\boldsymbol{Q}}}^{\rm (u)}( {{\mathit{\boldsymbol{q}}}, {\mathit{\boldsymbol{u}}}})+{\mathit{\boldsymbol{Q}}}^{\rm (c)}( {{\mathit{\boldsymbol{q}}}}) \end{align} $

(7)

其中，系统约束力$ {\mathit{\boldsymbol{Q}}}^{\rm (c)}({{\mathit{\boldsymbol{q}}}})={\mathit{\boldsymbol{A}}}^{\rm T}({{\mathit{\boldsymbol{A}}}{\mathit{\boldsymbol{M}}}^{-1}{\mathit{\boldsymbol{A}}}^{\rm T}})^{+}({\mathit{\boldsymbol{b}}}-{\mathit{\boldsymbol{Aa}}}) $，“$ + $”号表示矩阵的广义逆。注意，系统约束力$ {\mathit{\boldsymbol{Q}}}^{\rm (c)}({{\mathit{\boldsymbol{q}}}}) $计算公式由文[29]得到，详细推导可参考文[31]。

2.3 GPS传感器模型

系统的两端卫星的绝对位置$ \mathit{\boldsymbol{r}}_{i}^{\rm (g)} $可由GPS（全球定位系统）传感器直接确定，可表示为

$ \begin{align} \mathit{\boldsymbol{r}}_{i}^{\rm (g)} =\mathit{\boldsymbol{r}}_{i} +\mathit{\boldsymbol{v}}^{\rm (g)} \end{align} $

(8)

式中，$ \mathit{\boldsymbol{r}}_{i} =[ r_{i, x} \; \; {r_{i, y}} \; \; {r_{i, z}} ]^{\rm T}\in \mathbb{R}^{3} $表示卫星的实际绝对位置，$ {\mathit{\boldsymbol{v}}}^{\rm (g)}\in \mathbb{R}^{3} $表示GPS传感器测量噪声，本文假设为高斯白噪声。

2.4 系绳绳长伪测量模型

系绳的绳长作为有价值的先验信息，需要加以考虑。本文中将系绳的绳长约束表示为

$ \begin{align} \mathit{\boldsymbol{z}}^{(l)}=\mathit{\boldsymbol{L}}({\mathit{\boldsymbol{q}}})+\mathit{\boldsymbol{v}}^{(l)} \end{align} $

(9)

式中，$ \mathit{\boldsymbol{v}}^{(l)}\in \mathbb{R}^{N-1} $表示虚拟噪声，本文假设为1阶马尔可夫过程，并表示为

$ \begin{align} \mathit{\boldsymbol{\dot{v}}}^{(l)}=-\mathit{\boldsymbol{Q}}^{(l)}\mathit{\boldsymbol{b}}^{(l)}+\mathit{\boldsymbol{u}}^{(l)} \end{align} $

(10)

其中，$ \mathit{\boldsymbol{u}}^{(l)} $为高斯白噪声，$ \mathit{\boldsymbol{Q}}^{(l)}={\rm{diag}} \bigg(\dfrac{1}{\tau^{(l)}}, $ $ \cdots, $ $ \dfrac{1}{\tau^{(l)}}\bigg) $，$ \tau^{(l)} $为时间常数。

需要注意的是，实际情况中系绳形变很小，相对于系绳绳长可忽略。诚然系绳形变很小，但是确实存在，此处将考虑系绳小形变并将其建模为虚拟噪声$ \mathit{\boldsymbol{v}}^{(l)} $，这不仅符合实际情况，而且虚拟噪声的引入可解决基于伪测量法进行状态估计时数值发散的问题。

3 状态估计方案设计（Design of the state estimation scheme）

本文提出的状态估计框架如图 2所示。首先基于Udwadia-Kalaba方法建立系统的约束动力学模型。然后进行GPS传感器布局，基于伪测量法将系绳绳长视为“精确”的观测值并将该“精确”值与原观测方程组合形成新的观测方程，设计状态估计算法，进行可观测性分析。最后实现系统状态实时精确输出。

3.1 系统的状态方程

令$ \mathit{\boldsymbol{X}}=[{{\mathit{\boldsymbol{q}}}^{\rm T}} \; \; {\mathit{\boldsymbol{\dot{q}}}^{\rm T}} ]^{\rm T} $为状态变量，式(7) 可变形为

$ \begin{align} \mathit{\boldsymbol{\dot{X}}} & =\frac{{\rm d}\mathit{\boldsymbol{X}}}{{\rm d}t} \\ & =\begin{bmatrix} {\mathit{\boldsymbol{\dot{q}}}} \\ {\mathit{\boldsymbol{M}}^{-1}[{\mathit{\boldsymbol{Q}}^{\rm (u)}( {\mathit{\boldsymbol{q}}, \mathit{\boldsymbol{u}}})+\mathit{\boldsymbol{A}}^{\rm T}( {\mathit{\boldsymbol{AM}}^{-1}\mathit{\boldsymbol{A}}^{\rm T}})^{+}({\mathit{\boldsymbol{b}}-\mathit{\boldsymbol{Aa}}})}]} \end{bmatrix} \end{align} $

(11)

将上式写成函数形式为

$ \begin{align} \mathit{\boldsymbol{\dot{X}}}=\mathit{\boldsymbol{f}}({\mathit{\boldsymbol{X}}})+\mathit{\boldsymbol{\varGamma}} \end{align} $

(12)

式中，系统噪声$ \mathit{\boldsymbol{\varGamma}} \sim N({0, \mathit{\boldsymbol{Q}}^{(\Gamma)}}) $服从高斯分布，协方差矩阵$ \mathit{\boldsymbol{Q}}^{(\Gamma)}\in \mathbb{R}^{(6 N)\times (6 N)} $。

考虑系统的GPS传感器布局，则测量方程为

$ \begin{align} \mathit{\boldsymbol{z}}^{\rm (g)}({\mathit{\boldsymbol{X}}})=\mathit{\boldsymbol{H}}_{g} \mathit{\boldsymbol{X}}^{\rm (g)}+\mathit{\boldsymbol{V}}^{\rm (g)} \end{align} $

(13)

式中，测量噪声$ \mathit{\boldsymbol{V}}^{\rm (g)}\in \mathbb{R}^{6} $服从高斯分布，$ \mathit{\boldsymbol{H}}_{g} \in \mathbb{R}^{(6N)\times (6N)} $表示为

$ \begin{align} \mathit{\boldsymbol{H}}_{g} = \begin{bmatrix} {\mathit{\boldsymbol{I}}_{3}} & \cdots & {\bm0_{3\times 3}} & {\bm0_{3\times (3N)}} \\ \vdots & \ddots & \vdots & \vdots \\ {\bm0_{3\times 3}} & \cdots & {\mathit{\boldsymbol{I}}_{3}} & {\bm0_{3\times (3N)}} \end{bmatrix} \end{align} $

(14)

式中，$ \mathit{\boldsymbol{0}}_{3\times 3} \in \mathbb{R}^{3\times 3} $为$ 3\times 3 $零矩阵，$ \mathit{\boldsymbol{I}}_{3} \in \mathbb{R}^{3\times 3} $为$ 3\times 3 $单位矩阵。

基于伪测量法，将系绳绳长约束加入测量方程(13) 可得到

$ \begin{align} \mathit{\boldsymbol{Z}}_{k} = \underbrace {\begin{bmatrix} {\mathit{\boldsymbol{z}}^{\rm (g)}} \\ {\mathit{\boldsymbol{z}}^{(l)}} \end{bmatrix}}_{\mathit{\boldsymbol{h}}({{{\mathit{\boldsymbol{X}}}}_{k}})}+ \underbrace{\begin{bmatrix} {\mathit{\boldsymbol{V}}^{\rm (g)}} \\ {\mathit{\boldsymbol{v}}^{(l)}} \end{bmatrix}}_{\bm\varUpsilon_{k}} \end{align} $

(15)

式中，$ \mathit{\boldsymbol{\varUpsilon}}_{k} \sim N({0, \mathit{\boldsymbol{R}}_{k}^{(\Upsilon)}}) $表示测量方程扩展后的测量噪声，$ \mathit{\boldsymbol{R}}_{k}^{(\Upsilon)} $表示扩展后的测量噪声协方差。

将式(12)(15) 联立可得系统的状态方程为

$ \begin{align} \begin{cases} {\mathit{\boldsymbol{\dot{X}}}={\mathit{\boldsymbol{f}}}({\mathit{\boldsymbol{X}}})+\mathit{\boldsymbol{\varGamma}}} \\ {\mathit{\boldsymbol{Z}}_{k} =\mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}_{k}})+\mathit{\boldsymbol{\varUpsilon}}_{k}} \end{cases} \end{align} $

(16)

3.2 改进平方根无迹卡尔曼滤波

为了实施滤波，首先需要对系统状态方程进行离散化处理：

$ \begin{align} \begin{cases} \mathit{\boldsymbol{X}}_{k} =\mathit{\boldsymbol{X}}_{k-1} +\Delta T{\mathit{\boldsymbol{f}}} ({\mathit{\boldsymbol{X}}_{k-1}})+\mathit{\boldsymbol{\varGamma}}_{k-1} \\ \mathit{\boldsymbol{Z}}_{k} =\mathit{\boldsymbol{h}} ({\mathit{\boldsymbol{X}}_{k}})+\mathit{\boldsymbol{\varUpsilon}}_{k} \end{cases} \end{align} $

(17)

式中，$ \Delta T $为系统采样时间。

扩展卡尔曼滤波状态估计过程中雅可比矩阵计算复杂，而无迹卡尔曼滤波（UKF）存在因系统状态协方差矩阵无法实现Cholesky分解而导致滤波发散的问题^[32-33]。为此，本文开发了一种改进平方根无迹卡尔曼滤波（improved square root unscented Kalman filter，IUKF）算法。该算法包含2部分：EKF（扩展卡尔曼滤波）多步状态预测部分和SRUKF（平方根无迹卡尔曼滤波）状态更新部分。IUKF算法计算步骤如下：

步骤1：Sigma点计算

$ \begin{align} \begin{cases} \mathit{\boldsymbol{\varTheta}}_{k-1}^{(i)} =\hat{\mathit{\boldsymbol{X}}}_{k-1}, & i=0 \\ \mathit{\boldsymbol{\varTheta}}_{k-1}^{(i)} =\hat{\mathit{\boldsymbol{X}}}_{k-1} +({\eta \sqrt{\hat{\mathit{\boldsymbol{P}}}_{k-1}}})_{i}, & i=1, \cdots, n \\ \mathit{\boldsymbol{\varTheta}}_{k-1}^{(i)} =\hat{\mathit{\boldsymbol{X}}}_{k-1} -({\eta \sqrt{\hat{\mathit{\boldsymbol{P}}}_{k-1}}})_{i}, & i=n+1, \cdots, 2n \end{cases} \end{align} $

(18)

相应的权值为

$ \begin{align} \begin{cases} W_{\rm m}^{(i)} =\dfrac{\lambda} {n+\lambda}, & {i=0} \\[5pt] W_{\rm c}^{(i)} =\dfrac{\lambda} {n+\lambda} +({1-\alpha^{2}+\beta}), & {i=0} \\[5pt] W_{\rm m}^{(i)} =W_{\rm c}^{(i)} =\dfrac{1}{2( {n+\lambda})}, & {i=1, \cdots, 2n} \end{cases} \end{align} $

(19)

式中，$ \eta =\sqrt{n+\lambda} $，尺度因子$ \lambda =\alpha^{2}( {n+\kappa})-n $，$ \kappa $为常数，$ \alpha $为控制Sigma点分布的常数，一般取值范围为$ 1{\rm e}-4\leqslant \alpha \leqslant 1 $。

步骤2：多步状态预测

$ \begin{align} \hat{\mathit{\boldsymbol{X}}}_{k-1}^{(j)} =\hat{\mathit{\boldsymbol{X}}}_{k-1}^{(j-1)} +\Delta T\mathit{\boldsymbol{f}}({\hat{\mathit{\boldsymbol{X}}}_{k-1}^{(j-1)}}), \quad j=1, 2, \cdots, P \end{align} $

(20)

考虑到GPS传感器更新频率较低^[34]，无法满足航天器的实时导航需求，因此本文采用多步预测方法，即系统状态每预测$ P $步，状态更新一次。

步骤3：时间更新

$ \begin{align} & \hat{\mathit{\boldsymbol{\varTheta}}}_{k} =\hat{\mathit{\boldsymbol{X}}}_{k}^{({P})} \end{align} $

(21)

$ \begin{align} & \hat{\mathit{\boldsymbol{X}}}_{k} =\sum _{i=0}^{2n} {W_{\rm m}^{(i)}} \hat{\mathit{\boldsymbol{\varTheta}}}_{k}^{(i)} \end{align} $

(22)

$ \begin{align} & [{\mathit{\boldsymbol{\bar{Q}}}, {\mathit{\boldsymbol{S}}}_{k}^{-}}]\leftarrow \rm{qr} \\ & \left({\left[{\sqrt{W_{\rm c}^{(1)}} \left({\hat{\bm\varTheta}_{1:2n, k} - \left[{\underbrace {\hat{{\mathit{\boldsymbol{X}}}}_{k} \cdots \hat{{\mathit{\boldsymbol{X}}}}_{k}}_{2n}}\right]}\right)\sqrt{{\mathit{\boldsymbol{Q}}}_{k-1}^{(\Gamma)}}} \right]^{\rm T}, 0}\right) \end{align} $

(23)

$ \begin{align} & {\mathit{\boldsymbol{S}}}_{k}^{-} \leftarrow {\rm{chol}} \left({{\mathit{\boldsymbol{S}}}_{k}^{-}, \sqrt{| {W_{\rm c}^{(0)}} |}({\hat{\bm\varTheta}_{0, k} -\hat{{\mathit{\boldsymbol{X}}}}_{k}}), {\rm{sgn}} W_{\rm c}^{(0)}}\right) \end{align} $

(24)

$ \begin{align} & {\mathit{\boldsymbol{S}}}_{k}^{-} \leftarrow ({{\mathit{\boldsymbol{S}}}_{k}^{-}})^{\rm T} \end{align} $

(25)

$ \begin{align} & \mathit{\boldsymbol{\varTheta}}_{k} =\left[{\underbrace {\hat{\mathit{\boldsymbol{X}}}_{k} \cdots \hat{\mathit{\boldsymbol{X}}}_{k}}_{2n+1}}\right]+\eta \left[{\mathit{\boldsymbol{0}}_{n, 1}} \; \; {\sqrt{\hat{\mathit{\boldsymbol{P}}}_{k}}} \; \; {-\sqrt{\hat{\mathit{\boldsymbol{P}}}_{k}}} \right] \end{align} $

(26)

$ \begin{align} & \mathit{\boldsymbol{\varOmega}}_{k} =\mathit{\boldsymbol{h}}({\mathit{\boldsymbol{\varTheta}}_{k}}) \end{align} $

(27)

$ \begin{align} & \hat{\mathit{\boldsymbol{Z}}}_{k} =\sum _{i=0}^{2n} {W_{\rm m}^{(i)}} \mathit{\boldsymbol{\varOmega}}_{k}^{(i)} \end{align} $

(28)

式中，“qr”与“chol”为Matlab函数，“sgn”为符号函数。

步骤4：测量更新

$ \begin{align} & [{\mathit{\boldsymbol{\bar{Q}}}, {\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}}} ]\leftarrow \\ & {\rm{qr}}\left({ \left[{\sqrt{{W}_{\rm c}^{( 1)}} \left({\mathit{\boldsymbol{\varOmega}}_{1:2n, k} - \left[{\underbrace {\hat{\mathit{\boldsymbol{Z}}}_{k} \cdots \hat{\mathit{\boldsymbol{Z}}}_{k}}_{2n}} \right]}\right)\sqrt{{\mathit{\boldsymbol{R}}}_{k}^{(\Upsilon)}}} \right]^{\rm T}, 0}\right) \end{align} $

(29)

$ \begin{align} & {\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}} \leftarrow \rm{chol} \left({{\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}}, \sqrt{| {{W}_{\rm c}^{(0)}} |}({\mathit{\boldsymbol{\varOmega}}_{0, k} -\hat{\mathit{\boldsymbol{Z}}}_{k}}), {\rm{sgn}} {W}_{\rm c}^{(0)}}\right) \end{align} $

(30)

$ \begin{align} & {\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}} \leftarrow {\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}}^{\rm T} \end{align} $

(31)

$ \begin{align} & \mathit{\boldsymbol{P}}_{\mathit{\boldsymbol{X}}_{k}, \mathit{\boldsymbol{Z}}_{k}} =\sum _{i=0}^{2n} {W_{\rm c}^{(i)}} ({\mathit{\boldsymbol{\varTheta}}_{k}^{(i)} -\hat{\mathit{\boldsymbol{Z}}}_{k}})({\mathit{\boldsymbol{\varTheta}}_{k}^{(i)} -\hat{\mathit{\boldsymbol{Z}}}_{k}})^{\rm T} \end{align} $

(32)

$ \begin{align} & \mathit{\boldsymbol{K}}_{k} =\mathit{\boldsymbol{P}}_{\mathit{\boldsymbol{X}}_{k}, \mathit{\boldsymbol{Z}}_{k}} ({{\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}}^{\rm T}})^{-1}{\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}}^{-1} \end{align} $

(33)

$ \begin{align} & \mathit{\boldsymbol{X}}_{k} =\hat{\mathit{\boldsymbol{X}}}_{k} +\mathit{\boldsymbol{K}}_{k} ( {\mathit{\boldsymbol{Z}}_{k} -\hat{\mathit{\boldsymbol{Z}}}_{k}}) \end{align} $

(34)

$ \begin{align} & \mathit{\boldsymbol{U}}=\mathit{\boldsymbol{K}}_{k} {\mathit{\boldsymbol{S}}}_{\mathit{\boldsymbol{Z}}_{k}} \end{align} $

(35)

$ \begin{align} & {\mathit{\boldsymbol{S}}}_{k} ={\rm{chol}}({\mathit{\boldsymbol{S}}}_{k}^{-}, \mathit{\boldsymbol{U}}, {\tt'}-') \end{align} $

(36)

4 可观性分析（Observability analysis）

本文研究的直链式$ N $体STS仅安装2个GPS传感器，两端卫星状态显然是可观的。但是，整个系统可观吗？下面，将基于李导数的局部弱可观的秩判据方法严格证明系统的可观性^[35-36]。

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

$ \begin{align} \begin{cases} {\mathit{\boldsymbol{\dot{x}}} = {\mathit{\boldsymbol{f}}}({\mathit{\boldsymbol{x}}, \mathit{\boldsymbol{u}}})} \\ {\mathit{\boldsymbol{y}}={\mathit{\boldsymbol{h}}}({\mathit{\boldsymbol{x}}})} \end{cases} \end{align} $

(37)

式中$ \mathit{\boldsymbol{x}}\in \mathbb{R}^{n} $是状态矢量，$ \mathit{\boldsymbol{u}}=[{u_{1}, \cdots, u_{l}}]^{\rm T}\in \mathbb{R}^{l} $是控制输入矢量，$ \mathit{\boldsymbol{y}}=[{y_{1}, \cdots, y_{m}} ]^{\rm T}\in \mathbb{R}^{m} $是测量矢量，其分量为$ y_{k} =h_{k} ({\mathit{\boldsymbol{x}}}) $，$ k=1, \cdots, m $。

如果过程函数$ {\mathit{\boldsymbol{f}}}({\mathit{\boldsymbol{x}}, \mathit{\boldsymbol{u}}}) $的输入$ \mathit{\boldsymbol{u}} $是线性的，则可以写成一系列独立函数之和，其中每个独立函数对应一个控制输入矢量分量。则式(1) 可以写成

$ \begin{align} \begin{cases} {\mathit{\boldsymbol{\dot{x}}} = {\mathit{\boldsymbol{f}}}_{0} ({\mathit{\boldsymbol{x}}})+{\mathit{\boldsymbol{f}}}_{1} ({\mathit{\boldsymbol{x}}})u_{1} +\cdots +{\mathit{\boldsymbol{f}}}_{l} ({\mathit{\boldsymbol{x}}})u_{l}} \\ {\mathit{\boldsymbol{y}}={\mathit{\boldsymbol{h}}}({\mathit{\boldsymbol{x}}})} \end{cases} \end{align} $

(38)

式中，$ {\mathit{\boldsymbol{f}}}_{0} $是零控制输入分量对应的过程函数。

定义1：函数$ h_{k} ({{\mathit{\boldsymbol{x}}}}) $对$ {\mathit{\boldsymbol{f}}}_{i} $的$ k $阶李导数定义为

$ \begin{align} \begin{cases} {L_{{\mathit{\boldsymbol{f}}}_{i}}^{0} h_{k} ({{\mathit{\boldsymbol{x}}}})=h_{k} ({{\mathit{\boldsymbol{x}}}})} \\[3pt] {L_{{\mathit{\boldsymbol{f}}}_{i}}^{k} h_{k} ({{\mathit{\boldsymbol{x}}}})=\dfrac{\partial ({L_{{\mathit{\boldsymbol{f}}}_{i}}^{k-1} h_{k} ( {{\mathit{\boldsymbol{x}}}})})}{\partial \mathit{\boldsymbol{x}}}{\mathit{\boldsymbol{f}}}_{i} ({\mathit{\boldsymbol{x}}})} \end{cases}\kern-15pt, \quad k=1, 2, \cdots \end{align} $

(39)

定义2：函数$ h_{k} ({{\mathit{\boldsymbol{x}}}}) $对$ {\mathit{\boldsymbol{f}}}_{i} $和$ {\mathit{\boldsymbol{f}}}_{j} $的$ k $阶混合李导数定义为（这里先对$ {\mathit{\boldsymbol{f}}}_{i} $求$ k-1 $阶李导数）

$ \begin{align} L_{{\mathit{\boldsymbol{f}}}_{j} {\mathit{\boldsymbol{f}}}_{i}}^{k} h_{k} ({\mathit{\boldsymbol{x}}})=L_{{\mathit{\boldsymbol{f}}}_{j}}^{1} ({L_{{\mathit{\boldsymbol{f}}}_{{i}}}^{k-1} h_{k} ({\mathit{\boldsymbol{x}}})})=\nabla L_{{\mathit{\boldsymbol{f}}}_{\mathit{\boldsymbol{i}}}}^{k-1} h_{k} ({\mathit{\boldsymbol{x}}})\cdot {\mathit{\boldsymbol{f}}}_{j} \end{align} $

(40)

基于上述李导数表示，可观性矩阵由李导数求导的行向量组成，可定义为

$ \begin{align} \mathit{\boldsymbol{O}} \triangleq \begin{bmatrix}\nabla L^0 h_{k} ({\mathit{\boldsymbol{x}}})\\ \vdots\\ \nabla L_{{\mathit{\boldsymbol{f}}}_{i} \cdots {\mathit{\boldsymbol{f}}}_{j}}^{l} h_{k} ({\mathit{\boldsymbol{x}}})\end{bmatrix} \end{align} $

(41)

其中，$ i, j=0, \cdots, l $，$ k=1, \cdots, m $，$ l\in \mathbb{N} $。

命题（可观性秩判据）：如果式(41) 中定义的非线性系统可观性矩阵$ \mathit{\boldsymbol{O}} $是满秩的，则非线性系统(37) 局部弱可观。

推论：由于非线性系统(37) 是无限平滑的，可观性矩阵$ \mathit{\boldsymbol{O}} $由无限行向量组成。然而，要证明$ \mathit{\boldsymbol{O}} $满秩，只需证明其行向量的子集线性无关。

注意：关于如何选择合适的李导数以及对应的$ \mathit{\boldsymbol{O}} $的行向量尚无系统的方法。一般需要通过逐阶计算各个方向的李导数，从中挑选合适的李导数。

下面，将采用数学归纳法递推证明非线性系统(16) 的可观性。

首先，如果$ N=3 $，此时系统由3颗卫星和2根系绳组成，系统方程可表示为

$ \begin{align} \begin{cases} {\mathit{\boldsymbol{\dot{X}}}\mathit{\boldsymbol{=f}}_{0} ({\mathit{\boldsymbol{X}}})+\mathit{\boldsymbol{f}}_{1} ({\mathit{\boldsymbol{X}}})u_{1} +\cdots +\mathit{\boldsymbol{f}}_{9} ({\mathit{\boldsymbol{X}}})u_{9}} \\ {\mathit{\boldsymbol{Z}}=\mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}})} \end{cases} \end{align} $

(42)

式中$ \mathit{\boldsymbol{Z}} $是观测方程并且$ Z_{m} =h_{m} ({\mathit{\boldsymbol{X}}}) $，$ m=1, \cdots, 8 $。

1) 0阶李导数

由定义1可知，函数的0阶李导数是函数本身

$ \begin{align} L^{0}\mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}})=\mathit{\boldsymbol{h}}( {\mathit{\boldsymbol{X}}}) \end{align} $

(43)

因此，0阶李导数的梯度即为观测方程的雅可比矩阵，表示为

$ \begin{align} \nabla L^{0}\mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}})& =\nabla \mathit{\boldsymbol{h}}( {\mathit{\boldsymbol{X}}}) \\ & =\begin{bmatrix} {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 9}} \\ {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 9}} \\ {\mathit{\boldsymbol{a}}} & {-\mathit{\boldsymbol{a}}} & {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{0}}_{1\times 9}} \\ {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{b}}} & {-\mathit{\boldsymbol{b}}} & {\mathit{\boldsymbol{0}}_{1\times 9}} \end{bmatrix} \end{align} $

(44)

式中$ \mathit{\boldsymbol{0}}_{1\times 3} \in \mathbb{R}^{1\times 3} $为$ 1\times 3 $零矩阵，并且

$ \begin{align} \mathit{\boldsymbol{a}}& =\frac{\mathit{\boldsymbol{r}}_{1} -\mathit{\boldsymbol{r}}_{2}} {l_{1}} \end{align} $

(45)

$ \begin{align} \mathit{\boldsymbol{b}}& =\frac{\mathit{\boldsymbol{r}}_{2} -\mathit{\boldsymbol{r}}_{3}} {l_{2}} \end{align} $

(46)

2) 1阶李导数

$ \begin{align} L_{\mathit{\boldsymbol{f}}_{0}}^{1} \mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}}) =[{\mathit{\boldsymbol{\dot{r}}}_{1}} \; \; {\mathit{\boldsymbol{\dot{r}}}_{3}} \; \; {h_{1}} \; \; {h_{2}} ]^{\rm T} \end{align} $

(47)

式中$ h_{1} =({\mathit{\boldsymbol{r}}_{1} -\mathit{\boldsymbol{r}}_{2}})( {\mathit{\boldsymbol{\dot{r}}}_{1} -\mathit{\boldsymbol{\dot{r}}}_{\mathit{\boldsymbol{2}}}})/l_{1} $，$ h_{2} =({\mathit{\boldsymbol{r}}_{2} -\mathit{\boldsymbol{r}}_{3}})( \mathit{\boldsymbol{\dot{r}}}_{2} - $ $ \mathit{\boldsymbol{\dot{r}}}_{3})/l_{2} $。则

$ \begin{align} & \nabla L_{\mathit{\boldsymbol{f}}_{0}}^{1} \mathit{\boldsymbol{h}}({\mathit{\boldsymbol{X}}}) \\ =& \begin{bmatrix} {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\ {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} \\ {\mathit{\boldsymbol{x}}_{1}} & {-\mathit{\boldsymbol{x}}_{1}} & {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{a}}} & {-\mathit{\boldsymbol{a}}} & {\mathit{\boldsymbol{0}}_{1\times 3}} \\ {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{x}}_{2}} & {-\mathit{\boldsymbol{x}}_{2}} & {\mathit{\boldsymbol{0}}_{1\times 3}} & {\mathit{\boldsymbol{b}}} & {-\mathit{\boldsymbol{b}}} \\ \end{bmatrix} \end{align} $

(48)

式中$ \mathit{\boldsymbol{x}}_{1} $、$ \mathit{\boldsymbol{x}}_{2} $均为$ 1\times 3 $矩阵，具体表达式可不写出。

3) 2阶李导数

$ \begin{align} \nabla L_{\mathit{\boldsymbol{f}}_{1}}^{2} h_{7} ({\mathit{\boldsymbol{X}}}) =[{\mathit{\boldsymbol{c}}} \; \; {-\mathit{\boldsymbol{c}}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} ] \end{align} $

(49)

式中$ \mathit{\boldsymbol{c}} = [{c_{1}} \; \; {c_{2}} \; \; {c_{3}} ] $为$ 1\times 3 $矩阵，具体为

$ \begin{align} c_{1} & =\frac{(r_{1, y} -r_{2, y})^{2}+(r_{1, z} -r_{2, z})^{2}}{m_{1} l_{1}^{3}} \end{align} $

(50)

$ \begin{align} c_{2} & =-\frac{(r_{1, x} -r_{2, x})(r_{1, y} -r_{2, y})}{m_{1} l_{1}^{3}} \end{align} $

(51)

$ \begin{align} c_{3} & =-\frac{(r_{1, x} -r_{2, x})(r_{1, z} -r_{2, z})}{m_{1} l_{1}^{3}} \end{align} $

(52)

$ \begin{align} \nabla L_{\mathit{\boldsymbol{f}}_{2}}^{2} h_{7} ({\mathit{\boldsymbol{X}}}) & =[ {\mathit{\boldsymbol{d}}} \; \; {-\mathit{\boldsymbol{d}}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} \; \; {\mathit{\boldsymbol{0}}_{1\times 3}} ] \end{align} $

(53)

式中$ \mathit{\boldsymbol{d}} = [{d_{1}} \; \; {d_{2}} \; \; {d_{3}} ] $为$ 1\times 3 $矩阵，具体为

$ \begin{align} d_{1} & =-\frac{(r_{1, x} -r_{2, x})(r_{1, y} -r_{2, y})}{m_{1} l_{1}^{3}} \end{align} $

(54)

$ \begin{align} d_{2} & =\frac{(r_{1, x} -r_{2, x})^{2}+(r_{1, z} -r_{2, z})^{2}}{m_{1} l_{1}^{3}} \end{align} $

(55)

$ \begin{align} d_{3} & =-\frac{(r_{1, y} -r_{2, y})(r_{1, z} -r_{2, z})}{m_{1} l_{1}^{3}} \end{align} $

(56)

将求得的李导数梯度合并，构造可观性矩阵$ \mathit{\boldsymbol{O}}_{3} $：

$ \begin{align} \mathit{\boldsymbol{O}}_{3} =\begin{bmatrix} {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\ {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\ {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\ {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} \\ {\mathit{\boldsymbol{\varXi}}_{1}} & {-\mathit{\boldsymbol{\varXi}}_{1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\ {\mathit{\boldsymbol{X}}_{1}} & {-\mathit{\boldsymbol{X}}_{1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{\varXi}}_{1}} & {-\mathit{\boldsymbol{\varXi}}_{1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \end{bmatrix} \end{align} $

(57)

式中$ \mathit{\boldsymbol{X}}_{1} $为$ 3\times 3 $矩阵，具体表达式可不写出，$ \mathit{\boldsymbol{\varXi}}_{1} $由式(58) 确定并且$ \mathit{\boldsymbol{\varXi}}_{1} =\Im (r_{1, x} -r_{2, x}, $ $ r_{1, y} -r_{2, y}, $ $ r_{1, z} -r_{2, z}, $ $ m_{1}, l_{1}) $.

$ \begin{align} \Im ({\bar{x}, \bar{y}, \bar{z}, m, l}) =\begin{bmatrix} {\dfrac{\bar{x}}{l}} & {\dfrac{\bar{y}}{l}} & {\dfrac{\bar{z}}{l}} \\[7pt] {\dfrac{\bar{y}^{2}+\bar{z}^{2}}{ml^{3}}} & {-\dfrac{\bar{x}\bar{y}}{ml^{3}}} & {-\dfrac{\bar{x}\bar{z}}{ml^{3}}} \\[7pt] {-\dfrac{\bar{x}\bar{y}}{ml^{3}}} & {\dfrac{\bar{x}^{2}+\bar{z}^{2}}{ml^{3}}} & {-\dfrac{\bar{y}\bar{z}}{ml^{3}}} \end{bmatrix} \end{align} $

(58)

可知，当$ \mathit{\boldsymbol{\varXi}}_{1} $满秩时，可观性矩阵$ \mathit{\boldsymbol{O}}_{3} $必满秩。基于高斯消去法对$ \mathit{\boldsymbol{\varXi}}_{1} $处理后得$ \mathit{\boldsymbol{\varXi}}_{1} ' $，具体表达式为式(59)。可知$ \mathit{\boldsymbol{\varXi}}_{1} $必是满秩矩阵，除非$ r_{1, z} =r_{2, z} $或$ (r_{1, x} - r_{2, x})^{2}+ ({r_{1, y} -r_{2, y}})^{2}+({r_{1, z} -r_{2, z}})^{2}=0 $。显然，由于卫星1、2的质点并不重合，所以$ r_{1, z} \ne r_{2, z} $和$ (r_{1, x} - $ $ r_{2, x})^{2}+({r_{1, y} -r_{2, y}})^{2}+({r_{1, z} -r_{2, z}})^{2}\ne 0 $。因此，可观性矩阵$ \mathit{\boldsymbol{O}}_{3} $是满秩的，即非线性系统(16) 可观。

然后，假设当$ N=n $时非线性系统(16) 是可观的，则可观性矩阵$ \mathit{\boldsymbol{O}}_{n} $如式(60) 所示。其中$ \mathit{\boldsymbol{X}}_{n-1} $ $ \in $ $ \mathbb{R}^{3\times 3} $，$ \mathit{\boldsymbol{\varXi}}_{n-1} $ $ = $ $ \Im (r_{n-2, x} -r_{n-1, x}, $ $ r_{n-2, y} -r_{n-1, y}, $ $ r_{n-2, z} $ $ - $ $ r_{n-1, z}, $ $ m_{n-2}, l_{n-2}) $，此时$ \mathit{\boldsymbol{\varXi}}_{n-1} $是满秩矩阵。

最后，当$ N=n+1 $时，系统观测矩阵由$ n-1 $维扩展至$ n $维，即增加系绳伪测量值$ l_{n} =| {\mathit{\boldsymbol{r}}_{n} -\mathit{\boldsymbol{r}}_{n+1}}| $，经计算发现可观性矩阵$ \mathit{\boldsymbol{O}}_{n+1} $在$ \mathit{\boldsymbol{O}}_{n} $的基础上增加了6行，$ \mathit{\boldsymbol{O}}_{n+1} $具体表示为式(61)，其中$ \mathit{\boldsymbol{\varXi}}_{n} =\Im ( r_{n-1, x} -r_{n, x}, $ $ r_{n-1, y} -r_{n, y}, $ $ r_{n-1, z} -r_{n, z}, $ $ m_{n-1}, $ $ l_{n-1}) $。易知$ \mathit{\boldsymbol{\varXi}}_{n} $必定为满秩矩阵，除非$ r_{n-1, z} -r_{n, z} =0 $或$ ({r_{n-1, y} -r_{n, y}})^{2}+({r_{n-1, y} -r_{n, y}})^{2}+({r_{n-1, z} -r_{n, z}})^{2}=0 $。显然，这两式不可能成立。因而，$ \mathit{\boldsymbol{O}}_{n+1} $必是满秩矩阵。

综上，非线性系统(16) 是可观的，证毕。

$ \begin{align} & \mathit{\boldsymbol{\varXi}}_{1}' =\begin{bmatrix} {r_{1, x} -r_{2, x}} & {r_{1, y} -r_{2, y}} & {r_{1, z} -r_{2, z}} \\[-1pt] {({r_{1, x} -r_{2, x}})^{2}+({r_{1, y} -r_{2, y}})^{2}+({r_{1, z} -r_{2, z}})^{2}} & 0 & 0 \\[-1pt] 0 & {({r_{1, x} -r_{2, x}})^{2}+({r_{1, y} -r_{2, y}})^{2}+({r_{1, z} -r_{2, z}})^{2}} & 0 \end{bmatrix} \end{align} $

(59)

$ \begin{align} & \mathit{\boldsymbol{O}}_{n} =\begin{bmatrix} {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\[-1pt] {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\[-1pt] {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\[-1pt] {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{I}}_{3}} \\[-1pt] {\mathit{\boldsymbol{\varXi}}_{1}} & {-\mathit{\boldsymbol{\varXi}}_{1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\[-1pt] {\mathit{\boldsymbol{X}}_{1}} & {-\mathit{\boldsymbol{X}}_{1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{\varXi}}_{1}} & {-\mathit{\boldsymbol{\varXi}}_{1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\[-1pt] \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots \\[-1pt] {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{\varXi}}_{n-1}} & {-\mathit{\boldsymbol{\varXi}}_{n-1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \\[-1pt] {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{X}}_{n-1}} & {-\mathit{\boldsymbol{X}}_{n-1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & {\mathit{\boldsymbol{0}}_{3\times 3}} & \cdots & {\mathit{\boldsymbol{\varXi}}_{n-1}} & {-\mathit{\boldsymbol{\varXi}}_{n-1}} & {\mathit{\boldsymbol{0}}_{3\times 3}} \end{bmatrix} \end{align} $

(60)

(61)

5 数值仿真（Numerical simulation）

为验证本文算法的有效性，本节给出当$ N = 3 $时的数值仿真结果。系统状态初值见表 1。另外，系绳$ l_{1} =1000 $ m，系绳$ l_{2} =1000 $ m，GPS传感器测量误差为5 m，更新频率为1 Hz。在滤波过程中，控制Sigma点分布的常数$ \alpha =0.6 $，系统噪声为$ \mathit{\boldsymbol{Q}}^{(\Gamma)} $ $ =10^{-9}\times {\rm{diag}} [ {\mathit{\boldsymbol{Q}}_{1}^{(\Gamma)}} \; \; {\mathit{\boldsymbol{Q}}_{1}^{(\Gamma)}} \; \; {\mathit{\boldsymbol{Q}}_{1}^{(\Gamma)}} \; \; {\mathit{\boldsymbol{Q}}_{2}^{(\Gamma)}} \; \; {\mathit{\boldsymbol{Q}}_{2}^{(\Gamma)}} \; \; {\mathit{\boldsymbol{Q}}_{2}^{(\Gamma)}} ]\in \mathbb{R}^{18\times 18} $，其中$ \mathit{\boldsymbol{Q}}_{1}^{(\Gamma)} ={\rm{diag}} (1, 1, 1) $，$ \mathit{\boldsymbol{Q}}_{2}^{(\Gamma)} ={\rm{diag}} ({10^{-6}}, {10^{-6}}, {10^{-6}} ) $。测量噪声$ \mathit{\boldsymbol{R}}_{k}^{(\Upsilon)} ={\rm{diag}} [{\mathit{\boldsymbol{R}}_{1}^{(\Upsilon)}} \; \; {\mathit{\boldsymbol{R}}_{2}^{(\Upsilon)}} ]\in \mathbb{R}^{8\times 8} $，其中$ \mathit{\boldsymbol{R}}_{1}^{(\Upsilon)} =25{\mathit{\boldsymbol{I}}}_{6} $，$ \mathit{\boldsymbol{R}}_{2}^{(\Upsilon)} ={\rm{diag}} ( {10^{-4}}, {10^{-4}} ) $。

图 3和图 4为卫星1和卫星3滤波前后的误差对比图，为了方便观察，这里只画出了500 s内的滤波误差对比结果。图 5~图 7为卫星1在2个周期内在$ X $、$ Y $和$ Z $方向的位置估计结果图，图中将系统实际位置、GPS测量位置和状态估计结果进行对比。同样，图 8~图 10为卫星3在2个周期内在$ X $、$ Y $和$ Z $方向的位置估计结果对比图。对比结果表明，本文算法可大幅度减小GPS传感器的测量误差。

为了定量地分析所提算法的滤波效果，表 2给出了2个运动周期内系统状态均方误差在滤波前后的统计数据，从表 2可以看出，滤波后系统状态的误差约减小了90{%} 以上。位置估计精度达到分米级别，特别是在$ X $方向上，滤波精度可达厘米级。滤波结果表明，本文算法能够有效保证系统状态的估计精度。

考虑到GPS传感器更新频率较低，难以满足航天器实时导航的需求，本文采用每预测$ P $步状态更新一次的策略来实现系统状态的密切跟踪。图 11、图 12分别为卫星1和3在$ X $、$ Y $和$ Z $方向的状态跟踪图。图 13、图 14为卫星1和3在2个运动周期内的状态跟踪误差。仿真结果表明本文算法能保证系统在高动态运动条件下实现状态实时跟踪。

为考察卫星2的状态可观性，需要进一步开展仿真验证。图 15~图 17为卫星2在$ X $、$ Y $和$ Z $方向的状态跟踪图。图 18为卫星2在2个运动周期内的状态跟踪误差。仿真结果表明，尽管卫星2未安装GPS传感器，但是其状态确实是可观的。

综上可知，本文算法能够有效保证STS的状态估计精度和实时跟踪。并且在仅使用2个GPS传感器的条件下，$ N={3} $时系统确实是可观的。进一步，在上节可观性分析部分，从理论上已经严格证明了任意直链式$ N $（$ N\geqslant 3 $）体绳系系统的可观性，结合本节仿真结果可知直链式$ N $体STS在仅使用2个GPS传感器的条件下确实是可观的。

6 结论（Conclusion）

针对直链式$ N $体STS的约束状态估计问题，本文提出了一种基于伪测量法的约束状态估计方法。该方法将系绳绳长约束作为先验信息，在仅使用2个GPS传感器的条件下，实现了$ N $体STS状态的高精度估计和实时跟踪。同时，基于李导数的局部弱可观的秩判据方法给出了任意直链式$ N $（$ N\geqslant 3 $）体空间绳系系统的可观性的严格证明。但是，本文的状态估计方案仅适用于系绳弹性形变相对较小的情况，对于系绳弹性变形较大或系绳大幅摆动的情况尚需进一步研究。下一步，将进一步拓展本文工作，讨论多种构形的STS的最少传感器布局，研究系绳存在大变形条件下的STS状态估计和可观性。