CN108000521A - 一种无传感器式协作机器人碰撞检测方法 - Google Patents

Abstract

本发明公开了无传感器式协作机器人碰撞检测方法，基于协作机器人广义动量构造关节外力矩观测器的方法，实现实时对外部碰撞力以及摩擦力作用在关节上的力矩的观测。通过机器人正常运行时的观测值进行关节摩擦力矩模型求解，使得观测值只受碰撞力的影响。当观测值超出预设的阈值时认为机器人发生碰撞，机器人立刻停止运行，避免在人机协作过程中对人体造成伤害。本发明无需在机器人外部安装额外的传感器，只需通过机器人控制系统获取机器人的关节驱动力矩，关节位置，关节角度即可以实现碰撞检测功能，避免了增加机器人结构的复杂性以及提高机器人制造成本。通过阈值的设定可以改变碰撞检测的灵敏度，保证人机协作安全。

Description

一种无传感器式协作机器人碰撞检测方法

技术领域

本发明涉及机器人控制技术领域，特别是一种无传感器式协作机器人碰撞检测方法

背景技术

区别传统工业机器人需要用围栏将人与机器人隔离开，协作机器人的应用场合需要进行密切的人机协作，在人与机器人交互协作过程中难免会发生接触碰撞的情况，如果对此情况不进行检测以及合适处理，会对人身造成危害，无法保证人机协作安全。

常见的碰撞检测方法主要分为有传感器碰撞检测和无传感器碰撞检测，安装传感器的方法主要有在关节处安装力矩传感器以及在机器人本体上包裹一层敏感的“皮肤”传感器等，安装传感器不仅会增加机器人结构的复杂性还会提高机器人的制造成本。无传感器的碰撞检测主要是基于力矩差的碰撞检测，通过计算预测力矩和实际采集力矩差值来设定阈值，进而做出判断，但在计算预测力矩时候需要加速度，加速度的采集容易引入很大噪声。

中国专利申请CN104985598A公开了一种工业机器人碰撞检测方法，该方法通过运动轨迹实时预测关节理论驱动力矩，并实时地计算理论力矩与实际采样力矩之间的差值，当该差值超过预设阈值时候机器人停止运动。但在计算预测力矩时候需要引入关节加速度，机器人系统的关节加速度是通过关节速度差分得到的，当机器人速度存在抖动时候加速度非常容易引人很大的噪声，容易造成误检测。

中国专利申请CN103192413A公开了一种无传感器的机器人碰撞检测保护装置及方法，该方法主要是通过建立扰动观测器对外界干扰力进行观测并计算干扰力对机器人运动状态的影响，从而预测下一时刻的运动状态，通过预测的下一时刻的运动状态与事先规划的状态之间的差值与预设阈值作对比；若检测到差值超过预设阈值，则驱使机器人停止工作。该方法需要经过大量的碰撞试验才能通过神经网络方法使得扰动观测器输出的数值与实际的数值误差减小，因此该方法相对较为繁琐且误差难以控制。

中国专利申请CN102426391A公开了一种判断机器人运行是否发生碰撞的方法，该方法涉及两个变量,一个是机器人的位置误差,另一个是运行过程中电机的电流值。通过当前轨迹的位置误差和实时测量的电流值的合成一个碰撞标量,再与预设碰撞阈值比较来实现碰撞检测,但该方法需要在电机上安装电流传感器，这有可能需要改变机器人的安装结构，而且还需要增加机器人成本。

发明内容

本发明的目的在于克服已有技术的缺点，提供一种适用于轻型协作机器人的任何工况，不改变机器人结构以及不增加机器人制造成本，保证人机协作安全的无传感器式协作机器人碰撞检测方法。

本发明是通过以下技术方案实现：

1.无传感器式协作机器人碰撞检测方法，其特征在于包括以下步骤：

步骤一、采用拉格朗日方程建立机器人动力学模型，模型的表达式如下：

τ_e＝τ_c+τ_f

式中：q、分别为机器人待测定的旋转关节的关节角度、角速度、角加速度；M(q)为机器人惯性项系数矩阵，为n×n矩阵，n代表机器人的旋转关节数；代表科氏力和离心力项系数，为n×n矩阵；G(q)为重力项；τ_f为关节处传动过程中产生的摩擦力矩；τ_d为关节驱动力矩，近似于待测定关节的电机扭矩乘上该关节的减速比；τ_c为外部碰撞力等效作用在关节上的力矩，所述的τ_e为外力矩；

步骤二、求解拉格朗日方程中的机器人惯性项系数矩阵，科氏力和离心力项系数，重力项，具体步骤为：

(a)通过机器人控制系统采集各个关节角度、角速度、角加速度,然后进行算术平均值滤波处理，并通过solidworks软件得到机器人各个关节的惯性参数：质量、质心位置向量、惯性张量矩阵；

(b)通过所述的机器人各个关节的惯性参数计算获得惯性项系数矩阵，科氏力和离心力项系数，重力项的表达式；

步骤三、基于机器人广义动量方程得到外力矩观测器模型，求解外力矩观测器模型中机器人待测定的关节观测值向量r，当机器人发生碰撞时，则待测定的关节观测值向量r近似等于外力矩τ_e；

所述的外力矩观测器模型为：

式中，P为机器人广义动量，K_r为外力矩观测器的增益矩阵，r初始值为0， r(t)表示t时刻各个关节观测值向量r的值；

步骤四、机器人正常运动没有发生碰撞时，有τ_c＝0，此时，r≈τ_e＝τ_f，利用下式建立摩擦力矩模型以求解机器人待测定的关节传动过程中产生的摩擦力矩τ_f，得到机器人没有发生碰撞正常运行时的观测值r：

式中，τ_f为关节摩擦力矩，τ_h为静摩擦力矩，τ_s为最大静摩擦力矩，τ_k为库伦摩擦力矩，a₁、a₂、a₃分别为与速度相关摩擦系数；

通过采集多组机器人没有发生碰撞正常运动过程中的观测值r以及关节速度并通过MATLAB拟合曲线工具箱求解出摩擦力矩模型τ_f中的静摩擦力矩τ_h、最大静摩擦力矩τ_s、库伦摩擦力矩τ_k以及与速度相关摩擦系数a₁、a₂、a₃；

步骤五、通过公式τ_c＝r-τ_f，得到碰撞力作用在关节上的碰撞力矩τ_c，设置一个阈值τ_normal，当|τ_c|≥|τ_normal|时，机器人立刻停止运行。

本发明的有益效果是：本发明方法基于机器人广义动量实现实时对外部碰撞力以及摩擦力作用在关节上的力矩的观测，并根据机器人正常运行时候的外力矩观测值进行关节摩擦力矩求解，使得观测值只受到碰撞力的影响。本检测方法适用于轻型协作机器人的任何工况，保证人机协作安全。

本发明通过机器人正常运行时的观测值求解关节摩擦力矩模型，去除了关节摩擦力矩对观测值的影响，整个求解过程也无需使用任何传感器，即可获得关节摩擦力矩模型。

本发明的阈值依据正常运行时的观测值的百分比设定，可以根据实际应用场合需求设定灵活方便。

附图说明

图1是本发明无传感器式协作机器人碰撞检测方法的碰撞检测原理图；

图2是本发明碰撞发生时外力矩观测值曲线图。

具体实施方式

为了使本发明的技术方案更加清晰，以下结合附图对本发明做进一步详细说明。应当理解，此处所述的具体实例仅用以解释本发明，并不限定于本例。

如图1所示的本发明的无传感器式协作机器人碰撞检测方法，包括以下步骤：

步骤一、采用拉格朗日方程建立机器人动力学模型，模型的表达式如下：

τ_e＝τ_c+τ_f (2)

式(1)中：q、分别为机器人待测定的旋转关节的关节角度、角速度、角加速度；M(q)为机器人惯性项系数矩阵，为n×n矩阵，n代表机器人的旋转关节数；代表科氏力和离心力项系数，为n×n矩阵；G(q)为重力项；τ_f为关节处传动过程中产生的摩擦力矩；τ_d为关节驱动力矩，近似于待测定关节的电机扭矩乘上该关节的减速比；τ_c为外部碰撞力等效作用在关节上的力矩，所述的τ_e为外力矩，这样可以方便对摩擦力矩项的处理。

步骤二、求解拉格朗日方程中的机器人惯性项系数矩阵M(q)，科氏力和离心力项系数重力项G(q)，具体步骤为：

(a)通过机器人控制系统采集各个关节角度q、角速度角加速度然后进行算术平均值滤波处理，并通过solidworks软件得到机器人各个关节的惯性参数：质量m、质心位置向量p_c、惯性张量矩阵I；

(b)通过所述的机器人各个关节的惯性参数计算获得惯性项系数矩阵M(q)，科氏力和离心力项系数重力项G(q)的表达式。

步骤三、基于机器人广义动量方程得到外力矩观测器模型：通过求解外力矩观测器模型中机器人待测定的关节观测值向量r，若机器人发生碰撞，则待测定的关节观测值向量r近似等于外力矩τ_e，即r≈τ_e。

式中，P为机器人广义动量，K_r为外力矩观测器的增益矩阵，r初始值为0， r(t)表示t时刻各个关节观测值向量r的值。

通过机器人的广义动量方程推导外力矩观测器模型的过程参见IEEE会议论文集《Robotics andAutomation》2003年10月出版，第634-639页公开的《Actuator failure detection andisolation using generalized momenta》(基于广义动量的促动器故障检测与隔离)一文：

r≈τ_e具体推导过程如下：

将外力矩观测器模型两端进行拉氏变换得：

由上式(3)可知，该观测器形式为一低通滤波器，当增益K_r→∞时，若机器人没有发生碰撞，则观测值r将指数式收敛到0；若机器人发生碰撞，则r指数式收敛到τ_e，即r≈τ_e。

步骤四、机器人正常运动没有发生碰撞时，有τ_c＝0，此时，r≈τ_e＝τ_f。因此，利用下式建立摩擦力矩模型以求解机器人待测定的关节传动过程中产生的摩擦力矩τ_f(关节摩擦力矩模型主要和关节速度以及关节运动方向相关)，得到机器人没有发生碰撞正常运行时的观测值r：

式中，τ_f为关节摩擦力矩，τ_h为静摩擦力矩，τ_s为最大静摩擦力矩，τ_k为库伦摩擦力矩，a₁、a₂、a₃分别为与速度相关摩擦系数。

通过采集多组机器人没有发生碰撞正常运动过程中的观测值r以及关节速度并通过MATLAB拟合曲线工具箱求解出摩擦力矩模型τ_f中的静摩擦力矩τ_h、最大静摩擦力矩τ_s、库伦摩擦力矩τ_k以及与速度相关摩擦系数a₁、a₂、a₃。

步骤五、通过公式τ_c＝r-τ_f，得到碰撞力作用在关节上的碰撞力矩τ_c。理论上，机器人正常运行即没有发生碰撞时候τ_c应该趋近与0，但是系统动力学方程建模以及机器人控制系统采集数据时存在误差，所以设置一个阈值τ_normal，当|τ_c|≥ |τ_normal|时，机器人立刻停止运行，保证人身安全。

图2为碰撞发生时前三个关节外力矩观测值曲线。由于机器人各个关节的减速比不同，误差也不相同，所以各个关节的碰撞阈值设定也各不相同，但都遵循一个原则，可以根据各个关节观测值的正常值的百分比来设定，阈值设定越小，碰撞反应越快，反之则越慢，最终设定值也可参考实际的碰撞效果而定。

优选的|τ_c|≥|1.1τ_normal|时，机器人立刻停止运行，保证人身安全。

本发明适用于多自由度轻型模块化协作机器人，机器人结构可以参《工程设计学报》2016年出版第74-81页公开的《新型模块化可重构机器人设计与运动学分析》一文。

本发明基于机器人动力学模型以及机器人广义动量构造外力矩观测器，实现对关节处外力矩的观测，整个过程无需任何传感器，避免了增加机器人本体结构的复杂性以及提高机器人的制造成本。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不仅局限于此，凡是依据本发明的技术方案及其发明构思加以同等替换或改变，都应涵盖在本发明的保护范围内。

Claims (1)

1.无传感器式协作机器人碰撞检测方法，其特征在于包括以下步骤：

步骤一、采用拉格朗日方程建立机器人动力学模型，模型的表达式如下：

<mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;&amp;CenterDot;</mo> </mover> <mo>+</mo> <mi>C</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>,</mo> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>)</mo> </mrow> <mover> <mi>q</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>+</mo> <mi>G</mi> <mrow> <mo>(</mo> <mi>q</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;tau;</mi> <mi>f</mi> </msub> <mo>=</mo> <msub> <mi>&amp;tau;</mi> <mi>d</mi> </msub> <mo>-</mo> <msub> <mi>&amp;tau;</mi> <mi>c</mi> </msub> </mrow>

τ_e＝τ_c+τ_f

式中：q、分别为机器人待测定的旋转关节的关节角度、角速度、角加速度；M(q)为机器人惯性项系数矩阵，为n×n矩阵，n代表机器人的旋转关节数；代表科氏力和离心力项系数，为n×n矩阵；G(q)为重力项；τ_f为关节处传动过程中产生的摩擦力矩；τ_d为关节驱动力矩，近似于待测定关节的电机扭矩乘上该关节的减速比；τ_c为外部碰撞力等效作用在关节上的力矩，所述的τ_e为外力矩；

步骤二、求解拉格朗日方程中的机器人惯性项系数矩阵，科氏力和离心力项系数，重力项，具体步骤为：

(a)通过机器人控制系统采集各个关节角度、角速度、角加速度,然后进行算术平均值滤波处理，并通过solidworks软件得到机器人各个关节的惯性参数：质量、质心位置向量、惯性张量矩阵；

(b)通过所述的机器人各个关节的惯性参数计算获得惯性项系数矩阵，科氏力和离心力项系数，重力项的表达式；

步骤三、基于机器人广义动量方程得到外力矩观测器模型，求解外力矩观测器模型中机器人待测定的关节观测值向量r，当机器人发生碰撞时，则待测定的关节观测值向量r近似等于外力矩τ_e；

所述的外力矩观测器模型为：

式中，P为机器人广义动量，K_r为外力矩观测器的增益矩阵，r初始值为0，r(t)表示t时刻各个关节观测值向量r的值；

步骤四、机器人正常运动没有发生碰撞时，有τ_c＝0，此时，r≈τ_e＝τ_f，利用下式建立摩擦力矩模型以求解机器人待测定的关节传动过程中产生的摩擦力矩τ_f，得到机器人没有发生碰撞正常运行时的观测值r：

式中，τ_f为关节摩擦力矩，τ_h为静摩擦力矩，τ_s为最大静摩擦力矩，τ_k为库伦摩擦力矩，a₁、a₂、a₃分别为与速度相关摩擦系数；

通过采集多组机器人没有发生碰撞正常运动过程中的观测值r以及关节速度并通过MATLAB拟合曲线工具箱求解出摩擦力矩模型τ_f中的静摩擦力矩τ_h、最大静摩擦力矩τ_s、库伦摩擦力矩τ_k以及与速度相关摩擦系数a₁、a₂、a₃；

步骤五、通过公式τ_c＝r-τ_f，得到碰撞力作用在关节上的碰撞力矩τ_c，设置一个阈值τ_normal，当|τ_c|≥|τ_normal|时，机器人立刻停止运行。