CN106945045B - 基于ros和orocos的机器人控制的通信方法和系统 - Google Patents

Abstract

本发明涉及一种基于ROS和OROCOS的机器人控制的通信方法和系统，所述方法包括：通过第二接口接收算法处理程序发送的控制指令数据；将所述控制指令数据保存到电机指令缓冲队列中；根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站。本发明的技术，可以构成一个完整的机器人控制器软件，降低机器人控制系统开发成本，提高控制效果。

Description

基于ROS和OROCOS的机器人控制的通信方法和系统

技术领域

本发明涉及机器人控制技术领域，特别是涉及一种基于ROS和OROCOS的机器人控制的通信方法和系统。

背景技术

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

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

发明内容

基于此，有必要针对上述技术问题，提供一种基于ROS和OROCOS的机器人控制的通信方法和系统，降低系统开发成本，提高控制效果。

一种基于ROS和OROCOS的机器人控制的通信方法，包括：

通过第二接口接收算法处理程序发送的控制指令数据；

将所述控制指令数据保存到电机指令缓冲队列中；

根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站；

其中，所述控制指令数据是算法处理程序的机器人运动规划的响应函数根据控制指令计算得到，所述控制指令由控制器管理程序从接收端进行接收，所述控制器管理程序的控制指令触发函数通过调用算法处理的所述响应函数，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与通信管理程序进行通信连接。

一种基于ROS和OROCOS的机器人控制的通信系统，包括：

接收单元，用于通过第二接口接收算法处理程序发送的控制指令数据；

缓存单元，用于将所述控制指令数据保存到电机指令缓冲队列中；

发送单元，用于根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站；

其中，所述控制指令数据是算法处理程序的机器人运动规划的响应函数根据控制指令计算得到，所述控制指令由控制器管理程序从接收端进行接收，所述控制器管理程序的控制指令触发函数通过调用算法处理的所述响应函数，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与通信管理程序进行通信连接。

上述基于ROS和OROCOS的机器人控制的通信方法和系统，利用OROCOS的实时输入/输出接口与控制器管理程序和算法处理程序进行通信，由控制器管理程序接收控制指令，通过控制指令触发函数，调用算法处理程序的机器人运动规划的响应函数计算控制指令数据，对控制指令数据进行缓存后转发至机器人，实现控制指令的接收分发、解析和传输，可以构成一个完整的机器人控制器软件，降低机器人控制系统开发成本，提高控制效果。

附图说明

图1是本发明实施例的基于ROS和OROCOS的机器人控制的通信方法流程图；

图2是一种机器人控制系统的硬件结构模型；

图3是基于ROS和OROCOS的机器人控制方法搭建的软件架构图；

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

图5是设备状态机的状态变化示意图；

图6是本发明实施例的基于ROS和OROCOS的机器人控制的通信结构示意图。

具体实施方式

下面结合附图阐述本发明的基于ROS和OROCOS的机器人控制的通信方法和系统的实施例。

参考图1所示，图1是本发明实施例的基于ROS和OROCOS的机器人控制的通信方法流程图，包括：

S101，通过第二接口接收算法处理程序发送的控制指令数据；其中，所述控制指令数据是算法处理程序的机器人运动规划的响应函数根据控制指令计算得到，所述控制指令由控制器管理程序从接收端进行接收，所述控制器管理程序的控制指令触发函数通过调用算法处理的所述响应函数；

对于控制器管理程序，可以通过ICE开发的通信协议建立与控制端的通信连接，通过调用所述通信协议的动态异步远程过程调用方法输入的控制指令；

在一个实施例中，所述控制器管理程序的控制指令触发函数可以根据控制指令的类型，通过第一接口调用算法处理程序相应类型的响应函数；每种机器人运动规划的响应函数对应控制器管理程序的一个控制指令触发函数；所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口，用于算法处理程序与控制器管理程序进行通信连接。

所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与通信管理程序进行通信连接。。

S102，将所述控制指令数据保存到电机指令缓冲队列中；在此，可以按照顺序将控制指令数据存储到队列中。

S103，根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站；

在此，控制主站可以是CANOpen主站，CANopen是一种架构在控制局域网路(Controller Area Network，CAN)上的高层通信协协议，包括通信子协议及设备子协议。

上述实施例的技术方案，利用OROCOS的实时输入/输出接口与控制器管理程序和算法处理程序进行通信，由控制器管理程序接收控制指令，通过控制指令触发函数，调用算法处理程序的机器人运动规划的响应函数计算控制指令数据，对控制指令数据进行缓存后转发至机器人，实现控制指令的接收分发、解析和传输，可以构成一个完整的机器人控制器软件，降低机器人控制系统开发成本，提高控制效果。

在一个实施例中，所述通信方法，还可以通过第二接口向算法处理程序发送电机运行数据，通过所述第三接口向控制器管理程序发送反馈信息；其中，所述反馈信息包括事件报告信息、机器人电机运行的诊断数据；所述第三接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与控制器管理程序进行通信连接。

在一个实施例中，在接收算法处理程序发送的控制指令数据前，还可以进行初始化处理，具体可以如下：

(1)检测历史运行所记录的日志报告，若日志报告正常，将相关信息通过事件报告接口传递给控制器管理程序；若日志报告异常，则退出。

(2)电机驱动的初始化：

建立与电机驱动器的通信连接，将电机指令缓冲队列清空；

使能电机，若使能成功，读取电机的位置信息，根据位置信息计算机器人的机械臂的状态信息；其中，所述状态信息包括关节角度和机械臂的末端位姿；

(3)机械臂的初始化：

若机械臂任一关节角度与零度相差大于设定值，则调用点到点运动规划，根据所述末端位姿对机械臂进行回零运动规划；

具体的，可以依据电机的位置信息，计算机械臂任一关节角度与零度相差是否大于0.01度，若是，则执行回零运动，可以调用点到点运动规划，对回零运动进行规划。

作为实施例，在本发明实施例的基于ROS和OROCOS的机器人控制的通信方法，可以采用设备状态机来进行相关状态的控制，其中，所述设备状态机设有初始化、中断、暂停、恢复、手动示教和运转六种状态；所述使能为暂停、恢复、手动示教和运转四种状态的合集状态；使能状态的转移规则，对四个子状态均有效。

基于设备状态机，所述方法可以如下：

(1)根据用户设定的频率(如1KHz)读取设备状态机的状态信息；

(2)根据所述状态信息执行控制指令数据的发送操作；具体可以包括如下几种情况：

a)若设备状态机是初始化状态，执行机械臂的回零运动规划；

根据回零运动规划的轨迹，计算电机的控制指令数据，并将该控制指令数据发送给控制主站；

在回零成功，向设备状态机发送状态事件，将设备状态机转换为运转状态，并将该事件报告反馈至控制器管理程序。

b)若设备状态机是运转状态，从控制指令数据的输入通道中，读取控制指令数据，并存储到电机指令缓冲队列中；

读取控制主站的指令缓冲区的现有控制指令数据的数量，根据该数量大小从电机指令缓冲队列中取出指定数量的控制指令数据发送至控制主站。

c)若设备状态机是恢复状态，向设备状态机发送状态事件，将设备状态机转换为运转状态，并将该事件报告反馈至控制器管理程序；若恢复不成功，则向设备状态机发送中断事件，状态机转换为中断状态，将该事件报告反馈至控制器管理程序，并退出；

d)若设备状态机是暂停状态，检测控制指令数据的输入通道中是否有新的控制指令数据，若是，则读取控制指令数据并存储到电机指令缓冲队列中；

e)若设备状态机是手动示教状态，根据手动示教的运动规划轨迹，计算该时刻电机的控制指令数据，并将该单条控制指令数据发送至控制主站。

(3)读取电机状态，计算机械臂关节以及末端运动状态信息，并反馈至算法处理程序与控制器管理程序；

(4)检测控制主站的诊断信息，将该诊断信息反馈至算法处理程序与控制器管理程序，将设备状态机转换为运转状态，并将该事件报告发送至控制器管理程序。

另外，如果机器人的机械臂需要执行回零运动规划，则控制设备状态机保持初始化状态不变；否则，向设备状态机发送状态事件，将设备状态机的状态转换为运转状态。

本发明实施例中，所述机器人运动规划可以包括：点到点运动，直线运动，圆弧运动，手动示教，回到原点、暂停、恢复、急停等算法。

对于第二接口和第三接口，是指分别用于与控制器管理程序和用于与算法处理程序进行通信的接口；按照输入/输出接口方式划分，第二接口包括向算法处理程序的输入/输出接口，第三接口包括向控制器管理程序的输入/输出接口。

可以使用ROS的orocreate-catkin-pkg方法创建为ROS的Package，并在Package中继承OROCOS的RTT：：TaskContext类，利用OROCOS的RTT：：Input与RTT：：Output方法定义实时输入/输出接口。

为了更加清晰本发明的实施例的技术方案，下面阐述采用本发明的方法实现的实例。

本实例中，硬件和软件环境可以如下：

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

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

参考图3所示，图3是基于ROS和OROCOS的机器人控制方法搭建的软件架构图；在控制过程中，操作系统上运行控制器管理程序、算法处理程序和通信管理程序；通信管理程序对应运行本发明的基于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)对于控制器状态机，参考图4所示，图4是控制器状态机的状态变化示意图；可以设置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开发板上运行一个控制主站协议，该主站协议可以设置一个指令缓存区，最多可以存储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状态，执行机械臂的回零运动。此时，执行如下操作：

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

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

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

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

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

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

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

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

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

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

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

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

c)读取电机状态，计算机械臂关节以及末端运动状态信息，并通过输出数据通道，传递给算法处理程序与控制器管理程序；

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

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

a)关闭电机驱动使能；

b)关闭电机驱动连接。

(5)参考图5所示，图5是设备状态机的状态变化示意图。共有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方法定义控制器管理程序、算法处理程序和通信管理程序的属性。因此，保证了控制器管理程序、算法处理程序和通信管理程序之间的独立性与解耦性。

参考图6所示，图6是本发明实施例的基于ROS和OROCOS的机器人控制的通信系统结构示意图，包括：

接收单元101，用于通过第二接口接收算法处理程序发送的控制指令数据；

缓存单元102，用于将所述控制指令数据保存到电机指令缓冲队列中；

发送单元103，用于根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站；

其中，所述控制指令数据是算法处理程序的机器人运动规划的响应函数根据控制指令计算得到，所述控制指令由控制器管理程序从接收端进行接收，所述控制器管理程序的控制指令触发函数通过调用算法处理的所述响应函数，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与通信管理程序进行通信连接。

本发明的基于ROS和OROCOS的机器人控制的通信系统与本发明的基于ROS和OROCOS的机器人控制的通信方法一一对应，在上述基于ROS和OROCOS的机器人控制的通信方法的实施例阐述的技术特征及其有益效果均适用于基于ROS和OROCOS的机器人控制的通信系统的实施例中，特此声明。

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

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

Claims (10)

1.一种基于ROS和OROCOS的机器人控制的通信方法，其特征在于，包括：

通过第二接口接收算法处理程序发送的控制指令数据；

将所述控制指令数据保存到电机指令缓冲队列中；

根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站；

其中，所述控制指令数据是算法处理程序的机器人运动规划的响应函数根据控制指令计算得到，所述控制指令由控制器管理程序从接收端进行接收，所述控制器管理程序的控制指令触发函数通过调用算法处理的所述响应函数，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与通信管理程序进行通信连接。

2.根据权利要求1所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，所述控制器管理程序的控制指令触发函数根据控制指令的类型，通过第一接口调用算法处理程序相应类型的响应函数；每种机器人运动规划的响应函数对应控制器管理程序的一个控制指令触发函数；所述第一接口是基于ROS上创建OROCOS的实时输入/输出接口，用于算法处理程序与控制器管理程序进行通信连接。

3.根据权利要求2所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，还包括：

通过第二接口向算法处理程序发送电机运行数据，通过第三接口向控制器管理程序发送反馈信息；

其中，所述反馈信息包括事件报告信息、机器人电机运行的诊断数据；所述第三接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与控制器管理程序进行通信连接。

4.根据权利要求3所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，在接收算法处理程序发送的控制指令数据前，还包括：

检测历史运行所记录的日志报告，若日志报告正常，将相关信息通过事件报告接口传递给控制器管理程序；若日志报告异常，则退出。

5.根据权利要求3所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，在接收算法处理程序发送的控制指令数据前，还包括：

建立与电机驱动器的通信连接，将电机指令缓冲队列清空；

使能电机，若使能成功，读取电机的位置信息，根据位置信息计算机器人的机械臂的状态信息；其中，所述状态信息包括关节角度和机械臂的末端位姿；

若机械臂任一关节角度与零度相差大于设定值，则调用点到点运动规划，根据所述末端位姿对机械臂进行回零运动规划。

6.根据权利要求5所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，还包括：

根据用户设定的频率读取设备状态机的状态信息，根据所述状态信息执行控制指令数据的发送操作；其中，所述设备状态机设有初始化、中断、暂停、恢复、手动示教和运转六种状态；所述使能为暂停、恢复、手动示教和运转四种状态的合集状态；

读取电机状态，计算机械臂关节以及末端运动状态信息，并反馈至算法处理程序与控制器管理程序；

检测控制主站的诊断信息，将该诊断信息反馈至算法处理程序与控制器管理程序，将设备状态机转换为运转状态，并将该事件报告发送至控制器管理程序。

7.根据权利要求6所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，所述根据所述状态信息执行控制指令数据的发送操作的步骤包括：

若设备状态机是初始化状态，执行机械臂的回零运动规划；

根据回零运动规划的轨迹，计算电机的控制指令数据，并将该控制指令数据发送给控制主站；

在回零成功，向设备状态机发送状态事件，将设备状态机转换为运转状态，并将该事件报告反馈至控制器管理程序。

8.根据权利要求6所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，所述根据所述状态信息执行控制指令数据的发送操作的步骤包括：

若设备状态机是运转状态，从控制指令数据的输入通道中，读取控制指令数据，并存储到电机指令缓冲队列中；

读取控制主站的指令缓冲区的现有控制指令数据的数量，根据该数量大小从电机指令缓冲队列中取出指定数量的控制指令数据发送至控制主站。

9.根据权利要求6所述的基于ROS和OROCOS的机器人控制的通信方法，其特征在于，所述根据所述状态信息执行控制指令数据的发送操作的步骤包括：

若设备状态机是恢复状态，向设备状态机发送状态事件，将设备状态机转换为运转状态，并将该事件报告反馈至控制器管理程序；若恢复不成功，则向设备状态机发送中断事件，状态机转换为中断状态，将该事件报告反馈至控制器管理程序，并退出；

若设备状态机是暂停状态，检测控制指令数据的输入通道中是否有新的控制指令数据，若是，则读取控制指令数据并存储到电机指令缓冲队列中；

若设备状态机是手动示教状态，根据手动示教的运动规划轨迹，计算该时刻电机的控制指令数据，并将该单条控制指令数据发送至控制主站。

10.一种基于ROS和OROCOS的机器人控制的通信系统，其特征在于，包括：

接收单元，用于通过第二接口接收算法处理程序发送的控制指令数据；

缓存单元，用于将所述控制指令数据保存到电机指令缓冲队列中；

发送单元，用于根据机器人的控制主站的指令缓存区的控制指令数据数量，从电机指令缓冲队列中选择控制指令数据输出至所述控制主站；

其中，所述控制指令数据是算法处理程序的机器人运动规划的响应函数根据控制指令计算得到，所述控制指令由控制器管理程序从接收端进行接收，所述控制器管理程序的控制指令触发函数通过调用算法处理的所述响应函数，所述第二接口是基于ROS上创建OROCOS的实时输入/输出接口，用于与通信管理程序进行通信连接。