CN102981175B - 一种集群空间机器人主从式目标协同定位方法 - Google Patents

Abstract

本发明涉及一种集群空间机器人主从式目标协同定位方法，属于航天器导航技术领域。本发明充分利用集群内成员和目标航天器之间的几何关系，建立不具备测量功能的成员与目标航天器之间相对位置的间接量测方程；主空间机器人M结合目标航天器和从空间机器人Ci与自身的相对位置信息，在远距离阶段为从空间机器人Ci提供目标相对位置信息，再采用Kalman滤波理论实现了协同定位。本发明所采用的集群空间机器人主从式目标协同定位方法，可以明显减少集群内相对测量设备的数量，降低各成员系统复杂程度，提高其可靠性。

Description

一种集群空间机器人主从式目标协同定位方法

技术领域

本发明涉及一种集群空间机器人主从式目标协同定位方法，属于航天器导航技术领域。

技术背景

针对空间大规模航天器的在轨组装、在轨维护等技术是未来在轨服务领域内的重要发展方向，大规模航天器在轨操作任务极为复杂，单个空间机器人系统难以独立完成任务，而由多个成员组成的集群空间机器人进行协同操作，则是完成此类任务的有效途径。对目标航天器的协同定位是实现协同操作的前提条件。目标航天器的协同定位是指集群空间机器人通过集群内测量信息的共享，为不具备目标测量功能的成员空间机器人提供目标的相对位置信息。协同定位通过数据融合可为集群内成员空间机器人系统提供连续的、高精度的目标相对信息，同时可有效简化集群内测量设备的配置，此外协同定位具有高精度和高可靠的特点，因而地面移动机器人、水下无人航行器等领域得到了广泛的重视。然而，针对多航天器协同相对导航与定位问题的相关研究却十分少见，即便是在卫星编队技术领域，研究内容也仅限于动力学及协同控制问题，但是作为编队协同控制基础的协同导航定位问题却未提及。

发明内容

本发明针对集群空间机器人在轨协同操作任务，发明了一种集群空间机器人的主从式目标协同定位方法，采用该方法可以有效简化集群内的相对测量设备配置，同时能够为集群内不具备测量功能的成员提供目标航天器的相对位置信息。

本发明的特点是充分利用集群内成员和目标航天器之间的几何关系，建立不具备测量功能的成员与目标航天器之间相对位置的间接量测方程；主空间机器人M结合目标航天器和从空间机器人Ci与自身的相对位置信息，在远距离阶段为从空间机器人Ci提供目标相对位置信息，再采用Kalman滤波理论实现了协同定位。

本发明的集群空间机器人主从式目标协同定位方法具体包括如下步骤：

步骤1，根据集群空间机器人的相对测量设备配置方案，建立集群内的测量方程：

$\left\{\begin{array}{c}{\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{mt}}\left(t_k\right)={\mathrm{\ρ}}_{\mathrm{mt}}\left(t_k\right)+w_{\mathrm{mt}}\left(t_k\right)\\ {\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{mci}}\left(t_k\right)={\mathrm{\ρ}}_{\mathrm{mci}}\left(t_k\right)+w_{\mathrm{mci}}\left(t_k\right)\end{array}\right.---\left(1\right)$

其中ρ_mt(t_k)和ρ_mci～(t_k)分别表示t_k时刻目标航天器和从空间机器人Ci相对主空间机器人M的3维位置矢量，下标m表示主空间机器人，下标ci表示从空间机器人，下标t表示目标航天器，

表示测量值，w_mt(t)和w_mci(t_k)为3维测量位置误差矢量，误差的统计特性通过测量设备地面试验获得。

假设测量噪声为零均值高斯白噪声，且两个测量噪声不相关，即：

$\left\{\begin{array}{c}E\left[w_{\mathrm{mt}}\left(t_k\right)\right]=E\left[w_{\mathrm{mci}}\left(t_k\right)\right]=0\\ E\left[w_{\mathrm{mt}}\left(t_k\right)w_{\mathrm{mt}}^T\left(t_j\right)\right]=R_k^{\mathrm{mt}}{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[w_{\mathrm{mci}}\left(t_k\right)w_{\mathrm{mci}}^T\left(t_j\right)\right]=R_k^{\mathrm{mci}}{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[w_{\mathrm{mt}}\left(t_k\right)w_{\mathrm{mci}}^T\left(t_j\right)\right]=0\end{array}\right.---\left(2\right)$

其中，

为目标航天器和从空间机器人Ci测量噪声的非负定方差矩阵，δ_kj为Kronecker-δ函数。

所述相对测量设备配置方案为：集群空间机器人由主空间机器人M和从空间机器人Ci（i＝1，2，..，N）组成,其中主空间机器人M安装远距离和近距离测量设备，对目标航天器相对位置信息进行全程高精度测量；从空间机器人Ci只安装满足近距离操作要求的测量设备，无法在远距离条件下直接测得目标航天器相对位置信息；集群内成员均安装GPS接收机，通过星间数据链路并采用差分GPS算法实现精确相对定位。

步骤2，建立主空间机器人M、Ci和目标航天器的相对动力学模型。

假设目标航天器运行在圆轨道上，忽略地球扁率、太阳光压等摄动因素，以目标航天器轨道坐标系∑Ft作为建模参照，当目标航天器与集群空间机器人处于近距离（小于50km）时，集群内任意成员与目标航天器的相对轨道动力学可由线性化的C-W方程描述，将从空间机器人Ci与目标航天器的相对动力学模型用离散化的状态转移方程表示为：

X_tci(t_k+1)=Φ_tci(t_k+1,t_k)X_tci(t_k)   （3）

其中为相对位置和相对速度组成的向量；Φ_6×6表示状态转移矩阵，根据C-W方程和目标航天器的轨道角速度ω，令θ=ωΔt，Δt=t_k+1-t_k，S_θ=sinθ，C_θ=cosθ，则状态转移矩阵为：

${\mathrm{\Φ}}_{\mathrm{tci}}\left(\mathrm{\Δt}\right)=\left[\begin{array}{cccccc}\sin \mathrm{\θ}& 0& 6(\mathrm{\θ}-S_{\mathrm{\θ}})& \frac{({4S}_{\mathrm{\θ}}-3\mathrm{\θ})}{\mathrm{\ω}}& 0& \frac{(1-C_{\mathrm{\θ}})}{\mathrm{\ω}}\\ 0& C_{\mathrm{\θ}}& 0& 0& \frac{S_{\mathrm{\θ}}}{\mathrm{\ω}}& 0\\ 0& 0& 4-3C_{\mathrm{\θ}}& -\frac{2(1-C_{\mathrm{\θ}})}{\mathrm{\ω}}& 0& \frac{S_{\mathrm{\θ}}}{\mathrm{\ω}}\\ 0& 0& 6\mathrm{\ω}(1-C_{\mathrm{\θ}})& {4C}_{\mathrm{\θ}}-3& 0& {2S}_{\mathrm{\θ}}\\ 0& -\mathrm{\ω}{S}_{\mathrm{\θ}}& 0& 0& C_{\mathrm{\θ}}& 0\\ 0& 0& 3\mathrm{\ω}{S}_{\mathrm{\θ}}& -2S_{\mathrm{\θ}}& 0& \end{array}\right]---\left(4\right)$

步骤3，以线性Kalman滤波理论为基础，建立关于目标航天器与从空间机器人Ci相对状态X_tci的线性状态空间模型。线性状态空间模型包括两部分，一部分是以X_tci为系统状态的动力学方程，用于完成相对状态X_tci的预测更新；另一部分为关于相对状态X_tci的量测方程，用于完成对相对状态的修正。

步骤3.1，考虑系统动力学噪声，描述目标航天器与从空间机器人Ci的相对动力学特性的动力学方程为：

X_tci(t_k+1)=Φ_tci(t_k+1,t_k)X_tci(t_k)+G_tciγ_tci(k)（5）

其中γ_tci(k)为系统动力学噪声，G_tci为动力学噪声输入矩阵，满足零均值高斯白噪声假设，噪声非负定方差矩阵为Q_tci，

步骤3.2，利用主从空间机器人和目标航天器间的几何关系：

X_tci(t_k)=X_tm(t_k)+X_mci(t_k)（6）

建立关于相对状态X_tci的间接量测方程为：

$Z_{\mathrm{tci}}\left(t_k\right)=H_{\mathrm{tci}}(X_{\mathrm{tm}}\left(t_k\right)+X_{\mathrm{mci}}\left(t_k\right))+w_{\mathrm{tci}}\left(t_k\right)$

$=({\mathrm{\ρ}}_{\mathrm{tm}}+{\mathrm{\ρ}}_{\mathrm{mci}})+w_{\mathrm{tci}}\left(t_k\right)---\left(7\right)$

$={\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{tm}}\left(t_k\right)+{\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{tci}}\left(t_k\right)$

其中H_tci=[I_3×3 0]，w_tci(t_k)为等效量测噪声：

w_tci(t_k)=w_tm(t_k)+w_mci(t_k)

联合式（5）和式（7），建立关于X_tci的线性状态空间模型。

相对位置信息由主空间机器人M测量目标航天器得到；从空间机器人Ci通过GPS接收机获得自身在地心惯性坐标系内的位置信息，并通过星间数据链路将位置信息发送给主空间机器人M；主空间机器人M将自身GPS位置信息和接收到的从空间机器人GPS位置数据做差分运算，得到两者相对位置信息

步骤3.3，根据线性Kalman滤波理论，通过迭代进行状态更新和状态修正，获得X_tci的最优估计

从而获得目标航天器相对从空间机器人Ci的位置信息。

步骤3.3.1，主空间机器人M完成相对状态和滤波协方差的一步预测；

相对状态的一步预测： ${\tilde{X}}_{\mathrm{tci}}\left(t_{k+1}\right)={\mathrm{\Φ}}_{\mathrm{tci}}(t_{k+1},t_k){\hat{X}}_{\mathrm{tci}}\left(t_k\right)---\left(8\right)$

滤波估计协方差矩阵的一步预测：P_tci(t_k+1,t_k)=Φ_tciP_tci(t_k)Φ_tci+Q_tci（9）

其中，P_tci为滤波协方差矩阵。

步骤3.3.2，主空间机器人M根据步骤3.3.1得到的一步预测值，对滤波增益矩阵、相对状态和滤波估计协方差矩阵进行滤波修正。

滤波增益矩阵为： $K_{\mathrm{tci}}\left(t_{k+1}\right)=P_{\mathrm{tci}}(t_{k+1},t_k)H_{\mathrm{tci}}^T{H}_{\mathrm{tci}}^T{(H_{\mathrm{tci}}{P}_{\mathrm{tci}}(t_{k+1},t_k)H_{\mathrm{tci}}^T+R_k^{\mathrm{tci}})}^{-1}---\left(10\right)$

相对状态的修正方程为：

${\hat{X}}_{\mathrm{tci}}\left(t_{k+1}\right)={\tilde{X}}_{\mathrm{tci}}(t_{k+1},t_k)+K_{\mathrm{tci}}[Z_{\mathrm{tci}}\left(t_{k+1}\right)-H_{\mathrm{tci}}{\tilde{X}}_{\mathrm{tci}}(t_{k+1},t_k)]---\left(11\right)$

滤波估计协方差矩阵为：P_tci(t_k+1)=[I-K_tci(t_k+1)H_tci]P_tci(t_k+1,t_k)   （12）

其中，K_tci为滤波增益矩阵。

步骤3.3.3，主空间机器人M根据步骤3.3.2得到的最优估计

计算t_k+1时刻目标航天器相对从空间机器人Ci的相对位置：

${\mathrm{\ρ}}_{\mathrm{tci}}\left(t_{k+1}\right)=H_{\mathrm{tci}}{\hat{X}}_{\mathrm{tci}}\left(t_{k+1}\right)---\left(13\right)$

主空间机器人M通过星间链路将ρ_tci(t_k+1)发给从空间机器人Ci,同时将最优估计

返回步骤3.3.1，按照步骤3.3.1-步骤3.3.3所述方法，进行下一个时刻相对位置的计算。

有益效果

采用集群空间机器人主从式目标协同定位方法，可以明显减少集群内相对测量设备的数量，降低各成员系统复杂程度，提高其可靠性。

附图说明

图1为本发明的集群空间机器人组成示意图；

图2为本发明的协同定位方法中的滤波迭代流程图；

图3为实施实例中从空间机器人C1的标称绕飞轨迹；

图4为实施实例的目标协同定位轨迹；

图5为实施实例的目标协同定位误差。

具体实施方式

为详细说明本发明的目的和优点，现结合附图和实施实例进行进一步说明。

[实施实例初始条件]

目标航天器运行在高度为600km的圆轨道上，集群空间机器人由主空间机器人M和多个从空间机器人Ci组成，其中主空间机器人M安装有微波雷达、可见光相机、GPS接收机；

主空间机器人M可采用微波雷达在500-10000m的范围内对目标航天器相对位置进行测量；

主空间机器人M可采用GPS对自身在地心惯性坐标系内定位；

主空间机器人M可通过集群内星间链路与其他从空间机器人进行数据交互；

主空间机器人初始位置位为[-400m，0m，0m]，主空间机器人对目标航天器和从空间机器人C1的相对位置进行测量，各向测量精度均为5m(3σ)，且各向测量噪声均不相关。

从空间机器人C1的主要功能是完成对目标航天器的维修操作，在目标距离大于10m的条件下，不具备测量能力；

从空间机器人C1安装有GPS接收机，可获得自身在地心惯性坐标系内的位置数据，同时能够通过星间数据链路将GPS数据发送给主空间机器人M；

从空间机器人C1以目标航天器的轨道坐标系作为参照，从初始位置[-500m，0m，0m]开始，按照C-W制导模式沿空间椭圆对目标航天器实施绕飞观测，且在绕飞过程当中运动不受控制，从空间机器人C1的标称绕飞轨迹如图3所示，仿真时长为一个轨道周期。

[主从式目标协同定位方法实施步骤]

1、根据目标航天器的轨道高度，求得目标航天器轨道角速度ω；

2、将ω代入式（4），获得动力学方程中的状态转移矩阵Φ_tc1；

3、按照式式（5）和式（7）建立系统的线性状态空间；

4、按照现行卡曼滤波迭代过程的要求，给定滤波迭代所需要的的初始状态X_tc1(t₀)、初始滤波估计方差矩阵P_to1(t₀)。

5、给定测量误差方差矩阵

给定系统动力学噪声方差矩阵Q_tc1；

6、根据式（8）获得相对状态的一步预测：

7、根据式（9）获得滤波估计协方差矩阵的一步预测：

P_tc1(t₁,t₀)=Φ_tc1P_tc1(t₀)Φ_tc1+Q_tc1

8、根据式（10）获得滤波增益矩阵：

$K_{\mathrm{tc}1}\left(t_1\right)=P_{\mathrm{tc}1}(t_1,t_0)H_{\mathrm{tc}1}^T{(H_{\mathrm{tc}1}{P}_{\mathrm{tc}1}(t_1,t_0)H_{\mathrm{tc}1}^T+R^{\mathrm{tc}1})}^{-1}$

9、空间机器人M测量目标航天器并得到其相对位置信息

10、从空间机器人Ci通过GPS接收机获得自身在地心惯性坐标系内的位置信息，并通过星间数据链路将位置信息发送给主空间机器人M。

11、主空间机器人M将自身GPS位置信息和接收到的从空间机器人GPS位置数据做差分运算，得到两者相对位置信息

12、按照式（6）和式（7），获得目标相对从空间机器人位置信息的间接量测值 $Z_{\mathrm{tc}1}\left(t_1\right)={\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{tc}1}\left(t_1\right)+{\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{tm}}\left(t_1\right);$

13、将Z_tci(t₁)代入式（11），主空间机器人M并按照式（11）完成一步预测的修正并得到最优估计

14、按式（13）获得目标航天器相对从空间机器人Ci的位置信息ρ_tc1(t₁)；

15、主空间机器人通过星间数据链路将ρ_tc1(t₁)发送给从空间机器人C1，从而实现集群空间机器人的协同定位。

16、按式（12）完成对滤波估计协方差矩阵一步预测值的修正，得到P_tci(t₁)：

P_tc1(t₁)=[I-K_tc1(t₁)H_tc1]P_tc1(t₁,t₀)

17、转入第6步，重复上述滤波迭代过程。滤波迭代的流程如图2所示。

[协同定位方法实施具体结果说明]

图4是主从式目标协同定位方法获得的从空间机器人C1相对目标航天器的定位轨迹，比较图3和图4可以看出，在初始时刻，由于滤波初始条件给定的状态初始、方差初值与真实值之间存在着较大的偏差，因此定位误差较大，但随着滤波时间的推移，定位轨迹逐渐收敛到标称轨迹附近。图5给出了采用主从式目标协同定位方法时，从空间机器人C1相对目标航天器的协同定位误差，从图中可以看出，协同定位误差在滤波达到稳定时小于5m(3σ)。

Claims (5)

1.一种集群空间机器人主从式目标协同定位方法，其特征在于：包括如下步骤：

步骤1，根据集群空间机器人的相对测量设备配置方案，建立集群内的测量方程：

$\left\{\begin{array}{c}{\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{mt}}\left(t_k\right)={\mathrm{\ρ}}_{\mathrm{mt}}\left(t_k\right)+w_{\mathrm{mt}}\left(t_k\right)\\ {\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{mci}}\left(t_k\right)={\mathrm{\ρ}}_{\mathrm{mci}}\left(t_k\right)+w_{\mathrm{mci}}\left(t_k\right)\end{array}\right.---\left(1\right)$

其中ρ_mt(t_k)和ρ_mci(t_k)分别表示t_k时刻目标航天器和从空间机器人Ci相对主空间机器人M的3维位置矢量，下标m表示主空间机器人，下标ci表示从空间机器人，下标t表示目标航天器，表示测量值，w_mt(t)和w_mci(t_k)为3维测量位置误差矢量；

假设测量噪声为零均值高斯白噪声，且两个测量噪声不相关，即：

$\left\{\begin{array}{c}E\left[w_{\mathrm{mt}}\left(t_k\right)\right]=E\left[w_{\mathrm{mci}}\left(t_k\right)\right]=0\\ E\left[w_{\mathrm{mt}}\left(t_k\right)w_{\mathrm{mt}}^T\left(t_j\right)\right]=R_k^{\mathrm{mt}}{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[w_{\mathrm{mci}}\left(t_k\right)w_{\mathrm{mci}}^T\left(t_j\right)\right]=R_k^{\mathrm{mci}}{\mathrm{\δ}}_{\mathrm{kj}}\\ E\left[w_{\mathrm{mt}}\left(t_k\right)w_{\mathrm{mci}}^T\left(t_j\right)\right]=0\end{array}\right.---\left(2\right)$

其中，为目标航天器和从空间机器人Ci测量噪声的非负定方差矩阵，δ_kj为Kronecker-δ函数；

步骤2，建立主空间机器人M、Ci和目标航天器的相对动力学模型；

设目标航天器运行在圆轨道上，忽略地球扁率、太阳光压等摄动因素，以目标航天器轨道坐标系∑Ft作为建模参照，当目标航天器与集群空间机器人处于近距离时，将从空间机器人Ci与目标航天器的相对动力学模型用离散化的状态转移方程表示为：

X_tci(t_k+1)＝Φ_tci(t_k+1,t_k)X_tci(t_k)             （3）

其中

为相对位置和相对速度组成的向量；Φ_tci(t_k+1,t_k)表示状态转移矩阵，根据C-W方程和目标航天器的轨道角速度ω，令θ＝ωΔt，Δt＝t_k+1-t_k，S_θ＝sinθ，C_θ＝cosθ，则状态转移矩阵为：

${\mathrm{\Φ}}_{\mathrm{tci}}\left(\mathrm{\Δt}\right)=\left[\begin{array}{cccccc}\sin \mathrm{\θ}& 0& 6(\mathrm{\θ}-S_{\mathrm{\θ}})& \frac{(4S_{\mathrm{\θ}}-3\mathrm{\θ})}{\mathrm{\ω}}& 0& \frac{(1-C_{\mathrm{\θ}})}{\mathrm{\ω}}\\ 0& C_{\mathrm{\θ}}& 0& 0& \frac{S_{\mathrm{\θ}}}{\mathrm{\ω}}& 0\\ 0& 0& 4-3C_{\mathrm{\θ}}& -\frac{2(1-C_{\mathrm{\θ}})}{\mathrm{\ω}}& 0& \frac{S_{\mathrm{\θ}}}{\mathrm{\ω}}\\ 0& 0& 6\mathrm{\ω}(1-C_{\mathrm{\θ}})& 4C_{\mathrm{\θ}}-3& 0& 2S_{\mathrm{\θ}}\\ 0& -\mathrm{\ω}{S}_{\mathrm{\θ}}& 0& 0& C_{\mathrm{\θ}}& 0\\ 0& 0& 3\mathrm{\ω}{S}_{\mathrm{\θ}}& -2S_{\mathrm{\θ}}& 0& C_{\mathrm{\θ}}\end{array}\right]---\left(4\right)$

步骤3，建立关于目标航天器与从空间机器人Ci相对状态X_tci的线性状态空间模型；

步骤3.1，考虑系统动力学噪声，描述目标航天器与从空间机器人Ci的相对动力学特性的动力学方程为：

X_tci(t_k+1)＝Φ_tci(t_k+1,t_k)X_tci(t_k)+G_tciγ_tci(k)      （5）

其中γ_tci(k)为系统动力学噪声，G_tci为动力学噪声输入矩阵，满足零均值高斯白噪声假设，噪声非负定方差矩阵为Q_tci，

步骤3.2，利用主从空间机器人和目标航天器间的几何关系：

X_tci(t_k)＝X_tm(t_k)+X_mci(t_k)               （6）

建立关于相对状态X_tci的间接量测方程为：

$\begin{array}{c}{Z}_{\mathrm{tci}}\left(t_k\right)=H_{\mathrm{tci}}(X_{\mathrm{tm}}\left(t_k\right)+X_{\mathrm{mci}}\left(t_k\right))+w_{\mathrm{tci}}\left(t_k\right)\\ =({\mathrm{\ρ}}_{\mathrm{tm}}+{\mathrm{\ρ}}_{\mathrm{mci}})+w_{\mathrm{tci}}\left(t_k\right)\\ ={\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{tm}}\left(t_k\right)+{\stackrel{\‾}{\mathrm{\ρ}}}_{\mathrm{tci}}\left(t_k\right)\end{array}---\left(7\right)$

其中H_tci＝[I_3×3 0_3×3]，w_tci(t_k)为等效量测噪声：

w_tci(t_k)＝w_tm(t_k)+w_mci(t_k)

联合式（5）和式（7），建立关于X_tci的线性状态空间模型；

步骤3.3，根据线性Kalman滤波理论，通过迭代进行状态更新和状态修正，获得X_tci的最优估计

从而获得目标航天器相对从空间机器人Ci的位置信息；

步骤3.3.1，主空间机器人M完成相对状态和滤波协方差的一步预测；

相对状态的一步预测： ${\tilde{X}}_{\mathrm{tci}}\left(t_{k+1}\right)={\mathrm{\Φ}}_{\mathrm{tci}}(t_{k+1},t_k){\hat{X}}_{\mathrm{tci}}\left(t_k\right)---\left(8\right)$

滤波估计协方差矩阵的一步预测：P_tci(t_k+1,t_k)＝Φ_tciP_tci(t_k)Φ_tci+Q_tci   （9）

其中，P_tci为滤波协方差矩阵；

步骤3.3.2，主空间机器人M根据步骤3.3.1得到的一步预测值，对滤波增益矩阵、相对状态和滤波估计协方差矩阵进行滤波修正；

滤波增益矩阵为： $K_{\mathrm{tci}}\left(t_{k+1}\right)=P_{\mathrm{tci}}(t_{k+1},t_k)H_{\mathrm{tci}}^T{(H_{\mathrm{tci}}{P}_{\mathrm{tci}}(t_{k+1},t_k)H_{\mathrm{tci}}^T+R_k^{\mathrm{tci}})}^{-1}---\left(10\right)$

相对状态的修正方程为：

${\hat{X}}_{\mathrm{tci}}\left(t_{k+1}\right)={\tilde{X}}_{\mathrm{tci}}(t_{k+1},t_k)+K_{\mathrm{tci}}[Z_{\mathrm{tci}}\left(t_{k+1}\right)-H_{\mathrm{tci}}{\tilde{X}}_{\mathrm{tci}}(t_{k+1},t_k)]---\left(11\right)$

滤波估计协方差矩阵为：P_tci(t_k+1)＝[I-K_tci(t_k+1)H_tci]P_tci(t_k+1,t_k)    （12）

其中，K_tci为滤波增益矩阵；

步骤3.3.3，主空间机器人M根据步骤3.3.2得到的最优估计

计算t_k+1时刻目标航天器相对从空间机器人Ci的相对位置：

${\mathrm{\ρ}}_{\mathrm{tci}}\left(t_{k+1}\right)=H_{\mathrm{tci}}{\hat{X}}_{\mathrm{tci}}\left(t_{k+1}\right)---\left(13\right)$

主空间机器人M通过星间链路将ρ_tci(t_k+1)发给从空间机器人Ci，同时将最优估计

返回步骤3.3.1，按照步骤3.3.1-步骤3.3.3所述方法，进行下一个时刻相对位置的计算。

2.根据权利要求1所述的一种集群空间机器人主从式目标协同定位方法，其特征在于：所述相对测量设备配置方案为：集群空间机器人由主空间机器人M和从空间机器人Ci组成，i=1，2，..，N,其中主空间机器人M安装远距离和近距离测量设备；从空间机器人Ci只安装满足近距离操作要求的测量设备；集群内成员均安装GPS接收机，通过星间数据链路并采用差分GPS算法实现相对定位。

3.根据权利要求1所述的一种集群空间机器人主从式目标协同定位方法，其特征在于：步骤2所述近距离为小于50km。

4.根据权利要求1所述的一种集群空间机器人主从式目标协同定位方法，其特征在于：所述线性状态空间模型包括两部分，一部分是用于完成相对状态X_tci预测更新的、以X_tci为系统状态的动力学方程；另一部分为用于完成对相对状态修正的、关于相对状态X_tci的量测方程。

5.根据权利要求1所述的一种集群空间机器人主从式目标协同定位方法，其特征在于：所述相对位置信息

由主空间机器人M测量目标航天器得到；从空间机器人Ci通过GPS接收机获得自身在地心惯性坐标系内的位置信息，并通过星间数据链路将位置信息发送给主空间机器人M；主空间机器人M将自身GPS位置信息和接收到的从空间机器人GPS位置数据做差分运算，得到两者相对位置信息

Images