CN113607164B - 一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法 - Google Patents

Abstract

本发明提供了一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法，以实现最少传感器布局下的柔性系绳三维摆角高精度估计。本发明以二体绳系卫星系统为模型展开研究，首先采用珠点法对系绳进行离散并基于牛顿法建立二体绳系卫星系统动力学模型，然后开发了一种改进型平方根无迹卡尔曼滤波(IUKF)并对本发明提出方法的可行性进行了严格的证明，最后采用三次B样条曲线对获得的系绳珠点三维位置状态高精度进行拟合并处理后即可获得系绳的三维摆角。

Description

一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估 计方法

技术领域

本发明属于大尺度柔性系绳三维摆角估计技术领域，具体涉及一种千米级空间柔性系绳在最少传感器布局条件下的三维摆角高精度估计方法。

背景技术

空间系绳在灵活性、生命周期和功能拓展等方面优势显著，成为近年来国内外航天领域的研究热点之一。空间绳系系统由多根系绳将卫星连接组成，因此，空间绳系系统在空间实验、空间资源探索、对地观测以及空间攻防等任务中具有广泛应用前景，掌握空间绳系技术对推动我国空间技术发展具有重大的战略意义，必须尽快开展相关的基础理论和技术研究。

系绳的利用是空间绳系系统最重要的特征，相对于其他结构，系绳具有质量轻、阻尼小、抗拉强度高、柔软性好、大尺度及易卷取等特征，其在空间具有很大的应用优势。在日常生活中系绳亦有很大应用范围，例如：两电线杆之间连接着输电线、桥柱与桥墩之间连接着拉索，上述这些就可以看成是一个两端固定的绳系系统；纺织机两络筒之间连接着纺织线、电梯起重机与电梯厢之间连接着钢丝绳以及带动观光缆车运动的缆绳与缆车这些也是绳系系统。但是，系绳柔软、易发生大变形，不仅表现出蠕变、非线性刚度和阻尼、塑性变形等非线性现象，而且在周期往复加减载时还表现出明显的迟滞效应，因而系绳具有复杂的动力学特性。其动力学特性不仅在微观上与材料本身的分子结构有关，宏观上与系绳的缠绕编制方式等也有很大的关系，一般很难建立能完全表现系绳特性的精确模型。

在现有的绳系卫星研究中，研究者一般根据各自所侧重的问题，采用各种简化的模型对系绳进行描述。从对具有无限维自由度系绳的离散程度上来分，现有的模型大致可分为三种类型：刚性杆模型、珠点模型和连续体模型。空间绳系系统在轨运行时，系绳将会产生面内，面外的摆动和纵向、横向的振动，并且系绳的展开是一个临界稳定的过程，当展开速度超过临界速度时系统将会变得不稳定，而系绳的回收过程由于负阻尼存在始终是不稳定的。系绳的摆动运动与空间绳系系统的轨道运动和系绳两端卫星的姿态运动产生耦合，从而影响系统的轨道运动和卫星的姿态运动。由于系绳的摆动影响，将会导致系绳两端卫星的振动，因此需要对绳系系统加以稳定，这需要知道系绳的摆动信息，但是如何获得系绳的面内、面外摆角及其相应角速率是一个需要解决的工程难题。

因此，本发明针对一类大尺度柔性系绳，在仅使用两个低成本惯性传感器(例如：GPS传感器)条件下实现柔性系绳三维摆角的高精度状态估计。

发明内容

本发明提供了一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法，目的是实现最少传感器布局下的柔性系绳三维摆角高精度估计。本发明以二体绳系卫星系统为模型展开研究，首先采用珠点法对系绳进行离散并基于牛顿法建立二体绳系卫星系统动力学模型，然后开发了一种改进型平方根无迹卡尔曼滤波(IUKF)并对本发明提出方法的可行性进行了严格的证明，最后采用三次B样条曲线对获得的系绳珠点三维位置状态高精度进行拟合后获得系绳的三维摆角。

为实现上述目的，本发明所提供的技术解决方案是：

一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法，其特殊之处在于，包括以下步骤：

1)建立二体绳系卫星系统的状态空间模型，即获得二体绳系卫星系统的状态方程；

2)基于步骤1)的状态空间模型设计滤波算法，获得二体绳系卫星系统的状态；

3)根据二体绳系卫星系统的状态，进行曲线拟合，获得二体绳系卫星系统的大尺度柔性系绳的三位摆角。

进一步地，步骤1)中，所述状态空间模型包括动力学模型以及惯性传感器布局；

状态空间模型的建立包括基于牛顿法建立二体绳系卫星系统动力学模型以及在二体绳系卫星系统节点卫星进行惯性传感器布局；具体为：

考虑GPS传感器特点，定义地心惯性坐标系O-XYZ,其原点O固连于地心，OX轴位于赤道面内并指向春分点方向，OZ轴指向地球北极，OY轴由右手螺旋法则确定；为简化研究，对二体绳系卫星系统做以下假设：

a.相对于系绳长度，二体绳系卫星系统节点卫星尺寸可忽略不计，将卫星视为质点；

b.不考虑系绳弹性，将柔性离散为由一系列珠点和刚性杆连接而成；

c.二体绳系卫星系统运行于圆形开普勒轨道，除了地球引力，其他外界干扰均忽略；

按照牛顿法建立的系统动力学方程为

令为状态变量，则系统动力学方程可变形为

考虑二体绳系卫星系统噪声，将上式写成函数形式为

式中，系统噪声Γ～N(0，Q^(Γ))服从高斯分布，协方差矩阵

二体绳系卫星系统的两端卫星的绝对位置分别由其上的惯性传感器直接确定，表示为

式中，表示卫星的期望实际绝对位置，/>表示惯性传感器测量噪声；假设为高斯白噪声

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

z^(l)＝L(q)+v^(l)

式中，表示虚拟噪声，假设为高斯白噪声。

考虑二体绳系卫星系统的惯性传感器布局，基于伪测量法，合并系统的测量方程可得到

式中，Υ_k～N(0，R^(Υ))表示测量方程扩展后的测量噪声，R^(Υ)表示扩展后的测量噪声协方差，z^(g)(X)由下式确定

Z^(g)(X)＝H_gX^(g)+V^(g)

式中，测量噪声服从高斯分布，/>表示为

式中，为3×3单位矩阵。

将二体绳系卫星系统的动力学方程和测量方程联立可得二体绳系卫星系统的状态方程为

进一步地，步骤2)具体为：

2.1)为了实施滤波，对二体绳系卫星系统的状态方程进行离散化处理

式中，ΔT为系统采样时间；

2.2)设计滤波算法

所述滤波算法为改进型平方根无迹卡尔曼滤波算法，包括：多步状态预测部分和状态更新部分，具体如下：

2.2.1)Sigma点计算

相应的权值为：

式中，尺度因子λ＝α²(n+κ)-n，κ为常数，α为控制Sigma点分布的常数，一般取值范围为1e-4≤α≤1；

2.2.2)多步状态预测

考虑到惯性传感器更新频率较低，无法满足航天器的实时导航需求；为此，本发明采用多步预测方法，即系统状态每预测P步状态更新一次；

2.2.3)时间更新

Ω_k＝h(Θ_k)

2.2.4)测量更新

本发明仅使用了两个GPS传感器即可估计出来包括两端卫星和中间系绳珠点的位置状态，下面基于Lie导数可观性秩判据分析系统的对系统的可观性进行证明。

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

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

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

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

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

如果上式定义的非线性系统可观性矩阵O是满秩的，则本发明的非线性系统局部弱可观。

首先，如果柔性系绳被离散为一个珠点连接两个刚性杆，此时系统含有三个质点和两根系绳，系统方程可表示为

式中Z是观测方程并且Z_m＝h_m(x)，m＝1，...，8。

1.零阶李导数

函数的零阶李导数是函数本身。因此，零阶导数的梯度对应观测方程的雅克比矩阵表示为

式中0_3×3为3×3零矩阵，0_1×3为1×3零矩阵，a＝(r₁-r₂)/l₁，b＝(r₂-r₃)/l₂。

2.一阶李导数

式中则

式中x1，x2为1×3矩阵，具体表达式可不写出。

3.二阶李导数

式中c＝[c₁ c₂ c₃]，具体为： 和

式中d＝[d₁ d₂ d₃]，具体为

将求得的李导数梯度合并，构造可观矩阵O₃

式中，X₁为3×3矩阵，具体表达式可不写出，Ξ₁由下式确定并且

可知，当Ξ₁满秩时，可观性矩阵O₃必满秩，基于高斯消去法对Ξ₁处理后得Ξ₁′，具体表达式为下式，

可知Ξ₁必是满秩矩阵，除非r_1，z＝r_2，z或(r2，x_1，x ²+(r_1，y-r_2，y)²+(r2，z_1，z ²＝0))。显然，这不可能。可知，可观性矩阵O₃是满秩的。因此，当仅有两个GPS传感器时，中间珠点的位置可以被估计出来。

同理，如果中间若系绳被离散为两个珠点时，可看成两个含一个珠点的两体绳系卫星系统，按照以上步骤可证得中间两个珠点的位置可以被估计出来。

经归纳总结：当柔性系绳被离散为多个珠点时，在系统仅安装有两个惯性传感器的条件下，中间所有珠点位置均可以被估计出来。

进一步地，步骤3)具体为：

采用三次B样条曲线对获得的系绳珠点三维位置状态进行拟合；

经状态估计后得到系绳珠点的三维坐标后采用三次B样条曲线拟合的形式对得到的数据点进行处理；利用三次B样条曲线拟合原理对坐标点进行拟合，其原理如下：

已知系绳的珠点三维坐标组成一个点集{r₁，r₂，…r_n-1，r_n}，取此点集前4个离散点并重新记为B₀，B₁，B₂，B₃，假设其坐标分别为(x₀，y₀，z₀)，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x₃，y₃，z₃)；要使拟合后的曲线起始于B₀，终止于B₃，需要在起始端与终止端分别增加点B_s＝2B₀-B₁和B_e＝2B₃-B₂，然后组成新的点序列为B_s，B₀，B₁，B₂，B₃，B_e为控制点拟合三次B样条曲线；

同理，依次构建三次B样条曲线段序列{B_n，B_n+1，B_n+2，…，B_n+x}，其中，第k个三次B样条曲线可表示为

其中，B_k(u)为B样条曲线上的任意坐标点位置矢量；u为参量，且u∈[0，1]，分量式为

式中：

最后，根据获得系绳的拟合曲线后即可求得系绳在绝对坐标系下的横向摆角和纵向摆角。

进一步地，所述惯性闯传感器为GPS传感器。

本发明的优点是：

本发明给出了一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法。该方法与存在的系绳摆角相比以下方面存在优势：(1)系绳摆角具有高保真性，现有的摆角测量均假设大尺度系绳为刚性杆，这与现实情况不符合。本发明采用珠点模型对柔性系绳进行离散，可获得较为真实的系绳摆角状态；(2)该方法仅使用了两个低成本GPS传感器，成本较低，实现简单；(3)本方法并未使用传感器直接测量难以精确测量的大尺度系绳摆角，而是基于理论推导和曲线拟合在仅使用两个GPS传感器的条件下获得大尺度柔性系绳三维摆角，该方法具有创新性；(4)本发明仅使用两个惯性传感器可实现包括系绳三维摆角，两端卫星位置在内的系统状态，本发明充分利用了系统的耦合动力学模型和系绳约束先验信息，并经过严格理论证明了发明提出方法的可行性。本发明简单易行，可适用于一类大尺度柔性系绳的三维摆角估计。

附图说明

图1二体绳系卫星系统。

具体实施方式

以下结合附图和具体实施例对本发明的内容作进一步的详细描述：

步骤一：基于牛顿法建立二体绳系卫星系统动力学模型；

本发明研究的二体绳系卫星系统如附图1所示，其中两颗卫星上均只安装GPS传感器。考虑GPS传感器特点，定义地心惯性坐标系O-XYZ,其原点O固连于地心，OX轴位于赤道面内并指向春分点方向，OZ轴指向地球北极，OY轴由右手螺旋法则确定。为简化研究，对系统做以下假设：

a.相对于系绳长度，系统节点卫星尺寸可忽略不计，将卫星视为质点；

b.不考虑系绳弹性，将柔性离散为由一系列珠点和刚性杆连接而成；

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

按照牛顿法方法的系统动力学方程为

令为状态变量，则系统动力学方程可变形为

考虑系统噪声，将上式写成函数形式为

式中，系统噪声Γ～N(0，Q^(Γ))服从高斯分布，协方差矩阵系统的两端卫星的绝对位置/>可由GPS传感器直接确定，可表示为

式中，表示卫星的期望实际绝对位置，/>表示GPS传感器测量噪声，本文假设为高斯白噪声。

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

z^(l)＝L(q)+v^(l)

式中，表示虚拟噪声，本文假设为高斯白噪声。

考虑系统的传感器布局，基于伪测量法，合并系统的测量方程可得到

式中，Υ_k～N(0，R^(Υ))表示测量方程扩展后的测量噪声，R^(γ)表示扩展后的测量噪声协方差，z^(g)(X)由下式确定

z^(g)(X)＝H_gX^(g)+V^(g)

式中，测量噪声服从高斯分布，/>表示为

式中，为3×3单位矩阵。

将系统的动力学方程和测量方程联立可得系统的状态方程为

步骤二：为改进型平方根无迹卡尔曼滤波算法设计并进行严格理论证明；

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

式中，ΔT为系统采样时间；

本发明开发的IUKF包含：多步状态预测部分和状态更新部分。具体如下：

第一步：Sigma点计算

相应的权值为：

式中，尺度因子λ＝α²(n+κ)-n，κ为常数，α为控制Sigma点分布的常数，一般取值范围为1e-4≤α≤1；

第二步：多步状态预测

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

第三步：时间更新

Ω_k＝h(Θ_k)

第四步：测量更新

本发明仅使用了两个GPS传感器即可估计出来包括两端卫星和中间系绳珠点的位置状态，下面基于Lie导数可观性秩判据分析系统的对系统的可观性进行证明。

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

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

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

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

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

如果上式定义的非线性系统可观性矩阵O是满秩的，则本发明的非线性系统局部弱可观。

首先，如果柔性系绳被离散为一个珠点连接两个刚性杆，此时系统含有三个质点和两根系绳，系统方程可表示为

式中Z是观测方程并且Z_m＝h_m(x)，m＝1，...，8；

1.零阶李导数

函数的零阶李导数是函数本身。因此，零阶导数的梯度对应观测方程的雅克比矩阵表示为

式中0_3×3为3×3零矩阵，0_1×3为1×3零矩阵，a＝(r₁-r₂)/l₁，b＝(r₂-r₃)/l₂。

2.一阶李导数

式中则

式中x1，x2为1×3矩阵，具体表达式可不写出；

3.二阶李导数

式中c＝[c₁ c₂ c₃]，具体为： 和

式中d＝[d₁ d₂ d₃]，具体为

将求得的李导数梯度合并，构造可观矩阵O₃

式中X₁为3×3矩阵，具体表达式可不写出，Ξ₁由下式确定并且

可知，当Ξ₁满秩时，可观性矩阵O₃必满秩；基于高斯消去法对Ξ₁处理后得Ξ₁′，具体表达式为下式，

可知Ξ₁必是满秩矩阵，除非r_1，z＝r_2，z或(r2，x_1，x ²+(r_1，y-r_2，y)²+(r2，z_1，z ²＝0))；显然，这不可能。可知，可观性矩阵O₃是满秩的。因此，当仅有两个GPS传感器时，中间珠点的位置可以被估计出来。

同理，如果中间若系绳被离散为两个珠点时，可看成两个含一个珠点的两体绳系卫星系统，按照以上步骤可证得中间两个珠点的位置可以被估计出来。经归纳总结：当柔性系绳被离散为多个珠点时，在仅系统仅安装有两个GPS传感器的条件下，中间所有珠点位置可以被估计出来。

步骤三：采用三次B样条曲线对获得的系绳珠点三维位置状态进行精确拟合。

经状态估计后得到系绳珠点的三维坐标后采用三次B样条曲线拟合的形式对得到的数据点进行处理。利用三次B样条曲线拟合原理对坐标点进行拟合，其原理如下：已知系绳的珠点三维坐标组成一个点集{r₁，r₂，…r_n-1，r_n}，取此点集前4个离散点并重新记为B₀，B₁，B₂，B₃,假设其坐标分别为(x₀，y₀，z₀)，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x₃，y₃，z₃)。要使拟合后的曲线起始于B₀，终止于B₃，需要在起始端与终止端分别增加点B_s＝2B₀-B₁和B_e＝2B₃-B₂，然后组成新的点序列为B_s，B₀，B₁，B₂，B₃，B_e为控制点拟合三次B样条曲线。同理，依次构建三次B样条曲线段序列{B_n，B_n+1，B_n+2，…，B_n+x}，其中第k个三次B样条曲线可表示为

其中：B_k(u)为B样条曲线上的任意坐标点位置矢量；u为参量，且u∈[0，1]，分量式为

式中：

最后，根据获得系绳的拟合曲线后即可求得系绳在绝对坐标系下的横向摆角和纵向摆角。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。

Claims (2)

1.一种最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法，其特征在于，包括以下步骤：

1)建立二体绳系卫星系统的状态空间模型；

2)基于步骤1)的状态空间模型设计滤波算法，获得二体绳系卫星系统的状态；

3)根据二体绳系卫星系统的状态，进行曲线拟合，获得二体绳系卫星系统的大尺度柔性系绳的三位摆角；

步骤1)中，状态空间模型的建立包括基于牛顿法建立二体绳系卫星系统动力学模型以及在二体绳系卫星系统节点卫星上分别进行惯性传感器布局，具体为：

定义地心惯性坐标系O-XYZ，其原点O固连于地心，OX轴位于赤道面内并指向春分点方向，OZ轴指向地球北极，OY轴由右手螺旋法则确定；对二体绳系卫星系统做以下假设：

a.相对于系绳长度，二体绳系卫星系统节点卫星尺寸忽略不计，将卫星视为质点；

b.不考虑系绳弹性，将柔性系绳离散为由一系列珠点和刚性杆连接而成；

c.二体绳系卫星系统运行于圆形开普勒轨道，除地球引力，其他外界干扰均忽略；

按照牛顿法建立的系统动力学方程为：

令为状态变量，则系统动力学方程变形为

考虑二体绳系卫星系统噪声，将上式转换为函数形式

式中，系统噪声Γ～N(0，Q^(Г))服从高斯分布，协方差矩阵

二体绳系卫星系统两端卫星的绝对位置r_i ^(g)分别由其上的惯性传感器直接确定，表示为

r_i ^(g)＝r_i+v^(g)

式中，表示卫星的期望实际绝对位置，/>表示惯性传感器测量噪声；

将系绳的绳长约束表示为

Z⁽¹⁾＝L(q)+v⁽¹⁾

式中，表示虚拟噪声；

考虑二体绳系卫星系统的惯性传感器布局，基于伪测量法，合并系统的测量方程可得到

式中,γ_k～N(0，R^(γ))表示测量方程扩展的测量噪声，R^(γ)表示扩展后的测量噪声协方差，z^(g)(X)由下式确定

z^(g)(X)＝H_gX^(g)+V^(g)

式中，测量噪声服从高斯分布，/>表示为

式中，为3×3单位矩阵；

将二体绳系卫星系统的动力学方程和测量方程联立可得二体绳系卫星系统的状态方程为

步骤2)具体为：

2.1)对二体绳系卫星系统的状态方程进行离散化处理

式中，△T为系统采样时间；

2.2)设计滤波算法

所述滤波算法为改进型平方根无迹卡尔曼滤波算法，包括：多步状态预测部分和状态更新部分，具体如下：

2.2.1)Sigma点计算

相应的权值为：

式中，尺度因子λ＝α²(n+κ)-n，κ为常数，α为控制Sigma点分布的常数，取值范围为1e-4≤α≤1；

2.2.2)多步状态预测

2.2.3)时间更新

Ω_k＝h(Θ_k)

2.2.4)测量更新

步骤3)具体为：

采用三次B样条曲线对获得的系绳珠点三维位置状态进行拟合；

经状态估计后得到系绳珠点的三维坐标后采用三次B样条曲线拟合的形式对得到的数据点进行处理；利用三次B样条曲线拟合原理对坐标点进行拟合，其原理如下：

已知系绳的珠点三维坐标组成一个点集{r₁，r₂，…r_n-11，r_n}，取此点集前4个离散点并重新记为B₀，B₁，B₂，B₃，假设其坐标分别为(x₀，y₀，z₀)，(x₁，y₁，z₁)，(x₂，y₂，z₂)和(x₃，y₃，z₃)；要使拟合后的曲线起始于B₀，终止于B₃，需要在起始端与终止端分别增加点B_s＝2B₀-B₁和B_e＝2B₃-B₂，然后组成新的点序列为B_s，B₀，B₁，B₂，B₃，B_e为控制点拟合三次B样条曲线；

同理，依次构建三次B样条曲线段序列{B_n，B_n+1，B_n+2，…，B_n+x}，其中第k个三次B样条曲线可表示为

其中，B_k(u)为B样条曲线上的任意坐标点位置矢量；u为参量，且u∈[0，1]，分量式为

式中：

最后，根据获得系绳的拟合曲线即可求得系绳在绝对坐标系下的横向摆角和纵向摆角。

2.根据权利要求1所述最少传感器布局下的大尺度柔性系绳三维摆角高精度估计方法，其特征在于：

二体绳系卫星系统两端卫星上均设置有一个惯性传感器，所述惯性传感器为GPS传感器。