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

摘要

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

主权项

1.一种空间绳系机器人中远距离逼近位置估计方法，其特征在于：循环进行以下步骤，对空间绳系机器人中远距离逼近位置进行实时估计：步骤1：利用单目视觉相机获得第k时刻目标相对空间绳系机器人的方位角η_1k和η_2k：$\left\{\begin{array}{c}{\eta }_{1k}=\arctan \left(\frac{(u-u_0)d_x}{f}\right)\\ {\eta }_{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={\Phi }_{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为状态转移矩阵，${\Phi }_{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}{\eta }_{1k}\\ {\eta }_{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-{\sigma }^2)}{{(1+{\sigma }^2)}^2}\left[{\sigma }^{\times }\right]+\frac{8}{{(1+{\sigma }^2)}^2}{\left[{\sigma }^{\times }\right]}^2$I₃为3×3单位矩阵，σ为利用修正罗德里格斯参数表示的空间绳系机器人的姿态角，通过空间绳系机器人自身的姿态测量装置获得，$\left[{\sigma }^{\times }\right]=\left(\begin{array}{ccc}0& -{\sigma }_3& {\sigma }_2\\ {\sigma }_3& 0& -{\sigma }_1\\ -{\sigma }_2& {\sigma }_1& 0\end{array}\right)$σ₁、σ₂和σ₃分别为σ的三个分量；(x_p,y_p,z_p)^T为释放空间绳系机器人的机动平台在目标轨道坐标系下的位置坐标；W_k-1为系统白噪声向量，V_k观测白噪声向量。