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

权利要求书

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

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

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

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

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

步骤三、利用步骤一获得的^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_,γ_]；

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

步骤四、基于三角函数规划机械臂末端的期望路径，使步骤三获得的[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个元素，通过以下公式获得：

若时间变量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)；

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

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