[发明专利]一种基于混合任务优先级的双臂空间机器人协调控制方法

权利要求书

1.一种基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，包括以下步骤：

步骤1：设定双臂空间机器人系统的三个任务，并且设定优先级从高到低的顺序依次为：任务1：双臂末端的协调运动；任务2：主臂末端的轨迹跟踪以及任务3：避障；

步骤2：根据基座与机械臂的耦合作用，构建双臂空间机器人的运动学模型，具体为：

步骤2-1：确定双臂空间机器人系统的质心：

其中，r₀为惯性坐标系下机器人基座质心的位置；n_a，n_b分别为机器人A臂、B臂连杆的个数；m₀为基座的质量；^am_k，^bm_k分别为A臂、B臂第k个连杆的质量；M为系统的总质量，^ar_k为惯性坐标系下A臂第k个连杆质心的位置；^br_k为惯性坐标系下B臂第k个连杆质心的位置；r_g为惯性坐标系下系统质心的位置；

步骤2-2：确定双臂空间机器人基矢量：

基座的基矢量为e₀＝[cosθ₀,sinθ₀]^T；

A臂第k个连杆的基矢量分别为

B臂第k个连杆的基矢量分别为

其中，θ₀为基座的角度，^aθ_i为A臂第i个连杆的角度，^bθ_i为B臂第i个连杆的角度；

步骤2-3：确定两只机械臂末端的位置分别为：

其中，a₀为基座长度l₀的一半；^al_k、^bl_k分别为A臂、B臂第k个连杆的长度；

步骤2-4：确定主臂的末端速度，设置A臂为主臂，则A臂末端的速度为：

其中，^av_e为A臂末端的速度，^aJ为A臂的雅克比矩阵，^aθ、分别为A臂的n_a个连杆的角度、角速度向量，^bθ、为B臂的n_b个连杆的角度、角速度向量；θ＝[θ₀,^aθ^T,^bθ^T]^T为系统的角度向量，为系统的角速度向量；

步骤2-5：确定双臂空间机器人系统的相对雅克比矩阵J_R：

其中，是A臂末端的坐标系{A_b}相对于A臂第一个关节所在坐标系{A_e}的旋转矩阵；是B臂末端坐标系{B_b}相对于坐标系{A_e}的旋转矩阵；

步骤2-6：确定双臂空间机器人系统的运动学方程：

主臂的运动学方程为：

双臂末端的相对运动学方程为：

将所述惯性坐标系原点建立在双臂空间机器人系统的质心上，则r_g＝0，基座位置r₀为

步骤3：构建双臂空间机器人逆运动学方程，根据主臂末端任务和双臂末端相对任务要求，求得双臂空间机器人运动学逆解，具体为：

步骤3-1：设计自适应阻尼因子λ：

其中，ε＞0，为最小奇异边界值，λ₀为阻尼系数；λ_min为阻尼因子最小值，由雅克比矩阵奇异值分解得到；

步骤3-2：根据设定的任务1和任务2要求，获得双臂空间机器人的运动学逆解：

其中，是J_R的阻尼最小二乘逆，^aJ⁺是^aJ的阻尼最小二乘逆，v_Rd为期望相对末端运动速度；^av_ed为期望主臂末端的运动速度；

步骤4：构建基于梯度投影法的避障算法，具体为：

设计基于梯度投影法的避障算法，梯度函数为：

其中，d表示自由双臂空间机器人系统距离障碍物最近的距离，θ₀为基座的角度，^aθ_i和^bθ_i分别表示A臂的i个连杆的角度和B臂的i个连杆的角度；

步骤5：设计零空间自适应投影算子系数，保证任务之间的切换连续并且保证高优先级任务的执行顺序，具体为：

根据机械臂与障碍物的实时最短距离d设计零空间自适应投影算子系数K^#：

K^#＝a^#[tanh(λ₁-λ₂d)+1]

其中，a^#,λ₁,λ₂为设定的标量参数；

步骤6：构建闭环运动学控制器，实现机械臂末端相对运动以及主机械臂位置跟踪误差收敛，具体为：

闭环逆运动学控制器为：

其中，v_Rd为期望相对末端运动速度；^av_ed为期望A臂末端的运动速度；

P_E为投影算子：P_E＝P_RP_AB，P_AB＝P_A+P_B，

K_R,^aK_e为正定增益矩阵；ω₁,ω₂分别为任务1和任务2的权重，e_R和^ae分别为双臂末端的相对误差和A臂末端的跟踪误差，e_R＝[P_{R_d}-P_R,Φ_{R_d}-Φ_R]^T，^ae＝^ar_{e_d}-^ar_e，P_R、Φ_R分别为双臂末端的相对位置和相对姿态；P_{R_d}表示双臂末端期望相对位置；Φ_{R_d}表示双臂末端期望相对姿态；^ar_{e_d}表示A臂末端期望位置。