基于OROCOS的工业机器人控制软件设计

曾 玥，杜泽强，田会方，吴迎峰

(1.武汉理工大学 机电工程学院，湖北 武汉 430070；2.西安向阳航天材料股份有限公司，陕西 西安 710065)

工业机器人行业已有大半个世纪的历史了，一直热衷于适应电子、机械、材料和通信等方面的标准的革新[1-2]。但是，该行业当前的软件实现情况与20年前的计算机行业非常相似：每个制造商都有自己的专有软件、算法、数据结构以及编程语言。定义开放数据格式标准或编程语言的努力仅取得了很小的成功，因此软件交换的可能性实际上非常低[3-4]。工业机器人行业缺乏互操作性和开放性，这导致了新手会面临巨大的投入门槛和较低的创新交叉利用率。开放式机器人软件(open robot control software,OROCOS)解决了这一难点。

OROCOS适用于缠绕生产线机器人主要有两大优点 ：

(1)相对于常见的机器人操作系统(robot operating system,ROS)，OROCOS的最大优势是能够提供现成的硬实时操作。生产线上各个工位的执行时间一般情况下是确定的，也会有突发事件，因此对于机器人控制系统的实时性有要求。OROCOS实时工具包(real time transmit,RTT)可以让机器人在确定的时间内执行任务或响应关键事件[5]。

(2)如果缠绕的对象改变，生产线各个装置的位置分布和执行流程也会变化，因此期望机器人控制系统只需修改尽量少的程序，就能适应这些调整。OROCOS使用基于组件的软件开发(component-based software development,CBSD)技术能很好地满足这一点。OROCOS使用模块化控制结构可大大提高代码的可重用性，易于跟踪流程执行，减少编程错误[6]。

1 总体控制原理设计

缠绕生产线机器人的控制原理主要包括Qt人机交互界面和ORCOS机器人算法中间件，如图1所示。

图1 总体控制原理示意图

Qt人机交互界面基于Linux操作系统，主要包括机器人参数设置、手动控制、自动控制和动画仿真功能。操作者可以用手动控制进行调试，进而使用自动控制编写运动程序。

手动控制产生的指令或自动控制程序经命令解释器产生的指令信号传递给OROCOS机器人算法中间件。中间件将进行各种机器人算法分析，主要包括运动学分析和轨迹规划，由各个组件程序进行互联实现[7]。

2 OROCOS算法中间件设计

2.1 软件总体设计

图2显示了构成应用的组件。命令和消息处理组件在一个线程中运行，负责处理与人机界面程序的交互以及需要记录的任何信息。运动学和轨迹组件在一个单独的线程中运行，负责生成要发送给机器人的关节级命令。机器人组件负责通过以太网与机器人硬件进行交互。此组件在其自己的线程中以高于任何其他组件的频率运行，以便以所需的速率中继命令和更新信息。

图2 主要组件和关系图

CMake跨平台构建自动化系统用于管理应用程序中软件组件的编译。此系统允许使用独立于编译代码的系统生成配置文件。然后，该系统自动生成makefile，以便与程序中使用的Ubuntu Linux操作系统中的GNU编译器集合一起使用。

在运行时，通过以太网互联网协议从人机界面程序接收消息。这些消息被中继到消息处理组件，该组件处理这些消息以标识人机界面正在发出的请求。如果消息是请求信息，则消息处理程序将收集请求信息并将其发送到人机界面。如果消息是要执行的指令，则消息处理程序将在应用程序的其他组件中启动相应的命令或方法，并回复人机界面以指示指令的成功或失败。例如，笛卡尔点对点运动命令将被中继到轨迹生成组件。然后，轨迹生成器将开始顺序输出位于连接初始位置和目标位置的直线上的设定点，在每个更新周期向目标移动一步。在工作空间限制器检查以确保设置点不位于标记为“禁止”的区域之后，这些笛卡尔设置点将通过反向运动学组件，该组件将它们转换为关节空间表示。然后，所需的关节配置通过关节限制器，该限制器确保每个设定点不违反任何关节行程限制。然后，所允许的配置进入机器人组件，该组件指示控制硬件执行运动。人机界面在关节空间中发送的命令将绕过该系统的早期阶段，关节空间轨迹生成组件直接将其反馈到关节限制器中。

2.2 正逆运动学组件实现

所有通过机器人组件发送给机器人硬件的命令都用关节空间表示，但是系统的上层命令是用笛卡尔空间表示。因此，需要一个或多个运动学组件来表示之间的转换量。正运动学将关节角度转换为笛卡尔位置，逆运动学将笛卡尔运动转换为相应的关节角度或速率[8]。OROCOS运动学库(kinematics and dynamics library,KDL)提供了涉及机器人串联运动链的运动学计算功能。它定义了基本几何的类，包括向量和旋转矩阵。三维变换由Frame表示，它包含一个表示位移距离的向量和一个表示相对坐标轴的旋转矩阵。提供了组合变换和反变换计算的功能。还定义了Twist类表示速度和角速度的6x1矩阵，Wrench类表示力和力矩，并且可以通过一个框架从一个坐标系转换到另一个坐标系。

根据缠绕生产线机器人的机械结构，建立坐标系，如图3所示，得出D-H参数如表1所示。

图3 机器人坐标系

表1 机器人D-H参数

正逆运动学的D-H参数配置在configureHook()中完成，具体配置代码如下：

namespace KDL

{

Chain myrobot;//实例化对象

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0,0,0,0)));//轴1

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0.18,-M_PI/2,0,-M_PI×3/4)));//轴2

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0.6650,0,0,M_PI/4)));//轴3

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0.17,-M_PI/2,0.6750,0)));//轴4

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0,M_PI/2,0,0)));//轴5

myrobot.addSegment(Segment(Joint(Joint::RotZ),Frame::DH(0,-M_PI/2,0,0)));//轴6

ChainFkSolverPos_recursive fwdkin(myrobot);//正运动学求解器

}

上述代码中Chain类用于构建运动学链，它封装了串行运动学互连结构，本组件实例化了一个运动学链对象armChain。addSegment()函数为运动学链添加段(Segment)，缠绕生产线机器人为6自由度机器人，即需添加6个段。每个段的D-H参数包含在Frame中。

在运行更新(updateHook)期间，大部分工作是在KDL提供的运动学求解器中完成的。正向运动学求解器KDL :: ChainFkSolverPos_recursive使用当前关节角度来计算末端执行器位置，从第一个Frame开始，然后沿着运动学链连续右乘变换矩阵。逆位置运动学由KDL :: ChainIkSolverPos_NR_JL提供，它使用牛顿迭代法以及逆速度运动学求解器。逆速度运动学的实现采用雅可比伪逆的KDL求解器。

2.3 轨迹生成组件实现

从人机交互界面接收到的运动指令仅仅是一个目标位置或一串离散的路径点。但是，机器人执行组件需要不断更新所需位置。因此，必须实现轨迹生成功能，以产生更密的路径点序列，使机器人从起始位置平稳地移动到目标位置。KDL为轨迹生成功能提供了一些相关的类，本设计中需要用到的几个关键的类分别为：

KDL :: Path_RoundedComposite路径，由带有圆角的路径点组成。

KDL :: VelocityProfile_Trap梯形速度曲线。

KDL::Trajectory_Segment由VelocityProfile_Trap和Path_RoundedComposite组合成的一条基础轨迹。

KDL :: Trajectory_Composite由基础轨迹组成的复合轨迹，调用添加函数以添加轨迹。

轨迹规划的流程如图4所示，其代码编写在updateHook()函数中，它是以一个周期不断循环的，每次循环主要分为计算轨迹和运行轨迹。

图4 轨迹规划流程图

(1)计算轨迹。首先，遍历每个路径点位的位姿，将其添加到路径中，创建带有圆角的路径。然后，设置速度和加速度上限，创建一条从起始位置到终止位置的梯形速度曲线。最后将路径和速度曲线合成一条基础轨迹，添加到复合轨迹中。

(2)运行轨迹。首先判断轨迹是否创建成功，再判断当前时间是否在运行设置的轨迹运行时长内。然后从创建的轨迹中获取当前的期望状态，包括位置、速度和加速度信息。再给当前时间增加一个周期，这个周期是轨迹规划组件的循环周期。最后输出调整状态的命令，命令的参数同样包括位置、速度和加速度信息。如果当前时间超过设置运行时长，则轨迹规划完成。

3 人机交互界面软件设计

3.1 软件架构设计

统一建模语言(unified modeling language，UML)是一种编制软件蓝图的标准化语言，它的目标之一就是为开发团队提供标准通用的设计语言来开发和构建计算机应用[9]。如图5所示的UML图是使用ProcessOn软件制作，项目主要用到的类有Base、Component、EndEfector、Handler等，以下分别详细介绍这些类。

图5 UML类图

(1)Base类负责数据管理和与图形用户界面的通信，整个项目中的Base只实例化一次，并且将其汇总到所有与GUI(graphical user interface)相关的所有类中。与项目中的许多其他类一样，该类继承自属于所使用框架的QObject库。

(2)Component类是负责保存与其实例化各个段的所有信息的类。其构造函数接收在Base中创建的机器人实体的rootEntity作为参数。此类的参数与.obj模型的零件渲染密切相关。Component类还可以获取和设置定义零件配置所必需的点、轴和角度。Component类提供了动画配置，例如基于设置速度的动画持续时间。每个元素都会添加一个Controller类对象。

(3)EndEfector类是一个从Component继承并与机器人的末端执行器相关联的类。其中包含用于控制要执行的任务及其持续时间的方法和属性。末段执行器是用于夹持气瓶芯模，因此它的状态有夹紧、释放和保持。

(4)Controller类实例化每个Component的控制器。这些控制器的目标是每个Component的QTransform属性 ，即它们处理机器人每个段的空间变换。变换操作是在动画的每一帧中给出配置值。因此，存在与相应Component类链接的方法，例如零件点、轴和角度的获取和设置。Controller类中最重要的功能是更新Component变换特性的4×4矩阵，包含零件执行的平移和旋转信息。

3.2 基本手动功能设计

如图6所示的主界面包括一个主窗口(Widget类 )，以及用于启动和停止机器人的按钮。启动随着一系列的归位运动自动进行。停止动作会将机器人制动在当前位置，使其处于未激活状态。该主窗口还具有一个文本框，该文本框指示机器人的整体状态(启动或停止)，另外两个文本框提供有关每个关节的角度、相对速度和末端执行器配置的信息。按钮“设置初始特征”，“加载命令文件”和“选择模式”将打开其他各个窗口。

图6 主界面

在主窗口中，如果按下“设置初始特征”按钮，则将打开“属性”窗口。在此窗口中，用户可以输入机器人的所需参数：关节角度及其各自的速度。输入这些参数后，当用户按下“确定”按钮时，将加载这些数据，如果它们与先前的数据不同，则机器人将继续执行相应的运动。

如果用户从主窗口按下“选择模式”按钮，则会打开操作窗口。用户可以从该窗口完全控制机器人，通过窗口中相应的滑块控制每个关节的角度和速度来移动机器人。

3.3 自动控制功能设计

实际生产中，除非生产线的其他装置改变，机器人的运动应该是重复的。这需要设计一个自动控制功能，用户能将手动控制的指令记录下来，并且保存起来，使机器人能按照指令执行一连串动作。因此设计了一套类似G代码的运动指令，如表2所示。在操作窗口中，用户还可以通过在相应的文本框中逐行输入运动指令来使机器人运动。

表2 运动指令表

4 轨迹测试

轨迹路径如图7所示，选取8个末段执行器的路径点：①初始位置；②水平旋转工作台附近；③④在水平工作台挂钩上；⑤再次退到水平工作台附近；⑥在纤维缠绕机处；⑦在三抓卡盘中；⑧在纤维缠绕机附近。

图7 路径示意图

末端执行器路径点配置如下，Vector表示位置矢量，单位为毫米，EulerZYZ表示Z-Y-Z形式的欧拉角姿态矢量，单位为弧度。

path->Add(Frame(Rotation::EulerZYZ(M_PI/2,-M_PI/2,0),

Vector(0,-384.8,850.2)));// ①

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1037,0,735)));// ②

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1080,0,735)));// ③

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1080,0,745)));// ④

path->Add(Frame(Rotation::EulerZYZ(M_PI,-M_PI/2,0), Vector(1037,0,745)));// ⑤

path->Add(Frame(Rotation::EulerZYZ(-M_PI/2,-M_PI/2,-M_PI/2),Vector(-350,1172,1000)));// ⑥

path->Add(Frame(Rotation::EulerZYZ(-M_PI/2,-M_PI/2,-M_PI/2),Vector(-410,1172,1000)));// ⑦

path->Add(Frame(Rotation::EulerZYZ(-M_PI/2,-M_PI/2,-M_PI/2),Vector(-410,1000,1000)));// ⑧

笛卡尔路径测试结果如图8所示，得到1 559个插值路径点，12个路径段。

图8 笛卡尔路径规划图

如图9所示笛卡尔空间x、y、z方向的位置随时间变化，总体比较平缓，没有剧烈变化。速度随时间变化曲线为梯型，速度和加速度都在控制范围以内。

图9 笛卡尔各方向位置-时间和路径速度-时间变化图

对笛卡尔空间中的每一个路径点求逆解，即可得到关节空间中每个关节角随时间变化的曲线，如图10所示。对关节值求差分，可以得到角速度随时间变化的曲线,如图11所示。

图10 各关节角-时间图

图11 各关节角速度-时间图

经过以上求解，软件会自动生成G代码，保存在文件中。导入到Qt界面，逐行读取G代码，发送运动控制指令到伺服控制器。

5 结论

针对水处理缠绕罐自动缠绕及固化生产线中对工业机器人控制的需求，对机器人控制软件进行整体方案设计。在Linux和OROCOS系统下，用C++编写消息处理组件、正向运动学组件、工作空间限制器等组件并将其连接。用Qt设计人机交互界面，手动控制、自动控制等功能。进行3D动画仿真,机器人各个关节均可按照指令转动，满足设计预期要求。