基于电流的机器人故障检测与安全保护研究*

宋吉来，徐　方，邹风山

(1.中国科学院 沈阳自动化研究所，沈阳　110016；2.中国科学院大学，北京　100049；3.沈阳新松机器人自动化股份有限公司，沈阳　110068)



基于电流的机器人故障检测与安全保护研究*

宋吉来1,2,3，徐方1,2,3，邹风山3

(1.中国科学院 沈阳自动化研究所，沈阳110016；2.中国科学院大学，北京100049；3.沈阳新松机器人自动化股份有限公司，沈阳110068)

摘要：提出一种基于电流的机器人故障检测与安全保护方法。当机器人运行中发生齿轮、抱闸等故障或与外界环境发生碰撞时，电机电流会发生突变，因此可以根据这种异常变化判断故障和碰撞的发生。通过直接检测电机电流，利用电机模型与机器人动力学模型，估计外作用力，对故障或碰撞的发生间接测量，实现保护功能。结合动力学控制、关节转矩控制，并采用硅片传输机器人进行碰撞保护实验，实际运行结果验证了该方法的有效性。

关键词：故障检测；安全保护；机器人控制；动力学控制

0引言

随着科学技术的发展，机器人、数控机床等设备越来越多的应用在生产、加工、制造等领域。在这些应用中，故障检测与安全保护已然成为这一领域研究的主要问题[1-2]。当机器人与人或其它设备协同工作时，一次偶然的碰撞会影响整个生产线的效率，甚至对机器人及周围环境设备(特别是人)造成很大的伤害，避免这样的碰撞发生或及时判断出发生碰撞对于机器人、数控设备或人的保护是非常必要的。

根据传感方式的不同，多种策略被应用于机器人和数控设备的故障诊断与安全保护控制[3]。一种策略是采用六维力/力矩传感器，其安装在机器人腕部，通过假设末端执行器为刚性连接，作用在机器人末端的外力可以被计算出来。然而，这种高灵敏度的力/力矩传感器价格比较高，常应用于高精度力控制场合，如高精度装配及抛光等领域，限制了其应用范围。第二种策略是采用关节转矩传感器，其安装在机器人关节处[4]。如果关节转矩为τ，那么末端执行器所受外力可以通过τ=JTF计算得到，其中J为机器人的雅可比矩阵。这种方法对机器人的安全控制是很方便的，但传感器的安装增加了关节的柔性，加大了建模与控制的难度，甚至可能引起振动。第三种方法是结合电机模型与控制，采用状态观测器或扰动观测器。Sun和Mills描述了一种采用转矩控制模型的转矩观测器，其基于电机电流和位置信息[5]。Ohnishi设计了一种扰动观测器，以补偿电机的扰动转矩[6]。同时，基于扰动观测器和摩擦模型[7-8]，只需电机电流和位置信息可以得到机器人与环境作用的反应转矩，而不需要额外的力/力矩传感器。

当机器人运行中发生齿轮抱闸等故障或与外界环境发生碰撞时，电机电流会异常变化。本文提出一种基于电流的机器人故障诊断与安全保护方法，直接检测电机电流，利用电机模型与机器人动力学模型，估计外作用力，对故障或碰撞的发生间接测量，实现保护功能。将电机模型引入到力/位混合控制，结合动力学控制、关节转矩控制，在硅片传输机器人上进行碰撞保护实验。基于电机电流的机器人故障检测与安全保护，不需要额外的传感器，成本低廉，不降低机器人自身刚性，具有广泛的适用性。

1机器人构型

图1是典型SCARA结构的硅片传输机器人外形图。这种结构的机器人被广泛应用于洁净、真空环境下的半导体生产工业中。其结构特点包括：

①每个方向的运动由一个电机驱动；

②Z轴采用丝杠传动，为升降轴；

③SCARA类型手臂具有两个连杆、三个旋转关节、三组皮带和带轮。

图1　SCARA结构硅片传输机器人

图1中，机器人的运动包括三个自由度，升降、旋转和伸缩，运动空间为圆柱形空间。升降和旋转分别由单电机驱动。旋转运动为R轴方向，由单电机驱动。沿半径方向的伸缩运动为S轴方向，由于关节、皮带和带轮的连接关系，S轴的伸缩也由一个电机驱动，关节之间的耦合关系，使得机器人伸缩时末端工具始终保持沿S轴方向的直线运动。

2运动学与动力学模型

2.1运动学模型

硅片传输机器人的连杆参数如表1。

表1　硅片传输机器人连杆参数

根据DH法[4]以及连杆参数和角度关系，得到运动学正解为：

(1)

运动学逆解为：

(2)

2.2动力学模型

SCARA构型硅片传输机器人的动力学计算采用牛顿-欧拉法[4]，其包括计算速度和加速度的向外迭代以及计算力和力矩的向内迭代两部分。牛顿-欧拉方程的递推形式为：

(3)

SCARA构型手臂满足角度耦合关系：

(4)

以及连杆长度关系：

L3=L4

(5)

应用连杆参数和角度的耦合关系简化动力学解，可以得到机器人三个驱动轴动力学解为：

(6)

(7)

(8)

其中，m是各连杆质量的总和。mi为第i个连杆的质量，L为第3,4连杆的长度，Lmi为第i个连杆的质心位置。

3机器人的力/力矩检测

3.1电机转矩模型

在研究永磁同步电机数学模型时，常作如下假设[9]：

(1)忽略空间谐波，设三相绕组对称，气隙磁通按正弦分布；

(2)忽略磁路饱和，各绕组自感和互感恒定；

(3)忽略铁心、磁滞等损耗；

(4)不考虑频率、温度变化对绕组电阻影响。

永磁同步电机动态方程由磁链方程、电压方程和转矩方程组成。为分析及控制简单，通常采用忽略铁损时的d、q轴数学模型。保持d轴电流为0的id=0控制时，永磁同步电机的数学模型简化为如下方程：

磁链方程：

(9)

电压方程：

(10)

转矩方程：

(11)

这时的电流矢量随负载状态的变化，在q轴上移动。

3.2基于模型的力/力矩检测

当机器人采用减速器传动时，关节角度和转矩满足：

q=N-1qm

(12)

τ=Nτm

(13)

式中，N∈Rn×n是每个关节减速比的对角阵，q和qm分别为关节角度和电机角度，τ和τm分别为关节转矩和电机转矩。

重写完整动力学方程为以下形式：

(14)

考虑电机转子动力学，可以得到转子的转矩平衡方程为：

(15)

(16)

由此可得作用在关节上的外部转矩τext：

(17)

代入电机转矩模型和关节转矩关系，得到：

(18)

代入雅可比矩阵的力传递关系，得到基于模型的机器人末端6×1维广义外力Fext为：

(19)

机器人在运行中与外界环境发生碰撞或者出现齿轮、抱闸等故障时，电机电流会异常变化，基于模型的力/力矩检测值τext、Fext也会突变，当τext、Fext大于限定值时则判定故障的发生。此时可以通过动力下电等方式，实现安全保护功能。

4实验

采用基于模型的力/力矩检测方法，在硅片传输机器人上进行碰撞实验。图2为机器人在碰撞过程中的电机转矩曲线，可以看出，碰撞发生时电机转矩迅速增加，最大变化量约0.4N·m，可以在电机转矩达到最大值前检测到碰撞并实施有效保护。图3为采用模型的力/力矩检测进行碰撞判断，并在碰撞发生时动力下电实施安全保护的电机转矩曲线。可以看出，电机转矩在10ms左右增加约0.1N·m时，便可进行有效的碰撞检测并进行保护，转换到机器人末端的力保护精度约为0.1N。传统的采用六维力传感器的方式，传感器检测的精度一般为满量程的百分之一，以传感器量程10N为例，末端的力保护精度约为0.1N。对比可见，基于模型的力/力矩检测方法在检测和保护精度上与采用六维力传感器的方式相当，检测时间在10ms左右也满足系统需求，但其成本低廉，只采用机器人本身的电流和位置传感器，且不会因传感器的安装降低机器人自身的刚性，具有一定的优越性。

图2　碰撞过程电机转矩曲线图

图3　碰撞保护电机转矩曲线图

5结论

本文提出一种基于电机电流的机器人故障检测与安全保护方法，该方法不需要使用额外的传感器，仅根据电机电流以及系统模型来进行碰撞检测。将电机转矩模型和机器人动力学模型结合得到外力检测的力/力矩模型。最后采用硅片传输机器人进行碰撞保护实验，实验结果表明该方法与传统的采用六维力传感器的方式检测精度相当，可以在10ms量级的时间内判断出系统的异常外作用力，能够及时检测系统故障并保障机器人与设备的安全。

[参考文献]

[1]MazzocchiT,DiodatoA,CiutiG,etal.Smartsensorizedpolymericskinforsaferobotcollisionandenvironmentalinteraction[C].IntelligentRobotsandSystems(IROS),2015IEEE/RSJInternationalConferenceon.IEEE,2015:837-843.

[2] 李梦群,马维金,王爱玲,等.现代数控机床故障诊断及维修[M].北京:国防工业出版社,2009.

[3] 刘志刚,赵晓燕.浅谈数控机床机械故障诊断的方法[J].装备制造技术,2007(4):67-68.

[4]PhongLD,ChoiJ,KangS.Externalforceestimationusingjointtorquesensorsforarobotmanipulator.RoboticsandAutomation(ICRA),2012IEEEInternationalConferenceon.IEEE,2012:4507-4512.

[5]SunD,MillsJK.Developmentofpartialmodel-basedtorquecontrolofACinductionmotors.IEEETransactionsonRobotandAutomation,2001,17(1):100-107.

[6]OhnishiK,ShibataM,MurakamiT.Motioncontrolforadvancedmechatronics.IEEE/ASMETransactionsonMechatronics,1996,1(1):56-67.

[7]MurakamiT,YuF,OhnishiK.Torquesensorlesscontrolinmultidegree-of-freedommanipulator.IEEETransactionsonInducedElectronics.1993,40(2):259-265.

[8]O’SullivanTM,BinghamCM,SchofieldN.Observer-basedtuningoftwo-inertiaservo-drivesystemswithintegratedSAWtorquetransducers.IEEETransactionsonInducedElectronics.2007,54(2):108-109.

[9]ZhangY,ZhuJ.Directtorquecontrolofpermanentmagnetsynchronousmotorwithreducedtorquerippleandcommutationfrequency.PowerElectronics,IEEETransactionson,2011,26(1):235-248.

(编辑李秀敏)

文章编号：1001-2265(2016)07-0056-04

DOI:10.13462/j.cnki.mmtamt.2016.07.016

收稿日期：2016-03-08;修回日期：2016-03-18

*基金项目：国家科技重大专项：硅片集成传输系统研发和示范应用(2014ZX02103005)

作者简介：宋吉来(1983—)，男，辽宁营口人，中国科学院大学博士，研究方向为机器人运动控制与力控制，(E-mail)songjilai@siasun.com。

中图分类号：TH16；TG659

文献标识码：A

Robot Fault Detection and Safety Protection Based on Current

SONG Ji-lai1,2,3,XU Fang1,2,3,ZOU Feng-shan3

(1.ShenyangInstituteofAutomation,ChineseAcademyofSciences,Shenyang110016,China;2.UniversityofChineseAcademyofSciences,Beijing110049,China)

Abstract：An approach of robot fault detection and safety protection based on current is presented. When the robot operation, the occurrence of gear,holding brake fault or collision with the external environment,the motor current will change suddenly,so this abnormal changes can be used to determine fault and collision occurs.By directly detecting the motor current,followed by use of the motor model and the robot dynamics model to estimate external force,and indirect measuring fault or collision,collision protection can be achieved.Finally,combined with dynamics control and joint control torque,collision protection experiments are carried out on clean-room robot,and actual operating results demonstrate the effectiveness of this method.

Key words：fault detection;safety protection;robot control;dynamic control