[发明专利]一种空间绳系机器人中远距离逼近位置估计方法

权利要求书

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

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

η1k=arctan((u-u0)dxf)η2k=arctan((v-v0)dyf)]]>

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

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

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

Xk=Φk,k-1Xk-1+Wk-1Zk=Hk(Xk)+Vk]]>

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

Φk,k-1=100T000100T000100T000102nT0-n2T0010003n2T-2nT01]]>

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

Zk=η1kη2kLk]]>

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

Hk(Xk)=arctanR21xk+R22yk+R23zkR11xk+R12yk+R13zkarctanR31xk+R32yk+R33zkR11xk+R12yk+R13zk(xp-xk)2+(yp-yk)2+(zp-zk)2]]>

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

R=I3-4(1-σ2)(1+σ2)2[σ×]+8(1+σ2)2[σ×]2]]>

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

[σ×]=0-σ3σ2σ30-σ1-σ2σ10]]>

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