未知模型机器人的碰撞检测算法

袁 野，白瑞林，邢晓凡

（1.江南大学 轻工过程先进控制教育部重点实验室，无锡 214122；2.无锡信捷电气股份有限公司，无锡 214072）

0 引言

随着机器人应用的领域不断增加，机器人的安全问题也受到了越来越多的关注。当机器人在运动过程中与周围环境、操作人员发生碰撞时，会造成很大的损失。因此，碰撞检测这种能有效提高机器人安全性的方法受到了很多研究。比较容易实现的，可以为机器人每个关节单独设置一个力矩阈值，当发生碰撞时，关节力矩超过阈值，发出报警[2]。这种方法需要设置的较大阈值，不能快速有效地检测轻微的碰撞。此外也可以通过在机器人末端安装力矩传感器来检测碰撞[3]。但一般力矩传感器价格昂贵，且只能检测发生在末端的碰撞，在工业领域有一定的局限性。为了能够检测到机器人各个位置发生的碰撞，可以在机器人每个关节安装力矩传感器，并通过与机器人动力学估计的力矩偏差来实现检测[4]。这种基于动力学模型的方法更加精准，但计算估计力矩时需要获得关节加速度，且每个关节的力矩传感器增大了机器人的成本。机器人的关节伺服驱动器一般都能提供与关节电机输出力矩相关的电机瞬态电流，可以用来计算关节力矩。为了避免计算加速度，引入广义动量[9～11]，并进行改进，提高了机器人碰撞检测的灵敏度。

以上的基于动力学模型的碰撞检测算法都需要机器人精准的动力学参数，模型参数的偏差会影响检测的准确性。目前，大多数的工业机器人的控制方式都是基于运动学的，出厂只提供基本的连杆长度等参数，而没有动力学参数。因此，对于模型未知的工业机器人，在实验辨识出动力学参数的基础上，才能设计基于模型的碰撞检测算法。本文针对机器人模型未知的情况，提出通过整体辨识的方法得到动力学模型，并设计了基于广义动量的新型力矩观测器。该观测器通过添加高通滤波结构，减小了因模型偏差导致的观测值扰动，提高了碰撞检测算法的鲁棒性。本文提出的算法不需要加速度信号，也不需要额外的力矩传感器，减小了机器人系统的成本，能够在工业机器人上广泛应用。实验表明，该观测器可以有效对模型未知的机器人实现碰撞检测。

1 机器人动力学参数辨识

当机器人与外界发生碰撞时，用牛顿-欧拉法可以得到关节空间坐标系下的n自由度串联机器人的动力学方程：

不采用额外的关节力矩传感器下，可以根据关节电机电流和控制力矩之间的关系间接获取力矩值。通常，机器人的关节由伺服电机和减速机驱动，这种动力传输方式下控制力矩与电机电流之间成正比。关节控制力矩计算公式为：

式中，Im为关节电机电流，T为电机的转矩系数，N为减速机的减速比。

为了实现碰撞检测，需要能够实时观测碰撞力的等效力矩extτ，由式(1)和式(2)可知：

因此，观测碰撞力矩需要机器人的动力学参数以及运行中的状态参数。为了获取精准的动力学参数，本文基于整体实验的方法，辨识机器人的动力学参数[1]。

机器人动力学参数辨识一般包含动力学建模及线性化、激励轨迹设计、数据采样与处理、参数估计和模型验证5个步骤[1]。

未发生碰撞时，使用修改的牛顿-欧拉参数[5]可以将式(1)表示为惯性参数的线性形式：

式中，fc是库伦摩擦系统，fv是粘滞摩擦系数。

组合式(4)和式(5)，得到完整的动力学线性化方程：

机器人的惯性参数并不是都对力矩有影响，式6中的观测矩阵并不是列满秩矩阵，所以无法通过辨识计算动力学参数。通过线性关系消去、重组惯性参数可以得到一组最小参数集

[5,6]。被消去、重组的惯性参数其值可定义为零，将Y中对应的列消去就可以得到对应的观测矩阵Ymin。

为了减小测量噪声的影响，调高参数辨识的精度，需要精心设计激励轨迹。采用改进傅里叶级数作为激励轨迹[7]，驱动机器人运动。通过一定的采样频率，采集在多个时刻的关节角度和力矩，构成观测矩阵和力矩向量。采用观测矩阵条件数作为优化目标，使用遗传算法优化傅里叶级数中的参数。

2 力矩观测器设计

2.1 动量偏差观测器

机器人关节的伺服电机都带有编码器，可以反馈运行中的角度信号，关节角速度和角加速度则无法直接测量。直接的数值微分会放大测量误差，二次差分后的角加速度信号无法使用。为避免引入角加速度，本文基于广义动量，利用动量偏差观察碰撞力矩。

机器人的广义动量定义为：

式(7)对时间求导为：

通过对式(10)做卡普拉斯变换，可以分析出残差r观测碰撞力矩相当于是一个低通滤波器[10]，可通过改变K值来调节观测器的抗扰能力以及响应速度。

2.2 改进的观测器

工业机器人一般使用减速比较大的RV减速机，通过章节1中电机电流估计力矩实验辨识出来的动力学参数会有误差。此时，残差观测值r中不仅包含了碰撞力矩，也包含了参数误差产生的力矩。参数误差信号通常集中在低频区域[11,12]，低通滤波器并不能滤除参数误差的干扰，因此，本文将观测器扩展为二阶系统，设计了新型的机器人碰撞力矩观测器：

将式(11)积分并代入式(12)中可得：

式中K1，K2为对大于零的对角矩阵，其工作原理如图1所示。

图1 新型观测器工作原理图

将式(12)在时间上求两次导可得：

对式(14)同样做拉普拉斯变化，可以得到碰撞力矩观测器的传递函数为：

式(15)表明二阶系统的观测器可以等效为一个带通滤波器，并可以进一步推导为：

因此，本文设计的新型碰撞力矩观测器可以简化为一个一阶的低通滤波器和一个一阶的高通滤波器。可以通过合理设置高通滤波器的参数，滤除参数误差产生的力矩影响，提高碰撞检测的品质。

当机器人与外界发生碰撞时，可通过对应的观测器值判定碰撞位置以及碰撞方向，判定公式如下：

式中，Ni是每个关节的阈值，可以通过实验获得。从最末端关节到关节1，依次根据式(17)，计算Sign值，若为0，说明对应连杆没有发生碰撞；若为1，对应连杆发生正方向碰撞；若为-1，对应连杆发生反方向的碰撞。

另外，在机器人启动过程中，关节电机的输出控制力矩实时变化后，机器人由于惯性因素等位置和速度会发生滞后，碰撞力矩观测器会出现误检测。本文采用式(18)对观测器值r*进行修正，t0是机器人启动时间：

3 碰撞仿真测试

为了验证本文提出的新型碰撞检测观测器的有效性，并确保机器人本体实验时的安装，首先在MATLAB中借助Robotics工具箱搭建机器人动力学模型，同时文献[10]中的低通滤波观测器以及本文提出的新型观测器，通过数值仿真来验证观测器检测碰撞力矩的效果。

机器人模型误差产生的低频偏差信号与关节速度有关[8,9]，而碰撞力矩的频率较高，因此可以用本文的新型碰撞力矩观测器来过滤掉模型误差引入的扰动。机器人的位姿与自由度决定了模型偏差频率特性[12]：

机器人运动过程中控制速度的最大值为20°/s，根据式(19)，并考虑测量误差、抖动等因素，可以将高通滤波器的系统K2设置成值为1的对角矩阵。按照文献[9]，设置低通滤波器系统K1=20，来过滤系统中的高频噪声。

图2 机器人碰撞仿真测试结果

从图中2可以看出，当模型没有偏差时，文献[10]和本文的观测器都能够很好的观测碰撞力矩。在没有发生碰撞时，观测值几乎为零。发生碰撞后，观测值迅速增大并超过阈值，触发碰撞发生信号。碰撞结束后，观测值又重新恢复到零，解除碰撞发生信号。

图3 机器人模型偏差观测结果

从图3中可以发现，由于模型有一定的偏差，文献[10]中的观测器只能过滤高频的噪声，对于模型偏差导致的低频波动并没有效果，导致观测值不能很好的跟踪碰撞力矩，甚至会一直大于阈值，产生误检测。而本文提出的新型观测器，通过设置合理的高通滤波器参数，能够比较准确的过滤模型误差，跟踪碰撞力矩。

4 实验结果分析

图4 机器人系统

为了验证观测器的有效性，在信捷电气的串联六轴机器人上进行实验，该机器人整体的系统结构如图所示。运动控制器采用信捷电气的XG2型号PLC，该型号PLC与伺服驱动器之间可以通过EtherCAT总线进行通讯，通讯周期为2ms，并可以通过上位机采集所有电机中的位置信号和电流信号。

4.1 动力学参数辨识实验

根据章节1的流程建立式(6)形式的动力学线性化方程，并利用改进的遗传算法设计出激励轨迹。

图5 六关节激励轨迹

按照规划好的激励轨迹，驱动机器人运动重复运动50次，采集运动过程中的位置信号和电流信号，并进行数据预处理。预处理后的数据利用最小二乘法辨识出动力学参数值，并拟合关节的力矩输出。依旧以关节2和关节5为例，拟合效果如图6所示。

图6 机器人力矩预测

利用整体实验的方法辨识出的动力学模型参数，在运动平稳时能够较好的预测出关节力矩，但是在一些速度换向处会出现较大的偏差。这是因为建模中使用的摩擦模型较为简单，使得辨识出来的模型参数与真实的参数间存在误差，这样的误差会对观测器的效果产生一定的影响。

4.2 碰撞检测实验

辨识出模型的动力学参数后，将碰撞检测算法写入控制器软件中进行碰撞检测实验。驱动机器人执行简单的点到点运行模式，设置运行过程中各关节的最大速度为20°/s，采用仿真测试时设计的观测器参数值。在机器人运行过程中，人为在连杆5上施加正向碰撞力，在连杆2上施加反向碰撞力，实验观测结果如图7所示。

图7 机器人碰撞力矩观测结果

从图中可知，由于实验辨识出的动力学参数有误差，文献[10]的观测器，不能过滤掉低频的模型偏差，使得观测器的值一直在大范围波动。在发生碰撞时，碰撞力矩和模型偏差并不能很好区分出来。而本文采用的新型观测器，在没有发生碰撞时，输出较为平稳，能够有效地过滤掉模型偏差，因此，可以设置一个较小的阈值。当发生碰撞时，新型观测器的输出值迅速增大，超过阈值，触发碰撞报警信号。

以上实验结果表明，本文提出的新型观测器，相对于低通滤波器形式的观测器，可以使用在动力学模型不够精准的情况下。

5 结语

本文提出了一种适用于未知模型机器人的参数辨识方法，以及基于广义动量的新型碰撞检测算法。该算法无需机器人的关节加速度信号，也不需要额外的关节力矩传感器，只需要关节电机的位置信号以及驱动电流，有利于在常规机器人系统中推广应用。在未知模型参数的情况下，通过整体的实验辨识可以得出一组比较精准的动力学参数，减小了机器人的模型误差。利用观测器等效为带通滤波器的性质，进一步过滤掉模型偏差导致的扰动，实现了工业机器人系统的碰撞检测。后续工作将研究更加精准的摩擦模型，提高动力学参数辨识的精度，进一步提高算法的碰撞检测性能。