CN110196445B - 系绳约束下的空间三体系统多传感器精确定位方法 - Google Patents

Abstract

本发明提出一种系绳约束下的空间三体系统多传感器精确定位方法，以封闭的旋转三角形绳系卫星编队系统为模型展开研究，首先获取编队系统各个卫星中的星载全球定位系统传感器数据；根据绳系系统动力学模型建立状态方程，将系绳约束加入观测方程，基于扩展卡尔曼滤波器对全球定位系统的多个传感器的输出信号进行融合。该方法与常规多传感器数据融合方法相比在以下方面存在优势：(1)本方法将系统的物理约束加入数据融合模型，可有效提高多传感器数据融合的效果；(2)本方法可将多个低成本低精度的GPS传感器组合使用通过数据融合用于实现高精度的定位。

Description

系绳约束下的空间三体系统多传感器精确定位方法

技术领域

本发明涉及一种基于全球定位系统多传感器数据融合处理的方法，特别是一种在弹性系绳约束下的空间三体系统的基于扩展卡尔曼滤波的多传感器精确定位方法。

背景技术

空间系绳在灵活性、生命周期和功能拓展性等方面优势显著，成为近年来国内外航天领域的研究热点之一。空间三体系统是由三根空间系绳将空间航天器逐一连接而成的闭环新型空间系统，其在科学实验、空间资源探索、对地观测以及空间攻防等任务中已经开始发挥作用，具有广泛的应用前景。

实时高精度航天器定位技术不仅是航天器实时监控和跟踪的重要技术保障，同时也是实时调整和补偿航天器姿态的主要依据。作为航天领域的一项基础技术，其在航天器交会对接、在轨维护、目标捕获、群体空间机器人构型保持与重构以及太空攻防等空间任务的成功实施中是不可缺少的前提。受观测目标信息不完备、空间环境干扰大等因素的影响，实时高精度航天器的定位尚未找到较好的解决方案，一直是国内外学者研究的热点难题之一。

随着GPS等全球定位系统的开发和利用，为航天器导航定位提供了全新的方式。与传统的陀螺仪、星敏感器等位姿测量传感器相比，星载全球定位系统能够提供较为准确、连续完整的导航、定位服务。因此基于星载全球定位系统接收机定位技术逐渐发展成为低轨道航天器定位的有效途径。但全球定位系统定位技术依然存在定位精度有限、算法的解算效率较低等关键问题，因此产生了多种全球定位系统的定位方法：(1)单独使用全球定位系统进行定位的独立解算方法；(2)将全球定位系统测量信息与航天器姿态运动信息或其它传感器测量信息进行组合的滤波定位算法，但这些方法在航天器应用上依然存在着许多局限性。

因此，本发明提出将多个低精度的全球定位系统传感器组合使用，基于滤波算法实现全球定位系统多传感器数据融合，对于实现地球低轨道航天器的高精度定位具有重要的应用价值。

发明内容

本发明的目的是实现系绳约束下的空间三体系统的精确定位。以封闭的旋转三角形绳系卫星编队系统为模型展开研究，首先获取编队系统各个卫星中的星载全球定位系统传感器数据；根据绳系系统动力学模型建立状态方程，将系绳约束加入观测方程，基于扩展卡尔曼滤波器对全球定位系统的多个传感器的输出信号进行融合。

为了实现上述目的，本发明所采用的技术方案为：

所述一种系绳约束下的空间三体系统多传感器精确定位方法，其特征在于：包括以下步骤：

步骤1：构造空间三体系统的动力学模型：

建立的空间三体系统的动力学模型为

其中X＝[r₁ r₂ r₃]^T，

r_i＝[x_i y_i z_i]^T(i＝1,2,3)为空间旋转绳系卫星系统中三颗卫星质点在地心惯性坐标系中的位置矢量，μ为引力常数；所述空间旋转绳系卫星系统由三根系绳和三颗卫星组成，并且卫星通过系绳连接起来形成闭合的平面三角形状；

步骤2：基于细绳约束条件建立观测模型：

根据空间三体系统在运动的过程中系绳长度保持不变，建立观测模型为

Z＝H(X)

其中

H(X)＝[(r₁-r₂)² (r₂-r₃)² (r₁-r₃)²]^T，l₁、l₂、l₃为三根系绳长度；

步骤3：根据步骤1和步骤2得到的空间三体系统的动力学模型和观测模型，建立空间三体系统的状态方程和观测方程如下：

其中W(k)为过程噪声，其为均值为0，方差为Q的高斯白噪声；V(k+1)为观测噪声，其为均值为0，方差为R的高斯白噪声；

根据空间三体系统的状态方程和观测方程，采用扩展卡尔曼滤波算法对空间三体系统中每个卫星上的星载全球定位系统接收机的数据进行融合，实现对空间三体系统中的三个卫星位置精确定位。

进一步的优选方案，所述一种系绳约束下的空间三体系统多传感器精确定位方法，其特征在于：步骤3中根据空间三体系统的状态方程和观测方程，采用扩展卡尔曼滤波算法对空间三体系统中每个卫星上的星载全球定位系统接收机的数据进行融合的具体步骤为：

步骤3.1：初始化初始状态X(0)、Z(0)、协方差矩阵P(0)；

步骤3.2：确定状态方程

步骤3.3：确定观测方程Z(k+1)＝H(X(k+1))+V(k+1)；

步骤3.4：一阶线性化状态方程，将

一阶线性化处理后，得到X(k+1|k)＝f[k,X(k)]+W(k)，求解状态转移矩阵

步骤3.5：一阶线性化观测方程，将Z(k+1)＝H(X(k+1))+V(k+1)一阶线性化处理后，得到Z(k+1)＝h[k+1,X(k+1)]+V(k+1)，求解观测矩阵

步骤3.6：求协方差矩阵P(k+1|k)＝Φ(k+1)P(k|k)Φ^T(k+1)+Q；

步骤3.7：求卡尔曼滤波增益K(k+1)＝P(k+1|k)Θ^T(k)/[Θ(k)P(k+1|k)Θ^T(k)+R]；

步骤3.8：求状态更新X(k+1)＝X(k+1|k)+K(k+1)[Z(k+1)-Z(k)]；

步骤3.9：协方差更新P(k+1)＝[I_n-K(k+1)Θ(k+1)]P(k+1|k)，其中矩阵I_n为单位矩阵。

有益效果

本专利给出了系绳约束下的多星载全球定位系统接收机基于扩展卡尔曼滤波的数据融合的方法。该方法与常规多传感器数据融合方法相比在以下方面存在优势：(1)本方法将系统的物理约束加入数据融合模型，可有效提高多传感器数据融合的效果；(2)本方法可将多个低成本低精度的GPS传感器组合使用通过数据融合用于实现高精度的定位。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1空间三体系统的系统示意图；

图2基于扩展卡尔曼滤波实现多GPS传感器信息融合技术路线图。

具体实施方式

下面详细描述本发明的实施例，所述实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

本实施例的目的是实现系绳约束下的空间三体系统的精确定位。以封闭的旋转三角形绳系卫星编队系统为模型展开研究，首先获取编队系统各个卫星中的星载全球定位系统传感器数据；根据绳系系统动力学模型建立状态方程，将系绳约束加入观测方程，基于扩展卡尔曼滤波器对全球定位系统的多个传感器的输出信号进行融合。

为了实现上述目的，本实施例中的技术方案包括以下步骤：

步骤一：构造空间三体系统的动力学模型作为预测模型；

步骤二：基于细绳约束条件设计观测模型；

步骤三：设计扩展卡尔曼滤波算法，实现多GPS传感器的数据融合。

所述的步骤一中，为建立空间三体系统的动力学模型，做如下假设并定义如下坐标系：

(一)系统描述及建模假设

发明所研究的空间旋转绳系卫星系统由三根系绳和三颗卫星组成，并且卫星通过系绳连接起来形成闭合的平面三角形状。对该系统进行建模时，为了简化，同时又保证模型的准确程度，采取的假设有：

(1)卫星尺寸相对于整个系统可以忽略，将三颗卫星视为质点；

(2)三根系绳质量相对于卫星可忽略不计，不考虑系绳阻尼、弯曲变形等；

(3)整个系统只受到地球的万有引力，忽略太阳光压、电磁力等外力；

(4)系统在绕地球的圆轨道上做轨道运动，且系统绕地球旋转角速度为ω。

(二)空间三体系统坐标及物理量的定义

为对空间三体系统进行动力学建模，定义地心惯性坐标系O-XYZ,其原点O固连于地心，OX轴位于赤道面内并指向春分点方向，OZ轴指向地球北极，OY轴由由右手螺旋法则确定。系统中相关的物理量定义如下：

(1)三颗卫星质量分别为m₁、m₂、m₃；

(2)三根系绳长度分别为l₁、l₂、l₃。

(三)空间三体系统的动力学模型

对该系统采用拉格朗日法建立动力学模型，选取三颗卫星的在地心惯性坐标系下的位置坐标为广义坐标，则广义坐标定义为：

q_j＝[x₁ y₁ z₁ x₂ y₂ z₂ x₃ y₃ z₃]^T

卫星质点在地心惯性坐标系中的位置矢量可表示为：

(i＝1,2,3)

系统的动能包括三颗卫星质点的动能，可表示为：

系统的势能包括三个卫星所具有的重力势能，可表示为：

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

将系统的动能T与势能V带入拉格朗日方程：

其中，q_j为系统的广义坐标；Q_j为各自由度所对的非保守广义外力，由系统所受外力和外力矩产生，此处假设Q_j＝0。

具体的公式推导过程如下：

由系统的广义坐标q_j＝[x₁ y₁ z₁ x₂ y₂ z₂ x₃ y₃ z₃]^T可得：

又知系统的动能为

将

(i＝1,2,3)带入系统的动能可得到：

可得

于是可得：

由系统的势能

其中：

所以

从而求得：

将

带入拉格朗日动力学方程可得：

将上式整理并化简可得到：

式中：r_i＝[x_i y_i z_i]^T(i＝1,2,3)为卫星质点在地心惯性坐标系中的位置矢量。

令X＝[r₁ r₂ r₃]^T,

则上式可表示为：

所述的步骤二中，基于系绳约束建立观测模型，由于设定空间三体系统在运动的过程中系绳长度保持不变，于是可得：

展开可得：

令：

H(X)＝[(r₁-r₂)² (r₂-r₃)² (r₁-r₃)²]^T

则上式可表示为：Z＝H(X)。

所述的步骤三中，将步骤一、步骤二得到的动力学模型和观测模型处理后可得到空间三体系统的状态方程和观测方程如下：

式中：W(k)为过程噪声，其为均值为0，方差为Q的高斯白噪声；

V(k+1)为观测噪声，其也为均值为0，方差为R的高斯白噪声。

系统的状态方程和观测方程均为非线性方程，因此本发明采用扩展卡尔曼滤波算法来实现多GPS传感器的数据融合。具体步骤如下：

步骤3.1：初始化初始状态X(0)、Z(0)、协方差矩阵P(0)；

步骤3.2：确定状态方程

步骤3.3：确定观测方程Z(k+1)＝H(X(k+1))+V(k+1)；

步骤3.4：一阶线性化状态方程，将

一阶线性化处理后，得到X(k+1|k)＝f[k,X(k)]+W(k)，求解状态转移矩阵

步骤3.5：一阶线性化观测方程，将Z(k+1)＝H(X(k+1))+V(k+1)一阶线性化处理后，得到Z(k+1)＝h[k+1,X(k+1)]+V(k+1)，求解观测矩阵

步骤3.6：求协方差矩阵P(k+1|k)＝Φ(k+1)P(k|k)Φ^T(k+1)+Q；

步骤3.7：求卡尔曼滤波增益K(k+1)＝P(k+1|k)Θ^T(k)/[Θ(k)P(k+1|k)Θ^T(k)+R]；

步骤3.8：求状态更新X(k+1)＝X(k+1\k)+K(k+1)[Z(k+1)-Z(k)]；

步骤3.9：协方差更新P(k+1)＝[I_n-K(k+1)Θ(k+1)]P(k+1|k)，其中矩阵I_n为单位矩阵。

以上为设计的扩展卡尔曼滤波器的一个计算周期，不断循环这个计算周期过程就可以实现多GPS传感器的数据融合，从而实现空间三体系统的高精度定位。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (1)

1.一种系绳约束下的空间三体系统多传感器精确定位方法，其特征在于：包括以下步骤：

步骤1：构造空间三体系统的动力学模型：

建立的空间三体系统的动力学模型为

其中X＝[r₁ r₂ r₃]^T，

r_i＝[x_i y_i z_i]^T(i＝1,2,3)为空间旋转绳系卫星系统中三颗卫星质点在地心惯性坐标系中的位置矢量，μ为引力常数；所述空间旋转绳系卫星系统由三根系绳和三颗卫星组成，并且卫星通过系绳连接起来形成闭合的平面三角形状；

步骤2：基于细绳约束条件建立观测模型：

根据空间三体系统在运动的过程中系绳长度保持不变，建立观测模型为

Z＝H(X)

其中

H(X)＝[(r₁-r₂)² (r₂-r₃)² (r₁-r₃)²]^T，l₁、l₂、l₃为三根系绳长度；

步骤3：根据步骤1和步骤2得到的空间三体系统的动力学模型和观测模型，建立空间三体系统的状态方程和观测方程如下：

其中W(k)为过程噪声，其为均值为0，方差为Q的高斯白噪声；V(k+1)为观测噪声，其为均值为0，方差为R的高斯白噪声；

根据空间三体系统的状态方程和观测方程，采用扩展卡尔曼滤波算法对空间三体系统中每个卫星上的星载全球定位系统接收机的数据进行融合，实现对空间三体系统中的三个卫星位置精确定位，具体步骤为：

步骤3.1：初始化初始状态X(0)、Z(0)、协方差矩阵P(0)；

步骤3.2：确定状态方程

步骤3.3：确定观测方程Z(k+1)＝H(X(k+1))+V(k+1)；

步骤3.4：一阶线性化状态方程，将

一阶线性化处理后，得到X(k+1|k)＝f[k,X(k)]+W(k)，求解状态转移矩阵

步骤3.5：一阶线性化观测方程，将Z(k+1)＝H(X(k+1))+V(k+1)一阶线性化处理后，得到Z(k+1)＝h[k+1,X(k+1)]+V(k+1)，求解观测矩阵

步骤3.6：求协方差矩阵P(k+1|k)＝Φ(k+1)P(k|k)Φ^T(k+1)+Q；

步骤3.7：求卡尔曼滤波增益K(k+1)＝P(k+1|k)Θ^T(k)/[Θ(k)P(k+1|k)Θ^T(k)+R]；

步骤3.8：求状态更新X(k+1)＝X(k+1|k)+K(k+1)[Z(k+1)-Z(k)]；

步骤3.9：协方差更新P(k+1)＝[I_n-K(k+1)Θ(k+1)]P(k+1|k)，其中矩阵I_n为单位矩阵。

Images