CN108772838B - 一种基于外力观测器的机械臂安全碰撞策略 - Google Patents

Abstract

本发明公开一种基于外力观测器的机械臂安全碰撞策略，该机械臂安全碰撞策略首先建立外力干扰下的柔性关节机械臂动力学模型，并对其进行受力分析；采用双编码器的方法来采集电机端和连杆端的角度信息，该外力观测器里需要对关节位置与速度的测量，而不需要检测加速度

或惯性矩阵的逆矩阵，即可反映电流输入；对于多自由度的机械臂，广义动量p和矢量η的计算需要递推或迭代方法，通过该观测器可以对外力矩或外力进行检测，并依据观测器的观测值设定了动态阈值，当观测值超过动态阈值的限定范围，控制系统即认为碰撞发生，为保证操作人员和机器人的安全，将立刻切换到碰停模式，实现在不同运动速度下的安全反应。

Description

一种基于外力观测器的机械臂安全碰撞策略

技术领域

本发明属于工业机器人控制领域，具体是一种基于外力观测器的机械臂安全碰撞策略，该碰撞策略不依赖外接传感器实现机器人与障碍物之间的碰撞检测，也没有采取视觉检测障碍物的位置，而是采用基于广义动量的外力观测器进行检测外力的方法，实现在不同运动速度下的安全反应。

背景技术

安全是实现“人机共融”需要解决的首要问题，当机器人在非结构化的环境中运行时，安全问题是首要关注的问题，根据机器人应用环境与功能需求，其安全性可分为两类：自身安全性和交互安全性。实际上，人机共融导致机器人作业空间存在不确定性障碍物，极易导致意外碰撞。

根据机器人应用环境与功能需求，其安全性可分为两类：自身安全性和交互安全性。机器人自身安全性指，运动过程中避免因突发情况造成机器人本体的损坏。该类安全性的研究对象集中在机器人本身，主要研究机器人尚未与周围环境发生接触碰撞前对自身安全的保护，如过载，过压，运动奇点等。交互安全性是指，机器人与周围环境发生交互，在机器人自身安全运行的基础上，还需要保证其在与周围物或人的交互过程中，不会对其造成伤害。交互安全性侧重于同时考虑机器人和人的安全性，是机器人即将或已经与周围人员发生接触后所采取的保护性措施。

工业机器人的安全协作主要分为碰撞检测阶段与安全机制阶段，碰撞检测包括主动避碰和被动避碰两方面，主动避碰指机器人绕过障碍物到达目标点的路径规划，该类避碰一般采用避碰算法和外部视觉传感器(一般多采用基于多传感器融合的避碰算法)，有利用视觉传感器来判断障碍物的图像信息，推算机器人与障碍物之间的相对位置，判定机械臂是否与障碍物发生干涉。被动避碰是指机器人在接触到障碍物后的碰撞检测以及采取的安全反应策略。对于被动避碰，现阶段多采用两种方法进行碰撞检测，其中一种方法就是借助力传感器来检测外力。有的机械臂应用关节力传感器，在其各关节安装力矩传感器，可通过关节力矩的观测值来推断机械臂是否发生碰撞；有的在机械臂末端安装手腕式力传感器进行外力的碰撞检测，利用敏感皮肤检测机器人全身的外力碰撞，利用外部传感器碰撞检测的灵敏度会有所提高，但其不足之处为：外部传感器的碰撞检测会增加机器人的成本及布线的复杂程度。

另一种是不采用外部传感器来采集机械臂各关节的电流与力矩变化，有将读取的机械臂关节电流转换为力矩，将位置编码器的数值带入理论动力学模型中求得所需此时刻的驱动力矩，理论力矩与实际测量力矩进行对比判断碰撞情况；该算法需要机械臂各关节的速度与加速度，由于引入噪声会造成检测不准确，Je H等通过将测量相邻时刻各关节的电流变化差值与设定好的阈值进行对比来检测外力干扰；但此方法容易在机械臂运动过程换向时造成误检测，还有通过对系统能量的变化进行碰撞检测，该法的弊端在于只能检测运动过程中的外力，当机械臂静止时，影响检测的准确性与实时性。有文献(Luca A D,Mattone R.Sensorless Robot Collision Detection and Hybrid Force/MotionControl[C]//IEEE International Conference on Robotics and Automation.IEEE,2005:999-1004.)采取一种基于广义动量设计外力观测器，此法不引入加速度带来的噪声干扰，但是一阶低通滤波器的传递函数中增益太少不能保证准确性和快速性，此外只能检测持续缓慢的外力，不能检测高速急剧的外力。

静态阈值容易导致误检测，因此，提出一种动态阈值的设定方法，解决了机器人启停及运行过程中的误检测问题。本文提出了一种基于广义动量的外力观测器来进行外力干扰的检测，通过简化二连杆模型的受力分析，应用轻量机械臂平台对该观测器与动态阈值的设定方法进行实验验证，证实该观测器能准确快速的检测外力并识别外力的大小与位置。

发明内容

为解决现有技术的不足，本发明提供一种基于外力观测器的机械臂安全碰撞策略，该策略只需要对

的测量，而不需要检测加速度

或惯性矩阵M的逆矩阵，即可反映电流输入，且是采用基于广义动量的外力观测器检测外力的方法代替外接传感器实现机器人与障碍物之间的碰撞检测和视觉检测障碍物的位置。

本发明解决所述技术问题的技术方案是，设计一种基于外力观测器的机械臂安全碰撞策略，具体包括下述步骤：

步骤1：利用拉格朗日函数建立机械臂动力学模型；

步骤2：建立外力干扰下柔性关节机械臂动力学模型，并对其进行受力分析；

步骤3：采用双编码器的方法采集电机端角度θ和连杆端角度q和连杆端角速度

步骤4：将步骤3中得到的电机端角度θ和连杆端角度q输入到外力干扰下柔性关节机械臂的动力学模型中，得到由动力学模型计算而来的关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext；

步骤5：将步骤3中得到的速度

和步骤4中得到的关节接触转矩τo和外力矩等效到关节上的力矩τ_ext带入到动力学模型中，得到机械系统的广义动量p；

步骤6：将步骤5中得到的广义动量p输入到动力学模型中，并串联一个高通滤波结构，最终得到优化的外力观测器方程r；

步骤7：根据步骤6中所得到的优化的外力观测器方程r来判断是否有外力，如果r＝0，有外力，r≠0，则无外力；

步骤8：设置动态阈值。当出现高于动态阈值的一个或多个残差的突变证明该时刻发生碰撞，采取安全机制停止机械臂停止；否则没有发生碰撞，不采取安全机制。

与现有技术相比，本发明有益效果在于：仅需要通过双编码器测量电机端的角度信息θ和连杆端的角度信息q和角速度

而不需要检测加速度

或惯性矩阵M的逆矩阵，即可反映电流输入，运用基于广义动量的外力观测器即可判断是否有外力，作为力反馈信息代替多维力传感器，提高了检测的准确性与实时性，降低了开发成本和布线的复杂程度。

附图说明

图1是基于广义动量外力观测器的原理框图。

图2是碰撞检测原理示意图。

图3是一种实施例的二连杆机械臂模型图。

图4是一种实施例的双编码器关节结构示意图。

图5是一种实施例的二连杆机械臂受力模型简图。

图6是图5中的连杆一受力分析简图。

图7是图5中的连杆二受力分析简图。

图8是图5中的连杆二碰撞一关节力矩曲线图。

图9是图5中的连杆二碰撞二关节力矩曲线图。

图10是图5中的连杆一在静态阈值下的未碰撞与碰撞时的力矩曲线图。

图11是图5中的连杆一在优化后动态阈值下的未碰撞与碰撞时的力矩曲线图。

图12是图5中的连杆一在动态阈值下的未碰撞与碰撞时的力矩曲线图。

图13是图5中的连杆一在动态阈值下的未碰撞与碰撞时的力矩曲线图。

图14是六轴机械臂平台结构示意图。

具体实施方式

以下结合实施例和附图对本发明进行进一步的说明，但本发明的实施方式不限于此。

本发明提供一种基于外力观测器的机械臂安全碰撞策略(简称机械臂安全碰

撞策略)，具体包括下述步骤：

步骤1：利用拉格朗日函数建立机械臂动力学模型；

步骤2：建立外力干扰下柔性关节机械臂动力学模型，并对其进行受力分析；

步骤3：采用双编码器的方法采集电机端角度θ和连杆端角度q和连杆端角速度

步骤4：将步骤3中得到的电机端角度θ和连杆端角度q输入到外力干扰下柔性关节机械臂的动力学模型中，得到由动力学模型计算而来的关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext；

步骤5：将步骤3中得到的速度

和步骤4中得到的关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext带入到动力学模型中，得到机械系统的广义动量p；

步骤6：将步骤5中得到的广义动量p输入到动力学模型中，并串联一个高通滤波结构，最终得到优化的外力观测器方程r；

步骤7：根据步骤6中所得到的优化的外力观测器方程r来判断是否有外力，如果r＝0，有外力，r≠0，则无外力；

步骤8：设置动态阈值。当出现高于动态阈值的一个或多个残差的突变证明该时刻发生碰撞，采取安全机制停止机械臂停止；否则没有发生碰撞，不采取安全机制。

下面以具体实施例来验证本发明，该具体实施例选取简易的二连杆机械臂作为对象，但实施对象不限于此。

图3中二连杆机械臂由具有两个双编码器的关节链接的连杆构成，其中关节如图4所示，双编码器包含绝对值编码器和增量式编码器，利用谐波减速器进行扭矩传递。

一种基于外力观测器的二连杆机械臂安全碰撞策略，具体包括下述步骤：

步骤1：建立动力学模型，二连杆机械臂动力学模型分为连杆端动力学方程和电机端动力学方程，连杆端动力学方程如下：

电机端动力学方程如下：

式中q,θ∈Rⁿ，表示机械臂连杆和电机的位置；M(q)∈R^n×n，表示机械臂连杆惯量矩阵；

表示机械臂连杆端离心力矩和哥氏力矩矢量；G(q)∈Rⁿ表示重力矩。

步骤2：对二连杆机械臂进行平面内的受力分析，如图5所示，外力作用在连杆二上，对两个均质杆的受力进行分析。图6中F_x2、F_y2表示碰撞外力F在连杆二中的浮动坐标系下的两个分量，其中x₁代表在连杆一上外力作用点在连杆一浮动坐标系上的横坐标，其中x₂代表在连杆二上外力作用点在连杆二浮动坐标系上的横坐标。根据力的平衡原理及相互作用力原理可得：

F_y1＝F_y2＝F_y1′＝F_y2′

F_x1＝F_x2＝F_x1′＝F_x2′

由虚功原理推断出雅克比矩阵的应用：

τ^T×Δq+(-F)^T×Δx＝0 (3)

X＝X(q) (4)

此方程描述二连杆机械臂的位移关系，建立关节空间与操作空间的映射关系：

表示接触力处的速度，

为关节的速度，J(q)为雅克比矩阵，是关节到接触力处的速度映射的线性转换。得出公式为：

τ＝J^T×F (6)

但是(6)式中的F为相对于基坐标系的外力的分量。综合以上各式可得：

但是(7)式中的F为相对于基坐标系的外力的分量。根据第i个杆相对于基坐标系的齐次变换公式：

得到：F_i＝T_iF＝A₁A₂…A_iF (9)

步骤3：二连杆机械臂运动过程中，通过双编码器采集电机端角度θ_i,连杆端角度q_i,连杆端角速度

步骤4：通过步骤3中的三个参数，对关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext进行求解，动力学公式如下：

τ_ext＝J^T×F (10)

τ_o＝k_i(θ_i-q_i) (11)

其中F_n为外力，J_i＝diag(J₁J₂…J_n)为电机惯性矩阵，K＝diag(K₁K₂…K_n)为关节刚度矩阵，对角正定。

在(1)式的右边，τ_ot包含了对q执行的所有转矩，即通过双编码器弹性关节和来自外部环境传递的扭矩。其中

τ_ot＝τ_o+τ_ext

当后者不存在时，式变为

τ_ot＝τ_o＝k_i(θ_i-q_i)

当人与二连杆机械臂发生碰撞时，外部扭矩被作用于二连杆机械臂的每一个关节，因此可以通过观测外部扭矩变化来检测碰撞。二连杆机械臂连杆二发生碰撞时关节处力矩的曲线图如图8和图9所示。

步骤5：根据关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext的确切数据，对广义动量p求解，由矩阵

的斜对称性来进行分析可得：

机器人的总能量包括系统动能T及势能U：

E＝T+U (13)

其中

结合式(1)和式(13)，对式(12)两端同时对t进行求导可得：

式(13)代表机器人整个系统的能量平衡。

根据定义，机器人的广义动量为：

连杆端动力学方程(1)相关的广义动量

满足一阶方程：

其中η的分量由式(17)给出：

步骤6：依据动力学模型求出的广义动量p，求解残余矢量r，对残余矢量r的定义为：

r＝K₁[∫(η+τ_o+r)dt-p] (18)

其中K＝diag(K₁K₂…K_n)为关节刚度矩阵，对角正定，剩余矢量动态满足：

其中为了避免将突变信号误认为是高频噪声，对上述一阶低通滤波结构的观测器进行优化设计，在上述一阶低通滤波结构的基础上串联一个高通滤波结构，如式(20)所示。

只有反馈的外力观测器(20)会有产生很大的振荡，在碰撞检测时有一定的延迟，因此需构造调整函数h_e来作为观测器的前馈调节：

最终得到优化的外力观测器方程为

其中K₁K₂K₃为关节刚度；

步骤7：根据步骤6中所得到的优化的外力观测器方程r来判断是否有外力，如果r＝0，有外力，r≠0，则无外力；

步骤8：设置动态阈值；当出现高于动态阈值的一个或多个残差的突变证明该时刻发生碰撞，采取安全机制停止机械臂停止；否则没有发生碰撞，不采取安全机制。

采用六轴机械臂平台对本发明机械臂安全碰撞策略进行验证。六轴机械臂平台的机械臂有六个转动关节，参见图14，从底座到终端依次为关节一到六，关节一与基座链接，关节二直接与关节一相连，关节三通过杆长为425mm的连杆与关节二链接，再通过杆长为392mm的连杆与关节四相连，关节四与关节五直接连接，关节五与关节六直接相连。关节一、二的质量均为3.4kg，关节三的质量为2.65kg，关节四、五、六质量均为1.57kg。其中将底座第一、第四、五与末端关节位置固定，仅利用第二、三关节的数据进行碰撞检测，以连杆一和连杆二来表示，作为简化二连杆机械臂来进行验证试验(其中，1表示连杆一，2表示连杆二)。

本实施例采用的二连杆机械臂的动态阈值设置过程为：让简化成二连杆机械臂的六轴机械臂平台运行一组轨迹，根据规划的关节轨迹

实时动态计算出关节力矩的阈值，一旦由基于广义动量的外力观测器估计的力矩值超过了阈值，可判断出机器人受到了外力作用。根据理论动力学模型计算出关节力矩的阈值，如图11与图12、图13为动态阈值的验证过程，动态阈值是根据理论动力学模型得出的曲线推导得出，图12与图13都是由于不恰当的参数设置导致误检测，图11是经过参数的不断改进得出的优化动态阈值，即经过多次实验，采集关节未碰撞实时力矩，根据力矩设置切合机械臂运动时关节力矩的弯曲管状动态阈值所得。

一般机械臂对于碰撞的判断均是静态阈值，如图10，分别为肩关节(即为连杆一)未碰撞曲线、行程与回程碰撞曲线及静态阈值曲线，其中设定机械臂的角速度为是30°/s，加速度40°/s²。由图10可以看出，静态阈值无法准确检测到部分回程碰撞时的力矩突变曲线，造成误检测。如图11所示，与上述静态阈值相比，动态阈值能准确的检测出各个时刻及大小的外力干扰，并且在一定程度上避免误检测。当机械臂能够应用观测器准确观测外力变化，在机械臂控制系统中结合碰撞安全反应机制，能够实现人机协作的相对安全。

实验结果表明本发明设计的基于外力观测器的机械臂安全碰撞策略是可行有效的。

本发明未述及之处适用于现有技术。

Claims (1)

1.一种基于外力观测器的机械臂安全碰撞策略，其特征在于，所述机械臂为二连杆机械臂，其安全碰撞策略步骤为：

步骤1：利用拉格朗日函数建立外力干扰下柔性关节机械臂动力学模型，并对其进行受力分析，二连杆机械臂动力学模型分为连杆端动力学方程和电机端动力学方程，连杆端动力学方程如下：

电机端动力学方程如下：

式中q,θ∈Rⁿ，表示机械臂连杆和电机的位置；M(q)∈R^n×n，表示机械臂连杆惯量矩阵；

表示机械臂连杆端离心力矩和哥氏力矩矢量；G(q)∈Rⁿ表示重力矩；

步骤2：对二连杆机械臂进行平面内的受力分析，根据力的平衡原理及相互作用力原理可得：

F_y1＝F_y2＝F_y1′＝F_y2′

F_x1＝F_x2＝F_x1′＝F_x2′

由虚功原理推断出雅克比矩阵的应用：

τ^T×Δq+(-F)^T×Δx＝0 (3)

X＝X(q) (4)

此方程描述二连杆机械臂的位移关系，建立关节空间与操作空间的映射关系：

表示接触力处的速度，

为关节的速度，J(q)为雅克比矩阵，是关节到接触力处的速度映射的线性转换；得出公式为：

τ＝J^T×F (6)

但是(6)式中的F为相对于基坐标系的外力的分量；综合以上各式可得：

但是(7)式中的F为相对于基坐标系的外力的分量；根据第i个杆相对于基坐标系的齐次变换公式：

得到：F_i＝T_iF＝A₁A₂…A_iF (9)

步骤3：二连杆机械臂运动过程中，通过双编码器采集电机端角度θ_i,连杆端角度q_i,连杆端角速度

步骤4：通过步骤3中的三个参数，对关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext进行求解，动力学公式如下：

τ_ext＝J^T×F (10)

τ_o＝k_i(θ_i-q_i) (11)

其中F_n为外力，J_i＝diag(J₁J₂…J_n)为电机惯性矩阵，K＝diag(K₁K₂…K_n)为关节刚度矩阵，对角正定；

在(1)式的右边，τ_ot包含了对q执行的所有转矩，即通过双编码器弹性关节和来自外部环境传递的扭矩；其中

τ_ot＝τ_o+τ_ext (12)

当后者不存在时，式12变为

τ_ot＝τ_o＝k_i(θ_i-q_i)

当人与二连杆机械臂发生碰撞时，外部扭矩被作用于二连杆机械臂的每一个关节，因此可以通过观测外部扭矩变化来检测碰撞；

步骤5：根据关节接触转矩τ_o和外力矩等效到关节上的力矩τ_ext的确切数据，对广义动量p求解，由矩阵

的斜对称性来进行分析可得：

机器人的总能量包括系统动能T及势能U：

E＝T+U (14)

其中

结合式(1)和式(14)，对式(13)两端同时对t进行求导可得：

式(14)代表机器人整个系统的能量平衡；

根据定义，机器人的广义动量为：

连杆端动力学方程(1)相关的广义动量

满足一阶方程：

其中η的分量由式(18)给出：

步骤6：依据动力学模型求出的广义动量p，求解残余矢量r，对残余矢量r的定义为：

r＝K₁[∫(η+τ_o+r)dt-p] (19)

其中K₁为对角矩阵，大于0，剩余矢量满足：

其中为了避免将突变信号误认为是高频噪声，对上述一阶低通滤波结构的观测器进行优化设计，在上述一阶低通滤波结构的基础上串联一个高通滤波结构；最终得到优化的外力观测器方程为：

其中K₁K₂K₃为关节刚度；

步骤7：根据步骤6中所得到的优化的外力观测器方程r来判断是否有外力，如果r＝0，有外力，r≠0，则无外力；

步骤8：设置动态阈值；当出现高于动态阈值的一个或多个残差的突变证明该时刻发生碰撞，采取安全机制停止机械臂；否则没有发生碰撞，不采取安全机制。

Images