面向协作机器人的零力控制与碰撞检测方法研究

赵 彬，吴成东，孙若怀，姜 杨，吴兴茂

(1.东北大学信息科学与工程学院，辽宁沈阳 110819；2.新松机器人自动化股份有限公司，辽宁沈阳 110168；3.东北大学机器人科学与工程学院，辽宁沈阳 110169)

在3C（计算机、通信和消费电子）行业中，由于工作环境特殊，对协作机器人的运动性能、零力控制和碰撞检测等方面有严格要求[1–3]。本文对协作机器人的零力控制与碰撞检测问题进行研究，以解决机器人柔顺交互控制过程中，由于其模型无法精确求解导致其控制效果无法预测的问题。在关节空间不受限的情况下，冗余协作机器人与传统机器人不同之处在于冗余自由度极大地提高了规避奇异点的能力和操作的灵活性，有效地发挥了机器人的优势和特点。近年来，机器人柔顺交互控制技术取得了重要进展，Ugurlu等[4]为了通过柔顺运动来减少机器人上半身的压力，利用主动顺应性控制减少外骨骼支撑行走的上肢力量；控制器通过在每个关节的导纳控制器来控制力与位置的关系；机器人在与环境交互时表现出顺应性的行走特性，充分减少了所需的上半身使用的力。Giordano等[5]研究了一种臂式航天器的顺应反馈控制器，该控制器适用于机器人操作的前接触、接触和后接触阶段，消除了系统在稳态接触过程中的不稳定性；该控制器采用内外转置雅可比矩阵控制实现末端执行器的柔顺调节，同时通过调节机器人整个身体的质心和角动量实现接触后的稳定，并通过力反馈实现稳定的接触相位。Guo等[6]设计了一种新型的双臂移动操作机器人，并提出了一种面向安全的遥操作策略；与模仿人类的传统双臂装置不同，该装置采用了长臂和短臂的功能互补设计；从基于虚拟墙工作空间保护、基于最小距离计算自碰撞保护和基于开放运动规划库配置开关保护3个方面来保证操作安全。以上方法可实现对机器人的柔顺控制[7]，但是所涉及的柔顺控制均采用明确的模型，没有考虑到实际情况中实际模型与建立模型之间的差异，导致控制效果无法最优。冗余机器人零力控制和碰撞保护的难点在于：随着机器人构型空间维数的增加，在不受限空间对于操作目标有更多的操作位姿，导致常规解法无法对协作机器人进行运动学求解；机器人完全动力学的摩擦力模型为理想模型，模型的参数大多难以测量，不能用到实际机器人柔顺控制中[8–11]。

本文构建了一种New ton-MP通用迭代求逆运动学方法，优化求解了机器人从初始位姿到目标位姿的关节值。在柔顺交互控制中，零力控制与碰撞检测方法其难点在于协作机器人运动控制过程中摩擦力的干扰是无法避免的，而且摩擦力一般都较大无法忽略不计。如何对摩擦模型进行精确的建模和参数求解是一项技术难点。提出了一种基于加速度的3次摩擦力模型的完全动力学方程，对摩擦力模型进行多参数辨识求解。将动力学和摩擦力计算的力矩值给定到驱动器实现了协作机器人零力控制功能。将基于外力矩观测器模型和本文提出的One-c lass卷积神经网络两种碰撞检测方法进行对比。外力矩观测器建模通过检测外力矩观测器模型和阈值函数关系实现碰撞保护功能，但是，力矩观测器模型本质上还是需要一个确定的模型，其效果依赖模型的准确度。本文提出的One-class卷积神经网络碰撞检测方法成功地弥补了模型不确定的动态影响，可以实现未知摩擦模型的碰撞预测，有效地提高了碰撞保护的鲁棒性和准确性。

1 冗余关节运动学求解

运动学求解是协作机器人零力控制和碰撞检测的前提，研究的冗余协作机器人因其结构特性导致运动学上存在无穷多组解，利用传统几何关系和解析法无法求解。提出了一种求解冗余协作机器人运动学通用的逆运动学算法，将逆运动学求解问题转化为通用的New ton-MP迭代方法求解。

协作机器人不同以往传统工业机器人的最大优势在于人机柔顺交互控制。研究的冗余协作机器人机械构型和关节坐标系如图1所示，其关节构型为R-P-R-P-R-P-R。现有协作机器人主流负载都在20 kg以下，把功率和扭矩值限制在一定范围内，减少了机器人运动时的能耗，实现了随着机器人冗余度的增加提高避障、奇点规避和灵活性等性能。将冗余协作机器人运动学问题分为位置和姿态分别进行求解。

图1 冗余协作机器人关节坐标系Fig.1 Joint coordinate system of redundant cooperative robot

1）正运动学求解

图1展示了机器人初始位置和各关节坐标系的定义，D1～D6为机器人连杆长度。将冗余协作机器人的正运动学分为大臂4个关节（ θ1、 θ2、 θ3、 θ4）和末端手腕3个关节（ θ5、 θ6、 θ7）两部分进行分析。先计算，再右乘，获得末端总体解为。其中，为腕部坐标系相对于基坐标系的位置和姿态矩阵，{ el}为腕部坐标系，{ tcp}为末端坐标系。因为协作机器人正运动学求解容易，所以这里仅给出计算方法。

2）逆运动学New ton-MP法优化求解

将前4轴逆运动学问题转化为非线性New ton-MP优化迭代求解问题。机器人位置由关节的角度 θ1、θ2、 θ3、 θ4决定，采用New ton-MP算法进行求解，其算法步骤可分如下5个部分：

第1步：采集协作机器人当前各个关节值，通过正运动学计算出在世界坐标系下的初始位置向量P。建立逆运动学的逆运动学非线性方程F(Θ(k))。其中，Θ(k)=(,,···,)T为非线性逆运动学方程组第k次迭代值，k为New ton-MP算法执行的迭代次数，n为机器人轴的数；计算第k+1次迭代值的牛顿下山法迭代公式为：

式中， ω为迭代因子，J为 3×4阶的雅克比矩阵，即：

2 多参数辨识的零力控制

2.1 完整动力学建模

零力控制策略难点与多种因素相关，主要影响因素是如何能够建立完整动力学方程中的摩擦力项。本文引入遗传算法对摩擦力模型进行多参数辨识求解，构建完整动力学方程来提高零力控制准确性和快速性。完整动力学方程包含理论动力学和摩擦力，本文将摩擦力模型函数考虑进去，可以得到一个完整动力学模型：

式中：M为关节空间惯性力项（对称、正定），M∈Rn×n；C为向心力和（哥氏力）科里奥利力耦合力项，C∈Rn×n；G为重力项，G∈Rn×1；Tf为基于速度的摩擦力矩，Tf∈Rn×1； θ 为关节转动向量，θ ∈Rn×1；为转动加速度，为转动角速度。

对冗余协作机器人摩擦力矩进行分析，依靠实际工程经验，对摩擦力矩Tf进行建模，本文提出的摩擦力模型Tf为基于速度的3次函数：

式中，Tf为摩擦模型的力矩，A3、B2、C1和D0分别为本文定义的基于速度的3次摩擦力模型的参数系数。本文将基于速度3次摩擦力模型与真实摩擦力进行拟合，即可得到完整的摩擦力模型参数。第2.2节对摩擦力的参数系数进行遗传算法求解。

2.2 基于遗传算法的摩擦力多参数辨识

选用遗传算法解决多参数辨识问题，其原因是遗传算法的策略不依赖梯度信息，其解是全局最优的。提出的遗传算法利用最小二乘法快速筛选一批适合的初始参数，来提升传统遗传算法的收敛速度，以满足实时机器人控制系统对算法效率的要求；利用交叉运算和变异运算来实现个体之间的信息交换和局部搜索；利用遗传算法迭代辨识出一批最佳摩擦力参数，最终使得目标函数最优。因此，遗传算法可以更快速的找到全局最优解。更为准确的摩擦模型参数，可以使零力控制获得更好的控制效果。因为初值采用最小二乘法求解可以使得遗传算法收敛更快，所以可以将该算法移植到机器人的实时控制系统中。

遗传算法流程图如图2所示，包含以下6步：

1）遗传算法实质是利用最小二乘法快速选定一批局部最优的初始擦力模型参数值，作为遗传算法优化的初始父代个体，最小二乘具体形式如下：

图2 遗传算法多参数拟合流程图Fig.2 Flow chart of genetic algorithm multi-parameter fitting

2）初始化摩擦力模型种群参数。根据摩擦力模型的辨识参数和评估该群体中每个个体的误差精度条件参数，设定算法的相关参数，如将摩擦力估计模型中遗传算法的个体 γ 设为xˆ，种群大小根据实际情况进行设定，变异概率一般设为75%左右，最大遗传繁衍迭代次数L为3000。这些算法相关参数影响着遗传算法的精度和收敛效果。

3）根据待求解问题的特征，设定遗传算法的适应度目标函数H设定为：

式中，N为迭代次数，yi为力矩实际值，Tf为力矩理论值。随机产生种群个体并计算其相关的适应度，构建初始种群产生父母代个体，然后准备对摩擦力模型参数A3、B2、C1和D0进行多参数辨识。设当前迭代次数N为0，遗传迭代辨识开始。

4）判断是否满足终止条件。

条件1：计算摩擦力模型种群中每个个体的适应度，同时评估该群体中每个个体的适应度是否满足精度条件，适应性分数反映优势个体被选择的可能性。若适应度满足精度条件，则跳转步骤6），同时，输出最佳优势个体及其代表的最优多参数辨识解，否则进行下一步。

条件2：当前迭代次数N加1，若N大于L，则跳转出程序，并告知无法收敛。否则进行下一步遗传过程。

5）对摩擦力模型参数辨识问题进行编码，选择算子通过轮盘赌选择适合的父母个体。通过交叉运算和变异运算操作繁殖产生新的个体，用新个体替换最不适合的种群。在获得最佳适应度个体后,将个体的变量序列还原为摩擦力模型多参数，则摩擦力多参数辨识过程结束。

6）最终生成适应度高的个体，即满足摩擦模型参数辨识最优。

将真实摩擦力曲线利用改进遗传算法进行参数辨识，获取摩擦力曲线的参数值。将摩擦力和动力学加入到控制器中，利用驱动器的两种控制模式周期同步位置模式（CSP）和周期同步转矩模式（CST），完成零力控制和碰撞检测功能。

3 碰撞检测方法

碰撞检测的策略目标是区分协作机器人的正常运动行为和异常运动行为[10–15]。本文介绍了2阶外力矩观测器，通过引入比例微分（PD）控制和测速反馈控制解决碰撞保护方法。同时，也分析了2阶外力矩观测器缺点，由于传统观测器无法精确的给出模型，其最终碰撞检测效果难以预测。鉴于此，本文提出了One-class卷积神经网络碰撞检测方法，该碰撞检测方法成功地解决了传统观测器建模不准确的问题。

3.1 外力观测器建模

协作机器人的动力学平衡方程为：

式中， τe为外力矩， τm为理论力矩，M(θ)为对称正定矩阵，且满足A=-为反对称矩阵，由反对称矩阵的性质AT=-A，及实对称矩阵的性质AT=A，可得=+[16–18]。由式（10）求解外力矩 τe需要求解加速度，对位置反馈信息2次求导将会带来计算误差，引入噪音干扰其动量往往会出现突变。基于广义动量设计观测器模型可以不用再计算角加速度 θ¨，显然引入动量方程对碰撞检测的性能提高是十分有利的。机器人广义动量方程为：

式中，p为广义动量，等式两边对时间t求导，可得：

式中，s为s域，K1、K2为观测器增益矩阵，R(s)为s域下的传递函数。由传递函数可知，R将随着 τe的变化而变化，通过改变K1和K2的大小，可认为 τe约等于r(t)。r(t)为时域t下的传递函数，对于2阶系统而言，PD控制的微分信号具有超前性，能够在超调发生前对系统进行调节，起到提高系统实时性的作用，但关节速度、电机力矩等信息是从机器人的控制系统中采集得到的，难以避免地存在采样噪声，影响检测效果。PD控制相当于高通滤波器，可能会放大这种噪声的影响，这对系统是十分不利的，因此，引入测速反馈环节，加强系统的抗噪音干扰能力，减小PD环节对噪音的放大效果。引入PD控制和测速反馈控制后，观测器的传递函数为：

式中，K3为PD控制系数矩阵，Kd为测速反馈控制系数矩阵，K3、Kd均属于观测器增益矩阵。对式（14）进行拉氏反变换r(t)=L-1(R(s))，可得优化后的观测器模型：

式中，r1(t)、r2(t)为中间结果。

因此，采用式（13）的观测器模型，通过调整观测器增益K1、K2、K3、K t，可以较好地保证观测器系统的实时性和准确性，实现协作机器人对外部碰撞的观测。在真实系统中往往存在谐波减速器引起的关节摩擦力矩，关节摩擦力矩难以直接测量，但它往往在较小的可确定范围内波动。因此，若不进行关节摩擦力矩辨识，可设定碰撞阈值rth大于0，通过阈值函数来判断机器人是否发生了碰撞。碰撞阈值大小的设定很大程度上依靠经验，所以该方案在模型不确定的情况下，碰撞效果难以得到有效的保证。鉴于此，作者利用深度学习进行网络碰撞检测的方法可以准确地检测机器人的异常碰撞情况。

3.2 One-class卷积神经网络碰撞检测方法

对于碰撞检测问题，使用较为完善的摩擦力模型和外力动量观测器模型也很难得到准确的检测效果。碰撞检测实际上相当于一种单分类任务。对于单分类问题，由于缺乏负样本数据，以端到端方式训练此类网络变得困难。基于深度卷积神经网络（CNN）的方法在异常检测和目标识别问题上表现出令人印象深刻的性能[19–23]。该网络使用交叉熵损失来学习特定类别与另一个类别的不同特征[24–29]。单类分类检测的主要概念是从由正常观察组成的给定数据集中学习正常模型，以便可以通过与训练模型的偏差来检测异常。单分类不需要将未知对象样本考虑进去，仅仅考虑正常样本即可，除正样本其他均为非负样本。

图3为本文的One-class卷积神经网络结构。整个网络由一个特征提取网络和一个分类器组成。特征提取网络本质上是将输入的目标类正常力矩曲线嵌入到特征空间中。然后，将提取的特征附加伪负类数据，这些数据是从特征空间中的高斯函数生成的。接下来，将附加的特征输入到一个分类器中。神经网络使用二元交叉熵损失对整个网络进行端到端训练。分类网络为每个特征表示分配一个置信度分数。分类网络的输出为1或0。这里1对应于属于正目标类（正常运动行为）的数据样本，0对应于属于非正类的数据样本（异常运动行为）。

图3 One-class卷积神经网络Fig.3 Schemat icdiagram of One-c lass CNN

3.2.1 One-class卷积神经网络步骤

使用One-class卷积神经网络的步骤为：

1）构建正常运动行为的正目标类数据样本，对正常力矩曲线数据预处理，转换为One-class卷积神经网络可识别的数据格式。

2）对可识别的正常力矩曲线数据进行数据标准化的处理，以防止因样本中不同特征数值大小相差较大影响One-class卷积神经网络的分类器性能。

3）采用VGG、A lexnet和ResNet网络提取数据特征，得到一个向量编码。利用均值为0，标准差为0.1的高斯噪声分布，生成同等维度的编码向量，充当非正样本编码后的向量。

4）训练整个One-class卷积神经网络，利用softmax分类器网络对提取的特征进行分类检测；Oneclass卷积神经网络检测只关注与正常力矩曲线数据相似或者匹配程度，对于未知的力矩曲线数据不妄下结论。

5）对训练后的模型进行测试验证。

3.2.2特征提取与损失函数

在本文中，特征提取采用了VGG16作为骨干预训练模型。在训练时，冻结卷积层，只训练全连接层。假设提取的特征是D维的，附加特征为高斯噪声函数，其中， σ 和是高斯函数的参数，I是一个D×D单位矩阵。训练时加入一些伪负样本，在训练方式都设置好之后，发现测试效果有所提升，达到了不错的测试准确率。

由于将伪负数据附加到原始特征中，使用简单的全连接层后跟一个softmax回归层作为分类器网络。全连接层的维度与特征维度保持一致。softmax层的输出数量设置为2。设定学习率为 10-4，数据集进行归一化处理。利用二元交叉熵损失函数Lc训练整个网络：

式中：2K为样本数量；y为样本的标签值（y∈{0,3}，y为0 表未发生碰撞，y为1 表发生碰撞）；p1为网络的预测值，即y为1的sof tmax概率。

4 实验结论

下面对运动学，动力学，摩擦力模型辨识参数和One-class卷积神经网络进行验证[30]。因为真实的力矩曲线无法进行模拟和仿真，所以实验通过驱动器软件直接采集机器人的驱动器数据进行分析实验。

4.1 运动学实验

下面将圆锥面等距螺旋线方程输入到离线编程软件来验证运动学正反解，其圆锥面等距螺旋线方程为：

式中，t为时间，px、py、p z为机器人末端在笛卡尔坐标系的坐标值，H为圆锥面等距螺旋线的高， ω1为绕中心轴旋转角速度，R为圆锥面等距螺旋线平面的半径，v为上升线速度。图4为机器人求解轨迹的运动学验证结果。其中：红色曲线是利用螺旋线方程示教出来的机器人真实轨迹；绿色是将示教轨迹用逆运动学求解出关节值，然后将关节值作为正运动学的输入参数，求解出目标轨迹参数。可以看出，作机器人末端示教轨迹（红色）与运动学求解轨迹（绿色）图一致，可见逆运动学建模的正确性。

图4 运动学验证Fig.4 Kinematics solution verification

对机器人末端位置进行绝对误差分析。图5为运动学绝对误差示意图，表示了机器人的理论位置值与运动学真实位置值的绝对误差。由图5可知，误差满足工程实际需求，绝对误差在0.0001 3mm左右。为了计算的快速性，当误差阈值为0.0015 mm时，New ton-MP法的平均迭代的次数不到20次（每次16m s）。冗余协作机器人可以在保证稳定性的基础上满足实时系统的要求。

图5 运动学绝对误差Fig.5 Kinematic absolute error

4.2 零力控制实验

为证明摩擦力项在零力控制中的重要性，进行零力控制实验。图6为从驱动器软件采集的真实关节力矩和牛顿欧拉法动力学理论力矩的差异示意图。其中，红色曲线为牛顿–欧拉法动力学理论力矩曲线，蓝色曲线为驱动器采集的真实力矩曲线，理论力矩与真实力矩有较大的偏差是因为没有考虑到摩擦力项。其差异可以证明，如果摩擦力忽略不计，对零力控制会有很大的影响。理论摩擦力模型是一种理想化模型，在实际工程中不能满足实际使用。机器人动力学技术的许多工作都致力于获得准确摩擦力的模型和辨识摩擦力模型的参数。摩擦力建模的准确性直接影响零力控制的实际效果。

图6 动力学理论力矩与真实关节力矩Fig.6 Theoretical torque and actual joint torque

下面进行传统摩擦力模型和本文提出的摩擦力模型进行对比。传统摩擦力模型包括黏性摩擦和库伦摩擦，其摩擦力模型为：

式中：v为黏性摩擦系数，它的符号取决于关节速度；c为库伦摩擦常数。

图7为传统简单摩擦力模型计算拟合摩擦力矩与真实观测力矩曲线对比曲线，蓝色曲线为冗余协作机器人真实摩擦力曲线，红色曲线为传统摩擦力的模型的力矩变化曲线。对于传统摩擦力模型，当θ˙不等于0时，模型相当于一个线性理想方程，无法直接拟合真实摩擦力。

图7 传统模型拟合摩擦力矩与观察真实力矩曲线Fig.7 Fitting friction torque from traditional model and observing actual torque curve

对本文提出的基于速度3次摩擦力模型进行验证，利用遗传算法进行摩擦力矩曲线拟合。图8为计算拟合摩擦力矩与真实观测力矩曲线对比曲线，蓝色曲线为冗余协作机器人真实摩擦力曲线，红色曲线为本文所提出的摩擦力模型的力矩变化曲线。本实验验证了遗传算法求解摩擦力模型参数的正确性。虽然曲线近似相等，但是可以看到也有一些噪声毛刺是无法拟合的，所以该方法仅适用于零力控制。如果应用碰撞保护策略很难保证碰撞算法的准确性。

图8 拟合摩擦力矩与真实力矩曲线Fig.8 Fitting friction torque from the proposed model curve and observing actual torque

图9为遗传代数收敛情况，最终可以得到遗传算法的个体参数 γ为：

图9中，蓝色、红色、黄色和紫色曲线分别为A3、B2、C1和D0系数解，历经30代遗传，系数解的变化趋于平稳。由图9可以看出，采用最小二乘法快速选定一批参数初值，摩擦力参数值震荡较小，可以保证遗传算法在保证稳定性的基础上满足实时系统对算法效率的要求。

图9 遗传代数收敛情况Fig.9 Convergence of genetic algebra

图10为协作机器人零力控制实验截图，实验利用完全动力学方程实现重力补偿技术。经过实际交互操作测试，人手对机器人进行拖动示教，可以对复杂的操作工艺进行手把手示教，替代传统的示教模式。

图10 协作机器人零力控制实验截图Fig.10 Screenshots of collaborative robot zero-force control

4.3 碰撞检测实验

在本文中，One-class卷积神经网络特征提取采用了VGG16作为骨干预训练模型。如表1所示，经过验证后模型准确率为0.917。

表1 One-class卷积神经网络测试结果Tab.1 Test results of One-class CNN

表2为所提出One-class卷积神经网络碰撞检测算法与外力矩观测器的性能比较。从表2可以看出，Oneclass卷积神经网络碰撞检测算法具有更高的准确率。外力矩观测器碰撞检测算法的执行效率具有更高的优势。

表2 One-class卷积神经网络和外力矩观测器碰撞检测对比Tab.2 Comparison of collision detection between Oneclass convolutional neural network and external moment observer

图11为One-class卷积神经网络碰撞检测实验截图，示教协作机器人在开放的环境下自由动作（图11（a））；用手指在无外部干扰时进行碰撞实验（图11（b）），来验证碰撞检测效果和灵敏度；手指与协作机器人发生碰撞以后，One-c lass卷积神经网络检测到碰撞后使得机器人停止运动（图11（c））。图12为直接利用驱动器软件对机器人的第3个关节进行基于碰撞保护的电机转矩曲线，其他关节类似，这里不作过多的分析。

图11 协作机器人碰撞检测实验截图Fig.11 Screenshots of collaborative robot collision detection

图12 基于碰撞保护的电机转矩曲线Fig.12 Motor torque curve diagram based on collision protection

由图12可知，冗余协作机器人在受到外围环境碰撞后，电流突然急剧变化（如曲线中的红色部分），触发控制器中的碰撞保护模块，电流发生碰撞后降低至0（如曲线中的蓝色部分）；然后，手动恢复机器人允许状态继续执行碰撞任务，将机器人对人的伤害降低到最低；冗余协作机器人碰撞保护功能确保了人机协作的安全性；提出One-class卷积神经网络碰撞的方法优于外扭矩观测器的碰撞检测算法，在不依靠准确模型的基础上能够对异常碰撞进行很好的检测。

5 结 论

主要研究了协作机器人柔顺交互控制中零力控制和碰撞检测方法。建立了一种分析冗余协作机器人New ton-MP通用的逆运动学算法，该方法绝对误差达到0.000 13mm。对于零力控制问题，通过考虑摩擦力考虑进去形成完整动力学方程。同时，建立以关节速度为变量的3次摩擦模型，利用遗传算法对3次摩擦模型进行参数求解实现了零力控制。由于协作机器人发生碰撞情况特别复杂，很难进行对碰撞数据建立完整的数据集，导致无法形成有效的负样本。鉴于此，作者利用One-class分类检测方法来解决碰撞检测的端到端可训练的分类问题。One-class卷积神经网络在特征空间中引入了伪负高斯数据，并使用二元交叉熵损失对网络进行了训练。One-class卷积神经网络碰撞检测方法成功地补偿了模型不确定的动态影响，适用于检测各种碰撞。当检测到外部碰撞时，需要采取适当的碰撞响应策略来保护工作人员的安全。未来可以尝试用正样本和少量非正样本两个数据集来训练，同时也用两个数据集来测试。通过改变正样本和非正样本的比例，寻求进一步提升碰撞检测效果的可能。