CN112698574A - 一种基于混合任务优先级的双臂空间机器人协调控制方法 - Google Patents

Abstract

本发明提供了一种基于混合任务优先级的双臂空间机器人协调控制方法，首先设定双臂空间机器人系统的三个任务，并且设定优先级顺序；随后构建双臂空间机器人的运动学模型，依据模型构建双臂空间机器人逆运动学方程并根据任务1和任务2求得逆解；构建避障算法并设计零空间自适应投影算子系数实现任务3，最后构建闭环运动学控制器，实现机械臂末端相对运动以及主机械臂位置跟踪误差收敛。本发明考虑基座与机械臂之间的耦合，并设计了基于混合任务优先级的控制器，可以使得双臂空间机器人可以同时执行多个冲突任务而且可以提高系统的控制性能，以保证在障碍环境下，双臂空间机器人能够实现主臂末端的轨迹跟踪和双臂末端的协调控制。

Description

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

技术领域

本发明属于空间机器人控制领域，特别是一种基于混合任务优先级的双臂空间机器人协调控制方法。

背景技术

随着太空探索的不断深入，空间机器人代替宇航员执行危险操作已经是必然趋势，各国都大力开展研究。相较于单臂空间机器人，双臂空间机器人的工作灵活性更高、可操作性更强，控制难度相应也更高。不同于固定基座的机器人，在微重力环境下自由漂浮空间机器人存在多重动力学耦合，机械臂的运动会对空间机器人漂浮基产生扰动，反之，基座的运动也会影响机械臂末端操作的稳定性和准确性。因此，双臂空间机器人协调一直是空间机器人研究难点之一。

除了末端控制要求，空间机器人在运动过程中会遇到碎片等太空垃圾，机械臂的运动范围受到限制，若运动过程中无法避开障碍物，将对机械臂造成损害。针对机械臂多任务要求，目前常用的控制方法为：扩展任务法和任务优先级法。然而，前者易导致系统奇异；后者易导致低级任务的失效。因此，规划好多个冲突任务的优先级，考虑基座与机械臂在运动过程中的耦合影响，设计好合理的控制方法是非常重要的。

发明内容

本发明所解决的技术问题在于提供一种基于混合任务优先级的双臂空间机器人协调控制方法，以保证在障碍环境下，双臂空间机器人能够实现主臂末端的轨迹跟踪和双臂末端的协调控制。

实现本发明的技术方案为：一种基于混合任务优先级的双臂空间机器人协调控制方法，包括以下步骤：

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

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

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

步骤4：构建基于梯度投影法的避障算法；

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

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

本发明与现有技术相比，其显著优点在于：

(1)考虑基座与机械臂之间的耦合，利用改进的雅克比矩阵建立双臂空间机器人的运动学模型，从而获得机械臂末端的相对位置和姿态；

(2)采用了基于混合任务优先级的控制器，不仅可以使得双臂空间机器人可以同时执行多个冲突任务而且可以提高系统的控制性能；

(3)采用了双曲正切函数作为零空间投影算子的系数，使得低级任务可以实现连续切换；

(4)改进了基于自适应变阻尼因子的阻尼最小二乘法，用于求取雅克比矩阵的逆解，可以避免算法奇异。

下面结合附图对本发明做进一步详细描述。

附图说明

图1为本发明的双臂空间机器人系统协调控制方法框图。

图2为本发明的双臂空间机器人系统结构示意图。

图3为本发明的实施例中三连杆平面双臂空间机器人的初始结构示意图。

图4为本发明的实施例中三连杆平面双臂空间机器人的最终结构示意图。

图5为本发明的实施例中投影算子系数仿真示意图。

图6为本发明的实施例中基座的位置和角度仿真结果图。

图7为本发明的实施例中左臂和右臂连杆角度仿真结果图。

图8为本发明的实施例中双臂空间机器人系统与障碍物的最短距离仿真结果图。

图9为本发明的实施例中双臂末端相对位置误差仿真结果示意图。

图10为本发明的实施例中双臂空间机器人系统左臂末端位置跟踪误差仿真示意图。

具体实施方式

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

步骤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为惯性坐标系下系统质心的位置；

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

步骤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：确定双臂空间机器人系统的运动学方程：

主臂的运动学方程为：

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

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

步骤3-1：针对雅克比矩阵求逆时易出现奇异的问题，设计自适应阻尼因子λ：

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

以雅克比矩阵J为例，得到的雅克比矩阵的阻尼最小二乘逆为

J⁺＝J^T(JJ^T+λI)^-1

由于阻尼项的存在，该逆解将不会存在奇异问题。

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

其中，

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

步骤4：根据任务3的避障要求，根据双臂空间机器人系统安全阈值，，构建基于梯度投影法的避障算法，具体为：

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

为：

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

在具体实施梯度法进行避障时，除了可以采用传感器进行距离的测量，还可以预先确定距离障碍物最近的连杆，在此基础上计算出障碍物到该连杆的最短距离d。

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

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

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

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

λ₁与λ₂正相关，这些参数的选取与系统设定的避障安全阈值有关，系数可以根据系统与障碍物的实时最短距离d进行自适应调整，当系统与障碍物的距离小于安全阈值，随着距离越来越小，低级任务的权重越来越高，以确保避障任务的有效性。

步骤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臂末端期望位置。

一种基于混合任务优先级的双臂空间机器人协调控制系统，包含以下模块：

运动学模型构建模块：用于构建双臂空间机器人的运动学模型；

运动学逆解求解模块：用于构建双臂空间机器人逆运动学方程，根据主臂末端任务和双臂末端相对任务要求，求得双臂空间机器人运动学逆解；

避障模块：用于构建基于梯度投影法的机器人避障算法，并设计零空间自适应投影算子系数；

闭环运动学控制模块：用于构建闭环运动学控制器，实现机械臂末端相对运动以及主机械臂位置跟踪误差收敛。

一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：

步骤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为惯性坐标系下系统质心的位置；

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

步骤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：确定双臂空间机器人系统的运动学方程：

主臂的运动学方程为：

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

步骤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臂末端期望位置。

一种计算机可存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行以下步骤：

步骤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为惯性坐标系下系统质心的位置；

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

步骤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：确定双臂空间机器人系统的运动学方程：

主臂的运动学方程为：

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

步骤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臂末端期望位置。

下面结合实施例对本发明作进一步说明。

实施例

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

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

步骤2：结合图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为惯性坐标系下系统质心的位置；

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

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

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

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

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

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

本实施例中，以三连杆平面自由漂浮双臂空间机器人为仿真对象，相关参数如下：

基座

<sup>a</sup>L<sub>1</sub>

<sup>a</sup>L<sub>2</sub>

<sup>a</sup>L<sub>3</sub>

<sup>b</sup>L<sub>1</sub>

<sup>b</sup>L<sub>2</sub>

<sup>b</sup>L<sub>3</sub>

质量(kg)

m<sub>0</sub>＝40

<sup>a</sup>m<sub>1</sub>＝2

<sup>a</sup>m<sub>2</sub>＝1

<sup>a</sup>m<sub>3</sub>＝1

<sup>b</sup>m<sub>1</sub>＝2

<sup>b</sup>m<sub>2</sub>＝1

<sup>b</sup>m<sub>3</sub>＝1

长度(m)

l<sub>0</sub>＝1.5

<sup>a</sup>l<sub>1</sub>＝3

<sup>a</sup>l<sub>2</sub>＝3

<sup>a</sup>l<sub>3</sub>＝3

<sup>b</sup>l<sub>1</sub>＝3

<sup>b</sup>l<sub>2</sub>＝3

<sup>b</sup>l<sub>3</sub>＝3

圆形障碍物中心设置于r_c＝[-0.2m,5m]^T，半径为0.2米；双臂末端夹持的圆形工件半径为0.75米。

结合图3，系统的初始角度为

初始角速度为

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

A臂三个连杆基矢量分别为

^ae₁＝[cos(θ₀+^aθ₁),sin(θ₀+^aθ₁)]^T

^ae₂＝[cos(θ₀+^aθ₁+^aθ₂),sin(θ₀+^aθ₁+^aθ₂)]^T

^ae₃＝[cos(θ₀+^aθ₁+^aθ₂+^aθ₃),sin(θ₀+^aθ₁+^aθ₂+^aθ₃)]^T；

B臂三个连杆基矢量分别为

^be₁＝[cos(θ₀+^bθ₁),sin(θ₀+^bθ₁)]^T

^be₂＝[cos(θ₀+^bθ₁+^bθ₂),sin(θ₀+^bθ₁+^bθ₂)]^T

^be₃＝[cos(θ₀+^bθ₁+^bθ₂+^bθ₃),sin(θ₀+^bθ₁+^bθ₂+^bθ₃)]^T；

获取K₀＝0、^aK₁＝-0.1875、^aK₂＝-0.0938、^aK₂＝-0.0313、^bK₁＝-0.1875、^bK₂＝-0.0938、^bK₃＝-0.0313；a₀＝0.75；^aa₁＝^aa₂＝^aa₃＝^ba₁＝^ba₂＝^ba₃＝1.5

则惯性坐标系下，基座位置r0为

r₀＝-0.1875^ae₁-0.0938^ae₂-0.0313^ae₃-0.1875^be₁-0.0938^be₂-0.0313^be₃；

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

其中，

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

在本实施例中：

A臂的末端位置为

B臂的末端位置为

步骤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}的旋转矩阵；

本实施例中，

坐标系{A_b}相对于坐标系{A_e}的旋转矩阵

为

坐标系{B_b}相对于坐标系{A_e}的旋转矩阵

为

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

主臂的运动学方程为：

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

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

步骤3-1：针对雅克比矩阵求逆时易出现奇异的问题，设计自适应阻尼因子λ：

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

本实施例中，ε＝0.01，λ₀＝10

以雅克比矩阵J为例，得到的雅克比矩阵的阻尼最小二乘逆为

J⁺＝J^T(JJ^T+λI)^-1

由于阻尼项的存在，该逆解将不会存在奇异问题。

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

其中，

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

步骤4：根据任务3的避障要求，根据双臂空间机器人系统安全阈值，，构建基于梯度投影法的避障算法，具体为：

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

为：

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

在具体实施梯度法进行避障时，除了可以采用传感器进行距离的测量，还可以预先确定距离障碍物最近的连杆，在此基础上计算出障碍物到该连杆的最短距离d。

在本实施例中，结合图4，期望的双臂末端相对位置为P_{R_d}＝[1.5m,0m]^T，相对姿态为Φ_{R_d}＝0，左臂由初始位置沿直线运动至为^ar_{e_d}＝[-0.15m,6.7824m]^T。机械臂在运动过程中，左臂第三个连杆到障碍物的距离最近，计算障碍物到左臂第三个连杆的距离即为最短距离d，在此基础上计算梯度为

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

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

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

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

λ₁与λ₂正相关，这些参数的选取与系统设定的避障安全阈值有关，系数可以根据系统与障碍物的实时最短距离d进行自适应调整，当系统与障碍物的距离小于安全阈值，随着距离越来越小，低级任务的权重越来越高，以确保避障任务的有效性。

结合图5，本实施例中，a^#＝10，λ₁＝18，λ₂＝60，安全阈值D设为0.35米。 K^#随着d的增大而减小，并且在越接近安全阈值，其增大的速度越来越大，从而可以提高避障算法的权重，使系统实现安全避障。

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

闭环逆运动学控制器为：

其中，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臂末端期望位置。

本实施例中，K_R＝diag{30,30,30}，^aK_e＝diag{5,5}，由于任务1的双臂协调运动和任务2的主臂位置跟踪在本实施例中的同等重要，故选取权重参数为ω₁＝ω₂＝1。

基于MATLAB进行仿真，得到的仿真结果如图3-图10所示。

从仿真结果可以看出，本文所提出的控制算法可以保证自由漂浮双臂空间机器人在障碍约束下实现主臂的位置跟踪和双臂末端的协调运动。从图6中可以看出，基座的位姿受到了两臂运动的影响，发生了一些偏差，但是对机械臂的反作用被控制器成功补偿。图7为两只机械臂连杆的角度变化仿真图，从图中可以看出A臂和B臂的第2个连杆在避障算法的实现中起了关键作用。图8为障碍物与系统的最短距离仿真结果图。由图9和图10可以看出，任务1：双臂末端相对位置误差和任务2：左臂末端位置跟踪误差均达到10^-4m数量级。

本实施例采用基于混合优先级的双臂空间机器人协调控制，首先基于改进的相对雅克比矩阵建立了双臂空间机器人的运动学模型；针对雅克比矩阵逆解容易出现奇异的问题，设计了自适应阻尼因子的阻尼最小二乘法用于避免算法奇异；此外设计了投影算子系数，提高了控制器避障的实时性与有效性。仿真结果表明，双臂空间机器人系统在10秒以内达到稳定状态，并且稳定度都能达到10^-4m数量级，补偿了基座与机械臂之间的耦合影响，使得系统可以同时执行多个冲突任务，验证了本发明的有效性。

Claims (10)

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

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

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

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

步骤4：构建基于梯度投影法的避障算法；

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

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

2.根据权利要求1所述的基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，步骤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：确定双臂空间机器人系统的运动学方程：

主臂的运动学方程为：

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

3.根据权利要求2所述的基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，将所述惯性坐标系原点建立在双臂空间机器人系统的质心上，则r_g＝0，基座位置r₀为

4.根据权利要求1所述的基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，步骤3中所述求得双臂空间机器人系统运动学逆解具体包含以下步骤：

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

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

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

其中，

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

5.根据权利要求1所述的基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，所述步骤4中的构建基于梯度投影法的避障算法具体为：

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

为：

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

6.根据权利要求2所述的基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，所述步骤5中的设计零空间自适应投影算子系数具体为：

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

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

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

7.根据权利要求5所述的基于混合任务优先级的双臂空间机器人协调控制方法，其特征在于，所述步骤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臂末端期望位置。

8.一种基于混合任务优先级的双臂空间机器人协调控制系统，其特征在于，包含以下模块：

运动学模型构建模块：用于构建双臂空间机器人的运动学模型；

运动学逆解求解模块：用于构建双臂空间机器人逆运动学方程，根据主臂末端任务和双臂末端相对任务要求，求得双臂空间机器人运动学逆解；

避障模块：用于构建基于梯度投影法的机器人避障算法，并设计零空间自适应投影算子系数；

闭环运动学控制模块：用于构建闭环运动学控制器，实现机械臂末端相对运动以及主机械臂位置跟踪误差收敛。

9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-7中任一项所述方法的步骤。

10.一种计算机可存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-7中任一项所述的方法的步骤。

Images