CN103900561A - 一种空间绳系机器人中远距离逼近位置估计方法 - Google Patents

Abstract

本发明提出一种空间绳系机器人中远距离逼近位置估计方法，利用相机进行目标方位角测量，得到目标相对空间绳系机器人的方位角，结合从空间绳系机器人的空间系绳放绳机构获得的空间绳系机器人的系绳长度信息，利用无损卡尔曼滤波方法实现对空间绳系机器人的位置估计。本发明提出的空间绳系机器人中远距离逼近位置估计方法，充分利用了空间绳系机器人的系绳长度信息，结合了单目相机的视场角信息，实现对空间绳系机器人位置进行实时估计。

Description

一种空间绳系机器人中远距离逼近位置估计方法

技术领域

本发明涉及航天器导航技术领域，具体为一种空间绳系机器人中远距离逼近位置估计方法，该方法可以应用于目标逼近过程中，对空间绳系机器人的位置进行估计。

背景技术

空间绳系机器人由于其灵活、安全、燃料消耗低等特点，在空间在轨服务中有着广泛的作用，其中对失效卫星救助、太空垃圾清理等进行抓捕是其主要应用。从空间平台发射后，空间绳系机器人向目标逼近的过程中，需要对自身的位置进行实时控制，从而保证其沿着预定轨迹对目标进行逼近。该过程中，控制所需的位置信息的测量获取是十分重要的，其直接影响着空间绳系机器人逼近抓捕任务的成败，它成为空间绳系机器人领域的关键研究问题。

申请号：201210175411.5公开了一种用于交会对接的测量方法,利用两台照相机对目标飞行器上的标记灯进行成像,进而确定标记灯的位置,通过目标飞行器标记灯和对接口的相对位置关系确定追踪飞行器对接口和目标飞行器对接口的相对位置、相对姿态和相对速度。申请号：201210111347.4本发明公开了一种航天器交会对接相对位姿测量方法,该方法将双目视觉测量方法与普吕克直线方程相结合来进行航天器交会对接相对位姿的测量。以上提到的均为基于双目视觉的近距离位姿确定方法，不适用于空间绳系机器人中远距离位置估计。

发明内容

技术方案

本发明的目的在于提供一种空间绳系机器人中远距离目标逼近过程中的位置估计方法，该导航方案基于目标单目视觉方位角信息，结合空间系绳的长度信息，对空间绳系机器人逼近过程中的位置进行估计。

本发明的技术方案为：

所述一种空间绳系机器人中远距离逼近位置估计方法，其特征在于：循环进行以下步骤，对空间绳系机器人中远距离逼近位置进行实时估计：

步骤1：利用单目视觉相机获得第k时刻目标相对空间绳系机器人的方位角η_1k和η_2k：

$\left\{\begin{array}{c}{\mathrm{\η}}_{1k}=\arctan \left(\frac{(u-u_0)d_x}{f}\right)\\ {\mathrm{\η}}_{2k}=\arctan \left(\frac{(v-v_0)d_y}{f}\right)\end{array}\right.$

其中u、v分别为目标在相机的图像坐标系中位置，u₀、v₀分别为图像坐标系原点位置，d_x、d_y为相机单个像素的物理尺寸，f为相机的焦距；

步骤2：从空间绳系机器人的空间系绳放绳机构获得第k时刻空间系绳的长度L_k；

步骤3：根据空间绳系机器人的状态方程和量测方程，采用无损卡尔曼滤波方法对空间绳系机器人的位置进行实时估计；其中空间绳系机器人的状态方程和量测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k\left(X_k\right)+V_k\end{array}\right.$

其中

为第k时刻空间绳系机器人的状态向量，x_k、y_k和z_k为第k时刻空间绳系机器人在目标轨道坐标系下的位置；

和

为第k时刻空间绳系机器人在目标第k坐标系下的三轴速度；所述目标轨道坐标系中x轴为目标轨道切线方向，z轴沿地球半径方向指向地心，y轴垂直与x轴和z轴，并且满足右手定则；Φ_k,k-1为状态转移矩阵，

${\mathrm{\Φ}}_{k,k-1}=\left(\begin{array}{cccccc}1& 0& 0& T& 0& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 2\mathrm{nT}\\ 0& -n^{2}T& 0& 0& 1& 0\\ 0& 0& 3n^{2}T& -2\mathrm{nT}& 0& 1\end{array}\right)$

T为时间间隔，n为轨道平均角速度；Z_k为观测向量，

$Z_k=\left(\begin{array}{c}{\mathrm{\η}}_{1k}\\ {\mathrm{\η}}_{2k}\\ L_k\end{array}\right)$

由步骤1和步骤2得到；H_k(X_k)为观测非线性函数，

$H_k\left(X_k\right)=\left(\begin{array}{c}\arctan \frac{R_{21}{x}_k+R_{22}{y}_k+R_{23}{z}_k}{R_{11}{x}_k+R_{12}{y}_k+R_{13}{z}_k}\\ \arctan \frac{R_{31}{x}_k+R_{32}{y}_k+R_{33}{z}_k}{R_{11}{x}_k+R_{12}{y}_k+R_{13}{z}_k}\\ \sqrt{{(x_p-x_k)}^2+{(y_p-y_k)}^2+{(z_p-z_k)}^2}\end{array}\right)$

R₁₁、R₁₂、R₁₃、R₂₁、R₂₂、R₂₃、R₃₁、R₃₂和R₃₃分别为目标轨道坐标系到空间绳系机器人本体坐标系转化矩阵R的9个分量，

$R=I_3-\frac{4(1-{\mathrm{\σ}}^2)}{{(1+{\mathrm{\σ}}^2)}^2}\left[{\mathrm{\σ}}^{\×}\right]+\frac{8}{{(1+{\mathrm{\σ}}^2)}^2}{\left[{\mathrm{\σ}}^{\×}\right]}^2$

I₃为3×3单位矩阵，σ为利用修正罗德里格斯参数表示的空间绳系机器人的姿态角，通过空间绳系机器人自身的姿态测量装置获得，

$\left[{\mathrm{\σ}}^{\×}\right]=\left(\begin{array}{ccc}0& -{\mathrm{\σ}}_3& {\mathrm{\σ}}_2\\ {\mathrm{\σ}}_3& 0& -{\mathrm{\σ}}_1\\ -{\mathrm{\σ}}_2& {\mathrm{\σ}}_1& 0\end{array}\right)$

σ₁、σ₂和σ₃分别为σ的三个分量；(x_p,y_p,z_p)^T为释放空间绳系机器人的机动平台在目标轨道坐标系下的位置坐标；W_k-1为系统白噪声向量，V_k观测白噪声向量。

有益效果

本发明提出的空间绳系机器人中远距离逼近位置估计方法，充分利用了空间绳系机器人的系绳长度信息，结合了单目相机的视场角信息，实现对空间绳系机器人位置进行实时估计。

附图说明

图1：本发明的中远距离空间绳系机器人目标逼近示意图；

图2：本发明的滤波流程框图。

其中：1、目标；2、空间绳系机器人；3、机动平台；4、空间系绳。

具体实施方式

下面结合具体实施例描述本发明：

本实施例针对空间绳系机器人对目标逼近过程中的导航问题，设计“单目视觉测量+系绳长度测量”的中远距离空间绳系机器人位置估计方法，利用相机进行目标方位角测量，结合空间绳系机器人的系绳长度信息，利用UKF滤波算法实现对空间绳系机器人的位置估计。

测量装置：进行目标方位角测量的相机需要选用窄视场的相机，从而保证在距离目标中远距离时，对目标进行较为准确的方位角测量。空间绳系机器人的系绳释放机构需要具有系绳长度测量装置，能够实时提供系绳长度信息。

下面详细介绍空间绳系机器人中远距离位置估计步骤：

对于第k时刻的估计：

步骤1：利用单目视觉相机获得第k时刻目标相对空间绳系机器人的方位角η_1k和η_2k：

$\left\{\begin{array}{c}{\mathrm{\η}}_{1k}=\arctan \left(\frac{(u-u_0)d_x}{f}\right)\\ {\mathrm{\η}}_{2k}=\arctan \left(\frac{(v-v_0)d_y}{f}\right)\end{array}\right.$

其中u、v分别为目标在相机的图像坐标系中位置，u₀、v₀分别为图像坐标系原点位置，d_x、d_y为相机单个像素的物理尺寸，f为相机的焦距；

步骤2：从空间绳系机器人的空间系绳放绳机构获得第k时刻空间系绳的长度L_k；空间绳系机器人在释放过程中，采取被动放绳的方式，即需要系绳中存在一定的小拉力，从而保证系绳是在有阻尼的情况下工作的，对于百米量级的空间绳系机器人，系绳长度较短，系绳处于拉直状态。利用系绳释放机构的测量装置，可以实时得到系绳的长度L_k。

步骤3：根据空间绳系机器人的状态方程和量测方程，采用无损卡尔曼滤波方法对空间绳系机器人的位置进行实时估计；

其中空间绳系机器人的状态方程和量测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k\left(X_k\right)+V_k\end{array}\right.$

其中

为第k时刻空间绳系机器人的状态向量，x_k、y_k和z_k为第k时刻空间绳系机器人在目标轨道坐标系下的位置；

和

为第k时刻空间绳系机器人在目标第k坐标系下的三轴速度；所述目标轨道坐标系中x轴为目标轨道切线方向，z轴沿地球半径方向指向地心，y轴垂直与x轴和z轴，并且满足右手定则；Φ_k,k-1为状态转移矩阵，

${\mathrm{\Φ}}_{k,k-1}=\left(\begin{array}{cccccc}1& 0& 0& T& 0& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 2\mathrm{nT}\\ 0& -n^{2}T& 0& 0& 1& 0\\ 0& 0& 3n^{2}T& -2\mathrm{nT}& 0& 1\end{array}\right)$

T为时间间隔，n为轨道平均角速度；Z_k为观测向量，

$Z_k=\left(\begin{array}{c}{\mathrm{\η}}_{1k}\\ {\mathrm{\η}}_{2k}\\ L_k\end{array}\right)$

由步骤1和步骤2得到；H_k(X_k)为观测非线性函数，

$H_k\left(X_k\right)=\left(\begin{array}{c}\arctan \frac{R_{21}{x}_k+R_{22}{y}_k+R_{23}{z}_k}{R_{11}{x}_k+R_{12}{y}_k+R_{13}{z}_k}\\ \arctan \frac{R_{31}{x}_k+R_{32}{y}_k+R_{33}{z}_k}{R_{11}{x}_k+R_{12}{y}_k+R_{13}{z}_k}\\ \sqrt{{(x_p-x_k)}^2+{(y_p-y_k)}^2+{(z_p-z_k)}^2}\end{array}\right)$

R₁₁、R₁₂、R₁₃、R₂₁、R₂₂、R₂₃、R₃₁、R₃₂和R₃₃分别为目标轨道坐标系到空间绳系机器人本体坐标系转化矩阵R的9个分量，

$R=I_3-\frac{4(1-{\mathrm{\σ}}^2)}{{(1+{\mathrm{\σ}}^2)}^2}\left[{\mathrm{\σ}}^{\×}\right]+\frac{8}{{(1+{\mathrm{\σ}}^2)}^2}{\left[{\mathrm{\σ}}^{\×}\right]}^2$

I₃为3×3单位矩阵，σ为利用修正罗德里格斯参数表示的空间绳系机器人的姿态角，通过空间绳系机器人自身的姿态测量装置获得，

$\left[{\mathrm{\σ}}^{\×}\right]=\left(\begin{array}{ccc}0& -{\mathrm{\σ}}_3& {\mathrm{\σ}}_2\\ {\mathrm{\σ}}_3& 0& -{\mathrm{\σ}}_1\\ -{\mathrm{\σ}}_2& {\mathrm{\σ}}_1& 0\end{array}\right)$

σ₁、σ₂和σ₃分别为σ的三个分量；(x_p,y_p,z_p)^T为释放空间绳系机器人的机动平台在目标轨道坐标系下的位置坐标；W_k-1为系统白噪声向量，V_k观测白噪声向量。

无损卡尔曼滤波方法对非线性系统具有较好的估计特点，其滤波算法流程为：

(1)初始化均值和方差分别为：

$\left\{\begin{array}{c}{\stackrel{\‾}{X}}_0=E\left(X_0\right)\\ P_0=E\left\{(X_0-{\stackrel{\‾}{X}}_0){(X_0-{\stackrel{\‾}{X}}_0)}^T\right\}\end{array}\right.$

为了保证滤波收敛，初始误差阵P₀可以取为6阶单位矩阵。

产生2n+1个Sigma点组成的列向量χ_k（k=0,1,2…,2n），λ为调节系数。

${\mathrm{\χ}}_k=\left(\begin{array}{cc}\stackrel{\‾}{X}& k=0\\ \stackrel{\‾}{X}+{\left(\sqrt{(n+\mathrm{\λ})P_x}\right)}_k& k=\mathrm{1,2}\·\·\·,n\\ \stackrel{\‾}{X}{-\left(\sqrt{(n+\mathrm{\λ})P_x}\right)}_{k-n}& k=n+1,n+2\·\·\·,2n\end{array}\right.$

(2)时间更新：

状态预测值：

(χ_k,k-1)_i＝Φ_k,k-1·(χ_k-1)_i i=1,2,…,2n

状态预测值均值：

$\begin{array}{cc}{\widehat{\mathrm{\χ}}}_{k,k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(m\right)}{\left({\mathrm{\χ}}_{k,k-1}\right)}_i& i=\mathrm{1,2},\·\·\·,2n\end{array}$

状态预测误差矩阵：

$P_{k,k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}{[{\widehat{\mathrm{\χ}}}_{k,k-1}-{\left({\mathrm{\χ}}_{k,k-1}\right)}_i][{\widehat{\mathrm{\χ}}}_{k,k-1}-{\left({\mathrm{\χ}}_{k,k-1}\right)}_i]}^T+Q_k$

观测量预测值：

(Z_k,k-1)_i＝h(χ_k,k-1)_i i=1,2,…,2n

观测量预测均值：

$\begin{array}{cc}{\hat{Z}}_{k,k-1}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(m\right)}{\left(Z_{k,k-1}\right)}_i& i=\mathrm{1,2},\·\·\·,2n\end{array}$

测量更新：

$\left\{\begin{array}{c}{P}_{\mathrm{zz}}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}[{\hat{Z}}_{k,k-1}-{\left(Z_{k,k-1}\right)}_i]{[{\hat{Z}}_{k,k-1}-{\left(Z_{k,k-1}\right)}_i]}^T+R_k\\ P_{\mathrm{xz}}=\underset{i=0}{\overset{2n}{\mathrm{\Σ}}}{w}_i^{\left(c\right)}[{\left(X_{k,k-1}\right)}_i-{\hat{X}}_{k,k-1}]{[{\left(Z_{k,k-1}\right)}_i-{\hat{Z}}_{k,k-1}]}^T\end{array}\right.$

卡尔曼滤波增益：

$K=P_{\mathrm{xz}}{P}_{\mathrm{zz}}^{-1}$

滤波值：

${\hat{x}}_k={\hat{x}}_{k,k-1}+K(Z_k-{\hat{Z}}_{k,k-1})$

滤波误差矩阵：

P_k＝P_k,k-1-KP_zzK^T

其中：

为均值权值，

为方差权值，取值分别为：

$\left\{\begin{array}{cc}{w}_0^{\left(m\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}& \\ w_0^{\left(c\right)}=\frac{\mathrm{\λ}}{n+\mathrm{\λ}}+(1-{\mathrm{\α}}^2+\mathrm{\β})& \\ w_i^{\left(m\right)}=\frac{\mathrm{\λ}}{2(n+\mathrm{\λ})}& i=\mathrm{1,2},\·\·\·,2n\\ \mathrm{\λ}={\mathrm{\α}}^2(n+\mathrm{\κ})-n& \end{array}\right.$

n为状态矢量维数，α为尺度参数，决定Sigma点的分布范围，κ通常取0。

即为第k时刻空间绳系机器人位置信息。

Claims (1)

1.一种空间绳系机器人中远距离逼近位置估计方法，其特征在于：循环进行以下步骤，对空间绳系机器人中远距离逼近位置进行实时估计：

步骤1：利用单目视觉相机获得第k时刻目标相对空间绳系机器人的方位角η_1k和η_2k：

$\left\{\begin{array}{c}{\mathrm{\η}}_{1k}=\arctan \left(\frac{(u-u_0)d_x}{f}\right)\\ {\mathrm{\η}}_{2k}=\arctan \left(\frac{(v-v_0)d_y}{f}\right)\end{array}\right.$

其中u、v分别为目标在相机的图像坐标系中位置，u₀、v₀分别为图像坐标系原点位置，d_x、d_y为相机单个像素的物理尺寸，f为相机的焦距；

步骤2：从空间绳系机器人的空间系绳放绳机构获得第k时刻空间系绳的长度L_k；

步骤3：根据空间绳系机器人的状态方程和量测方程，采用无损卡尔曼滤波方法对空间绳系机器人的位置进行实时估计；其中空间绳系机器人的状态方程和量测方程为：

$\left\{\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}\\ Z_k=H_k\left(X_k\right)+V_k\end{array}\right.$

其中

为第k时刻空间绳系机器人的状态向量，x_k、y_k和z_k为第k时刻空间绳系机器人在目标轨道坐标系下的位置；

和

为第k时刻空间绳系机器人在目标第k坐标系下的三轴速度；所述目标轨道坐标系中x轴为目标轨道切线方向，z轴沿地球半径方向指向地心，y轴垂直与x轴和z轴，并且满足右手定则；Φ_k,k-1为状态转移矩阵，

${\mathrm{\Φ}}_{k,k-1}=\left(\begin{array}{cccccc}1& 0& 0& T& 0& 0\\ 0& 1& 0& 0& T& 0\\ 0& 0& 1& 0& 0& T\\ 0& 0& 0& 1& 0& 2\mathrm{nT}\\ 0& -n^{2}T& 0& 0& 1& 0\\ 0& 0& 3n^{2}T& -2\mathrm{nT}& 0& 1\end{array}\right)$

T为时间间隔，n为轨道平均角速度；Z_k为观测向量，

$Z_k=\left(\begin{array}{c}{\mathrm{\η}}_{1k}\\ {\mathrm{\η}}_{2k}\\ L_k\end{array}\right)$

由步骤1和步骤2得到；H_k(X_k)为观测非线性函数，

$H_k\left(X_k\right)=\left(\begin{array}{c}\arctan \frac{R_{21}{x}_k+R_{22}{y}_k+R_{23}{z}_k}{R_{11}{x}_k+R_{12}{y}_k+R_{13}{z}_k}\\ \arctan \frac{R_{31}{x}_k+R_{32}{y}_k+R_{33}{z}_k}{R_{11}{x}_k+R_{12}{y}_k+R_{13}{z}_k}\\ \sqrt{{(x_p-x_k)}^2+{(y_p-y_k)}^2+{(z_p-z_k)}^2}\end{array}\right)$

R₁₁、R₁₂、R₁₃、R₂₁、R₂₂、R₂₃、R₃₁、R₃₂和R₃₃分别为目标轨道坐标系到空间绳系机器人本体坐标系转化矩阵R的9个分量，

$R=I_3-\frac{4(1-{\mathrm{\σ}}^2)}{{(1+{\mathrm{\σ}}^2)}^2}\left[{\mathrm{\σ}}^{\×}\right]+\frac{8}{{(1+{\mathrm{\σ}}^2)}^2}{\left[{\mathrm{\σ}}^{\×}\right]}^2$

I₃为3×3单位矩阵，σ为利用修正罗德里格斯参数表示的空间绳系机器人的姿态角，通过空间绳系机器人自身的姿态测量装置获得，

$\left[{\mathrm{\σ}}^{\×}\right]=\left(\begin{array}{ccc}0& -{\mathrm{\σ}}_3& {\mathrm{\σ}}_2\\ {\mathrm{\σ}}_3& 0& -{\mathrm{\σ}}_1\\ -{\mathrm{\σ}}_2& {\mathrm{\σ}}_1& 0\end{array}\right)$

σ₁、σ₂和σ₃分别为σ的三个分量；(x_p,y_p,z_p)^T为释放空间绳系机器人的机动平台在目标轨道坐标系下的位置坐标；W_k-1为系统白噪声向量，V_k观测白噪声向量。

Images