CN114407013A - 一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法 - Google Patents

Abstract

一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，涉及轨迹规划领域。本发明是为了解决现有机械臂伺服规划技术应用到失效卫星上时无法锁定失效卫星对接环的抓捕点导致跟踪过程中机械臂末端轨迹在对接环上漂移的问题。本发明包括：获取机械臂基座系相对于末端工具系的位姿矩阵^eT_b；获得对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_g；利用^eT_b中机械臂基座系的姿态信息，以末端工具系为参考坐标系，重构^eT_g，获得重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]；基于三角函数规划机械臂末端期望路径，使[d_，γ_]在预设时间内收敛到抓捕容差内；利用机械臂末端期望路径获取机械臂细分关节轨迹。本发明用于规划机械臂伺服对接圆环的运动轨迹。

Description

一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法

技术领域

本发明涉及轨迹规划领域，特别涉及一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法。

背景技术

失效卫星没有合作的标志器，为了捕获这类卫星，在机械臂末端安装手眼相机，通过测量卫星上的典型特征获取末端工具系与目标之间的相对位姿。失效的卫星通用特征包括对接环，帆板支架，天线等。其中对接环有足够的强度，可作为末端工具的抓持接口，因此，对接环是失效卫星最为理想的伺服靶标和抓捕接口。此外，失效目标一般处于旋转耦合章动的翻滚状态，捕获这类目标对机械臂的伺服规划技术提出了巨大的挑战。

当前的机械臂伺服规划技术，主要针对合作目标，如日本的ETS-VII机械臂、美国的OrbitExpress机械臂和国际空间站机械臂等，其伺服对象往往是静止不动，因此对伺服规划的时间没有限制，另外，相机测量合作靶标可以准确锁定目标，其抓捕点坐标系与目标对象是固连的，因此，机械臂可以直接根据视觉信息的测量反馈，规划关节运动跟踪目标。但是失效卫星没有合作的标志器，且失效目标一般处于旋转耦合章动的翻滚状态。将现有技术应用到失效卫星中，一方面，翻滚卫星对接环为典型的圆特征，具有全向对称性，相机无法解算出目标的滚转状态，会导致反馈的抓捕点信息不是固定的抓捕点而是在对接圆环上滑动的动态抓捕点，导致跟踪过程中机械臂末端轨迹在对接环上漂移；另一方面，翻滚卫星与机械臂存在相对运动，机械臂无法在指定时间内完成目标的伺服跟踪就会导致目标漂出机械臂的工作空间。

发明内容

本发明目的是为了解决现有机械臂伺服规划技术应用到失效卫星上时无法锁定失效卫星对接环的抓捕点导致跟踪过程中机械臂末端轨迹在对接环上漂移的问题，同时还存在无法在指定时间内完成伺服导致目标漂出机械臂的工作空间的问题，而提出了一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法。

一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法具体过程为：

步骤一、根据机械臂关节角度信息获取机械臂基座系相对于末端工具系的位姿矩阵^eT_b；

所述机械臂包括7个关节；

步骤二、根据对接环抓捕点坐标系相对于末端工具系的位姿信息[d，γ]，获得对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_g；

其中，d是对接环抓捕点坐标系原点在末端工具系下的位置矢量，γ是对接环抓捕点坐标系相对于末端工具系的ZYX欧拉角；

步骤三、利用步骤一获得的^eT_b中机械臂基座系的姿态信息，以末端工具系为参考坐标系，重构步骤二获得的^eT_g，获得重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]；

其中，d_是重构后的对接环抓捕点坐标系原点在末端工具系下的位置矢量，γ_是重构后的对接环抓捕点坐标系相对于末端工具系的ZYX欧拉角；

步骤四、基于三角函数规划机械臂末端的期望路径，使步骤三获得的[d_，γ_]在预设时间内收敛到抓捕容差内；

步骤五、利用步骤四获得的机械臂末端的期望路径获取机械臂细分的关节轨迹；

所述机械臂细分的关节轨迹包括：关节位置矢量，关节速度矢量、关节加速度矢量。

本发明的有益效果为：

1、本发明引入了机械臂基座的位姿信息，重构了目标抓捕点，将伺服的目标位置锁定在了机械臂基座安装面的平行面与对接环所在圆的切点上，避免了机械臂跟踪翻滚卫星过程中，机械臂末端轨迹在对接环上漂移的问题，解决了相机测量全向对称的翻滚卫星对接环无法锁定目标抓捕点的问题。

2、本发明引入了时间的强约束，在指定时间内，实现了机械臂末端工具系姿态、Y、Z位置方向的位置与对接环抓捕点坐标系的对中；本发明在指定的完成伺服时间内，在X位置方向实现了末端工具系与抓捕点坐标系相对距离收敛，本发明在规划上实现了伺服时间严格可控，并保证先完成目标锁定，保证抓捕工具以最佳姿态接近目标对接环，避免了目标漂出机械臂的工作空间的问题。

附图说明

图1为机械臂坐标系；

图2为伺服规划方法中涉及到的主要坐标系；

图3为伺服规划方法流程图；

图4为机械臂伺服翻滚目标过程中的关键构型图；

其中，图4(a)是伺服初始构型图；图4(b)是伺服对中构型图；图4(c)是伺服完成构型图；

图5为机械臂伺服翻滚目标过程中末端工具系与重构抓捕点坐标系的相对位姿数据曲线图；

其中，图5(a)为相对距离数据曲线图；图5(b)为相对姿态数据曲线图；

图6为关节位置细分后得到的关节空间轨迹(位置、速度和加速度)；

其中，图6(a)为细分的关节位置曲线图；图6(b)为关节速度曲线图；图6(c)是关节加速度曲线图。

具体实施方式

具体实施方式一：结合图1-2本实施方式一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法具体过程为：

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

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

^eT_b表示由机械臂基座系至末端工具系的位姿矩阵，e是末端工具系，b是机械臂基座系，^en_b，^eo_b，^ea_b分别表示基座系在末端系下的单位矢量，^en_b为基座安装面的法线矢量，^ep_b表示基座系原点在末端系下的位置矢量，[^en_b，^eo_b，^ea_b]是机械臂基座系的姿态信息，fdkine()是正运动学求解函数；

表1机械臂DH参数表

步骤二、根据安装在机械臂末端的手眼相机反馈的对接环抓捕点坐标系相对于末端工具系的位姿信息[d，γ]，获得对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_g：

式中，d为手眼相机实时反馈的对接环与末端工具系之间的相对距离，γ为手眼相机实时反馈的对接环与末端工具系之间的相对姿态，^eT_g表示由对接环抓捕点坐标系至末端工具系的位姿矩阵，g代表对接环抓捕点坐标系，^en_g，^eo_g，^ea_g分别表示对接环抓捕点坐标系在末端工具系下的单位矢量，其中^en_g矢量为对接环所在平面的法线矢量，^ea_g矢量在对接环所在平面内，并由对接环抓捕点坐标系原点指向对接环所在圆⊙O的圆心O，^ep_g表示对接环抓捕点坐标系原点在末端工具系下的位置矢量，wz2mtrx()是位姿信息到位姿矩阵转换函数。

步骤三、利用步骤一获得的机械臂基座系的姿态信息^en_b、^eo_b、^ea_b，以末端工具系为参考坐标系，重构步骤二获得的^eT_g，获得重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息：

步骤三一、利用步骤一获得的机械臂基座系的姿态信息，获取重构后的对接环抓捕点坐标系在末端工具系下的姿态矩阵^eR_g_：

步骤三一一、将重构前对接环抓捕点坐标系的X轴矢量仍作为重构后对接环抓捕点坐标系得X轴矢量，即：

^en_{g_}＝^en_g (3)

其中，下标g_代表重构后的对接环抓捕点坐标系，重构前的对接环抓捕点坐标系即为步骤二获得的^eT_g；

步骤三一二、将矢量^en_b与^en_g叉乘得到重构后的对接环抓捕点坐标系的Y轴矢量，即：

^eo_{g_}＝^en_b×^en_g (4)

步骤三一三、坐标系为右手系，所以重构后的对接环抓捕点坐标系的Z轴矢量为：

^ea_{g_}＝^en_{g_}×^eo_{g_} (5)

步骤三一四、根据步骤三一一到步骤三一三获得X、Y、Z矢量获得重构后对接环抓捕点坐标系在末端工具系下的姿态矩阵：

^eR_{g_}＝[^en_{g_} ^eo_{g_} ^ea_{g_}] (6)

步骤三二、利用步骤三一获得的^eR_{g_}获取重构后的对接环抓捕点坐标系相对于重构前对接环抓捕点坐标系的相对姿态关系^gR_{g_}：

^gR_{g_}＝[^en_g ^eo_g ^ea_g]^{T e}R_{g_} (7)

步骤三三、利用步骤三二获得相对姿态关系^gR_{g_}获取重构后的对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_{g_}：

步骤三三一、利用步骤三二获得的相对姿态关系^gR_{g_}获取重构前对接环抓捕点坐标系^eT_g转换到重构后的对接环抓捕点坐标系^eR_{g_}绕X轴旋转的角度β：

^gR_{g_}＝RotX(β) (8)

其中，RotX(·)表示绕X轴旋转的单位位姿矩阵；

步骤三三二、利用β获取重构后的对接环抓捕点坐标系^eR_{g_}原点相对于重构前对接环抓捕点坐标系^eT_g原点的距离矢量^gp_{g_}：

^gp_g-＝[0 rsin(β) r-rcos(β)]^T (9)

其中，r是对接环半径，为已知量，[]^T是转置；

步骤三三三、利用步骤三三二获得的距离矢量^gp_{g_}获取重构后的对接环抓捕点坐标系^eR_{g_}相对于末端工具系的位姿矩阵^eT_{g_}：

^eT_{g_}＝^eT_g ^gT_{g_} (10)

其中，重构后的对接环抓捕点坐标系相对于重构前对接环抓捕点坐标系的位姿矩阵为：

步骤三四、利用^eT_{g_}获取重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]；

姿态采用ZYX欧拉角表示，作为机械臂伺服规划方法的输入：

[d_，γ_]＝mtrx2wz(^eT_{g_}) (12)

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

步骤四、基于三角函数规划机械臂末端的期望路径，使步骤三获得的[d_，γ_]在预设时间内收敛到抓捕容差内；

所述抓捕容差根据抓捕工具确定；

所述机械臂末端的期望路径，通过以下方式获得：

步骤四一、获取规划初始时刻t₀时目标抓捕点相对于末端工具系的相对位姿偏差[d_{init_}，γ_{init_}]，并设定伺服开始到对准时间为t_l，伺服开始到完成时间为t_f；

相对位姿偏差和t_l、t_f均为常值，其中伺服完成时间t_f应大于对准时间t_l；

步骤四二、采用三角函数对步骤四一获得的[d_{init_}，γ_{init_}]进行规划获得初始偏差补偿量[d_c，γ_c]中每个元素，具体如下：

对于位置的补偿量d_c中的第1个元素，即末端工具系原点与目标点在工具系X方向(前进方向)的距离补偿量，通过以下方式获取：

位置补偿量d_c和姿态补偿量γ_c分别有三个元素；

若时间变量t＜t_f：

其中，时间变量t是初始时刻t₀到当前时刻的经历的时间；

当时间变量t≥t_f：

d_c(1)＝0 (14)

其中，d_{init_}(1)是t₀时刻的位置的补偿量d_{init_}中的第一个元素；

对于d_c中的另外两个元素以及ZYX欧拉角姿态补偿量γ_c：，通过以下公式获得：

若时间变量t＜t_l：

若时间变量t≥t_l：

x＝0 (16)

其中，x代表d_c(2)，d_c(3)或γ_c的三个分量，x_init为对应的d_{init_}(2)，d_{init_}(3)或γ_{init_}的三个分量。

步骤四三、利用步骤四二获得的初始偏差补偿量中每个元素获得补偿初始位姿偏差的位姿矩阵T_fill-gap：

其中，RotZ(·)和RotY(·)分别表示绕Z轴和Y轴旋转的单位位姿矩阵；

步骤四四、利用步骤四三获得的T_fill-gap规划获取末端工具系的期望路径，并以末端工具系相对于机械臂基座系的位姿矩阵表示：

^bT_r＝(^eT_b)^{-1 e}T_{g_}T_fill-gap (18)

步骤五、利用步骤四获得的机械臂末端的期望路径获取细分的关节轨迹：

步骤五一、首先选定一个关节k，基于选定的关节位置的位置级逆运动学解算期望笛卡尔位姿对应的关节角度：

θ_d＝ikine(^bT_r，θ_k) (19)

其中，θ_d为解算得到的关节期望角度矢量，对应7个关节的期望角度，θ_k为指定的关节角度，ikine()是逆运动学求解函数，k为选定的关节标号，k取{1，2，3，5，6，7}即除最中间的关节以外的关节；

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

其中，interpolator()是插补函数，q_d为细分得到的关节位置矢量，

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

为细分得到的关节加速度矢量，三者共同构成了最终的机械臂伺服翻滚卫星对接圆环的关节空间轨迹。

实施例：

为本发明的有效性，以下结合图3至图6对本发明的技术方案做进一步描述，使用计算机仿真。

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

[步骤S1]根据关节角度反馈计算^eT_b；

[步骤S2]根据关节角度反馈计算^eT_g；

[步骤S3]目标对接环半径为813.25mm，重构新的抓捕点，计算得到^eT_{g_}；

[步骤S4]从^eT_{g_}解算出新的位姿反馈[d_，γ_]；

[步骤S5]判断是否首次进入伺服，若为首次进入伺服，记录下初始位姿偏差[d_{init_}，γ_{init_}]，否则直接进入下一步；

[步骤S6]判断伺服时间t是否已经超过设定的锁定目标时间t_l，若未超过，则位置Y、Z方向以及三个姿态各自按照下列公式计算补偿量，若已经超过锁定目标时间，补偿量为0；

x代表d_c(2)，d_c(3)或γ_c的三个分量，x_init为对应的d_{init_}(2)，d_{init_}(3)或γ_{init_}的三个分量。

[步骤S7]判断伺服时间t是否已经超过设定的伺服目标时间t_f，若未超过，则位置X方向(机械臂前进方向)按照下列公式计算补偿量，若已经超过锁定目标时间，补偿量为0：

[步骤S8]根据补偿量[d_c，γ_c]，得到补偿初始位姿偏差的位姿矩阵：

[步骤S9]得到机械臂末端的期望路径：

^bT_r＝(^eT_b)^{-1 e}T_{g_}T_fill-gap

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

θ_d＝ikine(^bT_r，θ₂)

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

Claims (10)

1.一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于所述方法具体过程为：

步骤一、根据机械臂关节角度信息获取机械臂基座系相对于末端工具系的位姿矩阵^eT_b；

所述机械臂包括7个关节；

步骤二、根据对接环抓捕点坐标系相对于末端工具系的位姿信息[d，γ]，获得对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_g；

其中，d是对接环抓捕点坐标系原点在末端工具系下的位置矢量，γ是对接环抓捕点坐标系相对于末端工具系的ZYX欧拉角；

步骤三、利用步骤一获得的^eT_b中机械臂基座系的姿态信息，以末端工具系为参考坐标系，重构步骤二获得的^eT_g，获得重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]；

其中，d_是重构后的对接环抓捕点坐标系原点在末端工具系下的位置矢量，γ_是重构后的对接环抓捕点坐标系相对于末端工具系的ZYX欧拉角；

步骤四、基于三角函数规划机械臂末端的期望路径，使步骤三获得的[d_，γ_]在预设时间内收敛到抓捕容差内；

步骤五、利用步骤四获得的机械臂末端的期望路径获取机械臂细分的关节轨迹；

所述机械臂细分的关节轨迹包括：关节位置矢量，关节速度矢量、关节加速度矢量。

2.根据权利要求1所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述机械臂关节角度信息通过机械臂关节位置传感器获得。

3.根据权利要求1所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述对接环抓捕点坐标系相对于末端工具系的位姿信息通过安装在机械臂末端的手眼相机获得。

4.根据权利要求3或2所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤一中的机械臂基座系相对于末端工具系的位姿矩阵^eT_b，如下：

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

^eT_b是由机械臂基座系至末端工具系的位姿矩阵，e是末端工具系，b是机械臂基座系，^en_b，^eo_b，^ea_b分别表示基座系在末端系下的单位矢量，^ep_b表示基座系原点在末端系下的位置矢量，[^en_b，^eo_b，^ea_b]是机械臂基座系的姿态信息，fdkine()是正运动学求解函数。

5.根据权利要求4所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤二中对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_g，如下：

其中，d是对接环与末端工具系之间的相对距离，γ对接环与末端工具系之间的相对姿态，^eT_g表示由对接环抓捕点坐标系至末端工具系的位姿矩阵，g代表对接环抓捕点坐标系，^en_g，^eo_g，^ea_g分别表示对接环抓捕点坐标系在末端工具系下的单位矢量，^ea_g矢量在对接环所在平面内，并由对接环抓捕点坐标系原点指向对接环所在圆⊙O的圆心O，^ep_g表示对接环抓捕点坐标系原点在末端工具系下的位置矢量，wz2mtrx()是位姿信息到位姿矩阵转换函数。

6.根据权利要求5所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤三中的利用步骤一获得的^eT_b中机械臂基座系的姿态信息，以末端工具系为参考坐标系，重构步骤二获得的^eT_g，获得重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]，包括以下步骤：

步骤三一、利用步骤一获得的机械臂基座系的姿态信息，获取重构后的对接环抓捕点坐标系在末端工具系下的姿态矩阵^eR_{g_}，具体包括以下步骤：

步骤三一一、将^eT_g的X轴矢量仍作为重构后对接环抓捕点坐标系的X轴矢量，即：

^en_{g_}＝^en_g (3)

其中，下标g_代表重构后的对接环抓捕点坐标系；

步骤三一二、将^eT_b中的矢量^en_b与^en_g叉乘得到重构后的对接环抓捕点坐标系的Y轴矢量：

^eo_{g_}＝^en_b×^en_g (4)

步骤三一三、获取重构后的对接环抓捕点坐标系的Z轴矢量：

^ea_{g_}＝^en_{g_}×^eo_{g_} (5)

步骤三一四、根据步骤三一一到步骤三一三获得的重构后的对接环抓捕点坐标系X、Y、Z矢量获取重构后对接环抓捕点坐标系在末端工具系下的姿态矩阵：

^eR_{g_}＝[^en_{g_} ^eo_{g_} ^ea_{g_}] (6)

步骤三二、利用步骤三一获得的^eR_{g_}获取重构后的对接环抓捕点坐标系相对于^eT_g的相对姿态关系^gR_{g_}：

^gR_{g_}＝[^en_g ^eo_g ^ea_g]^TeR_{g_} (7)

步骤三三、利用步骤三二获得的^gR_{g_}获取重构后的对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_{g_}：

步骤三三一、利用步骤三二获得的^gR_{g_}，获取将^eT_g转换到^eR_{g_}绕X轴旋转的角度β：

^gR_{g_}＝RotX(β) (8)

其中，RotX(·)表示绕X轴旋转的单位位姿矩阵；

步骤三三二、利用β获取^eR_{g_}原点与^eT_g原点之间的距离矢量^gp_{g_}：

^gp_{g_}＝[0 rsin(β) r-rcos(β)]^T (9)

其中，r是对接环半径，[]^T是转置；

步骤三三三、利用步骤三三二获得的^gp_g-获取重构后的对接环抓捕点坐标系相对于末端工具系的位姿矩阵^eT_{g_}：

^eT_{g_}＝^eT_g ^gT_{g_} (10)

其中，重构后的对接环抓捕点坐标系相对于^eT_g的位姿矩阵^gT_{g_}为：

步骤三四、利用步骤三三获得的^eT_{g_}获取重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]。

7.根据权利要求6所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤三四中利用步骤三三获得的^eT_{g_}获取重构后的对接环抓捕点坐标系相对于末端工具系的位姿信息[d_，γ_]采用ZYX欧拉角表示，具体如下：

[d_，γ_]＝mtrx2wz(^eT_{g_}) (12)

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

8.根据权利要求7所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤四中基于三角函数规划机械臂末端的期望路径，通过以下方式实现：

步骤四一、获取规划初始时刻t₀时目标抓捕点相对于末端工具系的相对位姿偏差[d_{init_}，γ_{init_}]，并设定伺服开始到对准时间t_l，伺服开始到完成时间t_f；

其中，t_l、t_f均为常值且t_f＞t_l；

步骤四二、采用三角函数对步骤四一获得的[d_{init_}，γ_{init_}]进行规划获得初始偏差补偿量[d_c，γ_c]中每个元素，具体如下：

首先，位置的补偿量d_c中的第1个元素，通过以下公式获得：

若时间变量t＜t_f，则：

其中，时间变量t是初始时刻t₀到当前时刻的经历的时间；

若时间变量t≥t_f，则：

d_c(1)＝0 (14)

其中，d_{init_}(1)是t₀时刻的位置的补偿量d_{init_}中的第一个元素；

然后，d_c中的另外两个元素以及姿态补偿量γ_c中的三个元素，通过以下公式获得：

若时间变量t＜t_l，则：

若时间变量t≥t_l：

x＝0 (16)

其中，x是d_c中第二个元素d_c(2)、d_c中第三个元素d_c(3)或γ_c中的三个元素，x_init为对应的d_{init_}(2)、d_{init_}(3)或γ_{init_}中的三个元素；

步骤四三、利用步骤四二获得的初始偏差补偿量中每个元素获得补偿初始位姿偏差的位姿矩阵T_fill-gap：

其中，RotZ(·)和RotY(·)分别表示绕Z轴和Y轴旋转的单位位姿矩阵；

步骤四四、利用步骤四三获得的T_fill-gap获取末端工具系的期望路径：

^bT_r＝(^eT_b)^-1eT_{g_}T_fill-gap (18)。

9.根据权利要求8所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤五中的利用步骤四获得的机械臂末端的期望路径获取机械臂细分的关节轨迹，包括以下步骤：

步骤五一、首先选定机械臂的一个关节k，基于选定的关节位置的位置级逆运动学解算期望笛卡尔位姿对应的关节角度θ_d：

θ_d＝ikine(^bT_r，θ_k) (19)

其中，θ_d为解算得到的关节期望角度矢量，对应7个关节的期望角度，θ_k为指定关节的角度，ikine()是逆运动学求解函数，k为选定的关节标号取{1，2，3，5，6，7}；

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

10.根据权利要求9所述的一种机械臂伺服翻滚卫星对接圆环的运动轨迹规划方法，其特征在于：所述步骤五二中的细分的关节轨迹，通过以下公式获得：

其中，interpolator()是插补函数，q_d为细分得到的关节位置矢量，

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

为细分得到的关节加速度矢量。

Images