CN106945044A - 机器人暂停运动控制方法和系统 - Google Patents

Abstract

本发明涉及一种机器人暂停运动控制方法和系统，所述方法包括：接收控制端传送的暂停运动指令；在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下；获取当前运动的目标点和运动的剩余时间，将目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；计算机械臂末端在暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度转发至控制主站；本发明可以构成一个完整的机器人暂停运动控制系统，降低机器人控制系统开发成本，提高控制效果。

Description

机器人暂停运动控制方法和系统

技术领域

本发明涉及机器人控制技术领域，特别是涉及一种机器人暂停运动控制方法和系统。

背景技术

Robot Operating System(ROS)是开源的机器人操作系统，可以为机器人开发者提供一个标准化的、开源的编程框架。但是ROS目前不支持实时线程操作。Open RobotControl Software(OROCOS)也是一种开源的机器人控制软件编程框架，它的特点是支持实时的线程操作，但是它的开放性，通用性没有ROS好。

因此，机器人暂停运动是机器人一种重要运动方式，目前在一些应用方案中，在ROS上采用OROCOS，但现有技术方案在搭建的架构上，不能构成一个完整的机器人控制器软件，在执行暂停运动时，无法充分利用ROS和OROCOS的特性，系统开发成本高，控制效果差。

发明内容

基于此，有必要针对上述技术问题，提供一种机器人暂停运动控制方法，降低系统开发成本，提高控制效果。

一种机器人暂停运动控制方法，包括：

接收控制端传送的暂停运动指令；

在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，执行所述暂停运动指令；

获取当前运动的目标点和运动的剩余时间，将所述目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；

计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。

一种机器人暂停运动控制系统，包括：总控模块、算法模块和通信管理模块；

所述总控模块，用于接收控制端传送的暂停运动指令；在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，执行所述暂停运动指令；

所述算法模块，用于获取当前运动的目标点和运动的剩余时间，将所述目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度；

所述通信管理模块，用于将所述目标角度、目标角速度和目标角加速度转发至控制主站。

上述机器人暂停运动控制方法和系统，在接收到控制端传送的暂停运动指令后，在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，计算暂停运动的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，并转发至控制主站；实现对机器人的暂停运动控制过程；该技术方案可以构成一个完整的机器人暂停运动控制系统，降低机器人控制系统开发成本，提高控制效果。

附图说明

图1是本发明实施例的机器人暂停运动控制方法流程图；

图2是机器人暂停运动控制系统结构示意图；

图3是总控模块执行算法流程图；

图4是算法模块执行算法流程图；

图5是一应用实例的机器人控制系统的硬件结构模型；

图6是基于ROS和OROCOS的搭建的软件架构图；

图7是控制器状态机的状态变化示意图；

图8是设备状态机的状态变化示意图。

具体实施方式

下面结合附图阐述本发明的机器人暂停运动控制方法的实施例。

本发明实施例中，所述暂停运动，是指机械臂暂停当前运动指令，速度降为0，当收到恢复指令后，将沿当前规划的轨迹继续运动，直到达到当前指令的目标位置。

参考图1所示，图1是本发明实施例的机器人暂停运动控制方法流程图，包括：

S10，接收控制端传送的暂停运动指令；

上述步骤中，可以利用预设的通信协议并以异步远程过程调用的方式接收暂停运动指令；

在此过程中，可以是操作者通过控制端的人机交互界面生成暂停运动指令，该指令无需传递参数；通过预设的通信协议，如基于IEC(The Internet CommunicationsEngine，互联网通信引擎)开发的通信协议，以异步远程过程调用的方式从人机交互界面接收暂停运动指令。

S20，在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，执行所述暂停运动指令；

本步骤中，通过判断当前机器人运动状态是否为可执行暂停运动的点到点运动、直线运动、圆弧运动或回到原点运动状态，若是，执行该暂停运动指令；

在一个实施例中，可以实时监测控制器状态机的状态，当转换为执行暂停运动状态时，调用暂停运动规划函数，开始执行急停运动规划流程；

对于判断当前机器人运动状态是否为可执行暂停运动的指定运动状态，可以通过判断控制器状态机是否为点到点运动、直线运动、圆弧运动、回到原点运动的四种状态之一，若是，执行调用所述暂停运动规划函数的步骤，否则，拒绝执行该暂停运行指令。

S30，获取当前运动的目标点和运动的剩余时间，将所述目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；

在一个实施例中，执行所述暂停运动指令后，可以异步触发暂停运动执行函数并通过第一接口调用暂停运动规划函数；其中，所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口；

进一步的，可以通过OROCOS的Operational Caller方法调用所述暂停运动规划函数；

执行所述暂停运动规划函数，获取并记录当前运动的目标点和运动的剩余时间，以及读取机械臂末端的当前位置、当前速度和当前加速度，并根据所述当前位置、当前速度和当前加速度计算暂停运动轨迹；

在一个实施例中，执行所述暂停运动规划函数，将所述控制器状态机转换为执行暂停运动(Active.Halt)状态。

作为实施例，若所述当前运动为点到点运动或回到原点运动，则计算暂停运动轨迹的方法可以包括如下步骤：

(A1)计算当前点到点或回到原点运动的目标点，根据当前运动的总时长与当前运动已经历的时间计算当前运动的剩余时间，记录所述目标点和剩余时间；

所述当前点到点或回到原点运动的目标点θ₁，当前运动的剩余时间：Tr＝T₀-t₀，其中T₀是当前运动的总时长，t₀是当前运动已经历的时间；

(A2)读取机械臂当前的关节角度，关节角速度，以及关节角加速度；

具体的，读取机械臂当前的关节角度θ₀，关节角速度关节角加速度

(A3)将当前时间记为0，依据最大加速度使速度降为0的原则，计算各个关节速度降为0的时间，记为暂停运行指令执行时间；

所述各个关节速度降为0的时间计算公式为：

其中，i代表关节的标号，选取T_i中的最大值，记为暂停运行指令执行时间T；

(A4)根据暂停运行指令执行时间T得到暂停运行轨迹。

在一个实施例中，若所述当前运动为直线运动，则计算暂停运动轨迹的方法可以包括如下步骤：

(B1)计算当前直线运动的目标点，根据当前运动的总时长与当前运动已经历的时间计算当前运动的剩余时间，记录所述目标点和剩余时间；

具体的，所述当前直线运动的目标点s_e，当前运动的剩余时间：Tr＝T₀-t₀，其中，T₀是当前运动的总时长，t₀是当前运动已经历的时间。

(B2)根据直线运动轨迹计算机械臂当前时刻的轨迹，关节角速度和关节角加速度；

具体的，当前直线运动轨迹已由已知算法规划，因此可以计算机械臂当前时刻的轨迹s₀，关节角速度关节角加速度

(B3)根据所述轨迹，关节角速度和关节角加速度计算暂停直线运动需要走过的轨迹，得到暂停运动轨迹，并记录恢复运动时机械臂沿直线已走过的轨迹；

具体的，暂停直线运动需要走过的轨迹计算公式为：

其中，a_max为直线运动最大的加速度；记录s₂＝s₀+s₁，其中，s₂为恢复运动时机械臂沿直线已走过的轨迹。

在一个实施例中，若所述当前运动为圆弧运动，则计算暂停运动轨迹的方法可以包括如下步骤：

(C1)计算当前圆弧运动的目标点，根据当前运动的总时长与当前运动已经历的时间计算当前运动的剩余时间，记录所述目标点和剩余时间；

所述当前圆弧运动的目标点s_e，当前运动的剩余时间：Tr＝T₀-t₀，其中，T₀是当前运动的总时长，t₀是当前运动已经历的时间；

(C2)根据圆弧运动轨迹计算机械臂当前时刻的轨迹，关节角速度和关节角加速度；

具体的，当前圆弧运动轨迹已由已知算法规划，因此可以计算机械臂当前时刻的轨迹s₀，关节角速度关节角加速度

(C3)根据所述轨迹，关节角速度和关节角加速度计算暂停圆弧运动需要走过的轨迹，得到暂停运行轨迹，并记录恢复运动时机械臂沿圆弧已走过的轨迹；

暂停圆弧运动需要走过的轨迹计算公式为：

其中，a_max为圆弧运动最大的加速度；记录s₂＝s₀+s₁，其中，s₂为恢复运动时机械臂沿圆弧已走过的轨迹。

S40，计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。

在一个实施例中，通过第二接口将所述机械臂关节的目标角度、目标角速度和目标角加速度发送给设备通信软件转发至控制主站；其中，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口。

作为实施例，对于计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的机械臂关节的目标角度、目标角速度和目标角加速度的方法，可以包括如下步骤：

(a)在机械臂开始在所述暂停运动轨迹上运动后，统计机械臂的运动时间t_n；

(b)若所述运动时间满足：t_n≤T，每隔设定时间τ计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；τ＝1/f，f表示刷新频率；一般情况下，所述τ为1毫秒；

(c)若运动时间t_n满足：t_n＞T，则停止运动；其中，T为暂停运行指令执行时间。

如上述实施例，若所述当前运动为点到点运动或回到原点运动，则计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括如下步骤：

(A5)根据机械臂当前的关节角速度和所述暂停运行指令执行时间计算各个机械臂关节的目标角加速度；

具体的，机械臂关节的目标角加速度计算公式为：

(A6)根据所述目标角加速度计算各个机械臂关节的目标角速度与关节目标角度；

具体的，各个机械臂关节的目标角速度与关节目标角度θ_i的计算公式分别为：

式中，θ₀为机械臂当前的关节角度，为关节角速度，为关节角加速度；t表示暂停运动过程中的时刻。

如上述实施例，若所述当前运动为直线运动，则计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括如下步骤：

(B4)计算暂停运动中机械臂任一时刻t的轨迹位置、轨迹速度和轨迹加速度：

具体的，任一时刻t的轨迹位置、轨迹速度和轨迹加速度：

其中，s₀为机械臂当前时刻的轨迹，为关节角速度，为关节角加速度，s为轨迹位置，为轨迹速度，为轨迹加速度；

(B5)根据所述轨迹位置、轨迹速度和轨迹加速度计算机械臂末端的目标位置、速度与加速度；

具体的，机械臂末端的目标位置、速度与加速度的计算公式为：

其中，P_t为目标位置，为速度，为加速度，是当前直线运动的单位向量；

(B6)使用逆运动学方法根据所述目标位置，速度与加速度计算机械臂关节的目标角度、目标角速度和目标角加速度；

具体的，所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：

θ_t＝invKinematics(P_t)

其中，θ_t为目标角度，为目标角速度，为目标角加速度，invKinematics()表示逆运动学的计算公式，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为的伪逆。

如上述实施例，若所述当前运动为圆弧运动，则计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括如下步骤：

(C4)计算暂停运动中机械臂任一时刻t的轨迹位置、轨迹速度和轨迹加速度；

具体的，任一时刻t的轨迹位置、轨迹速度和轨迹加速度：

其中，s₀为机械臂当前时刻的轨迹，为关节角速度，为关节角加速度，s为轨迹位置，为轨迹速度，为轨迹加速度；

(C5)根据所述轨迹位置、轨迹速度和轨迹加速度计算机械臂末端的目标位置、速度与加速度；

具体的，机械臂末端的目标位置P_t＝[x_t,y_t,z_t]^T：

其中，θ_t为圆弧P₀P_t对应的圆心角，满足a₁,a₂,a₃,b₁,b₂,b₃，R等参数均是当前圆弧运动轨迹已知参数；

机械臂末端的目标速度和加速度计算公式为：

其中，是点P_t的切向量；

(C6)使用逆运动学方法根据所述目标位置，速度与加速度计算机械臂关节的目标角度、目标角速度和目标角加速度。

所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：

θ_t＝invKinematics(P_t)

其中，θ_t为目标角度，为目标角速度，为目标角加速度，invKinematics()表示逆运动学的计算公式，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为的伪逆。

上述实施例的技术方案，利用OROCOS的实时输入/输出接口，通过设定通信协议以异步远程过程调用的方式接收暂停运动指令，启动暂停运动指令触发函数，调用机器人运动规划的暂停运动执行函数，根据该函数计算暂停运动的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通过设备通信软件将上述状态参数转发至控制主站，实现对机器人的暂停运动控制过程；该技术方案可以进行控制指令的接收、解析，算法调用、执行，参数实时计算和传输等功能，可以构成一个完整的机器人暂停运动控制系统，降低机器人控制系统开发成本，提高控制效果；另外结合了控制器状态机的应用，实现对算法处理过程的优化控制，进一步提高了控制效果。

针对于机器人暂停运动控制方法，本发明提供了该控制方法对应的机器人暂停运动控制系统，

参考图2所示，图2是机器人暂停运动控制系统结构示意图，包括：总控模块、算法模块和通信管理模块；

所述总控模块，用于接收控制端传送的暂停运动指令；在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，执行所述暂停运动指令；

所述算法模块，用于获取当前运动的目标点和运动的剩余时间，将所述目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度；

所述通信管理模块，用于将所述目标角度、目标角速度和目标角加速度转发至控制主站。

在一个实施例中，所述总控模块通过第一接口与算法模块进行通信连接，算法模块通过第二接口与通信管理模块进行通信连接，所述第一接口、第二接口是基于ROS上创建OROCOS的实时输入/输出接口；

所述总控模块执行暂停运动指令后，异步触发暂停运动执行函数并通过第一接口调用暂停运动规划函数；

所述算法模块执行所述暂停运动规划函数，并计算暂停运动轨迹；

以及通过第二接口将所述机械臂关节的目标角度、目标角速度和目标角加速度发送给通信管理模块。

上述实施例的技术方案，利用OROCOS的实时输入/输出接口，总控模块通过设定通信协议以异步远程过程调用的方式接收暂停运动指令，启动暂停运动指令触发函数，调用算法模块的机器人运动规划的暂停运动执行函数，根据该函数计算暂停运动的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通信管理模块将上述状态参数转发至控制主站，实现对机器人的暂停运动控制过程；该技术方案可以进行控制指令的接收、解析，算法调用、执行，参数实时计算和传输等功能，可以构成一个完整的机器人暂停运动控制系统，降低机器人控制系统开发成本，提高控制效果。

作为实施例，总控模块执行算法流程，可以参考图3所示，图3是总控模块执行算法流程图；具体如下：

1)操作者通过人机交互界面生成暂停运动指令。

2)总控模块通过通信协议，以异步远程过程调用的方式从人机交互界面接收暂停运动指令。

3)指令到达总控模块后，异步触发暂停运动执行函数，该函数首先判断控制器状态机是否为点到点运动，直线运动，圆弧运动，回到原点的四种状态之一。若不是，则拒绝执行此次暂停运行指令。

4)若控制器状态机是点到点运动，直线运动，圆弧运动，回到原点的四种状态之一，则执行如下操作：

a)通过OROCOS的Operational Caller方法调用算法模块的暂停运动规划函数。

b)将控制器状态机转换为执行暂停运动(Active.Halt)状态。

作为实施例，算法模块执行算法流程，可以参考图4所示，图4是算法模块执行算法流程图；具体如下：

1)暂停运动规划函数被总控模块调用，开始执行暂停运动规划流程。

2)若当前运动是回零运动或者点到点运动，则调用点到点暂停规划算法：

a)记录当前运动的目标点θ₁；

b)记录当前运动的剩余时间：Tr＝T–t。其中T是当前运动的总时长，t是当前运动已经历的时间；

c)读取机械臂当前的关节角度θ₀，关节角速度关节角加速度

d)将当前时间记为0，依据最大加速度使速度降为0的原则，计算各个关节速度降为0的时间：

其中，i代表关节的标号。选取T_i中的最大值，记为暂停指令执行时间，记为T。

e)计算各个关节的加速度为：

f)t时刻对应的速度与关节角位置分别为：

3)若当前运动是直线运动，则调用直线暂停的算法：

a)记录当前运动的目标点s_e。

b)记录当前运动的剩余时间：Tr＝T–t。其中T是当前运动的总时长，t是当前运动已经历的时间；

c)由于当前直线运动轨迹可以由五次多项式算法规划，因此可以计算机械臂当前时刻的轨迹s₀，关节角速度关节角加速度

d)计算暂停直线运动需要走过的轨迹：

其中，a_max就直线运动最大的加速度。

e)记录恢复运动时，机械臂沿直线已走过的轨迹：s₂＝s₀+s₁；

f)计算暂停运动中机械臂任一时刻t的轨迹，轨迹速度，轨迹加速度：

g)计算机械臂末端的目标位置，速度与加速度：

其中，是直线运动的单位向量。

h)使用逆运动学方法，计算关节角的位置：

θ_t＝invKinematics(P_t) (8)

i)关节的角速度与角加速度，可由如下表达式求出：

其中，J为雅可比(Jacobian)矩阵，为雅可比矩阵的一阶导，J⁺为的伪逆(Pseudoinverse)。

4)若当前运动是圆弧运动，则调用圆弧暂停的算法：

a)记录当前运动的目标轨迹s_e；

b)记录当前运动的剩余时间：Tr＝T–t。其中T是当前运动的总时长，t是当前运动已经历的时间；

c)由于当前圆弧运动轨迹可以由五次多项式算法规划，因此可以计算机械臂当前时刻的轨迹s₀，关节角速度关节角加速度

d)计算暂停圆弧运动需要走过的轨迹：

其中，a_max就直线运动最大的加速度。

e)记录恢复运动时，机械臂沿圆弧已走过的轨迹：s₂＝s₀+s₁

f)计算暂停运动中机械臂任一时刻t的轨迹，轨迹速度，轨迹加速度：

g)计算末端的目标位置P_t＝[x_t,y_t,z_t]^T：

其中，θ_t为圆弧P₀P_t对应的圆心角，满足a₁,a₂,a₃,b₁,b₂,b₃，R等参数可以由圆弧运动五次多项式规划算法求得；

h)求得末端的目标速度和加速度

其中，是点P_t的切向量；

i)使用逆运动学方法，计算关节角的位置：

θ_t＝invKinematics(P_t) (16)

其中，invKinematics()代表逆运动学的计算公式。

j)关节的角速度与角加速度，可由如下表达式求出：

其中，J为雅可比(Jacobian)矩阵，为雅可比矩阵的一阶导，J⁺为的伪逆(Pseudoinverse)。

5)运动时间t_n不超过运动时长T：

a)每隔1毫秒分别计算一次机械臂每个关节的目标角度，目标角速度和目标角加速度；

b)将机械臂每个关节的目标角度，目标角速度和目标角加速度，发送给通信管理模块。

6)运动时间t_n超过运动时长T，则停止运动。

作为实施例，通信管理模块，可以进一步用于执行如下算法：

读取机器人电机的状态信息，根据机器人运动模型计算机器人机械臂的状态信息，将机器人的状态信息通过第三接口反馈给总控模块，将机器人的状态信息通过第二接口反馈给算法模块；其中，所述电机的状态信息包括位置、速度和力矩等；所述机械臂的状态信息包括关节角度、关节角速度、关节角加速度、末端位姿、末端线速度、末端角速度、末端线加速度和末端线加速度等。

为了更加清晰本发明的实施例的技术方案，下面阐述采用本发明的硬件和软件环境应用实例：

参考图5所示，图5是一应用实例的机器人控制系统的硬件结构模型，在机器人控制器的搭建软件架构，运行于Linux操作系统，该Linux主机可以是X86架构的PC机，或者ARM芯片嵌入式架构的开发板，总控模块接入控制端的人机交互界面的控制指令。

Linux主机可以安装如下软件：安装Xenomai或者RTAI或者RT Preempt的实时内核补丁；安装ROS，OROCOS，rFSM等软件。

参考图6所示，图6是基于ROS和OROCOS的搭建的软件架构图；在控制过程中，操作系统上运行总控模块、算法模块和通信管理模块。

1、对于总控模块：

(1)总控模块使用ROS的orocreate-catkin-pkg方法创建ROS的Package，记为Ec_control_system，然后在Package中，通过继承OROCOS的RTT：：TaskContext类，记为Ec_control_system_component。

在Ec_control_system_component类的构造函数中，设置为执行如下操作：

a)利用OROCOS的RTT：：Input与RTT：：Output方法，对模块的输入，输出接口进行定义。

其中输入的接口包括：

①通信管理模块传入的诊断数据；

②通信管理模块传入的状态反馈信息：包括电机运行状态等等；

③控制器状态机的状态；

输出的接口包括：

①控制器状态机事件触发，输出给控制器状态机。

b)利用OROCOS的Operational Caller方法设置函数调用接口。

第一，设置事件报告的回调函数：对事件报告处理请求进行响应，包括生成错误的时间戳，事件级别等信息，并将事件信息，发送给人机交互界面显示。

第二，设置警报设置的回调函数：依据诊断信息，判断是否生成警报。比如，位置，速度，加速度是否超限等等。

第三，设置各种运动规划的控制指令触发函数，这些函数将对算法模块的相应响应函数进行调用。

c)调用OROCOS的Properties方法定义总控模块的属性，将总控模块定义一个机械臂关节个数的属性。

(2)在Ec_control_system_component的StartHook()成员函数中，设置执行如下操作：

a)检查日志报告是否正常，若异常直接退出，并将相关信息通过事件报告接口传递给总控模块处理；

b)通过ICE开发的通信协议建立与人机交互界面的通信连接，调用通信协议提供的动态异步远程过程调用(RPC)方法，对人机交互界面发起的控制指令进行响应的回调函数进行绑定。该回调函数，首先根据ICE(The Internet Communications Engine，互联网通信引擎)提供的远程过程异步调用方法传入的第一个参数，判断调用类型，然后依据此类型选择调用相应运动规划的运动指令触发函数。

(3)对于Ec_control_system_component的CleanUpHook()成员函数，为了使得该函数在总控模块结束运行时，实现自动调用，还可以设置为执行如下操作：

a)调用ICE开发的通信协议接口，关闭与人机交互界面的通信连接。

(4)对于控制器状态机，参考图7所示，图7是控制器状态机的状态变化示意图；可以设置Init、Ready、Fault、Active.Recovery、Active.Halt、Active.Hands、Active.ToZero、Active.PTP、Active.Line、Active.Circle、Active.Stop共十一个状态，分别代表初始化、等待指令输入、恢复、暂停、手动示教、回到原点、暂停运动、暂停运动、暂停运动、急停状态。其中，Active.Recovery、Active.Halt、Active.Hands、Active.ToZero、Active.PTP、Active.Line、Active.Circle、Active.Stop这八个状态组成一个Active状态的集合，Active的状态转移规则，对八个子状态均有效。例如，给八个状态中的任一个，写入“e_ready”事件，将控制器状态机的状态从当前状态转移到Ready状态(即等待指令输入状态)。

另外，还可以使用Lua语言，编写总控模块的启动文件，该启动文件设置为执行如下动作：

a)通过OROCOS的import方法，加载模块进行运行；

b)定义模块的刷新频率，线程的优先级别；

c)对模块的属性进行赋值；

d)通过OROCOS的connect方法，将总控模块的输入、输出接口和算法模块和通信管理模块的接口建立连接。

e)通过OROCOS的start方法，运行总控模块，总控模块将先调用StartHook()函数，然后按预设刷新频率，实时地周期性调用UpdateHook()函数。

2、对于算法模块：

算法模块使用ROS的orocreate-catkin-pkg方法创建ROS的Package，记为Ec_control_loop，然后在Package中，通过继承OROCOS的RTT：：TaskContext类，记为Ec_control_loop_component。

(1)在Ec_control_loop_component类的构造函数中，设置为执行如下操作：

a)算法模块利用OROCOS的RTT：：Input与RTT：：Output方法，对输入，输出接口进行定义。

其中输入的接口包括：

①通信管理模块传入的电机运行数据；

②通信管理模块传入的诊断数据；

③设备状态机的状态；

④控制器状态机的状态；

输出的接口包括：

①电机控制指令数据，输出给设备通信模块；

②设备状态机事件触发，输出给设备状态机；

③控制器状态机事件触发，输出给控制器状态机。

b)利用OROCOS的Operational Caller方法设置函数调用接口，设置事件报告的接口：该接口将触发总控模块的事件报告处理函数设置各种运动规划的返回原点指令响应函数。

c)调用OROCOS的Properties方法定义算法模块的属性，算法模块定义一个机械臂关节个数的属性。

(2)在Ec_control_loop_component的StartHook()成员函数中，设置为执行如下操作：

a)检查日志报告是否正常，若异常直接退出，并将相关信息通过事件报告接口传递给总控模块处理；

b)检查电机运行数据通道是否有数据，若无数据直接退出，并将相关信息通过事件报告接口传递给总控模块处理。

(3)对于Ec_control_loop_component类的UpdateHook()成员函数，设置该函数在算法模块运行时，按照用户设定的频率实时运行(如设为100Hz)，可以设置为执行如下操作：

a)读取控制器状态机状态；

b)根据控制器状态机的不同状态，执行不同操作：

Ⅰ、如果是暂停运动，暂停运动，暂停运动，手动示教，急停，回到原点状态。此时，执行如下操作：

如果指令缓冲区的控制指令个数小于20个，则将所有指令一起发送给通信管理模块，并将控制器状态机的状态改变为等待指令输入状态；

如果指令缓冲区的控制指令个数大于20个，则取指令队列末尾的20个，发送给通信管理模块；

Ⅱ、如果是暂停状态，则什么也不做。

(4)对于Ec_control_loop_component类，定义暂停运动，暂停运动，暂停运动，手动示教，急停，回到原点，暂停，恢复等函数调用接口，实现如下：

a)暂停运动，暂停运动，暂停运动，手动示教，回到原点的函数，内部实现如下：

检查控制器状态机是否处于等待指令输入状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块处理；

读取电机的当前的状态信息；

依据电机的当前状态，分别调用暂停运动，暂停运动，暂停运动，手动示教，回到原点的运动规划，并将生成的电机控制指令保存到指令缓冲区；

将控制器状态机设为相应的状态。比如暂停运动回调函数，则将控制器状态机设为暂停运动状态。

b)暂停函数，内部实现如下：

检查控制器状态机是否是暂停运动，暂停运动，暂停运动，手动示教，回到原点等状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块进行处理；

记录当前控制器状态机的当前状态，并将控制器状态机的状态转变为暂停状态。

c)恢复函数，内部实现如下：

检查控制器状态机是否是暂停状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块处理；

将控制器状态机的状态转变为暂停前的状态。

d)急停函数，内部实现如下：

Ⅰ、检查控制器状态机是否是暂停运动，暂停运动，暂停运动，手动示教，回到原点等状态。如果不是则退出，并将相关信息通过事件报告接口传递给总控模块处理；

Ⅱ、读取电机的当前的状态信息；

Ⅲ、将电机控制指令缓冲区清零；

Ⅳ、调用速度规划运动规划，让电机以最短时间，速度降为0，并将生成的电机控制指令保存到指令缓冲区。

(5)使用Lua语言，编写算法模块的启动文件，设置为执行如下动作：

a)通过OROCOS的import方法，加载算法模块；

d)定义算法模块的刷新频率，线程的优先级别；

c)对算法模块的属性进行赋值；

d)通过OROCOS的connect方法，将算法模块的输入，输出接口和总控模块和通信管理模块的接口建立连接。

e)通过OROCOS的start方法，运行算法模块，算法模块先调用StartHook()函数，然后按设置的刷新频率，实时地周期性调用UpdateHook()函数。

3、对于通信管理模块：

通信管理模块可以通过linux主机minicom中的ttyACM0与Arm开发板通信，可以在该Arm开发板上运行一个CANOpen主站协议，该主站协议可以设置一个指令缓存区，最多可以存储25个指令。

通信管理模块可以利用OROCOS的RTT：：Input与RTT：：Output方法与机器人算法模块和总控模块进行通信。

利用rFSM软件建立设备状态机，对通信管理模块的业务逻辑进行控制。

通信管理模块利用OROCOS的RTT：：Input与RTT：：Output方法与设备状态机连接，可改变设备状态机的状态，并读取状态。

(1)通信模块使用ROS的orocreate-catkin-pkg方法创建为ROS的Package，然后在Package中，通过继承OROCOS的RTT：：TaskContext类，创建一个OROCOS的实时模块，记为Ec_component。

在Ec_component类的构造函数中，设置为执行如下操作：

a)通信管理模块利用OROCOS的RTT：：Input与RTT：：Output方法，对输入，输出接口进行定义。

其中输入的接口包括：

①算法模块传入的控制指令数据；

②设备状态机的状态；

输出的接口包括：

①诊断数据，输出至算法模块和总控模块；

②电机运行数据和机械臂状态数据，输出给算法模块；

③状态机事件触发，输出给设备状态机。

b)利用OROCOS的Operational Caller方法定义函数调用接口，通信管理模块定义事件报告的接口，通过该接口触发总控模块的事件报告处理函数。

c)调用OROCOS的Properties方法定义通信管理模块的属性，通信管理模块定义一个机械臂关节个数的属性。

(2)在Ec_component的StartHook()成员函数中，设置为执行如下操作：

a)检查日志报告是否正常，若异常直接退出，并将相关信息通过事件报告接口传递给总控模块进行处理；

b)电机驱动初始化：

Ⅰ、通过ttyACM0与电机驱动器建立通信；

Ⅱ、电机指令缓冲队列清空；

Ⅲ、电机使能，如果使能成功，则进行下一步，否则退出；

Ⅳ、读取电机的位置，计算机器人的机械臂当前状态，包括关节角度，机械臂末端位姿；

c)机械臂状态初始化：

依据电机位置，判断机械臂是否需要执行回零运动。如果机械臂任一关节角度与零度相差大于0.01度，则执行回零运动，调用暂停运动规划，对回零运动进行规划。

d)改变设备状态机的状态：

如果机器人的机械臂需要执行回零运动，则控制设备状态机保持Init状态不变；否则，向设备状态机发送“e_nominal”事件，将设备状态机的状态转换为Active.Nominal。

(3)对于Ec_component的UpdateHook()成员函数，设置该函数在通信管理模块运行时，以用户设定的频率实时运行(如设为1KHz)，设置为执行如下操作：

a)读取设备状态机状态；

b)根据设备状态机的不同状态，执行不同操作：

Ⅰ、如果是Init状态，执行机械臂的回零运动。此时，执行如下操作：

读取系统的时钟，依据回零运动轨迹规划结果，计算该时刻电机的运动指令，并将该单条运动指令发送给CANOpen主站。

如果运动已回零成功，向设备状态机发送“e_nominal”事件，将设备状态机转换为Active.Nominal，并将该事件报告给总控模块。

Ⅱ、如果是Active.Nominal状态。此时，执行如下操作：

从控制指令数据的输入通道中，读取控制指令，并存储到电机指令缓冲队列中。

读取CANOpen主站指令缓冲区的现有指令个数，如果小于10个，则一次性从电机指令缓冲队列中取出15个运动指令发送给CANOpen主站。如果电机指令缓冲队列的指令个数小于15个，则一次性全部发送给CANOpen主站。

Ⅲ、如果是Active.Recovery状态。此时，通信管理模块处于恢复状态。

此时，依据诊断信息，对系统状态进行恢复，若恢复成功，向设备状态机发送“e_nominal”事件，将设备状态机转换为Active.Nominal。并将该事件报告给总控模块。

若恢复不成功，给状态机发送“e_fault”事件，状态机转换为Fault状态，将该事件报告给总控模块，并直接退出UpdateHook()。

Ⅳ、如果是Active.Halt状态。此时，模块处于暂停状态，执行如下操作：检查控制指令数据的输入通道中是否有新的指令，若有新指令，则读取控制指令，并存储到电机指令缓冲队列中。

Ⅴ、如果是Active.Hands状态。此时，模块处于手控模式，执行如下操作：

读取系统的时钟，依据运动轨迹规划结果，计算该时刻电机的运动指令，并将该单条运动指令发送给CANOpen主站。

Ⅵ、如果是Fault状态，则直接退出UpdateHook()。

c)读取电机状态，根据机械臂的模型计算机械臂关节以及末端运动状态信息，并通过输出数据通道，传递给算法模块与总控模块；

d)检查控制主站是否有错误报告信息，如果有错误信息，则将诊断信息传递给算法模块与总控模块。向设备状态机发送“e_recovery”事件，将设备状态机转换为Active.Recovery状态、并将该事件报告给总控模块。

(4)对于Ec_component的CleanUpHook()成员函数，该函数在模块结束运行时，自动调用，设置执行如下操作：

a)关闭电机驱动使能；

b)关闭电机驱动连接。

(5)参考图8所示，图8是设备状态机的状态变化示意图。共有Init(初始化)、Fault(中断)，Active.Recovery(恢复)、Active.Hands(手动示教)、Active.Halt(暂停)、Active.Nominal(运转)六个状态。Active.Recovery、Active.Hands、Active.Halt、Active.Nominal四个状态组成一个Active(使能)的状态集合，Active的状态转移规则，对四个子状态均有效。

(6)使用Lua语言，编写该模块的启动文件，该启动文件设置执行如下动作：

a)通过OROCOS的import方法，加载通信管理模块；

b)定义通信管理模块的刷新频率，线程的优先级别；

c)对通信管理模块的属性进行赋值；

d)通过OROCOS的connect方法，将通信管理模块的输入，输出接口和总控模块和算法模等的接口建立连接。

e)通过OROCOS的start方法，运行通信管理模块，通信管理模块先调用StartHook()函数，然后按定义好的刷新频率，实时地周期性调用UpdateHook()函数。

对于上述总控模块、算法模块和通信管理模块，设置为当运行后，如果用户需要中途停止该模块，同时按键盘的ctrl键与D键。

综上实施例，基于ROS和OROCOS，利用OROCOS的实时性特点，保证软件程序的实时性能；充分利用ROS的开放性，基于ROS、OROCOS开发的总控模块、算法模块和通信管理模块进行实时通信，共同构成一个完整的机器人控制器软件；进一步建立了控制器状态机和设备状态机，从而实现对总控模块、通信管理模块的业务逻辑进行有效管理。

通过OROCOS的RTT：：Input,RTT：：Output方法建立总控模块、算法模块和通信管理模块的数据输入、输出通道，通过OROCOS的Operational Caller方法定义函数调用接口，通过OROCOS的Properties方法定义总控模块、算法模块和通信管理模块的属性。因此，保证了总控模块、算法模块和通信管理模块之间的独立性与解耦性。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种机器人暂停运动控制方法，其特征在于，包括：

接收控制端传送的暂停运动指令；

在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，执行所述暂停运动指令；

获取当前运动的目标点和运动的剩余时间，将所述目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；

计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度转发至控制主站。

2.根据权利要求1所述的机器人暂停运动控制方法，其特征在于，若所述当前运动为点到点运动或回到原点运动，

所述获取并记录当前运动的目标点和运动的剩余时间，以及读取机械臂末端的当前位置、当前速度和当前加速度，并根据所述当前位置、当前速度和当前加速度计算暂停运动轨迹的方法包括如下步骤：

计算当前点到点或回到原点运动的目标点，根据当前运动的总时长与当前运动已经历的时间计算当前运动的剩余时间，记录所述目标点和剩余时间；

读取机械臂当前的关节角度，关节角速度，以及关节角加速度；

依据最大加速度使速度降为0的原则，计算各个关节速度降为0的时间，记为暂停运行指令执行时间；

根据暂停运行指令执行时间得到暂停运行轨迹。

所述计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括如下步骤：

根据机械臂当前的关节角速度和所述暂停运行指令执行时间计算各个机械臂关节的目标角加速度；

根据所述目标角加速度计算各个机械臂关节的目标角速度与关节目标角度。

3.根据权利要求2所述的机器人暂停运动控制方法，其特征在于，所述当前点到点或回到原点运动的目标点θ₁，当前运动的剩余时间：Tr＝T₀-t₀，其中T₀是当前运动的总时长，t₀是当前运动已经历的时间；

所述各个关节速度降为0的时间计算公式为：

$T_i=\frac{{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{{\stackrel{\·\·}{\mathrm{\θ}}}_{imax}}$

其中，i代表关节的标号，选取T_i中的最大值，记为暂停运行指令执行时间T；

机械臂关节的目标角加速度计算公式为：

${\stackrel{\·\·}{\mathrm{\θ}}}_{imax}=\frac{{\stackrel{\·}{\mathrm{\θ}}}_{i0}}{T}$

各个机械臂关节的目标角速度与关节目标角度θ_i的计算公式分别为：

${\stackrel{\·}{\mathrm{\θ}}}_i={\stackrel{\·}{\mathrm{\θ}}}_{i0}-{\stackrel{\·\·}{\mathrm{\θ}}}_{imax}t$

${\mathrm{\θ}}_i={\stackrel{\·}{\mathrm{\θ}}}_{i0}t-\frac{{\stackrel{\·\·}{\mathrm{\θ}}}_{imax}{t}^2}{2}$

式中，θ₀为机械臂当前的关节角度，为关节角速度，为关节角加速度；t表示暂停运动过程中的时刻。

4.根据权利要求1所述的机器人暂停运动控制方法，其特征在于，若所述当前运动为直线运动；

所述获取并记录当前运动的目标点和运动的剩余时间，以及读取机械臂末端的当前位置、当前速度和当前加速度，并根据所述当前位置、当前速度和当前加速度计算暂停运动轨迹的方法包括：

计算当前直线运动的目标点，根据当前运动的总时长与当前运动已经历的时间计算当前运动的剩余时间，记录所述目标点和剩余时间；

根据直线运动轨迹计算机械臂当前时刻的轨迹，关节角速度和关节角加速度；

根据所述轨迹，关节角速度和关节角加速度计算暂停直线运动需要走过的轨迹，得到暂停运动轨迹，并记录恢复运动时机械臂沿直线已走过的轨迹；

所述计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括如下步骤：

计算暂停运动中机械臂任一时刻t的轨迹位置、轨迹速度和轨迹加速度：

根据所述轨迹位置、轨迹速度和轨迹加速度计算机械臂末端的目标位置、速度与加速度；

使用逆运动学方法根据所述目标位置，速度与加速度计算机械臂关节的目标角度、目标角速度和目标角加速度。

5.根据权利要求4所述的机器人暂停运动控制方法，其特征在于，所述当前直线运动的目标点s_e，当前运动的剩余时间：Tr＝T₀-t₀，其中，T₀是当前运动的总时长，t₀是当前运动已经历的时间；

暂停直线运动需要走过的轨迹计算公式为：

$s_1=\frac{{{\stackrel{\·}{s}}_0}^2}{2a_{max}}$

其中，a_max为直线运动最大的加速度；记录s₂＝s₀+s₁，其中，s₂为恢复运动时机械臂沿直线已走过的轨迹；

任一时刻t的轨迹位置、轨迹速度和轨迹加速度：

$s={\stackrel{\·}{s}}_{0}t-\frac{a_{max}{t}^2}{2}$

$\stackrel{\·}{s}\left(t\right)={\stackrel{\·}{s}}_0-a_{max}t$

$\stackrel{\·\·}{s}\left(t\right)=a_{max}$

其中，s₀为机械臂当前时刻的轨迹，为关节角速度，为关节角加速度，s为轨迹位置，为轨迹速度，为轨迹加速度；

机械臂末端的目标位置、速度与加速度的计算公式为：

其中，P_t为目标位置，为速度，为加速度，是当前直线运动的单位向量；

所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：

θ_t＝invKinematics(P_t)

${\stackrel{\·}{\mathrm{\θ}}}_t=J^{+}{\stackrel{\·}{P}}_t$

${\stackrel{\·\·}{\mathrm{\θ}}}_t=J^{+}({\stackrel{\·\·}{p}}_t-\stackrel{\·}{J}{\stackrel{\·}{\mathrm{\θ}}}_t)$

其中，θ_t为目标角度，为目标角速度，为目标角加速度，invKinematics()表示逆运动学的计算公式，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为的伪逆。

6.根据权利要求1所述的机器人暂停运动控制方法，其特征在于，若所述当前运动为圆弧运动；

所述获取并记录当前运动的目标点和运动的剩余时间，以及读取机械臂末端的当前位置、当前速度和当前加速度，并根据所述当前位置、当前速度和当前加速度计算暂停运动轨迹的方法包括：

计算当前圆弧运动的目标点，根据当前运动的总时长与当前运动已经历的时间计算当前运动的剩余时间，记录所述目标点和剩余时间；

根据圆弧运动轨迹计算机械臂当前时刻的轨迹，关节角速度和关节角加速度；

根据所述轨迹，关节角速度和关节角加速度计算暂停圆弧运动需要走过的轨迹，得到暂停运行轨迹，并记录恢复运动时机械臂沿圆弧已走过的轨迹；

所述计算机械臂每个关节的目标角度、目标角速度和目标角加速度的方法包括如下步骤：

计算暂停运动中机械臂任一时刻t的轨迹位置、轨迹速度和轨迹加速度；

根据所述轨迹位置、轨迹速度和轨迹加速度计算机械臂末端的目标位置、速度与加速度；

使用逆运动学方法根据所述目标位置，速度与加速度计算机械臂关节的目标角度、目标角速度和目标角加速度。

7.根据权利要求6所述的机器人暂停运动控制方法，其特征在于，所述当前圆弧运动的目标点s_e，当前运动的剩余时间：Tr＝T₀-t₀，其中，T₀是当前运动的总时长，t₀是当前运动已经历的时间；

暂停圆弧运动需要走过的轨迹计算公式为：

$s_1=\frac{{{\stackrel{\·}{s}}_0}^2}{2a_{max}}$

其中，a_max为圆弧运动最大的加速度；记录s₂＝s₀+s₁，其中，s₂为恢复运动时机械臂沿圆弧已走过的轨迹；

任一时刻t的轨迹位置、轨迹速度和轨迹加速度：

$s={\stackrel{\·}{s}}_{0}t-\frac{a_{max}{t}^2}{2}$

$\stackrel{\·}{s}\left(t\right)={\stackrel{\·}{s}}_0-a_{\max }t$

$\stackrel{\·\·}{s}\left(t\right)=a_{max}$

其中，s₀为机械臂当前时刻的轨迹，为关节角速度，为关节角加速度，s为轨迹位置，为轨迹速度，为轨迹加速度；

机械臂末端的目标位置P_t＝[x_t,y_t,z_t]^T：

$\left\{\begin{array}{c}{x}_t=x_c+R\cos \left({\mathrm{\θ}}_t\right)a_1+R\sin \left({\mathrm{\θ}}_t\right)b_1\\ y_t=y_c+R\cos \left({\mathrm{\θ}}_t\right)a_2+R\sin \left({\mathrm{\θ}}_t\right)b_2\\ z_t=z_c+R\cos \left({\mathrm{\θ}}_t\right)a_3+R\sin \left({\mathrm{\θ}}_t\right)b_3\end{array}\right.$

其中，θ_t为圆弧P₀P_t对应的圆心角，满足a₁,a₂,a₃,b₁,b₂,b₃，R等参数均是当前圆弧运动轨迹已知参数；

机械臂末端的目标速度和加速度计算公式为：

其中，是点P_t的切向量；

所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：

θ_t＝invKinematics(P_t)

${\stackrel{\·}{\mathrm{\θ}}}_t=J^{+}{\stackrel{\·}{P}}_t$

${\stackrel{\·\·}{\mathrm{\θ}}}_t=J^{+}({\stackrel{\·\·}{p}}_t-\stackrel{\·}{J}{\stackrel{\·}{\mathrm{\θ}}}_t)$

其中，θ_t为目标角度，为目标角速度，为目标角加速度，invKinematics()表示逆运动学的计算公式，J为雅可比矩阵，为雅可比矩阵的一阶导，J⁺为的伪逆。

8.根据权利要求1至7任一项所述的机器人暂停运动控制方法，其特征在于，还包括：

在执行该暂停运动指令后，异步触发暂停运动执行函数并通过第一接口调用暂停运动规划函数；其中，所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口；

执行所述暂停运动规划函数，并计算暂停运动轨迹；

以及通过第二接口将所述机械臂关节的目标角度、目标角速度和目标角加速度发送给设备通信软件转发至控制主站；其中，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口。

9.根据权利要求8所述的机器人暂停运动控制方法，其特征在于，计算机械臂关节的目标角度、目标角速度和目标角加速度的方法包括：

在机械臂开始在所述暂停运动轨迹上运动后，统计机械臂的运动时间t_n；

若所述运动时间满足：t_n≤T，每隔设定时间τ计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；若运动时间t_n满足：t_n＞T，则停止运动；

其中，T为暂停运行指令执行时间。

10.一种机器人暂停运动控制系统，其特征在于，包括：总控模块、算法模块和通信管理模块；

所述总控模块，用于接收控制端传送的暂停运动指令；在当前机器人处于点到点运动、直线运动、圆弧运动或回到原点运动的任意一种当前运动状态下，执行所述暂停运动指令；

所述算法模块，用于获取当前运动的目标点和运动的剩余时间，将所述目标点和运动的剩余时间保存为恢复当前运动的轨迹计算参数；读取机械臂末端的当前位置、当前速度和当前加速度，并计算机械臂末端由当前位置、当前速度和当前加速度变化到停止时所经过的暂停运动轨迹；计算机械臂末端在所述暂停运动轨迹上运行时，机械臂关节在各个位置的目标角度、目标角速度和目标角加速度；

所述通信管理模块，用于将所述目标角度、目标角速度和目标角加速度转发至控制主站。