0 引言

近年来，机器人取得了很多突破性的进展，它也逐步深入到人们的生产生活中，并在人类的生活中扮演着越来越重要的角色^[1-2]。为了提供更加灵巧的操作和更容易理解的动作，通过模仿人类上半身设计的双臂机器人不断涌现，如日本本田公司研发的ASIMO^[3]、日本软银集团和法国Aldebaran Robotics共同研发的Pepper机器人^[4]、瑞士ABB公司研发的YUMI^[5]、北京理工大学黄强老师团队研发的“汇童”系列机器人^[6]、深圳优必选公司研发的Walker^[7]等。相比于传统的工业机械臂，双臂机器人在运动过程中，不仅需要考虑机器人和外界环境的碰撞，而且需要考虑机器人自身的碰撞^[8]，例如通过机器人示教来完成一些精细的操作任务时，需要考虑机器人双臂之间的避碰及机器人单臂和躯干之间的碰撞，同时还需要为示教后的机器人运动留下适当的安全距离。由于在运动过程中，双臂机器人各杆件的相互位置是不断变化的，所以相比避免与外界环境碰撞，双臂机器人的自避碰运动控制更加复杂。

目前常见的机器人自避碰方法可以分为两类：全局规划和局部控制^[9]。全局规划方法能够根据机器人周围环境的信息规划出一条从起始位置到目标位置的无碰撞路径，通常在机器人运动前一次性完成规划，如RRT(rapidly-exploring random tree)算法^[10-11]、A^*算法^[12]、深度学习算法^[13]等。局部控制方法把机器人避碰当作一个控制问题，在机器人运动过程中通过底层控制实现实时避障，具有运算量小、实时性高的特点。常见的局部控制方法主要包括人工势场法和距离函数法，其中距离函数法具有方法简单、易使用的优点，应用较为广泛^[14]。人工势场法最初由文[15]提出，其通过对障碍物定义的排斥势场和对目标位置定义的吸引势场的共同作用对机器人进行运动学约束，但此类方法会陷入局部极小值，同时还会因为机械臂关节数量的增加而导致计算量过大。文[16]通过改进RRT算法与人工势场法的结合实现了工业机械臂避障路径规划，其通过建立临时目标点来解决算法的局部极小值问题，但也使得算法的复杂程度上升，效率变低。文[17]改进了人工势场法中的引力势能函数和斥力势能函数，并加入了虚拟吸引点，避免算法陷入局部极值，实现了双机械臂避障路径规划。文[18]在笛卡儿空间中构建改进的势场函数，并引入基于目标速度的吸引速度和依据障碍物速度变化的排斥速度，最终将两种速度在机械臂关节空间合成，实现机械臂避障路径规划和动态目标追踪，同时利用添加虚拟障碍物的方法避免机械臂陷入局部极小值。距离函数法最初由文[19]提出，其通过将机械臂关节处特征点和障碍物间的距离梯度函数引入到机械臂运动控制中，在机械臂运动过程中实现实时动态避障，该方法具有计算简单易使用，可实现实时避障的优点，但特征点选取对避障结果会产生较大的影响。文[20]提出了最大距离原则法(maximum distance criterion，MXDC)，该方法并不局限于将机械臂关节处作为特征点，其也将连杆的中点作为特征点，从而使得两个机械臂运动过程中尽量远离，从而避免碰撞，但并没有提出具体的、通用的、可靠的特征点选取方法。文[21]优化了特征点选取对机械臂避障效果的影响。此外，部分研究人员通过避障规划和避障控制的结合，解决双臂机器人运动过程中自身碰撞问题。文[22]利用机械臂之间的危险场信息，提出了一种用于避免自碰撞的轨迹规划方法，并设计了一种基于信号补偿和反步法的稳健轨迹跟踪控制器，确保了双臂空间机器人运动过程中不会发生自身碰撞。文[23]提出了一种基于离线轨迹的在线避障算法，该算法将机械臂的运行分为沿离线轨迹的主运动和避障的次运动，通过主运动和次运动之间实时切换以避开障碍物，实现双臂机器人自身避碰。

本研究针对双臂机器人自避碰问题提出了一种面向实时自避碰的双臂机器人力矩控制策略，如图 1所示。为了避免距离函数法中特征点选取的问题，本研究首先根据双臂机器人的结构参数利用基本几何体构建了机器人的动态骨架包围盒模型。其次，根据机器人运动过程中的实时关节角度利用机械臂正向运动学驱动机器人动态骨架包围盒做实时跟踪运动，并通过距离描述函数实时检测设定包围盒间的最小距离，从而将双臂机器人关节间距离检测简化为包围盒间距离检测。为了避免人工势场法计算量过大的问题，本研究根据阻抗控制规则提出了一种双臂机器人自避碰力矩控制算法，根据机械臂关节空间阻抗控制规则，将运动中各关节间的距离转换为避碰力矩，并将避碰力矩和其他任务的期望力矩相加得到控制双臂机器人运动的实际力矩，从而避免了双臂机器人在运动过程中的自碰撞。最后，本研究在双臂机器人上对算法的有效性和实用性进行了实验验证。

1 自避碰策略 1.1 双臂机器人动态骨架包围盒构建

为了避免双臂机器人在运动过程中，其双臂之间及单臂和躯干之间发生碰撞，需要考虑机器人上所有可能发生碰撞的点，并在运动过程中实时求取可能碰撞点之间的最小距离。为了更加简单和快速地计算出双臂机器人双臂之间及双臂和机器人躯干、头部等杆件之间的最小距离，本研究首先根据双臂机器人的结构参数并利用机械臂正向运动学构建了机器人的骨架结构，如图 2(b)所示，然后结合机器人的外形尺寸利用简单几何体(如圆柱体、球体等)建立了双臂机器人的动态骨架包围盒，如图 2(c)所示。表 1展示了双臂机器人动态骨架包围盒的参数。

为了更容易地描述动态骨架包围盒中侧边躯干的位置，本研究将机器人的基坐标系固连在机器人的腰部，机器人的躯干包围盒也与此坐标系固连在一起，如图 2(b)所示。根据图 2(a)所示的双臂机器人结构可知：在双臂运动过程中，可以认为机器人的头部和底盘相对于机器人的躯干是固定不变的，即机器人的头部和底盘在机器人基坐标中的位姿是固定不变的，所以根据机器人的结构尺寸易求得各固定包围盒的中心位置，并且机器人的双臂永远不会和底盘发生碰撞。

在双臂机器人运动过程中，实时读取机器人每个关节的角度值，并利用式(1)~式(4)进行机械臂正向运动学运算，从而求得机器人各个关节在机器人腰部基坐标系下的位姿。

(1)

(2)

(3)

(4)

式中，_jⁱT表示关节j相对于关节i的位姿变换矩阵，_i^baseT表示关节i相对于双臂机器人腰部基坐标系的位姿变换矩阵，_hand⁷T表示机器人手掌中心坐标系相对于关节7的位姿变换矩阵。

在笛卡儿空间中，利用求得的双臂机器人各个关节相对于机器人腰部基坐标的位姿信息，并结合表 1中双臂机器人动态骨架包围盒的参数信息，驱动机器人动态骨架包围盒跟随真实机器人实时运动。表 1中，R表示球体或圆柱体包围盒的半径长度，L表示圆柱体包围盒的高度。

1.2 动态骨架包围盒最小距离计算

在双臂机器人运动过程中，由于机器人的大臂、小臂和手部的空间位姿在不断地发生变化，所以为了防止双臂机器人在运动过程中的自碰撞，需要实时计算机器人双臂杆件之间的最小距离，以及单臂杆件和机器人头部、躯干等杆件之间的最小距离。根据双臂机器人的结构参数及各关节的运动限位，本研究只需对特定杆件之间做最小距离计算即可，如表 2所示。

根据表 1和表 2中的数据参数，本研究将机器人各杆件间的最小距离计算简化为3类：球与球之间的最小距离、圆柱体与圆柱体之间的最小距离、球与圆柱体之间的最小距离。

已知圆柱a和圆柱b的半径分别为r_a和r_b，x_a1和x_a2表示圆柱a轴线的极值点，x_b1和x_b2表示圆柱b轴线上的极值点，圆柱a和圆柱b轴线上的任意一点可以表示为

(5)

(6)

式中，向量u_a和u_b以及标量δ_a和δ_b定义为

从而可以得出圆柱a和圆柱b之间的最小距离为

(7)

同理，易知球与球之间的最小距离，以及球与圆柱体之间的最小距离都可以由圆柱体与圆柱体之间最小距离的求解形式变换得出，在此不再重复。

1.3 双臂机器人自避碰力矩控制算法 1.3.1 建立机械臂动力学模型

对于只存在转动关节的操作臂，利用拉格朗日方法易推导出操作臂的动力学方程为

(8)

式中，τ表示加在各关节上的n×1阶广义力矩矢量；q、分别表示操作臂的关节变量(矢量)、关节速度矢量及关节加速度矢量；D(q)表示操作臂的质量矩阵，是n×n阶的对称矩阵；表示n×1阶的非线性科氏力和离心力矢量；G(q)表示n×1阶的重力矢量。

进一步地，根据式(8)可以得到操作臂关节空间的状态模型：

(9)

式中，J表示雅可比矩阵；F表示外界作用力，F=[f^T，τ^T]^T；表示机械臂关节运动时的摩擦力和关节阻尼产生的力。

1.3.2 力矩控制策略

根据关节变量q的运动误差计算相应的关节力矩，阻抗控制规则可以表示为

(10)

式中，q_d为期望的关节变量；K_P和K_d分别为系统希望控制的刚度矩阵和阻尼矩阵，其由任务特性决定的，即在机械臂工作中需要达到的关节特性。

当机器人的双臂相互靠近或者机器人的单臂与自身躯干、头部等部位相互靠近达到设定的安全距离d_safe时，假定障碍物(本研究将除运动的机械臂之外的其它杆件均视为障碍物，如机器人的躯干、头部、以及停止运动的机械臂等)会对运动的机械臂产生一个排斥力，将其推开，从而实现双臂机器人自避碰的目的。因此，实现双臂机器人自避碰的核心问题是找到一个作用于整个手臂的平滑的排斥力。

本研究根据机械臂关节空间阻抗控制规则，对即将发生碰撞的机械臂产生一个虚拟的避碰力矩τ_virtual，拉大机械臂和障碍物之间的距离，并将避碰力矩和其他任务的期望力矩进行叠加，从而得到控制机械臂运动的总力矩，实现双臂机器人自避碰的效果。双臂机器人自避碰过程伪代码如算法1所示，算法的输入为机器人各关节变量q、机器人各关节速度矢量，其他任务期望力矩τ_d，自避碰算法运行标志f_run，设定的安全距离d_safe；算法的输出为机器人各关节控制力矩τ；同时算法中fk(q)函数表示机械臂正向运动学(forward kinematics)计算；q_temp为机器人各关节临时变量；SetCollisionBox(T)函数表示根据机器人各关节的位姿建立机器人动态骨架包围盒。

算法1  双臂机器人自避碰
输入：q，，τd，frun，dsafe； 输出：τ 1. qtemp←q； 2. while (frun) 3.   T←fk(q)； 4.   B←SetCollisionBox(T)； 5.   dmin←min(distance(B))； 6.   if dmin < dsafe then 7.     ； 8.   else 9.     τvirtual←0； 10.     qtemp←q； 11.   end if 12.   τ←τd+τvirtual； 13.   output τ 14. end while

在双臂机器人自避碰算法中，根据机械臂关节空间阻抗控制规则，利用机器人各关节变量q、机器人各关节速度矢量计算得到避碰力矩τ_virtual，然后将避碰力矩τ_virtual和其他任务的期望力矩τ_d相加，得到控制双臂机器人运动的实际力矩τ，其控制框图如图 4所示。

2 实验验证及结果分析

在双臂机器人自避碰算法的验证实验中，本研究以大型仿人服务机器人Walker2为研究对象对所提出算法的有效性及实用性进行了验证。Walker2机器人的每条机械臂上均布置了7个关节，在基于重力补偿的零力控制模式(利用每个关节采集的电流数据计算外力力矩，再通过动力学模型实时补偿重力、摩擦力等额外力)下，机器人的两条机械臂可以被自由拖动。

2.1 算法验证实验

在零力控制模式下，对本文提出的双臂机器人自避碰算法设置了两种验证方式：单臂避碰实验和双臂避碰实验。如图 5和图 6所示，单臂避碰实验为：在零力控制模式下，拖动双臂机器人的任意一条机械臂接近机器人的躯干、头部等部位，并实时记录设定关节间的最小距离和各关节扭矩的变化；双臂避碰实验为：在零力控制模式下，拖动双臂机器人的任意一条机械臂靠近另外一条机械臂，以及同时拖动双臂使其相互靠近，并实时记录设定关节间的最小距离和各关节扭矩的变化。本验证实验设定的安全距离d_safe为0. 08 m，实验结果如图 7和图 8所示，其中图 7(a)~图 7(d)分别对应右臂避碰实验中的右手和躯干避碰过程、右小臂和躯干避碰过程、右手和头部避碰过程，以及右小臂和头部避碰过程中右臂关节力矩的变化规律；图 8(a)~图 8(c)分别对应双臂避碰实验中的右臂运动、左臂运动及双臂同时运动避碰过程中双臂关节力矩的变化规律。

在零力控制模式下，开启本研究所提出的双臂机器人自避碰算法后，实验者任意拖动机械臂朝向机器人躯干、头部或另一侧机械臂运动，在两者相互靠近达到设定安全距离d_safe时，可以感受到排斥力，并且随着机械臂不断靠近，排斥力也不断增大，从而避免了双臂机器人由于自身发生物理碰撞造成损坏。如图 7和图 8所示，在初始运动阶段(d_min>d_safe)时，机器人手臂各关节的扭矩在零点附近摆动；在自避碰运动阶段(d_min < d_safe)，机器人手臂各关节的扭矩随着检测的设定关节间最小距离的变小而不断变大，从而推开运动的机械臂，实现双臂机器人自避碰的目的，从而再次验证了本研究所提出策略的有效性。由图 7和图 8可知，当双臂机器人手臂和设定关节间的最小距离小于设定的安全距离d_safe时，本研究所提出的算法就会立即产生虚拟排斥力，可以看出本研究所提出的自避碰算法的实时性较高。

2.2 真机应用实验

在真机应用实验中，本研究通过设定有自碰撞发生的示教轨迹来验证所提出的双臂机器人自避碰算法的有效性及实用性。如图 9(a)所示，双臂机器人右臂的末端的预设轨迹为从起点向终点状态做直线运动。

初始状态时，双臂机器人右臂的关节角为

终止状态时，右臂的关节角为

左臂固定关节角为

安全距离d_safe为0. 08 m。图 9(b)展示了在不启用自避碰算法时，右臂的第4关节、第6关节和手掌中心的运动轨迹。图 9(c)展示了在启用自避碰算法时，右臂的第4关节、第6关节和手掌中心的运动轨迹。图 10(a)展示了在不启用自避碰算法时，右臂关节角的变化规律。图 10(b)展示了在启用自避碰算法时，右臂关节角的变化规律。

由图 9可知，相比于不启用自避碰算法的运动过程，启用自避碰算法时，机器人右臂的运动轨迹发生了明显的变化，使其避免了和左臂的碰撞，从而验证了本研究提出的面向实时自避碰的双臂机器人力矩控制策略的有效性。如图 10(a)所示，在不启用本研究所提出的自避碰算法的情况下，运行至4 s左右时，右臂关节角的运动趋势发生变化，双臂机器人的左右臂发生碰撞；如图 10(b)所示，在启用本研究所提出的自避碰算法的情况下，运行至3 s左右时，自避碰算法生效，右臂关节角的运动趋势发生变化，使其避开左臂，避免了双臂机器人的自避碰，同时，在避碰的运动过程中，机器人右臂的关节角连续平稳变化，也再次验证了本文所提出的自避碰算法的实用性。

3 结论和后期工作

本研究根据双臂机器人的结构参数和正向运动学模型构建了机器人动态骨架包围盒，同时利用根据阻抗控制规则提出的双臂机器人自避碰力矩控制算法将关节间距离转换为避碰力矩，并将避碰力矩和其他任务的期望力矩相加得到控制双臂机器人运动的实际力矩，从而实现了双臂机器人实时自避碰。实验结果表明：当双臂机器人的设定杆件相互靠近且达到设定的安全距离时，本研究所提出的自避碰策略能够及时对机器人的运动关节产生一组平滑的避碰力矩，从而避免了机器人自身发生碰撞，同时在有自碰撞发生的运动轨迹上，本研究提出的自避碰策略也能够改变机械臂碰撞区域的运动轨迹，避免双臂机器人发生自碰撞，验证了本研究提出算法的有效性及实用性。

在本研究的基础上，后续会从两个方面开展相关研究：一方面后续研究会引入视觉，如可以通过深度相机获取机器人周围环境的点云信息，将机器人自避碰算法扩展到机器人与外部环境(包括外部物体和操作人员)间的实时避碰，以保护机器人及操作人员的安全。另一方面，将本文所提出的面向实时自避碰的双臂机器人力矩控制策略和机械臂笛卡儿空间路径规划相结合，利用笛卡儿空间路径规划方法分别为机器人左右臂快速规划出一条避障路径，在双臂运动过程中，利用自避碰算法保证双臂机器人运动过程中的安全性。