CN108015774B - 一种无需传感器的机械臂碰撞检测方法 - Google Patents

Abstract

本发明公开了一种无需传感器的机械臂碰撞检测方法，包括：计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵，根据该正交投影矩阵得到机械臂末端负载的力矩变换矩阵；建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器，通过动量偏差观测器得到机械臂关节外力矩矢量，根据该外力矩矢量与机械臂末端负载的力矩变换矩阵计算出机械臂的碰撞力矩；将机械臂的碰撞力矩与预设阈值相比较，判断机械臂是否发生碰撞。本发明采用的碰撞检测方法，能够实现末端带有负载的机械臂碰撞检测，简单高效，成本低。

Description

一种无需传感器的机械臂碰撞检测方法

技术领域

本发明涉及工业机器人领域，尤其涉及一种无需传感器的机械臂碰撞检测方法。

背景技术

现代自动化工业生产中串联多关节机械臂的使用较为普及，在机械臂的正常工作过程中，可能与周围的人或物体发生碰撞，这种情况不但可能对机械臂造成损伤，也可能对工作人员造成伤害。此外，近年逐渐发展起来的人机协作机器人无需保护栅栏，可与工作人员在同一环境中协同工作，在人机协同工作时，工作人员有时需要主动碰触正在工作中的机械臂以暂停机械臂的运行，所以对于机械臂无论是主动正常碰撞还是被动异常碰撞都必须被检测出来。综合以上可知，为了保障机械臂的正常运作以及操作人员的人身安全，碰撞检测逐渐成为机械臂必不可少的功能。

由于传感器价格较高，为了降低机械臂总体成本，传统的机械臂碰撞检测方法较为常用的是无传感器检测方式。一般机械臂出厂前由专业人员建立好机械臂本体的动力学模型，通过复杂的参数辨识实验过程经过计算得到机械臂本体参数，将机械臂本体的动力学模型和机械臂本体参数相结合得到机械臂的完整动力学模型，将由机械臂的完整动力学模型得到的理论力矩与通过机械臂关节电流得到的实际力矩进行比较，从而判断机械臂是否发生碰撞。该方法一般需要比较精确的机械臂本体动力学模型，而出厂后机械臂实际工作过程中末端会带上一定负载或负载工作中会发生变化，机械臂本体动力学模型已经不能适应这种带有负载的情况，原有的碰撞检测会带来较大误差甚至误报漏报，必须重新对机械臂进行动力学建模和参数辨识，而这种工作一般需要精通机器人控制的专业人员现场进行操作以及大量计算，普通操作工人无法进行，造成实际应用中的不便，增加工厂运营成本并且无法实现较高的碰撞检测精度。

发明内容

本发明的目的旨在解决所述技术缺陷，提出一种无需传感器的机械臂碰撞检测方法，计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵PF，根据该正交投影矩阵PF得到机械臂末端负载的力矩变换矩阵P；建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器并通过动量偏差观测器得到机械臂关节外力矩矢量r，根据该外力矩矢量r与机械臂末端负载的力矩变换矩阵P计算出机械臂的碰撞力矩τ_new；将机械臂的碰撞力矩τ_new与预设阈值相比较，判断机械臂是否发生碰撞。

为了实现上述目的，本发明提供一种无传感器的机械臂碰撞测方法，包括如下步骤：

步骤一：计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵PF，根据该正交投影矩阵PF得到机械臂末端负载的力矩变换矩阵P；

步骤二：建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器，通过所述机械臂动量偏差观测器得到机械臂关节外力矩矢量r，根据该外力矩矢量r与所述机械臂末端负载的力矩变换矩阵P计算出机械臂的碰撞力矩τ_new；

步骤三：将所述机械臂的碰撞力矩τ_new与预设阈值相比较，判断机械臂是否发生碰撞。

进一步的，所述的步骤一中，计算机械臂末端负载的力矩变换矩阵P具体步骤如下：

步骤1.1：根据机械臂驱动器反馈的数据得到机械臂关节反馈的关节角度q、角速度

将关节角速度

进行差分得到关节角加速度

步骤1.2：根据机械臂本体的D-H参数建立机械臂速度雅克比矩阵J，根据反馈的关节角速度

和机械臂速度雅克比矩阵J得到机械臂末端的速度矢量υ，表达式为：

步骤1.3：将所述机械臂末端的速度矢量υ差分，得到机械臂末端的加速度矢量a，将该机械臂末端的加速度矢量a和重力矢量g求和得到机械臂末端总的加速度矢量A，将机械臂末端负载质量设置为1kg，并将该机械臂末端负载质量点乘机械臂末端总的加速度矢量A，得到机械臂末端总的力矢量F，其中，机械臂末端负载质量的大小不影响碰撞检测精度；

步骤1.4：根据机械臂本体的D-H参数建立机械臂力雅克比矩阵J_F，根据机械臂末端力矢量F和机械臂力雅克比矩阵J_F，计算得到机械臂关节力矩矢量τ_t，表达式为：

τ_t＝J_F·F

步骤1.5：根据机械臂关节力矩矢量τ_t，建立机械臂关节力矩矩阵BF＝τ_t，接着计算出机械臂关节力矩矢量τ_t的正交投影矩阵PF，该正交投影矩阵PF使任意关节力矩矢量投影到机械臂关节力矩矢量τ_t所在的矢量空间S_payload上，其表达式为：

PF＝BF·(BF^T·BF)^-1·BF^T

步骤1.6：根据正交投影矩阵PF，计算力矩变换矩阵P，该力矩变换矩阵P使任意关节力矩矢量投影到矢量空间S_payload的正交矢量空间⊥S_payload上，表达式为：

P＝I-PF

对于末端带有负载的机械臂在工作过程中受到外力时，实际外力引起的机械臂关节力矩表达式为：

τ_ext＝τ_payload+τ_c

式中，τ_payload为负载力矩项；τ_c为机械臂发生碰撞产生的力矩项；τ_ext为实际外力引起的机械臂关节力矩；

由于矢量空间S_payload和矢量空间⊥S_payload总是相互正交的，而且机械臂末端负载对关节产生的力矩矢量总是存在于矢量空间S_payload，所以将该变换矩阵P乘以机械臂外力矩矢量，即可消除负载力矩项τ_payload对机械臂关节产生的力矩，其表达式为：

P·τ_ext＝P·τ_c+P·τ_payload＝P·τ_c

通过该式子，实际外力引起的机械臂关节力矩τ_ext经过力矩变换矩阵P的转换之后，力矩变换矩阵P抵消了负载力矩项τ_payload。

实际碰撞检测方法需要机械臂本体动力学模型，机械臂在出厂或测试阶段通过参数辨识已经得到较高精度机械臂本体动力学模型，因此，当机械臂在工作过程中末端加入负载，会导致原本的精确动力学模型发生变化，降低机械臂碰撞检测的精度，甚至导致机械臂碰撞检测功能失效。本发明所提供的方法避免了机械臂末端负载对机械臂碰撞检测精度的影响，甚至对于任意不同形状、大小、重量的负载也有同样的效果。

进一步的，所述的步骤二中，计算机械臂的碰撞力矩的具体步骤如下：

步骤2.1：机械臂本体动力学模型的表达式为：

式中，M(q)为惯性项；

为非线性耦合项；G(q)为重力项，τ_f为摩擦力项，τ为由机械臂动力学模型计算得到的指令力矩；

由于实际机械臂关节加速度

是由位置的二次微分得到的，通常含有较大噪音，与实际真实的关节加速度值相差较大，所以本发明根据机械臂受到外力时，机械臂广义动量发生变化的原理建立机械臂动量偏差观测器；

步骤2.2：根据机械臂本体动力学模型中的惯性项M(q)，计算机械臂的广义动量p，表达式为：

步骤2.3：根据机械臂本体动力学模型中的惯性项M(q)和非线性耦合项

计算机械臂本体动力学模型中非线性耦合项的转置矩阵

表达式为：

步骤2.4：机械臂在受外力情况下的机械臂动力学表达式为：

式中，τ_mot为电机输出力矩。将受外力情况下的机械臂动力学表达式进行变换，建立机械臂动量偏差观测器，表达式为：

式子中，r为机械臂关节外力矩矢量，K为机械臂动量偏差观测器调节系数。

将械臂动量偏差观测器的式子求导，得到：

对上式进行拉普拉斯变换得到械臂动量偏差观测器的传递函数：

从械臂动量偏差观测器的传递函数这个式子看出，该表达式描述的是一个一阶系统，系统的输入为实际外力引起的机械臂关节力矩τ_ext，输出为机械臂关节外力矩矢量r，该动量偏差观测器能够很好地跟踪外力矩的变化，通过调节机械臂动量偏差观测器调节系数K的大小改变系统上升时间。一阶系统设计方便，不会产生较大的震荡，不会影响碰撞检测的准确性，而且不需要用到机械臂关节加速度

就能建立该动量偏差观测器。

步骤2.5：将步骤一中得到的所述力矩变换矩阵P点乘机械臂关节外力矩矢量r得到机械臂的碰撞力矩τ_new，表达式为：

τ_new＝P·r

进一步的，所述步骤三中，将机械臂的碰撞力矩τ_new与预设阈值相比较，判断机械臂是否发生碰撞，表达式为：

式子中，TH1为力矩负向恒定阈值；TH2为力矩正向恒定阈值；通过参数d的数值来判断机械臂是否发生碰撞，当d为1时，检测出机械臂发生碰撞；当d为0时，机械臂无碰撞发生。

传统的无传感器机械臂碰撞检测方法中，有的采用通过动态阈值检测机器人是否发生碰撞，该方法是在机械臂重复作业工作中，若检测到机械臂完成一个作业周期，对于用于判断机械臂是否发生碰撞的动态阈值进行更新，这种方法增加了机械臂整体系统的复杂度，本发明所提出的无传感器的机械臂碰撞测方法相对简单实用高效，精度高，而且本发明采用的是恒定阈值，不需要动态更新阈值，仅仅通过调节阈值的大小就改变碰撞检测的精度，避免漏判和误判的现象。

与现有技术相比，本发明的有益效果：

1、本发明采用的负载消除方法适用于末端任意不同形状、大小、重量的负载，即使在末端负载不确定的情况下，也能够消除负载对机械臂关节力矩的影响，在不失碰撞检测精度的情况下，简单实用高效；

2、本发明对机械臂是否发生碰撞采用的是通过恒定阈值进行判断，不需要动态更新阈值，仅仅通过调节阈值的大小就改变碰撞检测的精度，避免漏判和误判的现象；

3、本发明采用的机械臂碰撞检测方法，对末端加速度的获取无需对机械臂配置额外的传感器，成本低，结构简单，减小系统的复杂度，增强系统的抗干扰能力。

附图说明

图1是本发明方法的整体流程图；

图2是本发明的计算机械臂末端负载力矩变换矩阵的流程图；

图3是本发明的机械臂动量偏差观测器结构框图；

图4是采用传统方法得到的机械臂关节四外力矩随时间变化的曲线图；

图5是本发明方法得到的机械臂关节五外力矩随时间变化的曲线图。

具体实施方式

下面详细描述本发明的实施例。但不应将此理解为本发明上述主题的范围仅限于以下的实施例，凡基于本发明内容所实现的技术均属于本发明的范围。另外还需要说明的是，为了便于描述，附图中仅表示出了与本发明相关的部分而非全部内容。

如图1所示，图1是本发明一个实施例的无需传感器的机械臂碰撞检测方法的流程图，本具体实施例采用的机械臂本体是七轴协作机器人，具体步骤如下：

步骤一：计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵PF，根据该正交投影矩阵PF得到机械臂末端负载的力矩变换矩阵P。

其中，如图2所示，计算机械臂末端负载的力矩变换矩阵步骤如下：

步骤1.1：在机械臂正常工作过程中，实时采集机械臂驱动器反馈的数据，将这些反馈的数据进行低通滤波处理，得到机械臂关节反馈的关节角度q、角速度

将关节角速度

进行差分得到关节角加速度

步骤1.2：根据机械臂本体的D-H参数建立机械臂速度雅克比矩阵J，根据反馈的关节角速度

和机械臂速度雅克比矩阵J得到机械臂末端的速度矢量υ，表达式为：

步骤1.3：将机械臂末端的速度矢量υ差分，得到机械臂末端的加速度矢量a，将该末端的加速度矢量a和重力矢量g求和得到机械臂末端总的加速度矢量A，为了方便计算，将机械臂末端负载质量设置为1kg，并将该机械臂末端负载质量点乘机械臂末端总的加速度矢量A，得到机械臂末端总的力矢量F；

步骤1.4：根据机械臂本体的D-H参数建立机械臂力雅克比矩阵J_F，根据机械臂末端力矢量F和机械臂力雅克比矩阵J_F，计算得到机械臂关节力矩矢量τ_t，表达式为：

τ＝J_F·F

步骤1.5：根据所述机械臂关节力矩矢量τ_t，建立机械臂关节力矩矩阵BF＝τ_t，接着计算出所述机械臂关节力矩矢量τ_t的正交投影矩阵PF，表达式为：

PF＝BF·(BF^T·BF)^-1·BF^T

步骤1.6：根据正交投影矩阵PF，计算力矩变换矩阵P，表达式为：

P＝I-PF

步骤二：建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器，通过所述机械臂动量偏差观测器得到机械臂关节外力矩矢量r，根据该外力矩矢量r与所述机械臂末端负载的力矩变换矩阵P计算出机械臂的碰撞力矩τ_new。

其中，计算机械臂的碰撞力矩τ_new的步骤如下：

步骤2.1：机械臂本体动力学模型的表达式为：

步骤2.2：根据机械臂本体动力学模型中的惯性项M(q)，计算机械臂的广义动量，表达式为：

步骤2.3：根据机械臂本体动力学模型中的惯性项M(q)和非线性耦合项

计算机械臂本体动力学模型中非线性耦合项的转置矩阵，表达式为：

步骤2.4：建立机械臂动量偏差观测器，表达式为：

式子中，r为机械臂关节外力矩矢量，K为机械臂动量偏差观测器调节系数。机械臂动量偏差观测器结构框架图如图3所示，其中：

步骤2.5：将步骤一中得到的所述力矩变换矩阵P点乘机械臂关节外力矩矢量r得到机械臂的碰撞力矩τ_new，表达式为：

τ_new＝P·r

步骤三：将机械臂碰撞力矩τ_new与预设阈值相比较，判断机械臂是否发生碰撞。

其中，将机械臂的碰撞力矩τ_new与预设阈值相比较，判断机械臂是否发生碰撞，表达式为：

式子中，TH1为力矩负向恒定阈值；TH2为力矩正向恒定阈值；通过参数d的数值来判断机械臂是否发生碰撞，当d为1时，检测出机械臂发生碰撞；当d为0时，机械臂无碰撞发生。

为了验证本发明所提出的无需传感器的机械臂碰撞检测方法的正确性，在matlab软件中的simulink平台下，用matlab软件中自带的simmechanics模块搭建机械臂碰撞检测系统进行仿真验证，对于机械臂无论哪个关节被检测出碰撞都视为机械臂发生碰撞。其中，机械臂末端负载为2kg，质心位置在末端坐标系下的坐标为(0,0,0.1)，单位为米(m)。仿真时间从0开始到第5秒结束，仿真过程中，仿真时间到第4秒时，在机械臂连杆五的质心处施加了一个笛卡尔空间外力(2,0,0)，单位为牛顿(N)，持续时间为0.01秒，仿真结束后，得到的结果如图4、图5所示。

采用传统的无传感器机械臂碰撞检测方法得到的机械臂的关节四外力矩随时间变化的曲线如图4所示，曲线平滑的部分是由机械臂末端负载引起的外力矩，曲线上在第4秒时的力矩突变是由机械臂连杆五突然被施加碰撞力引起的。如果使用力矩恒定阈值，会把负载所引起的外力矩检测出来，碰撞检测会出现误判的现象。

采用本发明所提供的无传感器机械臂碰撞检测方法得到的机械臂的关节五外力矩随时间变化的曲线如图5所示，曲线上在第4秒时的力矩突变是由机械臂连杆五突然被施加碰撞力引起的，机械臂发生碰撞时所受外力矩至少是正常运行时所受外力矩的10倍，所以通过设置恒定阈值能够检测出机械臂是否发生碰撞，该碰撞检测方法具有很高的灵敏度，不会出现误判和漏判的现象。

比较两幅图的曲线可知，对于带有负载的机械臂发生碰撞时，传统的方法通过预设的恒定阈值无法检测出机械臂发生碰撞，本发明所提供的无传感器机械臂碰撞检测方法过预设的恒定阈值很精确地检测出机械臂发生碰撞，仿真结果表明，本发明所提出的无需传感器的机械臂碰撞检测方法实现了末端带有负载的机械臂碰撞检测。

在matlab平台上进行碰撞检测的仿真验证之后，考虑实际应用情况，在机械臂实体上进行碰撞检测试验，将本发明所提供的无需传感器的机械臂碰撞检测方法用C语言编写成代码，下载到机械臂控制器里的程序中。

编写示教程序使机械臂每次运行轨迹相同，第一次试验中保持机械臂的末端夹持的负载不变，在每次机械臂运动过程中操作人员主动碰撞机械臂的不同部位，记录参数d的值；第二次试验中改变机械臂的末端夹持的负载的质量，保证每次机械臂运动过程中操作人员主动碰撞机械臂的相同部位，记录参数d的值，其中，操作人员主动碰撞机械臂不会使操作人员出现痛感，试验结果如下表1所示：

表1机械臂碰撞检测结果表

根据表1的试验数据可知，对于末端带有不同质量负载的机械臂，在机械臂运动过程中操作人员触碰机械臂相同的部位，通过设置正向、负向恒定阈值能够检测出机械臂发生碰撞；对于末端带有相同负载的机械臂，在机械臂运动过程中操作人员触碰机械臂不同的部位，通过设置正向、负向恒定阈值能够检测出机械臂发生碰撞，并且碰撞过程中操作人员不会出现痛感，所以本发明所提供的碰撞检测方法可以实现带有任意负载的无传感器机械臂碰撞检测。

Claims (3)

1.一种无需传感器的机械臂碰撞检测方法，其特征在于：

步骤一：计算机械臂末端负载对机械臂产生的关节力矩矢量的正交投影矩阵PF，根据该正交投影矩阵PF得到机械臂末端负载的力矩变换矩阵P；

步骤二：建立机械臂本体动力学模型，根据该动力学模型中的各项建立机械臂动量偏差观测器，通过所述机械臂动量偏差观测器得到机械臂关节外力矩矢量r，根据该外力矩矢量r与所述机械臂末端负载的力矩变换矩阵P计算出机械臂的碰撞力矩

；

所述步骤二中，计算机械臂的碰撞力矩

的具体步骤如下：

步骤2.1：建立机械臂本体动力学模型，表达式为：

式中，M(q)为惯性项；

为非线性耦合项；G(q)为重力项，

为摩擦力项，τ为由机械臂动力学模型计算得到的指令力矩；

步骤2.2：根据机械臂本体动力学模型中的惯性项M(q)，计算机械臂的广义动量p，表达式为：

；

步骤2.3：根据机械臂本体动力学模型中的惯性项M(q)和非线性耦合项

，计算机械臂本体动力学模型中非线性耦合项的转置矩阵

表达式为：

；

步骤2.4：建立机械臂动量偏差观测器，表达式为：

式子中，

为机械臂关节电机输出力矩，r为机械臂关节外力矩矢量，K为机械臂动量偏差观测器调节系数；

步骤2.5：将步骤一中得到的所述力矩变换矩阵P点乘机械臂关节外力矩矢量r得到机械臂的碰撞力矩

，表达式为：

；

步骤三：将所述机械臂的碰撞力矩

与预设阈值相比较，判断机械臂是否发生碰撞。

2.如权利要求1所述的无需传感器的机械臂碰撞检测方法，其特征在于：所述步骤一中，计算机械臂末端负载的力矩变换矩阵P具体步骤如下：

步骤1.1：根据机械臂驱动器反馈的数据得到机械臂关节反馈的关节角度q、角速度

，将关节角速度

进行差分得到关节角加速度

；

步骤1.2：根据机械臂本体的D-H参数建立机械臂速度雅克比矩阵J，根据反馈的关节角速度

和机械臂速度雅克比矩阵J得到机械臂末端的速度矢量

，表达式为：

；

步骤1.3：将所述机械臂末端的速度矢量

差分，得到机械臂末端的加速度矢量a，将该机械臂末端的加速度矢量a和重力矢量g求和得到机械臂末端总的加速度矢量A，将机械臂末端负载质量点乘机械臂末端总的加速度矢量A，得到机械臂末端总的力矢量F；

步骤1.4：根据机械臂本体的D-H参数建立机械臂力雅克比矩阵

，根据机械臂末端力矢量F和机械臂力雅克比矩阵

，计算得到机械臂关节力矩矢量

，表达式为：

； 步骤1.5：根据所述机械臂关节力矩矢量

，建立机械臂关节力矩矩阵

，接着计算出所述机械臂关节力矩矢量

的正交投影矩阵PF，表达式为：

；

步骤1.6：根据所述正交投影矩阵PF，计算机械臂末端负载的力矩变换矩阵P，表达式

为：

。

3.如权利要求2所述的无需传感器的机械臂碰撞检测方法，其特征在于：所述步骤三中，将机械臂的碰撞力矩

与预设阈值相比较，判断机械臂是否发生碰撞，表达式为：

式子中，TH1为力矩负向恒定阈值；TH2为力矩正向恒定阈值；通过参数d的数值来判断机械臂是否发生碰撞，当d为1时，检测出机械臂发生碰撞；当d为0时，机械臂无碰撞发生。

Images