基于运动学方程的故障诊断方法研究

刘 鹏 ，张 伟 ，王 强 ，季毅巍

（1.辽宁石油化工大学 信息与控制工程学院，抚顺 113000；2.中国科学院 沈阳自动化研究所 机器人国家重点实验室，沈阳 110016；3.上海微小卫星工程中心，上海 200050）

常见的机械臂故障诊断方法是利用观测器（组）产生残差集，依据某种准则或阈值对残差进行估计和决策。对于控制理论与计算机技术，其实质性进步表现在使观测器的故障诊断技术能够合理地应用于复杂过程中，需要考虑建模误差的非线性系统和时变系统，然而就机械臂的故障诊断而言，大多数使用理论值与真实值之间的偏差，对偏差进行仿真分析。机械臂的故障主要分为传感器故障和执行器故障。

传感器故障，是在控制系统中用来检测系统参数的传感器发生恒增益变化、卡死或恒偏差，而不能准确测量信息或者出现偏差，实际表现为测量值与真实值之间的偏差超过了系统可以接受的阈值。

执行器故障，是控制系统中用来执行控制命令的执行器发生恒增益变化、卡死或恒偏差从而不能准确地执行命令，实际表现为执行器的动作输出不符合它的输入命令。在机械臂故障诊断中，执行器指的是机械臂系统中控制关节的伺服电机，而传感器指的是机械臂系统中测量、检测关节角位移的码盘，具体如图1所示。

图1 关节角平面Fig.1 Joint angle plan

现有的机械臂故障诊断，主要是通过电机的码盘或转速计等传感器测量各个关节的转动的实际角度，然后再用理想模型观测器测量理论角度，通过不同时刻实际值与理论值的关系设计阈值来检测和识别故障，在机械臂故障诊断领域这种方法的研究报告和文献已有很多[1-6]。这些研究均建立在动力学基础之上，忽略了机械臂运动学方程的重要性，未用到机械臂的D-H参数及运动学方程，未利用机械臂末端位置相对于基坐标在空间上的传递关系。

显然，由于机械臂抖动的存在，无论传感器测量值与真实值之间的差值怎么变化，都将产生一个很小的偏差，导致机械臂运动学末端理论位置与实际位置产生微小的不同，而这个不同与机械臂每个连杆的长度是分不开的。然而，产生偏差的原因有很多，例如建模不准确、控制对象的参数在执行任务中发生突变等。

在此，基于运动学故障诊断的角度，运用运动学方程及余弦定理，把机械臂当作一个整体，通过对机械臂末端位置的测量值与真实值之间的偏差，利用残差仿真图间的关系来识别故障，完善了以前故障诊断方法中的不足，同时对机械臂传感器进行诊断。

1 D-H参数与运动学方程

1.1 连杆D-H参数描述

机械臂系统通过一系列关节连接组成，关节通过一个个连杆进行连接。把坐标系固定在机械臂每一个连杆机构中，将它们坐标系间的相对位置和方向用齐次变换矩阵来描述，如果要得到机械臂末端关节的位置姿态，首先必须给每个关节都定义一个坐标系，再计算后一关节相对于前一关节的坐标变换矩阵，最后将其叉乘到一起，得到末端相对于基坐标的位姿，连杆到连杆的坐标变换关系如图2所示。

图2 机械臂连杆变换关系示意Fig.2 Manipulator connecting rod transformation relationship diagram

机械臂各连杆都可以用图2中的 4个参数ai，di，αi，θi来描述。 其中，ai和 αi描述连杆本身特征；di和θi描述相邻两连杆之间的关系。对旋转关节来说，θi为变量，其他3个参数不变；对移动关节来说，di为变量，另外3个参数不变。这种用连杆参数描述机构运动关系的规则称为Denavit-Hartenberg方法，简称D-H方法。

设固连在基座的坐标系为坐标系｛O｝，与连杆固连的坐标系为｛i｝，则连杆参数定义为αi-1为zi-1与zi之间的夹角，绕xi轴旋转；ai-1为zi-1到zi之间的距离，沿着xi轴正方向；θi为xi-1与xi之间的夹角，绕 zi-1轴旋转；di为 xi-1到 xi之间的距离，沿着 zi-1轴正方向。 ai-1非负；di，αi-1，θi的值可正、可负。

1.2 连杆变换和运动学方程

相邻连杆坐标系之间的连杆变换与 ai，di，αi，θi这4个连杆参数有关。如果所有连杆坐标系都已经被规定，那么相邻两连杆之间的相对关系便可以用2个旋转和2个平移这4个基本子变换建立起来，即：①rot（zi-1，θi），绕 zi-1轴转动 θi角度；②trans（zi-1，di），沿 zi-1轴移动距离 di；③trans（xi，ai-1），沿xi-1轴移动距离 ai-1；④rot（zi-1，αi-1），绕 xi-1轴转动αi-1角度。

由此可知，连杆i相对于i-1的位姿可以用变换矩阵i-1Ti描述。由于这些基本子变换都是相对于基坐标进行变换的，根据“从左到右”原则，得到坐标系｛Oi-1｝到｛Oi｝的连杆变换矩阵为

则相邻两关节坐标系间的连杆变换矩阵为

由齐次变换矩阵的乘法可知

2 判断故障的依据

机械臂的每个关节都与2个连杆相连 （除去2个端点），如图1所示。假设把连杆的另外两端相连接，利用余弦定理计算出替代连杆的理论值。然而，在机械臂运动时，每个关节测量角度的传感器（码盘）包含了一个均值为零的正态分布的噪声信息ξ，ξ∈N（0，σ2），用含有噪声的角度测量值以及余弦定理计算理论值，与通过运动学方程计算出的真实值进行对比，构造残差。利用多个残差间的关系特性来进行故障诊断，由于对各个关节角度的测量相互独立，所以各个关节间的测量噪声互不相关，但是对故障做出甄别的程度以及采取的措施与采用的传感器数量及配置密切相关。

3 试验验证

根据上述故障诊断原理，利用现有的实验室资源，以平面型三连杆机械臂为例说明这种故障诊断方法，如图3所示。图中，机械臂的每个关节都具有1个测量关节角度的传感器 （码盘）和1个执行器（舵机），各连杆长度为L1=1.8 m，L2=1.2 m，L3=1.6m；原点O与关节1重合，点C为机械臂末端手的位置，点A和点B分别对应于关节2和关节3，OB和AC都是虚拟连杆，OC为机械臂工作空间的半径。

图3 机械臂传感器配置及坐标系Fig.3 Sensor collocation sketch and coordinates of manipulator

3.1 基于运动学方程

根据运动学方程公式，计算机械臂末端手相对于基坐标的位姿，求其替代连杆OC长度，具体方法如下：

根据式（1）和式（2），三连杆机械臂末端手相对于基坐标的位姿为

因此得到机械臂工作空间半径替代连杆OC长度为

同理，由于每个测量角度的传感器包含了一个均值为零、正态分布的噪声信息 ξ，ξ∈N（0，σ2），因此实际计算得到的机械臂工作空间半径虚拟连杆OC的表达式为

综上所述，机械臂工作空间半径替代连杆OC的长度为

3.2 基于余弦定理

由图3（b）结合余弦定理，可得关节2对应的替代连杆OB的理论长度为

由于测量角度的传感器包含了一个均值为零、正态分布的噪声信息 ξ2，ξ2∈N（0，σ2），故实际计算得到的虚拟连杆OB的长度为

用替代连杆长度的平方来替代连杆的长度，则替代连杆OB的残差为

由所得的关节2和关节3所对应的虚拟连杆OB，AC以及机械臂工作空间半径虚拟连杆OC，即可判断出机械臂每个关节的故障。

4 仿真分析

将各数据带入方程中，用Matlab软件进行仿真分析。仿真结果如图4、图5所示。图4表明机械臂各个关节都正常时，每个关节所对应的虚拟连杆残差及工作空间半径残差的关系，都可以看做是均值为零的噪声序列。

图4 正常时虚拟连杆残差及工作空间半径残差的关系Fig.4 Normal virtual link residual and work space radius residual relationship

图5 虚拟连杆OB，AC残差及工作空间半径OC残差的关系Fig.5 Relationship between the virtual link residual OB，AC and the working radius residual OC

由图5（a）可见，机械臂末端超出了可接受区域，表示发生了故障，比较上面2个虚拟连杆的残差关系，极有可能是虚拟连杆OB所对应的关节2发生了故障。同理，图5（b）表明极有可能是虚拟连杆OC对应的关节3发生了故障。对于图5（c），机械臂末端超出了可接受区域，表示发生了故障，但是关节2、关节3所对应的虚拟连杆误差均值都基本为零，并没有太大变化。这种情况说明第2关节发生了故障（由于其前一个连杆长度为零，并具备与参考点相重合的特性，因而不能直接判断，只能通过间接方法判断）。

5 结语

本文提出了一种基于机械臂运动学方程的故障诊断方法。以三连杆机械臂为例，利用运动学方程和余弦定理，将残差的设计与机械臂末端位置以及连杆长度的变化相联系；将2种不同计算方法做对比，分析残差；最后利用Matlab软件对其进行仿真分析。从仿真图中可以看出故障所在，完善了大部分基于动力学的故障诊断方法中没有利用运动学方程以及连杆长度的不足。

[1]Visinsky M L，Cavallaro J R，Walker I D.Robotic fault detection and fault tolerance：a survey[J].Reliability Engineering and System Safety，1994，46（2）：139-158.

[2]Rolf Isermann.Model based fault detection and diagnosis methods[C]//Proceedings of the American ControlConferences.Seattle Washington：[s.l.]，1995：1605-1609.

[3]Visinsky M L.Fault detection and fault tolerance in robotics[C]//Proceedings of NASA Space Operations Applications Research Symposium.Houston，Texas：[s.l.]，1991：262-271.

[4]Visinsky M L，Cavallaro J R，Walker I D.Adaptive fault detection and tolerance for robots[C]//First World Automation Conference.1994：205-210.

[5]Horak D T.Failure detection in dynamic systems with modeling errors[J].Journal of Guidance Control and Dynamics，1988，11（6）：508-516.

[6]Horak D T.Experimental estimation of modeling errors in dynamic systems[J].AIAA Journal of Guidance Control and Dynamics，1989，12（5）：653-658.