CN115416030B - 一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法 - Google Patents

Abstract

一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，涉及轨迹规划技术领域，针对现有技术中大噪声的测量反馈无法规划机械臂平滑末端运动的问题，本申请通过全局相机的测量数据，规划机械臂的运动轨迹，实现机械臂在手眼相机视场范围内稳定跟踪翻滚卫星对接环，解决了手眼相机测量视场小，初始状态下翻滚卫星对接环不在手眼相机测量视场中，机械臂无法伺服目标的问题。本申请相比于一般的伺服方法，仅将全局相机的测量数据作为机械臂末端速度调整的判据，并不直接引入闭环计算，从根本上解决了大误差的测量数据无法规划平滑末端轨迹的问题。

Description

一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法

技术领域

本发明涉及轨迹规划技术领域，具体为一种机械臂远距离跟踪翻滚卫星对接环的运动 规划方法。

背景技术

失效卫星除了自旋运动外，还存在章动，机械臂捕获这类目标，需要依靠视觉信息导 引。视觉测量系统可获得机械臂的当前位姿与期望位姿的偏差，根据相机安装的不同位置， 可分为手眼相机系统和全局相机系统。其中，手眼相机安装在机械臂的末端，测量视场较小，而翻滚卫星运动范围大，初始状态往往不在手眼相机的测量视场内，导致手眼相机无法给出测量信息，造成任务失败，仅使用单一的手眼相机系统已不能满足抓捕翻滚卫星对接环的任务需求。全局相机安装在机械臂之外，相比手眼相机具有更大的视场范围，可导引机械臂运动，使得手眼相机视场捕获目标的对接环，但全局相机测量误差大，实时动态测量过程中，测量数据存在较大波动，如何基于含有较大测量误差的全局数据规划平滑的 机械臂轨迹是一个难点。

当前机械臂抓捕翻滚卫星的运动规划技术，主要基于手眼相机的测量反馈，并没有考 虑初始状态下，目标的对接环不在手眼相机测量范围内的问题，为了手眼相机能够稳定的 测量，机械臂需要依靠视场范围更大的全局相机的测量反馈，从远距离运动至对接环附近，并持续跟踪对接环，使得手眼相机能够稳定测量目标对接环的位姿，之后再基于手眼测量 信息伺服抓捕目标；然而全局相机测量误差大，误差分布特性为随机分布，现有规划技术 无法基于大噪声的测量反馈平滑规划机械臂末端运动，常用的滤波手段无法有效剔除相机 测量的随机噪声。

发明内容

本发明的目的是：针对现有技术中大噪声的测量反馈无法规划机械臂平滑末端运动的 问题，提出一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法。

本发明为了解决上述技术问题采取的技术方案是：

一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，包括以下步骤：

步骤一：首先通过全局相机获取翻滚卫星实时测量数据[d，α，β，0]，然后获取翻滚卫星 对接环的半径r，之后根据测量数据[d，α，β，0]和翻滚卫星对接环的半径r得到对接环目标坐 标系相对于全局相机坐标系的位姿矩阵^gT_t，

其中，d表示对接环圆心坐标系原点在全局相机坐标系下的位置矢量，α和β分别表示 对接环圆心坐标系相对于全局相机坐标系的偏航角和俯仰角，滚转角度始终为0；

步骤二：根据机械臂关节位置安装的传感器实时反馈的关节角度信息得到机械臂基座 系相对于末端工具系的位姿矩阵^eT_b；

步骤三：根据对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t以及机械臂基 座系相对于末端工具系的位姿矩阵^eT_b得到对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t，然后使用转化函数将位姿矩阵^eT_t转化为位姿信息[d_，γ_]；

步骤四：根据位姿信息[d_，γ_]得到末端工具系相对于基座系的位姿矩阵^bT_t：

步骤五：利用末端工具系相对于基座系的位姿矩阵^bT_t得到机械臂细分的关节轨迹， 所述机械臂细分的关节轨迹包括：关节位置矢量、关节速度矢量，关节加速度矢量。

进一步的，所述步骤一的具体步骤为：

步骤一一：首先通过全局相机获取翻滚卫星实时测量数据[d，α，β，0]，然后根据测量数 据[d，α，β，0]得到对接环圆心坐标系相对于末端工具系的位姿矩阵^gT_o，^gT_o表示为：

^gT_o＝wz2mtrx(d，α，β)

其中，wz2mtrx()表示位姿信息到位姿矩阵的转换函数；

步骤一二：获取对接环的半径r，并根据对接环的半径r以及对接环圆心坐标系相对于 末端工具系的位姿矩阵^gT_o得到对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t，^gT_t表示为：

其中，I表示3×3的单位矩阵，0表示1行3列的0矢量。

进一步的，所述机械臂基座系相对于末端工具系的位姿矩阵^eT_b表示为：

^eT_b＝fdkine(θ，l，^bT₀)

其中，θ表示所有关节角度，l表示机械臂连杆长度参数，^bT₀表示常值矩阵，

fdkine()表示正运动学求解函数。

进一步的，所述步骤三的具体步骤为：

步骤三一：获取服务飞行器上全局相机坐标系相对于机械臂基座坐标系的位姿矩阵^bT_g，然后根据^gT_t、^eT_b以及^bT_g得到对接环目标坐标系相对于末端工具系的位姿矩 阵^eT_t，^eT_t表示为：

^eT_t＝^eT_b ^bT_g ^gT_t；

步骤三二：将^eT_t转换为位姿信息[d_，γ_]，位姿信息[d_，γ_]表示为：

[d_，γ_]＝mtrx2wz(^eT_t)

其中，d_表示对接环目标坐标系原点在末端工具系下的位置矢量，γ_表示对接环目标 坐标系相对于末端工具系的ZYX欧拉角，mtrx2wz()表示位姿矩阵到位姿信息的转换函数。

进一步的，所述步骤四的具体步骤为：

步骤四一：基于位姿信息[d_，γ_]分别计算6个方向上的标志位，标志位表示为：

其中，[i]表示矢量的第i个元素，i的取值为1～6，^eV_t，k表示k时刻机械臂的末端速度， a_e表示末端速度调整幅值，a_e为常值矢量，|[d_，γ_][i]|表示矢量[d_，γ_]的第i个元素的绝对 值，sign()表示符号函数，6个方向包括3个位置方向和3个姿态方向；

步骤四二：利用标志位得到k+1时刻机械臂的末端速度^eV_t，k+1[i]，具体为：

flag[i]＞0，^eV_t，k+1[i]＝^eV_t，k[i]+a_e[i]·Δt·sign([d_，γ_][i])

flag[i]≤0，

其中，λ表示均衡系数，v_max表示机械臂末端速度的限幅值，v_max为常值矢量，Δt表示计算周期；

当k+1时刻机械臂的末端速度^eV_t，k+1[i]大于v_max[i]时，对^eV_t，k+1[i]进行修正，表示为：

^eV_t，k+1[i]＝v_max[i]·sign([d_，γ_][i])；

步骤四三：将修正后的^eV_t，k+1[i]转换为位姿矩阵^eT_t，并以末端工具系相对于基座系的位姿矩阵^bT_t表示。

进一步的，所述位姿矩阵^eT_t表示为：

其中，rotx、roty和rotz分别表示绕z，y，x的单位旋转矩阵函数。

进一步的，所述末端工具系相对于基座系的位姿矩阵^bT_t表示为：

^bT_t＝(^eT_d)^{-1 e}T_t。

进一步的，所述步骤五的具体步骤为：

步骤五一：指定关节，然后基于指定关节的位置级逆运动学将末端工具系相对于基座 系的位姿矩阵^bT_t转换为机械臂关节空间轨迹θ_d，所述机械臂关节空间轨迹θ_d表示为：

θ_d＝ikine(^bT_t)

其中，ikine()表示位置级逆运动学函数；

步骤五二：对θ_d进行二次多项式插补，得到细分的关节轨迹：

其中，q_d为细分得到的关节位置矢量，

为细分得到的关节速度矢量，/>

为细分得到 的关节加速度矢量，interpolator()是插补函数。

进一步的，所述机械臂为7关节机械臂。

进一步的，所述步骤五一中指定的关节为：关节1、关节2、关节3、关节5、关节6 以及关节7。

本发明的有益效果是：

1、本申请通过全局相机的测量数据，规划机械臂的运动轨迹，实现机械臂在手眼相机 视场范围内稳定跟踪翻滚卫星对接环，解决了手眼相机测量视场小，初始状态下翻滚卫星 对接环不在手眼相机测量视场中，机械臂无法伺服目标的问题。

2、本申请相比于一般的伺服方法，仅将全局相机的测量数据作为机械臂末端速度调整 的判据，并不直接引入闭环计算，从根本上解决了大误差的测量数据无法规划平滑末端轨 迹的问题。

附图说明

图1为机械臂坐标系示意图；

图2为运动规划方法中涉及到的主要坐标系示意图；

图3为运动规划方法流程图；

图4为机械臂跟踪翻滚卫星对接环过程中的跟踪初始构型图；

图5为机械臂跟踪翻滚卫星对接环过程中的跟踪到位构型图；

图6为机械臂跟踪翻滚卫星对接环过程中的继续保持跟踪构型图；

图7为机械臂跟踪过程中末端工具系与对接环目标坐标系的相对距离数据曲线图；

图8为机械臂跟踪过程中末端工具系与对接环目标坐标系的相对姿态数据曲线图；

图9为关节位置细分后得到的细分的关节位置曲线图；

图10为关节位置细分后得到的关节速度曲线图；

图11为关节位置细分后得到的关节加速度曲线图。

具体实施方式

需要特别说明的是，在不冲突的情况下，本申请公开的各个实施方式之间可以相互组 合。

具体实施方式一：参照图1和图2具体说明本实施方式，本实施方式所述的一种机械 臂远距离跟踪翻滚卫星对接环的运动规划方法，包括以下步骤：

步骤一：首先通过全局相机获取翻滚卫星实时测量数据[d，α，β，0]，然后获取翻滚卫星 对接环的半径r，之后根据测量数据[d，α，β，0]和翻滚卫星对接环的半径r得到对接环目标坐 标系相对于全局相机坐标系的位姿矩阵^gT_t，

其中，d表示对接环圆心坐标系原点在全局相机坐标系下的位置矢量，α和β分别表示 对接环圆心坐标系相对于全局相机坐标系的偏航角和俯仰角，滚转角度始终为0；

步骤二：根据机械臂关节位置安装的传感器实时反馈的关节角度信息得到机械臂基座 系相对于末端工具系的位姿矩阵^eT_b；

步骤三：根据对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t以及机械臂基 座系相对于末端工具系的位姿矩阵^eT_b得到对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t，然后使用转化函数将位姿矩阵^eT_t转化为位姿信息[d_，γ_]；

步骤四：根据位姿信息[d_，γ_]得到末端工具系相对于基座系的位姿矩阵^bT_t：

步骤五：利用末端工具系相对于基座系的位姿矩阵^bT_t得到机械臂细分的关节轨迹， 所述机械臂细分的关节轨迹包括：关节位置矢量、关节速度矢量，关节加速度矢量。

具体实施方式二：本实施方式是对具体实施方式一的进一步说明，本实施方式与具体 实施方式一的区别是所述步骤一的具体步骤为：

步骤一一：首先通过全局相机获取翻滚卫星实时测量数据[d，α，β，0]，然后根据测量数 据[d，α，β，0]得到对接环圆心坐标系相对于末端工具系的位姿矩阵^gT_o，^gT_o表示为：

^gT_o＝wz2mtrx(d，α，β)

其中，wz2mtrx()表示位姿信息到位姿矩阵的转换函数；

步骤一二：获取对接环的半径r，并根据对接环的半径r以及对接环圆心坐标系相对于 末端工具系的位姿矩阵^gT_o得到对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t，^gT_t表示为：

其中，I表示3×3的单位矩阵，0表示1行3列的0矢量。

具体实施方式三：本实施方式是对具体实施方式二的进一步说明，本实施方式与具体 实施方式二的区别是所述机械臂基座系相对于末端工具系的位姿矩阵^eT_b表示为：

^eT_b＝fdkine(θ，l，^bT₀)

其中，θ表示所有关节角度，l表示机械臂连杆长度参数，^bT₀表示常值矩阵，

fdkine()表示正运动学求解函数。

具体实施方式四：本实施方式是对具体实施方式三的进一步说明，本实施方式与具体 实施方式三的区别是所述步骤三的具体步骤为：

步骤三一：获取服务飞行器上全局相机坐标系相对于机械臂基座坐标系的位姿矩阵^bT_g，然后根据^gT_t、^eT_b以及^bT_g得到对接环目标坐标系相对于末端工具系的位姿矩 阵^eT_t，^eT_t表示为：

^eT_t＝^eT_b ^bT_g ^gT_t；

步骤三二：将^eT_t转换为位姿信息[d_，γ_]，位姿信息[d_，γ_]表示为：

[d_，γ_]＝mtrx2wz(^eT_t)

其中，d_表示对接环目标坐标系原点在末端工具系下的位置矢量，γ_表示对接环目标 坐标系相对于末端工具系的ZYX欧拉角，mtrx2wz()表示位姿矩阵到位姿信息的转换函数。

具体实施方式五：本实施方式是对具体实施方式四的进一步说明，本实施方式与具体 实施方式四的区别是所述步骤四的具体步骤为：

步骤四一：基于位姿信息[d_，γ_]分别计算6个方向上的标志位，标志位表示为：

其中，[i]表示矢量的第i个元素，i的取值为1～6，^eV_t，k表示k时刻机械臂的末端速度， 前3个元素为线速度矢量，后三个元素为角速度矢量，角速度矢量采用ZYX欧拉角的一阶 导数表示，第0时刻(初始)的^eV_t，0[i]＝0，a_e表示末端速度调整幅值，a_e为常值矢量， |[d_，γ_][i]|表示矢量[d_，γ_]的第i个元素的绝对值，sign()表示符号函数，6个方向包括3个 位置方向和3个姿态方向；

步骤四二：利用标志位得到k+1时刻机械臂的末端速度^eV_t，k+1[i]，具体为：

flag[i]＞0，^eV_t，k+1[i]＝^eV_t，k[i]+a_e[i]·Δt·sign[d_，γ_][i])

flag[i]≤0，

其中，λ表示均衡系数，v_max表示机械臂末端速度的限幅值，v_max为常值矢量，Δt表示计算周期；

当k+1时刻机械臂的末端速度^eV_t，k+1[i]大于v_max[i]时，对^eV_t，k+1[i]进行修正，表示为：

^eV_t，k+1[i]＝v_max[i]·sign([d_，γ_][i])；

步骤四三：将修正后的^eV_t，k+1[i]转换为位姿矩阵^eT_t，并以末端工具系相对于基座系的位姿矩阵^bT_t表示。

具体实施方式六：本实施方式是对具体实施方式五的进一步说明，本实施方式与具体 实施方式五的区别是所述位姿矩阵^eT_t表示为：

其中，rotx、roty和rotz分别表示绕z，y，x的单位旋转矩阵函数。

具体实施方式七：本实施方式是对具体实施方式六的进一步说明，本实施方式与具体 实施方式六的区别是所述末端工具系相对于基座系的位姿矩阵^bT_t表示为：

^bT_t＝(^eT_b)^{-1 e}T_t。

具体实施方式八：本实施方式是对具体实施方式七的进一步说明，本实施方式与具体 实施方式七的区别是所述步骤五的具体步骤为：

步骤五一：指定关节，然后基于指定关节的位置级逆运动学将末端工具系相对于基座 系的位姿矩阵^bT_t转换为机械臂关节空间轨迹θ_d，所述机械臂关节空间轨迹θ_d表示为：

θ_d＝ikine(^bT_t)

其中，ikine()表示位置级逆运动学函数；

步骤五二：对θ_d进行二次多项式插补，得到细分的关节轨迹：

其中，q_d为细分得到的关节位置矢量，

为细分得到的关节速度矢量，/>

为细分得到 的关节加速度矢量，三者共同构成了最终的机械臂跟踪翻滚卫星对接环的关节空间轨迹， interpolator()是插补函数。

具体实施方式九：本实施方式是对具体实施方式八的进一步说明，本实施方式与具体 实施方式八的区别是所述机械臂为7关节机械臂。

具体实施方式十：本实施方式是对具体实施方式九的进一步说明，本实施方式与具体 实施方式九的区别是所述步骤五一中指定的关节为：关节1、关节2、关节3、关节5、关节6以及关节7。

视觉伺服开始，每个周期内做如下计算：

步骤一、根据安装在服务卫星上的全局相机实时反馈的测量数据[d，α，β，0]和翻滚卫星 对接环的半径r获得对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t：

步骤一一、根据全局相机的测量数据[d，α，β，0]，获得对接环圆心坐标系相对于末端 工具系的位姿矩阵^gT_o：

^gT_o＝wz2mtrx(d，α，β)

式中，d是对接环圆心坐标系原点在全局相机坐标系下的位置矢量，α和β是对接环圆 心坐标系相对于全局相机坐标系的偏航角和俯仰角，滚转角度始终为0，^gT_o表示由对接环 圆心坐标系至全局相机坐标系的位姿矩阵，g代表全局相机坐标系，o代表对接环圆心坐标 系，wz2mtrx()是位姿信息到位姿矩阵的转换函数。

步骤一二、根据对接环半径r，得到对接环目标坐标系相对于全局相机坐标系的位姿矩 阵^gT_t：

式中，下标t代表对接环目标坐标系；

步骤二、根据机械臂关节位置传感器实时反馈的关节角度信息获取机械臂基座系相对 于末端工具系的位姿矩阵^eT_b；机械臂的关节坐标系如图1所示，对应的DH参数表如表1所示；

^eT_b＝fdkine(θ，l，^bT₀)

其中，θ为7个关节角度的矢量，l为机械臂连杆长度参数即为连杆偏距d_i的值，^bT₀为 常值矩阵，

^eT_b表示由机械臂基座系至末端工具系的位姿矩阵，e是末端工具系，b是机械臂基座系，fdkine()是正运动学求解函数；

表1机械臂DH参数表

步骤三、根据步骤一和步骤二得到的^gT_t和^eT_b，进而计算对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t，并使用转化函数将位姿矩阵转化为位姿信息[d_，γ_]：

步骤三一、根据步骤一获得的^gT_t和步骤二获得的^eT_b，以及服务飞行器上机械臂与全局相机的安装关系，由下式计算出对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t：

^eT_t＝^eT_b ^bT_g ^gT_t

式中，^bT_g是服务飞行器上全局相机坐标系相对于机械臂基座坐标系的位姿矩阵，为 常值矩阵，可通过标定得到。

步骤三二、将上一步得到的对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t转换 为位姿信息[d_，γ_]：

[d_，γ_]＝mtrx2wz(^eT_t)

式中，d_是对接环目标坐标系原点在末端工具系下的位置矢量，γ_是对接环目标坐标 系相对于末端工具系的ZYX欧拉角，mtrx2wz()是位姿矩阵到位姿信息的转换函数。

步骤四、根据设计的机械臂末端速度加减速调整方法，规划机械臂笛卡尔空间下的轨 迹：

步骤四一、基于位姿信息反馈，分别计算6个方向上判断下一时刻是否开始减速或者 继续加速的标志位：

式中，i的取值为1～6，[i]表示矢量的第i个元素，^eV_t，k是第k时刻在末端工具系下表 示的机械臂期望末端速度矢量，前3个元素为线速度矢量，后三个元素为角速度矢量，直接采用ZYX欧拉角的一阶导数表示，第0时刻(初始)的^eV_t，0[i]＝0，a_e是设定的末端 速度调整幅值，为常值矢量，|[d_，γ_][i]|表示矢量[d_，γ_]的第i个元素的绝对值，sign()是符号函数。

步骤四二、为避免频繁切换加速和减速状态，利用均衡系数λ平滑速度调整过程：

flag[i]＞0，^eV_t，k+1[i]＝^eV_t，k[i]+a_e[i]·Δt·sign([d_，Y_][i])

flag[i]≤0，

式中，^eV_t，k+1[i]是第(k+1)时刻在末端工具系下表示的机械臂期望末端速度矢量，λ是引入的均衡系数，v_max是设定的机械臂末端速度的限幅值，为常值矢量。

当得到的下一时刻机械臂的末端速度^eV_t，k+1[i]大于设定的最大值v_max[i]时，对其进行如下修正：

^eV_t，k+1[i]＝v_max[i]·sign([d_，γ_][i])

步骤四三、将得到的末端速度信息转换为位姿矩阵^eT_t，并以末端工具系相对于基座 系的位姿矩阵表示为^bT_t：

^bT_t＝(^eT_b)^{-1 e}T_t

步骤五、进一步地基于指定关节2位置的位置级逆运动学，将步骤四得到的机械臂笛 卡尔空间轨迹转换为机械臂关节空间轨迹：

θ_d＝ikine(^bT_t，θ₂)

其中，θ_d为解算得到的关节期望角度矢量，对应7个关节的期望角度，θ₂为指定的关节2角度，ikine()是位置级逆运动学函数。对θ_d进行二次多项式插补，得到细分的关节轨迹：

其中，qd为细分得到的关节位置矢量，

为细分得到的关节速度矢量，/>

为细分得到 的关节加速度矢量，三者共同构成了最终的机械臂跟踪翻滚卫星对接环的关节空间轨迹， interpolator()是插补函数。

实施例：

搭建计算机仿真平台，启动伺服，具体实施步骤如附图3所示，在每一个规划周期内 做如下运算：

[步骤S1]根据全局相机反馈的相对位姿计算^gT_o；

[步骤S2]目标对接环半径为813.25mm，计算^gT_t；

[步骤S3]根据关节位置传感器的关节角度反馈计算^eT_b；

[步骤S4]根据标定的机械臂基座系与全局相机坐标系相对位姿得到^bT_g，计算^eT_t；

[步骤S5]从^eT_t解算出目标位姿信息[d_，γ_]；

[步骤S6]计算末端期望速度矢量6个元素对应的开始减速或者继续加速的标志位：

[步骤S7]判断标志位是否大于0，若大于0，按下式计算下一时刻的末端速度：

^eV_t，k+1[i]＝^eV_t，k[i]+a_e[i]·Δt·sign([d_，γ_][i])

否则下一时刻的末端速度为：

[步骤S8]判断计算得到的下一时刻末端速度是否超过设定值，若超过，则修正为：

^eV_t，k+1[i]＝v_max[i]·sign([d_γ-][i])

[步骤S9]解算下一时刻的位姿变化矩阵：

[步骤S10]计算目标末端工具系相对于基座系的位姿矩阵：

^bT_t＝(^eT_b)^{-1 e}T_t

[步骤S11]基于指定关节2位置的位置级逆运动学解算期望笛卡尔位姿对应的关节角 度，并对其转换得到关节空间轨迹：

θ_d＝ikine(^bT_t，θ₂)

[步骤S12]判断是否到位，若已经伺服到位则结束规划，否则，进入下一控制周期，重回[步骤S1]继续进行规划。

图4、图5和图6为机械臂跟踪翻滚卫星对接环过程中的关键构型图，其中图4为伺服初始构型，图5为伺服过程中构型，图6为伺服结束(目标进入手眼相机视场)。

图7和图8为机械臂跟踪过程中末端工具系与对接环目标坐标系的相对位姿数据曲线 图，其中图7为相对距离数据曲线图，图8为相对姿态数据曲线图。

图9、图10以及图11为关节位置细分后得到的关节空间轨迹(位置、速度和加速度)， 其中图9为关节位置曲线图，图10为关节速度曲线图，图11为关节加速度曲线图。

需要注意的是，具体实施方式仅仅是对本发明技术方案的解释和说明，不能以此限定 权利保护范围。凡根据本发明权利要求书和说明书所做的仅仅是局部改变的，仍应落入本 发明的保护范围内。

Claims (9)

1.一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于包括以下步骤：

步骤一：首先通过全局相机获取翻滚卫星实时测量数据[d，α，β，0]，然后获取翻滚卫星对接环的半径r，之后根据测量数据[d，α，β，0]和翻滚卫星对接环的半径r得到对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t，

其中，d表示对接环圆心坐标系原点在全局相机坐标系下的位置矢量，α和β分别表示对接环圆心坐标系相对于全局相机坐标系的偏航角和俯仰角，滚转角度始终为0；

步骤二：根据机械臂关节位置安装的传感器实时反馈的关节角度信息得到机械臂基座系相对于末端工具系的位姿矩阵^eT_b；

步骤三：根据对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t以及机械臂基座系相对于末端工具系的位姿矩阵^eT_b得到对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t，然后使用转化函数将对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t转化为位姿信息[d_，γ_]；

步骤四：根据位姿信息[d_，γ_]得到对接环目标坐标系相对于机械臂基座系的位姿矩阵^bT_t：

步骤五：利用对接环目标坐标系相对于机械臂基座系的位姿矩阵^bT_t得到机械臂细分的关节轨迹，所述机械臂细分的关节轨迹包括：关节位置矢量、关节速度矢量，关节加速度矢量；

所述步骤一的具体步骤为：

步骤一一：首先通过全局相机获取翻滚卫星实时测量数据[d，α，β，0]，然后根据测量数据[d，α，β，0]得到对接环圆心坐标系相对于全局相机坐标系的位姿矩阵^gT_o，^gT_o表示为：

^gT_o＝wz2mtrx(d，α，β)

其中，wz2mtrx()表示位姿信息到位姿矩阵的转换函数；

步骤一二：获取对接环的半径r，并根据对接环的半径r以及对接环圆心坐标系相对于全局相机坐标系的位姿矩阵^gT_o得到对接环目标坐标系相对于全局相机坐标系的位姿矩阵^gT_t，^gT_t表示为：

其中，I_3×3表示3×3的单位矩阵，0_1×3表示1行3列的0矢量。

2.根据权利要求1所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述机械臂基座系相对于末端工具系的位姿矩阵^eT_b表示为：

^eT_b＝fdkine(θ，l，^bT₀)

其中，θ表示所有关节角度，l表示机械臂连杆长度参数，^bT₀表示常值矩阵，

fdkine()表示正运动学求解函数。

3.根据权利要求2所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述步骤三的具体步骤为：

步骤三一：获取服务飞行器上全局相机坐标系相对于机械臂基座系的位姿矩阵^bT_g，然后根据^gT_t、^eT_b以及^bT_g得到对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t，^eT_t表示为：

^eT_t＝^eT_b ^bT_g ^gT_t；

步骤三二：将^eT_t转换为位姿信息[d_，γ_]，位姿信息[d_，γ_]表示为：

[d_，γ_]＝mtrx2wz(^eT_t)

其中，d_表示对接环目标坐标系原点在末端工具系下的位置矢量，γ_表示对接环目标坐标系相对于末端工具系的ZYX欧拉角，mtrx2wz()表示位姿矩阵到位姿信息的转换函数。

4.根据权利要求3所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述步骤四的具体步骤为：

步骤四一：基于位姿信息[d_，γ_]分别计算6个方向上的标志位，标志位表示为：

其中，[i]表示矢量的第i个元素，i的取值为1～6，^eV_t，k表示k时刻机械臂的末端速度，a_e表示末端速度调整幅值，a_e为常值矢量，|[d_，γ_][i]|表示矢量[d_，γ_]的第i个元素的绝对值，sign()表示符号函数，6个方向包括3个位置方向和3个姿态方向；

步骤四二：利用标志位得到k+1时刻机械臂的末端速度^eV_t，k+1[i]，具体为：

flag[i]＞0，^eV_t，k+1[i]＝^eV_t，k[i]+a_e[i]·Δt·sign([d_，γ_][i])

其中，λ表示均衡系数，v_max表示机械臂末端速度的限幅值，v_max为常值矢量，Δt表示计算周期；

当k+1时刻机械臂的末端速度^eV_t，k+1[i]大于v_max[i]时，对^eV_t，k+1[i]进行修正，表示为：

^eV_t，k+1[i]＝v_max[i]·sign([d_，γ_][i])；

步骤四三：将修正后的^eV_t，k+1[i]转换为位姿矩阵^eT_t，并以对接环目标坐标系相对于机械臂基座系的位姿矩阵^bT_t表示。

5.根据权利要求4所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述对接环目标坐标系相对于末端工具系的位姿矩阵^eT_t表示为：

其中，rotx、roty和rotz分别表示绕z，y，x的单位旋转矩阵函数。

6.根据权利要求5所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述对接环目标坐标系相对于机械臂基座系的位姿矩阵^bT_t表示为：

^bT_t＝(^eT_b)^-1eT_t。

7.根据权利要求6所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述步骤五的具体步骤为：

步骤五一：指定关节，然后基于指定关节的位置级逆运动学将对接环目标坐标系相对于机械臂基座系的位姿矩阵^bT_t转换为机械臂关节空间轨迹θ_d，所述机械臂关节空间轨迹θ_d表示为：

θ_d＝ikine(^bT_t)

其中，ikine()表示位置级逆运动学函数；

步骤五二：对θ_d进行二次多项式插补，得到细分的关节轨迹：

其中，q_d为细分得到的关节位置矢量，

为细分得到的关节速度矢量，/>

为细分得到的关节加速度矢量，interpolator()是插补函数。

8.根据权利要求7所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述机械臂为7关节机械臂。

9.根据权利要求8所述的一种机械臂远距离跟踪翻滚卫星对接环的运动规划方法，其特征在于所述步骤五一中指定的关节为：关节1、关节2、关节3、关节5、关节6以及关节7。

Images