CN107030695A - 机器人返回原点运动控制方法和系统 - Google Patents

机器人返回原点运动控制方法和系统 Download PDF

Info

Publication number
CN107030695A
CN107030695A CN201710272269.9A CN201710272269A CN107030695A CN 107030695 A CN107030695 A CN 107030695A CN 201710272269 A CN201710272269 A CN 201710272269A CN 107030695 A CN107030695 A CN 107030695A
Authority
CN
China
Prior art keywords
origin
joint
target
return
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710272269.9A
Other languages
English (en)
Other versions
CN107030695B (zh
Inventor
阳方平
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangzhou Shiyuan Electronics Thecnology Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN201710272269.9A priority Critical patent/CN107030695B/zh
Publication of CN107030695A publication Critical patent/CN107030695A/zh
Application granted granted Critical
Publication of CN107030695B publication Critical patent/CN107030695B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1612—Programme controls characterised by the hand, wrist, grip control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1689—Teleoperation

Abstract

本发明涉及一种机器人返回原点运动控制方法和系统，所述方法包括：接收返回原点运动指令；获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，分别计算机械臂降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；分别计算机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，计算各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度，发送给设备通信软件转发至控制主站。本发明可以构成一个完整的机器人返回原点运动控制系统，降低机器人控制系统开发成本，提高控制效果。

Description

机器人返回原点运动控制方法和系统
技术领域
本发明涉及机器人控制技术领域，特别是涉及一种机器人返回原点运动控制方法和系统。
背景技术
Robot Operating System(ROS)是开源的机器人操作系统，可以为机器人开发者提供一个标准化的、开源的编程框架。但是ROS目前不支持实时线程操作。Open RobotControl Software(OROCOS)也是一种开源的机器人控制软件编程框架，它的特点是支持实时的线程操作，但是它的开放性，通用性没有ROS好。
因此，机器人返回原点运动是机器人一种重要运动方式，目前在一些应用方案中，在ROS上采用OROCOS，但现有技术方案在搭建的架构上，不能构成一个完整的机器人控制器软件，在执行返回原点运动时，无法充分利用ROS和OROCOS的特性，系统开发成本高，控制效果差。
发明内容
基于此，有必要针对上述技术问题，提供一种机器人返回原点运动控制方法，降低系统开发成本，提高控制效果。
一种机器人返回原点运动控制方法，包括：
接收控制端传送的返回原点运动指令；
获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，根据所述关节角度、关节角速度和关节角加速度分别计算机械臂由当前的关节角度、关节角速度和关节角加速度降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；
分别计算所述机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，得到返回原点运动轨迹；
计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度，发送给设备通信软件转发至控制主站。
一种机器人返回原点运动控制系统，包括：总控模块、算法模块和通信管理模块；
所述总控模块，用于接收控制端传送的返回原点运动指令；
所述算法模块，用于获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，根据所述关节角度、关节角速度和关节角加速度分别计算机械臂由当前的关节角度、关节角速度和关节角加速度降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；分别计算所述机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，得到返回原点运动轨迹；计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度；
所述通信管理模块，用于将所述目标角度、目标角速度和目标角加速度转发至控制主站。
上述机器人返回原点运动控制方法和系统，接收控制端传送的返回原点运动指令，计算返回原点运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通过设备通信软件将上述状态参数转发至控制主站，实现对机器人的返回原点运动的过程控制；该技术方案可以构成一个完整的机器人返回原点运动控制系统，降低机器人控制系统开发成本，提高控制效果。
附图说明
图1是本发明实施例的机器人返回原点运动控制方法流程图；
图2是机器人返回原点运动控制系统结构示意图；
图3是总控模块执行算法流程图；
图4是算法模块执行算法流程图；
图5是算法模块执行算法状态转换图；
图6是一应用实例的机器人控制系统的硬件结构模型；
图7是基于ROS和OROCOS的搭建的软件架构图；
图8是控制器状态机的状态变化示意图；
图9是设备状态机的状态变化示意图。
具体实施方式
下面结合附图阐述本发明的机器人返回原点运动控制方法的实施例。
本发明实施例中，所述原点指机械臂的各个关节角度都为0，返回原点运动，是指机械臂的关节角从当前关节角度θ0运动到原点的运动过程。
参考图1所示，图1是本发明实施例的机器人返回原点运动控制方法流程图，包括：
S10，接收控制端传送的返回原点运动指令；
上述步骤中，可以通过预设的通信协议并以异步远程过程调用的方式接收返回原点运动指令；
在此过程中，可以是操作者通过人机交互界面生成返回原点运动指令，该指令无需传递参数；通过预设的通信协议，如基于IEC(The Internet Communications Engine，互联网通信引擎)开发的通信协议，以异步远程过程调用的方式从人机交互界面接收返回原点运动指令。
S20，获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，根据所述关节角度、关节角速度和关节角加速度分别计算机械臂由当前的关节角度、关节角速度和关节角加速度降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；
在一个实施例中，在接收到所述返回原点运动指令后，异步触发返回原点运动执行函数，通过第一接口并根据所述返回原点运动执行函数调用返回原点运动规划函数；其中，所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口。
在一个实施例中，在调用返回原点运动规划函数前，可以首先由返回原点运动执行函数判断控制器状态机是否为准备(Ready)状态；
若是，通过OROCOS的Operational Caller方法调用所述返回原点运动规划函数，并将控制器状态机切换为执行返回原点运动(Active.toZero)状态；若否，则拒绝执行此次指令；
其中，所述控制器状态机的可被改变状态，并读取状态，设有初始化、指令等待、指令执行、中断和使能对应的状态。
S30，分别计算所述机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，得到返回原点运动轨迹；
在一个实施例中，在调用所述返回原点运动规划函数后，读取机械臂关节当前的节角度、关节角速度和关节角加速度，并根据所述节角度、关节角速度和关节角加速度计算返回原点运动轨迹。
进一步地，在调用返回原点运动规划函数后，可以根据所述返回原点运动规划函数执行返回原点运动规划流程，并检查控制器状态机是否为执行返回原点运动状态；若是，执行所述计算返回原点的运动轨迹的步骤，否则，退出执行流程。
作为实施例，计算返回原点的运动轨迹的方法，可以包括如下：
(1)获取机械臂当前的关节角度θ0、关节角速度和关节角加速度以及将目标关节角度，关节角速度，关节角加速度设为0；
(2)按照最快加速度与最大速度返回原点原则，根据所述关节角度θ0、关节角速度和关节角加速度计算返回原点运动需要的时间T；具体的，对于运行时长T的计算公式，可以如下：
式中，当前的关节角度θ0，关节角速度关节角加速度i表示任一关节；
(3)根据所述关节角度θ0、关节角速度和关节角加速度以及返回原点运动需要的时间T，生成返回原点运动轨迹；
具体的，所述返回原点运动轨迹的计算公式为：
S(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5
a0＝θ0
式中，S(t)是五次多项式，t是运行时间；ai，i＝1,…,5是系数。
S40，计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度，发送给设备通信软件转发至控制主站；
在一个实施例中，在计算各个位置的目标角度、目标角速度和目标角加速度后，通过第二接口将所述目标角度、目标角速度和目标角加速度发送给设备通信软件转发至控制主站；其中，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口；
在此过程中，实时计算各个位置的参数，然后作为控制指令输出至机器人进行控制其在运行轨迹上的位置，快速回到原点；
所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：
θt＝a0+a1t+a2t2+a3t3+a4t4+a5t5
式中，任意时刻t时的关节角位置θt、角速度和角加速度
作为实施例，计算目标角度、目标角速度和目标角加速度的方法包括：
1)在机械臂开始运动后，由0开始每隔设定时间τ统计机械臂的运动时间tn；τ＝1/f，f表示刷新频率；
即由零开始统计时间，按照刷新率不断计算机械臂各个关节的目标角度、目标角速度和目标角加速度；上述实施例中，一般情况下，所述τ为1毫秒；
2)若所述运动时间满足：tn≤aT，每隔设定时间τ依据所述计算模型分别计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；一般情况下，所述a＝70％；
3)若运动时间tn满足：aT＜tn＜T，将标志位更改为过渡状态，将控制器状态机状态设定为准备状态；
作为实施例，在过渡状态下，若有新指令输入，则启动过渡运动规划；若没有新指令输入，则每隔设定时间依据所述计算模型分别计算一次机械臂每个关节的目标角度，目标角速度和目标角加速度，并发送给设备通信软件。
4)若运动时间tn满足：tn≥T，点到点运动结束，将标志位更改为等待状态。上述实施例中，一般情况下，所述a＝70％，τ为1毫秒。
所述通信管理模块，可以进一步用于读取机器人电机的状态信息，根据机器人运动模型计算机器人机械臂的状态信息，将机器人的状态信息通过第三接口反馈给总控模块，将机器人的状态信息通过第二接口反馈给算法模块；其中，所述电机的状态信息包括位置、速度和力矩等；所述机械臂的状态信息包括关节角度、关节角速度、关节角加速度、末端位姿、末端线速度、末端角速度、末端线加速度和末端线加速度等。
上述实施例的技术方案，利用OROCOS的实时输入/输出接口，通过设定通信协议以异步远程过程调用的方式接收返回原点运动指令，启动返回原点运动指令触发函数，调用机器人运动规划的返回原点运动执行函数，根据该函数计算返回原点运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通过设备通信软件将上述状态参数转发至控制主站，实现对机器人的返回原点运动控制过程；该技术方案可以进行控制指令的接收、解析，算法调用、执行，参数实时计算和传输等功能，可以构成一个完整的机器人返回原点运动控制系统，降低机器人控制系统开发成本，提高控制效果；另外结合了控制器状态机和标志位的应用，实现对算法处理过程的优化控制，进一步提高了控制效果。
针对于机器人返回原点运动控制方法，本发明提供了该控制方法对应的机器人返回原点运动控制系统，
参考图2所示，图2是机器人返回原点运动控制系统结构示意图，包括：总控模块、算法模块和通信管理模块；
所述总控模块，用于接收控制端传送的返回原点运动指令；
所述算法模块，用于获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，根据所述关节角度、关节角速度和关节角加速度分别计算机械臂由当前的关节角度、关节角速度和关节角加速度降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；分别计算所述机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，得到返回原点运动轨迹；计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度；
所述通信管理模块，用于将所述目标角度、目标角速度和目标角加速度转发至控制主站。
在一个实施例中，所述总控模块可以通过第一接口与算法模块进行通信连接，算法模块通过第二接口与通信管理模块进行通信连接，所述第一接口、第二接口是基于ROS上创建OROCOS的实时输入/输出接口；
所述总控模块在接收到所述返回原点运动指令后，异步触发返回原点运动执行函数，通过第一接口并根据所述返回原点运动执行函数调用算法模块的返回原点运动规划函数；
所述算法模块将通过第二接口将所述目标角度、目标角速度和目标角加速度发送给通信管理模块；
所述通信管理模块将所述目标角度、目标角速度和目标角加速度转发至控制主站。
上述实施例的技术方案，利用OROCOS的实时输入/输出接口，总控模块通过设定通信协议以异步远程过程调用的方式接收返回原点运动指令，启动返回原点运动指令触发函数，调用算法模块的机器人运动规划的返回原点运动执行函数，根据该函数计算返回原点的运动轨迹，并实时计算机械臂各个关节在该运动轨迹上运行的目标角度、目标角速度和目标角加速度等状态参数，通信管理模块将上述状态参数转发至控制主站，实现对机器人的返回原点运动控制过程；该技术方案可以进行控制指令的接收、解析，算法调用、执行，参数实时计算和传输等功能，可以构成一个完整的机器人返回原点运动控制系统，降低机器人控制系统开发成本，提高控制效果。
作为实施例，总控模块执行算法流程，可以参考图3所示，图3是总控模块执行算法流程图；具体如下：
(1)操作者通过人机交互界面生成返回原点运动指令。该指令无需传递参数；
(2)返回原点运动指令通过通信协议，以异步远程过程调用的方式从人机交互界面发送给总控模块；
(3)指令到达总控模块后，会异步触发点到点运动执行函数。该函数首先判断控制器状态机是否为准备(Ready)状态。若不是Ready状态，则拒绝执行此次指令；
(4)若控制器状态机是Ready状态，则执行如下操作：
4.1、则通过OROCOS的Operational Caller方法调用算法模块的返回原点运动规划函数；
4.2、将控制器状态机转换为Active.toZero状态；
(5)判断控制器状态机是否重新变为Ready状态。若是Ready状态，则本次返回原点运动完成。
作为实施例，算法模块执行算法流程，可以参考图4所示，图4是算法模块执行算法流程图；具体如下：
(1)算法模块的返回原点运动规划函数被总控模块调用后，开始执行返回原点运动规划流程；
(2)检查控制器状态机是否为Active.toZero状态，若不是则退出；
(3)读取机械臂当前的关节角度θ0、关节角速度和关节角加速度设定目标关节角度、关节角速度和关节角加速度都为0，生成返回原点运动轨迹；计算过程如下：
按照最快加速度与最大速度返回原点原则，可求得各个关节返回原点的时间：
选取公式(1)计算得到返回时间最大值作为运动时长T。
返回原点的运动轨迹用如下公式表示：
S(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5
式中，t是运行时间；ai,i＝1,…,5是系数，则可求得系数：
a0＝θ0
根据以下公式计算出任意时刻t时的关节角位置θt，角速度角加速度
θt＝a0+a1t+a2t2+a3t3+a4t4+a5t5 (3)
(4)将运动时间tn记为0，参考图5，图5是算法模块执行算法状态转换图，将算法模块的状态标志位设为运动状态。
(5)算法模块的UpdateHook()函数检测到状态标志位为运动状态后，且运动时间tn不超过运动时长T的70％；
5.1、每隔1毫秒依据公式(3)、(4)、(5)分别计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；
5.2、将机械臂每个关节的目标角度、目标角速度和目标角加速度，发送给通信管理模块。
(6)若运动时间tn满足：0.7T＜tn＜T：
6.1、将算法模块的标志位更改为过渡状态；
6.2、将控制器状态机状态设定为Ready状态；
6.3、有新指令输入，则启动过渡运动规划；
6.4、每隔1毫秒依据公式(3)、(4)、(5)分别计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；并将机械臂每个关节的目标角度，目标角速度和目标角加速度，发送给通信管理模块。
(7)若运动时间tn满足：tn≥T：
本次返回原点运动结束，将算法模块的标志位更改为等待状态。
对于通信管理模块，其用于读取电机的状态，包括位置、速度、力矩；依据机器人运动模型，计算机器人的状态，包括关节角度、关节角速度、关节角加速度、末端位姿末端线速度、末端角速度、末端线加速度、末端线加速度等；将机器人的状态反馈给总控模块和算法模块。
为了更加清晰本发明的实施例的技术方案，下面阐述采用本发明的硬件和软件环境应用实例：
参考图6所示，图6是一应用实例的机器人控制系统的硬件结构模型，在机器人控制器的搭建软件架构，运行于Linux操作系统，该Linux主机可以是X86架构的PC机，或者ARM芯片嵌入式架构的开发板，总控模块接入控制端的人机交互界面的控制指令。
Linux主机可以安装如下软件：安装Xenomai或者RTAI或者RT Preempt的实时内核补丁；安装ROS，OROCOS，rFSM等软件。
参考图7所示，图7是基于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)对于控制器状态机，参考图8所示，图8是控制器状态机的状态变化示意图；可以设置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)参考图9所示，图9是设备状态机的状态变化示意图。共有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.一种机器人返回原点运动控制方法，其特征在于，包括：
接收控制端传送的返回原点运动指令；
获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，根据所述关节角度、关节角速度和关节角加速度分别计算机械臂由当前的关节角度、关节角速度和关节角加速度降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；
分别计算所述机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，得到返回原点运动轨迹；
计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度，发送给设备通信软件转发至控制主站。
2.根据权利要求1所述的机器人返回原点运动控制方法，其特征在于，计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度的步骤包括：
在机械臂开始在所述返回原点运动轨迹运动后，统计机械臂的运动时间tn；
若所述运动时间满足：tn≤aT，每隔设定时间τ依据分别计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度；a是系数，T为返回原点运动需要的时间；
若运动时间tn满足：aT＜tn＜T，将标志位更改为过渡状态，将控制器状态机状态设定为准备状态；
若运动时间tn满足：tn≥T，返回原点运动结束，将标志位更改为等待状态。
3.根据权利要求2所述的机器人返回原点运动控制方法，其特征在于，所述运行时长T的计算公式为：
式中，当前的关节角度关节角速度关节角加速度i表示任一关节，最大关节角度最大关节角速度最大关节角加速度
4.根据权利要求3所述的机器人返回原点运动控制方法，其特征在于，所述返回原点运动轨迹的计算公式为：
S(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5
a0＝θ0
式中，S(t)是五次多项式，t是运行时间；ai，i＝1,…,5是系数。
5.根据权利要求4所述的机器人返回原点运动控制方法，其特征在于，所述机械臂关节的目标角度、目标角速度和目标角加速度的计算公式为：
θt＝a0+a1t+a2t2+a3t3+a4t4+a5t5
式中，任意时刻t时的关节角位置θt、角速度和角加速度
6.根据权利要求2所述的机器人返回原点运动控制方法，其特征在于，在过渡状态下，若有新指令输入，则启动过渡运动规划；若没有新指令输入，则每隔设定时间τ计算一次机械臂每个关节的目标角度、目标角速度和目标角加速度，并发送给设备通信软件。
7.根据权利要求1至6所述的机器人返回原点运动控制方法，其特征在于，还包括：在接收到所述返回原点运动指令后，异步触发返回原点运动执行函数，通过第一接口并根据所述返回原点运动执行函数调用返回原点运动规划函数；在调用所述返回原点运动规划函数后，执行所述获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度的步骤；
以及通过第二接口将所述目标角度、目标角速度和目标角加速度发送给设备通信软件；其中，所述第一接口、第二接口是基于ROS上创建OROCOS的实时输入/输出接口。
8.根据权利要求7所述的机器人返回原点运动控制方法，其特征在于，还包括：
在调用返回原点运动规划函数前，所述返回原点运动执行函数判断控制器状态机是否为准备状态；
若是，通过OROCOS的Operational Caller方法调用所述返回原点运动规划函数，并将控制器状态机切换为执行返回原点运动状态；若否，则拒绝执行此次指令；
其中，所述状态机的可被改变状态，并读取状态，设有初始化、指令等待、指令执行、中断和使能对应的状态。
9.根据权利要求8所述的机器人返回原点运动控制方法，其特征在于，还包括：
在调用返回原点运动规划函数后，根据所述返回原点运动规划函数执行返回原点运动规划流程，并检查控制器状态机是否为执行返回原点运动状态；若是，执行所述计算返回原点的运动轨迹的步骤，否则，退出执行流程。
10.一种机器人返回原点运动控制系统，其特征在于，包括：总控模块、算法模块和通信管理模块；
所述总控模块，用于接收控制端传送的返回原点运动指令；
所述算法模块，用于获取机械臂的当前关节角度、当前关节角速度和当前关节角加速度；按照最快加速度与最大速度返回原点原则，根据所述关节角度、关节角速度和关节角加速度分别计算机械臂由当前的关节角度、关节角速度和关节角加速度降为0时所需要的运行时间，将运行时间中的最大值设为原点运动指令的执行时间；分别计算所述机械臂在原点运动指令的执行时间内，由当前关节角度、当前关节角速度和当前关节角加速度回到原点经过的轨迹，得到返回原点运动轨迹；计算机械臂各个关节在所述返回原点运动轨迹上运行的各个位置的目标角度、目标角速度和目标角加速度；
所述通信管理模块，用于将所述目标角度、目标角速度和目标角加速度转发至控制主站。
CN201710272269.9A 2017-04-19 2017-04-19 机器人返回原点运动控制方法和系统 Active CN107030695B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710272269.9A CN107030695B (zh) 2017-04-19 2017-04-19 机器人返回原点运动控制方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710272269.9A CN107030695B (zh) 2017-04-19 2017-04-19 机器人返回原点运动控制方法和系统

Publications (2)

Publication Number Publication Date
CN107030695A true CN107030695A (zh) 2017-08-11
CN107030695B CN107030695B (zh) 2019-07-09

Family

ID=59536219

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710272269.9A Active CN107030695B (zh) 2017-04-19 2017-04-19 机器人返回原点运动控制方法和系统

Country Status (1)

Country Link
CN (1) CN107030695B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019227307A1 (zh) * 2018-05-29 2019-12-05 深圳市大疆创新科技有限公司 地面机器人控制方法及地面机器人
CN112060091A (zh) * 2020-09-04 2020-12-11 苏州华兴源创科技股份有限公司 机器人回原点的方法、装置、机器人和存储介质
CN113232018A (zh) * 2021-04-27 2021-08-10 成都飞机工业（集团）有限责任公司 一种机器人姿态快速复位的方法
CN114845841A (zh) * 2019-10-09 2022-08-02 川崎重工业株式会社 控制方法、控制装置、机器人系统、程序及记录介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6039204A (ja) * 1983-08-11 1985-03-01 Fujitsu Ltd 多関節ロボツトの速度制御方法
JPS6436303A (en) * 1987-07-31 1989-02-07 Matsushita Electric Ind Co Ltd Angular acceleration control method
CN1399762A (zh) * 1999-05-28 2003-02-26 布鲁克斯自动化公司 基于预先规定的时间上最优的轨迹形状的机器人操纵器
CN1798637A (zh) * 2003-06-02 2006-07-05 本田技研工业株式会社 用于多关节型机器人的教导数据准备方法
CN103543677A (zh) * 2013-09-30 2014-01-29 北京大学深圳医院 一种配药机器人的回零控制系统及其实现方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS6039204A (ja) * 1983-08-11 1985-03-01 Fujitsu Ltd 多関節ロボツトの速度制御方法
JPS6436303A (en) * 1987-07-31 1989-02-07 Matsushita Electric Ind Co Ltd Angular acceleration control method
CN1399762A (zh) * 1999-05-28 2003-02-26 布鲁克斯自动化公司 基于预先规定的时间上最优的轨迹形状的机器人操纵器
CN1798637A (zh) * 2003-06-02 2006-07-05 本田技研工业株式会社 用于多关节型机器人的教导数据准备方法
CN103543677A (zh) * 2013-09-30 2014-01-29 北京大学深圳医院 一种配药机器人的回零控制系统及其实现方法

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019227307A1 (zh) * 2018-05-29 2019-12-05 深圳市大疆创新科技有限公司 地面机器人控制方法及地面机器人
CN110637265A (zh) * 2018-05-29 2019-12-31 深圳市大疆创新科技有限公司 地面机器人控制方法及地面机器人
CN114845841A (zh) * 2019-10-09 2022-08-02 川崎重工业株式会社 控制方法、控制装置、机器人系统、程序及记录介质
CN114845841B (zh) * 2019-10-09 2024-02-06 川崎重工业株式会社 控制方法、控制装置、机器人系统、程序及记录介质
CN112060091A (zh) * 2020-09-04 2020-12-11 苏州华兴源创科技股份有限公司 机器人回原点的方法、装置、机器人和存储介质
CN112060091B (zh) * 2020-09-04 2022-02-22 苏州华兴源创科技股份有限公司 机器人回原点的方法、装置、机器人和存储介质
CN113232018A (zh) * 2021-04-27 2021-08-10 成都飞机工业（集团）有限责任公司 一种机器人姿态快速复位的方法
CN113232018B (zh) * 2021-04-27 2022-05-10 成都飞机工业（集团）有限责任公司 一种机器人姿态快速复位的方法

Also Published As

Publication number Publication date
CN107030695B (zh) 2019-07-09

Similar Documents

Publication Publication Date Title
CN106926243A (zh) 机器人示教运动控制方法和系统
CN106950924B (zh) 机器人圆弧运动控制方法和系统
CN106985140A (zh) 机器人点到点运动控制方法和系统
CN106914900B (zh) 基于ros和orocos的机器人控制系统
CN106945044A (zh) 机器人暂停运动控制方法和系统
CN107030695A (zh) 机器人返回原点运动控制方法和系统
CN106959694A (zh) 机器人直线运动控制方法和系统
CN102581850B (zh) 基于GSK-Link总线的模块化机器人控制装置及控制方法
CN102195845B (zh) 一种实现主控板主备切换的方法、装置和设备
CN106945045A (zh) 基于ros和orocos的机器人控制的通信方法和系统
CN109347884B (zh) 一种实时以太网到现场总线的转换方法及装置、存储介质
KR20070075957A (ko) 멀티 타스킹을 위한 타스크 기반 로봇 제어시스템
CN104281086A (zh) 一种基于EtherCAT通信协议的自动氩弧焊接设备软PLC控制系统
CN110838961B (zh) 一种通用航空总线消息调度系统
Schmidt et al. Distributed real-time protocols for industrial control systems: Framework and examples
CN114416274A (zh) 一种虚拟资源管理系统及其方法
CN106956265B (zh) 基于ros和orocos的机器人运动规划的执行方法和系统
KR20070052641A (ko) 로봇 제어를 위한 로봇 서버, 이를 포함하는 컨텐츠 제공시스템 및 그 방법
CN106965181A (zh) 基于ros和orocos的机器人控制方法和系统
CN116197919B (zh) 机器人控制系统及控制方法
CN110611599A (zh) 一种网络控制系统及其控制方法
WO2015117458A1 (zh) 故障信息收集方法、装置及系统
KR102259503B1 (ko) 이벤트 구동 방식 기반의 실시간 시뮬레이션 제어 방법 및 시스템
CN102819508B (zh) 响应来自安防监控子系统信号的方法
CN100493041C (zh) 基于路由交换机的自适应可变时间片包转发调度方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant