CN111376242B - 基于模式控制的机器人拖动示教方法和系统 - Google Patents

Abstract

本发明公开了基于模式控制的机器人拖动示教方法和系统，包括数据获取部、逻辑控制部和运动控制部，所述数据获取部收集所述运动控制部及所述逻辑控制部所需的数据；逻辑控制部根据数据获取部接收的指令来判断机器人当前的参数合理性，并判定机器人的运动模式；所述运动控制部根据所述数据获取部与逻辑控制部的结果，将示教力代入力‑速度关系映射方程，解算机器人的速度指令，再将所述速度指令代入速度‑位置关系映射方程，依据速度解算出机器人的指令位置。该方法和系统既可以保证机器人操作的柔顺性，又可以在较大的示教力的情况下保持低速稳定运动，提高示教精度。

Description

基于模式控制的机器人拖动示教方法和系统

技术领域

本发明涉及机器人示教系统领域,特别是基于模式控制的机器人拖动示教方法和系统。

背景技术

拖动示教允许操作者在脱离示教器和上位机的情况下直接手动接牵引机器人移动并完成点位或运动轨迹的记录。相比较传统示教方式，拖动示教使用门槛低且更加直观，因而有着广阔的应用前景。

基于位置控制的机器人拖动示教算法的核心是建立示教力与机器人速度之间的映射关系。通常情况下，力-速度映射关系用一个方程表示，速度和示教力呈正相关，即示教力越大，机器人速度越大。为保证拖动稳定性，必须引入运动死区，当示教力处于死区内时，速度为零。上述方法简单易行，但是仍有很多待改进的地方，例如：

(1)为保证拖动的柔顺性，在低速段，速度梯度较大。这样设计的优点是使得外界在施加较小助力的情况下就可以操纵机器人以较快速度运动；缺点是示教系统无法实现平稳的低速拖动，这样会直接影响拖动与示教精度。

(2)为保证拖动稳定性(例如消除抖动和拖动误触发)，要引入较大的运动死区，这无疑会降低拖动示教的灵敏度和精度。

(3)没有考虑机器人位置、示教力超过指定范围等异常情况的处理。

现有技术中也开发了一些执行方法或系统用于示教操作，例如，中国专利CN105479459 A中公开了一种人类操纵机器人运动的方法，虽然其能够实现一定程度的示教控制，但是其仍未能解决以上提出的问题。

发明内容

本发明的技术方案是：基于模式控制的机器人拖动示教系统，包括：数据获取部、逻辑控制部以及运动控制部。

数据获取部，获取逻辑控制部和运动控制部所需的相关数据，具体包括：机器人实际关节位置、实际关节速度、关节电流，以及外界施加在机器人本体上的示教力。

逻辑控制部，根据机器人相关数据判断机器人当前的工作状态，如果目标参数处于不合理范围，则认定机器人处于异常状态并进行异常处理；如果目标参数处于合理范围，则认定机器人处于正常状态并进入拖动示教模式控制。

对于所述逻辑控制部，包括以下四种示教模式：错误急停模式(ErrorStop)，示教过程中目标参数超出规定范围，此时机器人工作在异常状态，系统急停并且报错；等待模式(TeachWait)，机器人处于待机模式，等待外力的触发；拖动模式(TeachMove)，指示教系统根据示教力实时更新指令位置，使机器人按照外界的意愿运动；停止模式(TeachStop)，要求机器人迅速停止。

ErrorStop模式进入条件为：目标参数超出规定范围，所述目标参数包括实际位置、实际速度、关节转矩、关节电流；ErrorStop模式离开的条件为目标参数处于规定范围之内且机器人运动已经停止。

TeachWait模式进入条件为：(a)当前处于TeachStop模式且运动已停止，(b)当前处于TeachMove模式且外界施加的示教力不满足触发条件；TeachWait模式离开的条件是外界施加的示教力满足触发条件。

TeachMove模式进入条件为：当前处于TeachWait模式且外界施加的示教力满足触发条件；TeachMove模式离开的条件为：(a)外界施加的示教力不满足触发条件，(b)外界施加的示教力与运动方向瞬时相反。

TeachStop模式进入的条件为：当前处于TeachMove模式且外界施加的示教力与运动方向瞬时相反；TeachStop模式离开的条件为：(a)外界施加的示教力与运动方向相同且满足触发条件，(b)外界施加的示教力与运动方向持续相反且满足触发条件，(c)机器人运动已停止。

运动控制部，从数据获取部获得外界施加在机器人本体上的示教力，从逻辑控制部获取当前运动模式，由示教力通过力-速度关系映射方程，解算机器人的期望速度，再由速度-位置关系映射方程，依据速度解算出机器人的指令位置。

力-速度关系映射方程的具体说明如下：

假设当前模式为ErrorStop，速度指令为零，Vexp＝0。

假设当前模式为TeachWait，速度指令为零，Vexp＝0。

假设当前模式为TeachStop，速度指令为零，Vexp＝0。

假设当前模式为TeachMove，则将示教力F带入以下方程求解速度指令Vexp：

其中[F_t-,F_t+]为运动死区，V_exp是速度指令，V_max为设定最大运动速度，V_min为设定极限速度，m和n为设定阶次，x_i和y_i为待求对应阶次项系数。

已知机器人期望速度，计算机器人的指令位置，速度-位置关系映射方程的具体说明如下：

其中，P_k为第k次的指令位置，P_k-1为第k-1次的指令位置，P_k-2为第k-2次的指令位置，t为平滑系数，T为插补周期。

为了进一步的阐述系统的优势，同时给出了一个基于上述系统的机器人拖动示教方法：其逻辑控制部具体操作步骤包括：

S1、异常检测：检测机器人的实际位置、实际速度、关节电流是否符合预设的合理范围；

S2、错误处理：当机器人处于异常状态，将机器人当前的运作模式切换至错误急停模式；

S3、示教模式切换：根据机器人当前的运行状态以及各模式的进入、离开条件进行调整切换。

其中，在S1异常检测步骤中，异常情况的判定条件为：

A.关节位置大于预设最大值，且关节速度大于零，则判定机器人为正向越位异常；

B.关节位置小于预设最小值，且关节速度大于零，则判定机器人为正向越位异常；

C.关节电流大于电流额定值，则判定机器人此时电机或驱动过载异常；

D.示教力大于预设最大值，则判定施加在机器人上的示教力超出承受范围。

本发明的优点是：

1、该算法既可以保证系统的柔顺性，又可以在较大的示教力的情况下保持低速稳定运动，提高示教精度；

2、引入合理范围内的死区，提高系统的稳定性，可以很好的降低抖动性，提高拖动示教的防误触性能。

3、拥有完善的异常运动和错误参数的防护机制，既可以保障机器人的安全性能也可以保障操作者的人身安全。

附图说明

下面结合附图及实施例对本发明作进一步描述：

图1为示教系统结构原理图；

图2为逻辑控制部实施流程图；

图3为运动控制部实施流程图；

图4为各模式切换实施原理图；

图5为示教力速度映射关系示意图；

具体实施方式

实施例：

如附图1-5所示，基于模式控制的机器人拖动示教系统，包括三个主要部分：数据获取部、逻辑控制部以及运动控制部，三个部分可以在不同的任务里运行，也可以放在同一个任务中运行，其中运动控制部负责对机器人进行实时运动插补，因而需要放在实时任务中运行，例如放在一个周期为1ms的硬实时任务当中。

·数据获取部

数据获取部获取本系统所需的各种数据，例如机器人的实际关节位置P_r、实际关节速度V_r、关节电流I_r以及外界施加在机器人本体上的示教力F。对于采用Ehtercat总线式架构的机器人系统，可以方便的通过总线获取电机的编码器读数、关节电机电流，通过简单的乘法和差分运算即可计算出关节位置P_r、关节速度V_r。

外界施加在机器人本体上的示教力可以通过多种方法获取，例如在机器人末端安装多维力感知机构，并将操纵手柄安装于感知机构另一侧，通过力感知机构可以方便地解算施加在机器人本体上的示教力F_r。此处的示教力F_r是一个六维矢量，包含三个轴向力分量和三个转矩分量。

·逻辑控制部

根据机器人相关数据判断机器人当前的工作状态，如果目标参数处于不合理范围，则认定机器人处于异常状态并进行异常处理；如果目标参数处于合理范围，则认定机器人处于正常状态并进入拖动示教模式进行控制。

关于逻辑控制部，对其操作包括以下步骤：

S1、异常检测：主要是检测机器人实际位置、实际速度以及负载是否位于合理区间。工作在位置环的机器人拖动示教实施方案包括两种：一种是基于机器人末端的六维力感知机构辨识外界的示教力，另一种是基于关节电流估计外界的示教力。当机器人末端或关节同外部环境发生接触时，有可能出现力感知机构过载或关节电流过载的情况。另外当机器人超出指定运动范围时，有可能与机器人本体上的限位挡块或者外围环境发生撞击，从而引起设备和操作人员损伤，异常检测模块主要对于各种异常情况加以防护，其具体实施方案如下：

如果Pr>P+(预设最大值)并且Vr>0，则认定机器人正向越位。

如果Pr<P-(预设最小值)并且Vr>0，则认定机器人负向越位。

如果I>Im(额定电流)，则认定机器人此时电机或驱动过载。

如果F>Fm(预设最大值)，则认定外界施加在机器人本体上的示教力超出了机器人的承受范围。

如果满足上面任一种情况，则认为机器人处于异常情况，转到S2步骤；否则处于正常情况，转到S3步骤。

S2、错误处理：如果机器人处于异常状态，则将示教模式切换为错误急停(ErrorStop)模式，并立即终止当前运动，返回错误码。

S3、示教模式切换：根据机器人当前运行状态控制示教模式，其模式切换原理如下：

ErrorStop模式进入条件为：目标参数超出规定范围，所述目标参数包括实际位置、实际速度、关节转矩、关节电流；ErrorStop模式离开的条件为目标参数处于规定范围之内且机器人运动已经停止。TeachWait模式进入条件为：(a)当前处于TeachStop模式且运动已停止，(b)当前处于TeachMove模式且外界施加的示教力不满足触发条件；TeachWait模式离开的条件是外界施加的示教力满足触发条件。TeachMove模式进入条件为：当前处于TeachWait模式且外界施加的示教力满足触发条件；TeachMove模式离开的条件为：(a)外界施加的示教力不满足触发条件，(b)外界施加的示教力与运动方向瞬时相反。TeachStop模式进入的条件为：当前处于TeachMove模式且外界施加的示教力与运动方向瞬时相反；TeachStop模式离开的条件为：(a)外界施加的示教力与运动方向相同且满足触发条件，(b)外界施加的示教力与运动方向持续相反且满足触发条件，(c)机器人运动已停止。

例举一较佳实施例进行说明：

初始模式为TeachWait，即有Mode＝TeachWait，定义示教力死区下限F_t-，死区上限F_t+，停止临界速度V_ml，以及上一时刻的示教力F_last。其中死区由实测获取，V_ml可以取一个合理的小值，例如V_ml＝0.01度/s。

假如当前时刻为TeachWait模式：如果F>F_t+，或者F<F_t-，则将示教模式切换为TeachMove；如果F_t-<F<F_t+，则示教模式不变，仍处于TeachWait模式。

假如当前时刻为TeachMove模式：如果F·F_last<0，则认为拖动力方向发生改变，认为外界有停止或反向拖动的意愿，如果时间在停止意愿判断范围内，则认为有有停止意愿，因而需要立即停止当前运动，如果时间不在停止意愿判断范围，则认为有反向拖动意愿，立即转换方向进入反向TeachMove模式。将机器人示教状态切换到TeachStop模式。如果F·F_last>0，则需要继续判断触发条件：如果有F_t-<F<F_t+则认定外界双手脱离机器人，将机器人示教状态切换到TeachWait模式，否则保持TeachMove模式。

假如当前时刻为TeachStop模式：如果|V_r|<V_ml，则切换到TeachWait模式，否则保持TeachStop模式。

假如当前时刻为ErrorStop模式：如果|V_r|<V_ml，则切换到TeachStop模式，否则保持ErrorStop模式。

·运动控制部

从数据获取部获得外界施加在机器人本体上的示教力，从逻辑控制部获取当前运动模式，由示教力通过力-速度关系映射方程，计算机器人的期望速度，由期望速度通过速度-位置关系映射方程，计算机器人的指令位置。

力-速度关系映射方程的具体说明如下：

假设当前模式为ErrorStop，速度指令为零，Vexp＝0。

假设当前模式为TeachWait，速度指令为零，Vexp＝0。

假设当前模式为TeachStop，速度指令为零，Vexp＝0。

假设当前模式为TeachMove，则将示教力F带入以下方程求解速度指令Vexp：

其中[F_t-,F_t+]为运动死区，V_exp是速度指令，V_max为设定最大运动速度，V_min为设定极限速度，m和n为设定阶次，x_i和y_i为待求对应阶次项系数。

为顺利求解x_i，y_i，必须引入若干控制点，控制点个数由m和n的值决定，建议的控制点包括：(F_t,0)，(F₁+F_t,V_max)。并且所取参数和控制点必须使力-速度映射关系满足以下条件：

当F_t<|F|<F₁+F_t时，V_exp从0值单调递增至最大速度V_max。

当|F|>F₁+F_t时，V_exp从V_max单调递减至极限值V_min。

设m＝2,n＝2,x₀＝0,引入控制点(F₂,V₀)则令力-速度映射关系如下：

上述力-速度映射关系兼顾拖动的柔顺性和良好的低速操纵特性。

V_min为最小速度，V₀为期望速度，V_max为最大速度，F₁和F₂为待设定参数，0<V_min<V₀<V_max，F_t<F₁<F₂，且有以下对应关系

当|F|＝F₁+F_t时，V_exp＝V_max，因而减小F₁增大V_max之后可以提升机械手拖动的柔顺性。

当|F|＝F₂时，V_exp＝V₀，调节F₂和V₀可以控制机械手低速拖动特性。

当|F|＝+∞时，V_exp＝V_min,设置拖动力取大值情况下速度极限。

已知机器人期望速度，计算机器人的指令位置，速度-位置关系映射方程的具体说明如下：

其中，P_k为第k次的指令位置；P_k-1为第k-1次的指令位置；P_k-2为第k-2次的指令位置；t为平滑系数取0.01-0.05，一般取0.02；T为插补周期，例如取1ms。

相比较TeachStop模式，ErrorStop要求机械手以更快的速度终止当前运动，因而平滑系数t可以采用更大的取值。

上述实施例仅例示性说明本发明的原理及其功效，而非用于限制本发明的。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明的所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (7)

1.基于模式控制的机器人拖动示教系统，包括数据获取部、逻辑控制部和运动控制部，所述数据获取部收集所述运动控制部及所述逻辑控制部所需的数据；其特征在于：所述逻辑控制部根据数据获取部接收的指令来判断机器人当前的参数合理性，并判定机器人的运动模式；所述运动控制部根据所述数据获取部与逻辑控制部的结果，将示教力代入力-速度关系映射方程，解算机器人的速度指令，再将所述速度指令代入速度-位置关系映射方程，依据速度解算出机器人的指令位置；

所述运动控制部执行的力-速度关系映射方程为：

其中，F为示教力，V_exp为速度指令，[F_t-,F_t+]为运动死区，V_max为设定最大运动速度，V_min为设定极限速度，m和n为设定阶次，xi和yi为待求对应阶次项系数；

所述速度-位置关系映射方程为：

其中，P_k为第k次的指令位置，P_k-1为第k-1次的指令位置，P_k-2为第k-2次的指令位置，t为平滑系数，T为插补周期。

2.根据权利要求1所述的基于模式控制的机器人拖动示教系统，其特征在于：所述逻辑控制部判定出的运动模式包括错误急停模式、示教等待模式、示教移动模式、示教停止模式；机器人当前的参数不符合预定值即进入所述错误急停模式。

3.根据权利要求2所述的基于模式控制的机器人拖动示教系统，其特征在于：示教等待模式的进入条件是：a.机器人处于示教停止模式且机器人运动停止，或b.机器人外部施加的示教力值达不到触发示教移动模式的预设值；示教等待模式的离开条件是：a.机器人外部施加的示教力值达到触发示教移动模式的预设值。

4.根据权利要求3所述的基于模式控制的机器人拖动示教系统，其特征在于：示教移动模式的进入条件是：机器人当前处于示教等待模式且机器人外部施加的示教力值达到触发示教移动模式的预设值；示教移动模式的离开条件是：a.机器人外部施加的示教力值达不到触发示教移动模式的预设值，或b.机器人外部施加的示教力与机器人运动方向瞬时相反。

5.根据权利要求4所述的基于模式控制的机器人拖动示教系统，其特征在于：示教停止模式的进入条件是：机器人当前处于示教移动模式且机器人外部示教的示教力与机器人的运动方向瞬时相反；示教停止模式的离开条件是：a.机器人外部施加的示教力与机器人运动方向相同且该示教力值满足触发示教移动的预设值，或b.机器人外部施加的示教力与机器人运动方向持续相反且该示教力值满足触发示教移动的预设值，或c.机器人运动停止。

6.一种基于模式控制的机器人拖动示教的方法，其特征在于：采用权利要求5所述的机器人拖动示教系统，所述逻辑控制部的具体操作步骤包括：

S1、异常检测：检测机器人的实际位置、实际速度、关节电流是否符合预设的合理范围；

S2、错误处理：当机器人处于异常状态，将机器人当前的运作模式切换至错误急停模式；

S3、示教模式切换：根据机器人当前的运行状态以及各模式的进入、离开条件进行调整切换。

7.根据权利要求6所述的基于模式控制的机器人拖动示教的方法，其特征在于：在S1异常检测步骤中，异常情况的判定条件为：

A.关节位置大于预设最大值，且关节速度大于零，则判定机器人为正向越位异常；

B.关节位置小于预设最小值，且关节速度大于零，则判定机器人为负向越位异常；

C.关节电流大于电流额定值，则判定机器人此时电机或驱动过载异常；

D.示教力大于预设最大值，则判定施加在机器人上的示教力超出承受范围。

Images