[发明专利]一种集群空间机器人主从式目标协同定位方法

摘要

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

主权项

1.一种集群空间机器人主从式目标协同定位方法，其特征在于：包括如下步骤：步骤1，根据集群空间机器人的相对测量设备配置方案，建立集群内的测量方程：ρ‾mt(tk)=ρmt(tk)+wmt(tk)ρ‾mci(tk)=ρmci(tk)+wmci(tk)---(1)]]>其中ρ_mt(t_k)和ρ_mci(t_k)分别表示t_k时刻目标航天器和从空间机器人Ci相对主空间机器人M的3维位置矢量，下标m表示主空间机器人，下标ci表示从空间机器人，下标t表示目标航天器，表示测量值，w_mt(t)和w_mci(t_k)为3维测量位置误差矢量；假设测量噪声为零均值高斯白噪声，且两个测量噪声不相关，即：E[wmt(tk)]=E[wmci(tk)]=0E[wmt(tk)wmtT(tj)]=RkmtδkjE[wmci(tk)wmciT(tj)]=RkmciδkjE[wmt(tk)wmciT(tj)]=0---(2)]]>其中，为目标航天器和从空间机器人Ci测量噪声的非负定方差矩阵，δ_kj为Kronecker-δ函数；步骤2，建立主空间机器人M、Ci和目标航天器的相对动力学模型；设目标航天器运行在圆轨道上，忽略地球扁率、太阳光压等摄动因素，以目标航天器轨道坐标系∑Ft作为建模参照，当目标航天器与集群空间机器人处于近距离时，将从空间机器人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θ，则状态转移矩阵为：Φtci(Δt)=sinθ06(θ-Sθ)(4Sθ-3θ)ω0(1-Cθ)ω0Cθ00Sθω0004-3Cθ-2(1-Cθ)ω0Sθω006ω(1-Cθ)4Cθ-302Sθ0-ωSθ00Cθ0003ωSθ-2Sθ0---(4)]]>步骤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）建立关于相对状态L_tci的间接量测方程为：Ztci(tk)=Htci(Xtm(tk)+Xmci(tk))+wtci(tk)]]>=(ρtm+ρmci)+wtci(tk)---(7)]]>=ρ‾tm(tk)+ρ‾tci(tk)]]>其中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的线性状态空间模型；步骤3.3，根据线性Kalman滤波理论，通过迭代进行状态更新和状态修正，获得X_tci的最优估计从而获得目标航天器相对从空间机器人Ci的位置信息；步骤3.3.1，主空间机器人M完成相对状态和滤波协方差的一步预测；相对状态的一步预测：X~tci(tk+1)=Φtci(tk+1,tk)X^tci(tk)---(8)]]>滤波估计协方差矩阵的一步预测：P_tci(t_k+1,t_k)=Φ_tciP_tci(t_k)Φ_tci+Q_tci（9）其中，P_tci为滤波协方差矩阵；步骤3.3.2，主空间机器人M根据步骤3.3.1得到的一步预测值，对滤波增益矩阵、相对状态和滤波估计协方差矩阵进行滤波修正；滤波增益矩阵为：Ktci(tk+1)=Ptci(tk+1,tk)HtciTHtciT(HtciPtci(tk+1,tk)HtciT+Rktci)-1---(10)]]>相对状态的修正方程为：滤波估计协方差矩阵为：P_tci(t_k+1)=[I-K_tci(t_k+1)H_tci]P_tci(t_k+1,t_k)   （12）其中，L_tci为滤波增益矩阵；步骤3.3.3，主空间机器人M根据步骤3.3.2得到的最优估计计算L_k+1时刻目标航天器相对从空间机器人Ci的相对位置：ρtci(tk+1)=HtciX^tci(tk+1)---(13)]]>主空间机器人M通过星间链路将ρ_tci(t_k+1)发给从空间机器人Ci,同时将最优估计返回步骤3.3.1，按照步骤3.3.1-步骤3.3.3所述方法，进行下一个时刻相对位置的计算。