[发明专利]一种基于对接环的捕获目标卫星的方法与系统

权利要求书

1.一种基于对接环的捕获目标卫星的方法，其特征在于，包括以下步骤：

S1、系统控制手眼相机测量得到空间机器人机械臂末端点与对接环中心点的相对位置矢量Δr和相对姿态θ；

S2、系统根据对接环面的法向量z_A，求得所述空间机器人机械臂末端点与对接环中心点的相对位置矢量Δr在对接环面的投影b_n，得出距离机械臂末端最近的抓捕点，并计算得到机械臂末端点到对接环“动态最近点”的相对位置偏差Δp和相对姿态偏差ΔO；

S3、系统判断所述相对位置偏差模||Δp||和相对姿态偏差模||ΔO||是否小于设定的临界值ε_p和ε_o，若二者均小于临界值，则机械臂末端的手爪闭合，实现目标的捕获；反之，则进入S4；

S4、系统根据所得的相对位置偏差Δp和相对姿态偏差ΔO，根据以下公式规划机械臂末端的线速度与角速度，

其中，K_p、K_v分别为规划的比例、微分参数；分别表示目标卫星的线速度与角速度；

S5、系统根据S4中得到的机械臂末端规划的线速度和角速度，通过雅可比矩阵反解，求得机械臂各关节的角速度；

S6、系统根据所得的关节角速度，驱动机械臂各关节运动，直至机械臂末端与“动态最近点”的相对位置偏差模和相对姿态偏差模小于设定的临界值ε_p和ε_o，手抓闭合并完成目标卫星的捕获。

2.根据权利要求1所述的基于对接环的捕获目标卫星的方法，其特征在于：所述机械臂末端点到对接环“动态最近点”的相对位置偏差Δp的计算公式为其中，R为对接环的半径。

3.根据权利要求2所述的基于对接环的捕获目标卫星的方法，其特征在于：所述机械臂末端的线速度满足

其中，t_s为设定的平滑启动时间，v_ed、v_em分别表示规划的线速度以及设定的最大线速度值，v_ex、v_ey、v_ez分别表示规划的机械臂末端的线速度在机械臂末端坐标系x-y-z三个方向的分量，t是时间，表示机械臂末端线速度，即机械臂末端实际的线速度。

4.根据权利要求3所述的基于对接环的捕获目标卫星的方法，其特征在于：机械臂末端的角速度满足

其中，ω_ed、ω_em分别表示规划的角速度以及设定的最大角速度值，ω_ex、ω_ey、ω_ez分别表示规划的机械臂末端的角速度在机械臂末端坐标系x-y-z三个方向的分量，t是时间，表示机械臂末端角速度，即机械臂末端实际的角速度。

5.根据权利要求4所述的基于对接环的捕获目标卫星的方法，其特征在于：所述临界值ε_p小于等于50毫米，ε_o小于等于5°。

6.根据权利要求5所述的基于对接环的捕获目标卫星的方法，其特征在于，所述关节角的角速度求解公式为：

其中，表示关节角速度，表示在末端坐标系中，关于机械臂参数的雅克比矩阵，分别表示在末端坐标系中，机械臂末端相对于基座的速度与角速度。

7.一种基于对接环的捕获目标卫星的系统，其特征在于，包括：

第一模块，用于执行步骤S1、系统控制手眼相机测量得到空间机器人机械臂末端点与对接环中心点的相对位置矢量Δr和相对姿态θ；

第二模块，用于执行步骤S2、系统根据对接环面的法向量z_A，求得所述空间机器人机械臂末端点与对接环中心点的相对位置矢量Δr在对接环面的投影b_n，得出距离机械臂末端最近的抓捕点，并计算得到机械臂末端点到对接环“动态最近点”的相对位置偏差Δp和相对姿态偏差ΔO；

第三模块，用于执行步骤S3、系统判断所述相对位置偏差模||Δp||和相对姿态偏差模||ΔO||是否小于设定的临界值ε_p和ε_o，若二者均小于临界值，则机械臂末端的手爪闭合，实现目标的捕获；反之，则进入S4；

第四模块，用于执行步骤S4、系统根据所得的相对位置偏差Δp和相对姿态偏差ΔO，根据以下公式规划机械臂末端的线速度与角速度，

其中，K_p、K_v分别为规划的比例、微分参数；分别表示目标卫星的线速度与角速度；

第五模块，用于执行步骤S5、系统根据S4中得到的机械臂末端规划的线速度和角速度，通过雅可比矩阵反解，求得机械臂各关节的角速度；

第六模块，用于执行步骤S6、系统根据所得的关节角速度，驱动机械臂各关节运动，直至机械臂末端与“动态最近点”的相对位置偏差模和相对姿态偏差模小于设定的临界值ε_p和ε_o，手抓闭合并完成目标卫星的捕获。