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

摘要

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

权利要求

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

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

步骤2：根据基座与机械臂的耦合作用，构建双臂空间机器人的运动学模型，具体为：步骤2‑1：确定双臂空间机器人系统的质心：其中，r0为惯性坐标系下机器人基座质心的位置；na，nb分别为机器人A臂、B臂连杆的个a b

数；m0为基座的质量；mk，mk分别为A臂、B臂第k个连杆的质量；M为系统的总质量，a b

rk为惯性坐标系下A臂第k个连杆质心的位置；rk为惯性坐标系下B臂第k个连杆质心的位置；rg为惯性坐标系下系统质心的位置；

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

基座的基矢量为e0＝[cosθ0,sinθ0]；

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

其中，θ0为基座的角度，θi为A臂第i个连杆的角度，θi为B臂第i个连杆的角度；

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

a b

a0为基座长度l0的一半；lk、lk分别为A臂、B臂第k个连杆的长度；

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

其中，ve为A臂末端的速度，J为A臂的雅克比矩阵，θ、 分别为A臂的na个连杆的角度、b a T b T T角速度向量，θ、 为B臂的nb个连杆的角度、角速度向量；θ＝[θ0,θ,θ] 为系统的角度向量， 为系统的角速度向量；

步骤2‑5：确定双臂空间机器人系统的相对雅克比矩阵JR：其中， 是A臂末端的坐标系{Ab}相对于A臂第一个关节所在坐标系{Ae}的旋转矩阵；

是B臂末端坐标系{Bb}相对于坐标系{Ae}的旋转矩阵；

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

双臂末端的相对运动学方程为：将所述惯性坐标系原点建立在双臂空间机器人系统的质心上，则rg＝0，基座位置r0为步骤3：构建双臂空间机器人逆运动学方程，根据主臂末端任务和双臂末端相对任务要求，求得双臂空间机器人运动学逆解，具体为：步骤3‑1：设计自适应阻尼因子λ：其中，ε＞0，为最小奇异边界值，λ0为阻尼系数；λmin为阻尼因子最小值，由雅克比矩阵奇异值分解得到；

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

其中， 是JR的阻尼最小二乘逆，J是J的阻尼最小二乘逆，vRd为期望相对末端运动速a

度；ved为期望主臂末端的运动速度；

步骤4：构建基于梯度投影法的避障算法，具体为：设计基于梯度投影法的避障算法，梯度函数 为：a b

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

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

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

K＝a[tanh(λ1‑λ2d)+1]#

其中，a ,λ1,λ2为设定的标量参数；

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

闭环逆运动学控制器为：a

其中，vRd为期望相对末端运动速度；ved为期望A臂末端的运动速度；

PE为投影算子：PE＝PRPAB， PAB＝PA+PB，a a

KR,Ke为正定增益矩阵；ω1,ω2分别为任务1和任务2的权重，eR和e分别为双臂末端的T a a a

相对误差和A臂末端的跟踪误差，eR＝[PR_d‑PR,ΦR_d‑ΦR] ，e＝re_d‑ re，PR、ΦR分别为双臂末端的相对位置和相对姿态；PR_d表示双臂末端期望相对位置；ΦR_d表示双臂末端期望相对a

姿态；re_d表示A臂末端期望位置。

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

运动学模型构建模块：用于构建双臂空间机器人的运动学模型，具体为：确定双臂空间机器人系统的质心：其中，r0为惯性坐标系下机器人基座质心的位置；na，nb分别为机器人A臂、B臂连杆的个a b

数；m0为基座的质量；mk，mk分别为A臂、B臂第k个连杆的质量；M为系统的总质量，a b

rk为惯性坐标系下A臂第k个连杆质心的位置；rk为惯性坐标系下B臂第k个连杆质心的位置；rg为惯性坐标系下系统质心的位置；

确定双臂空间机器人基矢量：T

基座的基矢量为e0＝[cosθ0,sinθ0]；

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

其中，θ0为基座的角度，θi为A臂第i个连杆的角度，θi为B臂第i个连杆的角度；

确定两只机械臂末端的位置分别为：其中，

a b

a0为基座长度l0的一半；lk、lk分别为A臂、B臂第k个连杆的长度；

确定主臂的末端速度，设置A臂为主臂，则A臂末端的速度为：a a a

其中，ve为A臂末端的速度，J为A臂的雅克比矩阵，θ、 分别为A臂的na个连杆的角度、b a T b T T角速度向量，θ、 为B臂的nb个连杆的角度、角速度向量；θ＝[θ0,θ,θ]为系统的角度向量， 为系统的角速度向量；

确定双臂空间机器人系统的相对雅克比矩阵JR：其中， 是A臂末端的坐标系{Ab}相对于A臂第一个关节所在坐标系{Ae}的旋转矩阵；

是B臂末端坐标系{Bb}相对于坐标系{Ae}的旋转矩阵；

确定双臂空间机器人系统的运动学方程：主臂的运动学方程为：

双臂末端的相对运动学方程为：将所述惯性坐标系原点建立在双臂空间机器人系统的质心上，则rg＝0，基座位置r0为运动学逆解求解模块：用于构建双臂空间机器人逆运动学方程，根据主臂末端任务和双臂末端相对任务要求，求得双臂空间机器人运动学逆解，具体为：设计自适应阻尼因子λ：其中，ε＞0，为最小奇异边界值，λ0为阻尼系数；λmin为阻尼因子最小值，由雅克比矩阵奇异值分解得到；

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

其中， 是JR的阻尼最小二乘逆，J是J的阻尼最小二乘逆，vRd为期望相对末端运动速a

度；ved为期望主臂末端的运动速度；

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

#

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

K＝a[tanh(λ1‑λ2d)+1]#

其中，a ,λ1,λ2为设定的标量参数；

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

其中，vRd为期望相对末端运动速度；ved为期望A臂末端的运动速度；

PE为投影算子：PE＝PRPAB， PAB＝PA+PB，a a

KR,Ke为正定增益矩阵；ω1,ω2分别为任务1和任务2的权重，eR和e分别为双臂末端的T a a a

相对误差和A臂末端的跟踪误差，eR＝[PR_d‑PR,ΦR_d‑ΦR] ，e＝re_d‑ re，PR、ΦR分别为双臂末端的相对位置和相对姿态；PR_d表示双臂末端期望相对位置；ΦR_d表示双臂末端期望相对a

姿态；re_d表示A臂末端期望位置。

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

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

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附图说明

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

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

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

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

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

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

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

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

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

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

具体实施方式

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

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

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

[0031] 步骤2‑1：确定双臂空间机器人系统的质心：

[0032]

[0033] 其中，r0为惯性坐标系下机器人基座质心的位置；na，nb分别为机器人A臂、 B臂连a b
杆的个数；m0为基座的质量；mk，mk分别为A臂、B臂第k个连杆的质量；M为系统的总质量，
a b
rk为惯性坐标系下A臂第k 个连杆质心的位置；rk为惯性坐标系下
B臂第k个连杆质心的位置；rg为惯性坐标系下系统质心的位置；

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

[0035]

[0036] 步骤2‑2：确定双臂空间机器人基矢量：

[0037] 基座的基矢量为e0＝[cosθ0,sinθ0]T；

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

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

[0040] 其中，θ0为基座的角度，aθi为A臂第i个连杆的角度，bθi为B臂第i个连杆的角度；

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

[0042]

[0043]

[0044] 其中，a b
a0为基座长度l0的一半；lk、lk分别为A臂、B臂第k个连杆的长
度；

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

[0046]

[0047] 其中，ave为A臂末端的速度，aJ为A臂的雅克比矩阵，aθ、 分别为A 臂的na个连杆的b a T b T T
角度、角速度向量，θ、 为B臂的nb个连杆的角度、角速度向量；θ＝[θ0,θ,θ] 为系统的
角度向量， 为系统的角速度向量；

[0048] 步骤2‑5：确定双臂空间机器人系统的相对雅克比矩阵JR：

[0049]

[0050] 其中， 是A臂末端的坐标系{Ab}相对于A臂第一个关节所在坐标系{Ae} 的旋转矩阵 ； 是 B臂 末 端坐 标 系{ B b} 相 对于 坐标 系 {A e }的 旋 转矩 阵 ；

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

[0052] 主臂的运动学方程为：

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

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

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

[0056]

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

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

[0059] J+＝JT(JJT+λI)‑1

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

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

[0062]a + a

[0063] 其中， 是JR的阻尼最小二乘逆，J是J的阻尼最小二乘逆，vRd为期望相对末端运a
动速度；ved为期望主臂末端的运动速度。

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

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

[0066]

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

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

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

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

[0071] K#＝a#[tanh(λ1‑λ2d)+1]# #

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

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

[0074] 闭环逆运动学控制器为：

[0075] 其中，vRd为期望相对末端运动速度；aved为期望A臂末端的运动速度；

[0076] PE为投影算子：PE＝PRPAB， PAB＝PA+PB，

[0077] KR,aKe为正定增益矩阵；ω1,ω2分别为任务1和任务2的权重，eR和ae分别为双臂末T a a a
端的相对误差和A臂末端的跟踪误差，eR＝[PR_d‑PR,ΦR_d‑ΦR]，e＝re_d‑re，PR、ΦR分别为
双臂末端的相对位置和相对姿态；PR_d表示双臂末端期望相对位置；ΦR_d表示双臂末端期望
a
相对姿态；re_d表示A臂末端期望位置。

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

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

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

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

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

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

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

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

[0086] 步骤2‑1：确定双臂空间机器人系统的质心：

[0087]

[0088] 其中，r0为惯性坐标系下机器人基座质心的位置；na，nb分别为机器人A臂、 B臂连a b
杆的个数；m0为基座的质量；mk，mk分别为A臂、B臂第k个连杆的质量；M为系统的总质量，
a b
rk为惯性坐标系下A臂第k 个连杆质心的位置；rk为惯性坐标系下
B臂第k个连杆质心的位置；rg为惯性坐标系下系统质心的位置；

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

[0090]

[0091] 步骤2‑2：确定双臂空间机器人基矢量：

[0092] 基座的基矢量为e0＝[cosθ0,sinθ0]T；

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

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

[0095] 其中，θ0为基座的角度，aθi为A臂第i个连杆的角度，bθi为B臂第i个连杆的角度；

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

[0097]

[0098]

[0099] 其中，a b
a0为基座长度l0的一半；lk、lk分别为A臂、B臂第k个连杆的长
度；

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

[0101]

[0102] 其中，ave为A臂末端的速度，aJ为A臂的雅克比矩阵，aθ、 分别为A 臂的na个连杆的b a T b T T
角度、角速度向量，θ、 为B臂的nb个连杆的角度、角速度向量；θ＝[θ0,θ,θ] 为系统的
角度向量， 为系统的角速度向量；

[0103] 步骤2‑5：确定双臂空间机器人系统的相对雅克比矩阵JR：

[0104]

[0105] 其中， 是A臂末端的坐标系{Ab}相对于A臂第一个关节所在坐标系{Ae} 的旋转矩阵 ； 是 B臂 末端 坐 标系 { Bb } 相对 于坐 标 系{ Ae }的 旋 转矩 阵 ；

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

[0107] 主臂的运动学方程为：

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

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

[0110] 步骤3‑1：设计自适应阻尼因子λ：

[0111]

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

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

[0114]a + a

[0115] 其中， 是JR的阻尼最小二乘逆，J是J的阻尼最小二乘逆，vRd为期望相对末端运a
动速度；ved为期望主臂末端的运动速度。

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

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

[0118]

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

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

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

[0122] K#＝a#[tanh(λ1‑λ2d)+1]

[0123] 其中，a#,λ1,λ2为设定的标量参数。

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

[0125] 闭环逆运动学控制器为：

[0126] 其中，vRd为期望相对末端运动速度；aved为期望A臂末端的运动速度；

[0127] PE为投影算子：PE＝PRPAB， PAB＝PA+PB，

[0128] KR,aKe为正定增益矩阵；ω1,ω2分别为任务1和任务2的权重，eR和ae分别为双臂末T a a a
端的相对误差和A臂末端的跟踪误差，eR＝[PR_d‑PR,ΦR_d‑ΦR]，e＝re_d‑re，PR、ΦR分别为
双臂末端的相对位置和相对姿态；PR_d表示双臂末端期望相对位置；ΦR_d表示双臂末端期望
a
相对姿态；re_d表示A臂末端期望位置。

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

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

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

[0132] 步骤2‑1：确定双臂空间机器人系统的质心：

[0133]

[0134] 其中，r0为惯性坐标系下机器人基座质心的位置；na，nb分别为机器人A臂、 B臂连a b
杆的个数；m0为基座的质量；mk，mk分别为A臂、B臂第k个连杆的质量；M为系统的总质量，
a b
rk为惯性坐标系下A臂第k 个连杆质心的位置；rk为惯性坐标系下
B臂第k个连杆质心的位置；rg为惯性坐标系下系统质心的位置；

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

[0136]

[0137] 步骤2‑2：确定双臂空间机器人基矢量：

[0138] 基座的基矢量为e0＝[cosθ0,sinθ0]T；

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

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

[0141] 其中，θ0为基座的角度，aθi为A臂第i个连杆的角度，bθi为B臂第i个连杆的角度；

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

[0143]

[0144]

[0145] 其中，a b
a0为基座长度l0的一半；lk、lk分别为A臂、B臂第k个连杆的长
度；

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

[0147]a a a

[0148] 其中，ve为A臂末端的速度，J为A臂的雅克比矩阵，θ、 分别为A臂的na个连杆的b a T b T T
角度、角速度向量，θ、 为B臂的nb个连杆的角度、角速度向量；θ＝[θ0,θ,θ] 为系统的
角度向量， 为系统的角速度向量；

[0149] 步骤2‑5：确定双臂空间机器人系统的相对雅克比矩阵JR：

[0150]

[0151] 其中， 是A臂末端的坐标系{Ab}相对于A臂第一个关节所在坐标系{Ae} 的旋转矩阵 ； 是 B臂 末 端坐 标 系{ B b} 相 对于 坐标 系 {A e }的 旋 转矩 阵 ；

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

[0153] 主臂的运动学方程为：

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

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

[0156] 步骤3‑1：设计自适应阻尼因子λ：

[0157]

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

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

[0160]

[0161] 其中， 是JR的阻尼最小二乘逆，aJ+是aJ的阻尼最小二乘逆，vRd为期望相对末端运a
动速度；ved为期望主臂末端的运动速度。

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

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

[0164]

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

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

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

[0168] K#＝a#[tanh(λ1‑λ2d)+1]

[0169] 其中，a#,λ1,λ2为设定的标量参数。

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

[0171] 闭环逆运动学控制器为：

[0172] 其中，vRd为期望相对末端运动速度；aved为期望A臂末端的运动速度；

[0173] PE为投影算子：PE＝PRPAB， PAB＝PA+PB，

[0174] KR,aKe为正定增益矩阵；ω1,ω2分别为任务1和任务2的权重，eR和ae分别为双臂末T a a a
端的相对误差和A臂末端的跟踪误差，eR＝[PR_d‑PR,ΦR_d‑ΦR]，e＝re_d‑re，PR、ΦR分别为
双臂末端的相对位置和相对姿态；PR_d表示双臂末端期望相对位置；ΦR_d表示双臂末端期望
a
相对姿态；re_d表示A臂末端期望位置。

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

[0176] 实施例

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

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

[0179] 步骤2：结合图2，根据基座与机械臂的耦合作用，构建双臂空间机器人的运动学模型，具体包含以下步骤：

[0180] 步骤2‑1：确定双臂空间机器人系统的质心：

[0181]

[0182] 其中，r0为惯性坐标系下机器人基座质心的位置；na，nb分别为机器人A臂、 B臂连a b
杆的个数；m0为基座的质量；mk，mk分别为A臂、B臂第k个连杆的质量；M为系统的总质量，
a b
rk为惯性坐标系下A臂第k 个连杆质心的位置；rk为惯性坐标系下
B臂第k个连杆质心的位置；rg为惯性坐标系下系统质心的位置；

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

[0184]

[0185] 步骤2‑2：确定双臂空间机器人基矢量：

[0186] 基座的基矢量为e0＝[cosθ0,sinθ0]T；

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

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

[0189] 其中，θ0为基座的角度，aθi为A臂第i个连杆的角度，bθi为B臂第i个连杆的角度；

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

[0191]   基座 aL1 aL2 aL3 bL1 bL2 bL3a a a b b b
质量(kg) m0＝40 m1＝2 m2＝1 m3＝1 m1＝2 m2＝1 m3＝1
a a a b b b
长度(m) l0＝1.5 l1＝3 l2＝3 l3＝3 l1＝3 l2＝3 l3＝3

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

[0193] 结合图3，系统的初始角度为 初始角速度为

[0194] 基座的基矢量为e0＝[cosθ0,sinθ0]T；

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

[0196] ae1＝[cos(θ0+aθ1),sin(θ0+aθ1)]T

[0197] ae2＝[cos(θ0+aθ1+aθ2),sin(θ0+aθ1+aθ2)]T

[0198] ae3＝[cos(θ0+aθ1+aθ2+aθ3),sin(θ0+aθ1+aθ2+aθ3)]T；

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

[0200] be1＝[cos(θ0+bθ1),sin(θ0+bθ1)]T

[0201] be2＝[cos(θ0+bθ1+bθ2),sin(θ0+bθ1+bθ2)]T

[0202] be3＝[cos(θ0+bθ1+bθ2+bθ3),sin(θ0+bθ1+bθ2+bθ3)]T；

[0203] 获取K0＝0、aK1＝‑0.1875、aK2＝‑0.0938、aK2＝‑0.0313、bK1＝‑0.1875、bK2＝‑b a a a b b b
0.0938、K3＝‑0.0313；a0＝0.75；a1＝a2＝a3＝a1＝a2＝a3＝1.5

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

[0205] r0＝‑0.1875ae1‑0.0938ae2‑0.0313ae3‑0.1875be1‑0.0938be2‑0.0313be3；

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

[0207]

[0208]

[0209] 其中，a b
a0为基座长度l0的一半；lk、lk分别为A臂、B 臂第k个连杆的
长度；

[0210] 在本实施例中：

[0211] A臂的末端位置为

[0212]

[0213] B臂的末端位置为

[0214]

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

[0216]a a a

[0217] 其中，ve为A臂末端的速度，J为A臂的雅克比矩阵，θ、 分别为A臂的na个连杆的b a T b T T
角度、角速度向量，θ、 为B臂的nb个连杆的角度、角速度向量；θ＝[θ0,θ,θ] 为系统的
角度向量， 为系统的角速度向量；

[0218] 步骤2‑5：确定双臂空间机器人系统的相对雅克比矩阵JR：

[0219]

[0220] 其中， 是A臂末端的坐标系{Ab}相对于A臂第一个关节所在坐标系{Ae} 的旋转矩阵 ； 是 B 臂末 端 坐标 系 {B b} 相 对于 坐 标系 { Ae }的 旋 转矩 阵 ；

[0221] 本实施例中，

[0222] 坐标系{Ab}相对于坐标系{Ae}的旋转矩阵 为

[0223]

[0224] 坐标系{Bb}相对于坐标系{Ae}的旋转矩阵 为

[0225]

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

[0227] 主臂的运动学方程为：

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

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

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

[0231]

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

[0233] 本实施例中，ε＝0.01，λ0＝10

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

[0235] J+＝JT(JJT+λI)‑1

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

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

[0238]

[0239] 其中， 是JR的阻尼最小二乘逆，aJ+是aJ的阻尼最小二乘逆，vRd为期望相对末端运a
动速度；ved为期望主臂末端的运动速度。

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

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

[0242]

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

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

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

[0246]

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

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

[0249] K#＝a#[tanh(λ1‑λ2d)+1]# #

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

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

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

[0253] 闭环逆运动学控制器为：

[0254] 其中，vRd为期望相对末端运动速度；aved为期望A臂末端的运动速度；

[0255] PE为投影算子：PE＝PRPAB， PAB＝PA+PB，

[0256] KR,aKe为正定增益矩阵；ω1,ω2分别为任务1和任务2的权重，eR和ae分别为双臂末T a a a
端的相对误差和A臂末端的跟踪误差，eR＝[PR_d‑PR,ΦR_d‑ΦR]，e＝re_d‑re，PR、ΦR分别为
双臂末端的相对位置和相对姿态；PR_d表示双臂末端期望相对位置；ΦR_d表示双臂末端期望
a
相对姿态；re_d表示A臂末端期望位置。

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

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

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

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