基于ROS的工业机器人运动规划仿真及试验

王 旭,朱晓春,刘汉忠,唐 鹏,徐 天

(南京工程学院自动化学院, 江苏 南京 211167)

随着我国制造业逐渐从传统制造方式向智能制造方式转型,工业机器人的应用领域也在不断扩展,对机器人控制技术的要求越来越高[1].然而,在工业机器人迅速发展的同时,也存在着诸多问题：国内外机器人厂商大多开发自己特有的机器人系统,可移植性差;机器人系统普遍实时性差;机器人系统开发过程中工作量大、成本高[2].为解决当前机器人所面临的问题,各国开发者致力于研发一种通用的机器人系统[3].其中,开源机器人操作系统(robot operating system,ROS)是通讯机制、工具软件包、机器人应用功能和机器人生态系统的集合体,具有跨平台、多语言支持、易扩展的特点[4],能够降低机器人开发的难度,被越来越多的机器人研究者所采用[5].

近年来,很多研究人员基于ROS进行机器人运动规划的研究.李浩等[6]基于ROS,采用优化后的S形轨迹规划算法,使机器人的关节速度、加速度和加加速度的变化平滑,保证了机器人运动的平滑性和稳定性；郭建根等[7]采用快速搜索随机树算法,结合ROS的运动规划插件,实现了机器人在复杂情况下的无碰撞路径规划仿真；刘汝佳等[8]采用五次多项式插值算法对轨迹规划后的路径点进行拟合,并通过仿真试验验证该方法的有效性.本文基于ROS对机器人的运动规划进行研究,采用三次样条插值算法对规划后的路径点信息进行插值,通过仿真和试验验证机器人运动规划的控制效果.

1 ROS的构建

ROS是构建在Linux系统上的一种应用程序框架,其整合了可视化工具rqt和Rviz、机械臂Moveit!功能包等工具软件和功能包.为了实现机器人的轨迹规划,需要进行统一机器人描述格式(unified robot description format,URDF)模型创建和Moveit!功能包配置.

1.1 URDF模型创建

在ROS中, URDF是一种描述机器人仿真模型所设定的XML格式.为了创建机器人的URDF模型,首先需要在SolidWorks软件中按照机器人的实际尺寸对其建模,并在机器人的每个旋转关节建立对应的旋转轴和参考坐标系;然后利用SolidWorks软件中的sw2urdf插件来生成机器人的URDF模型;最后在ROS的工作空间中,通过编写对应机器人模型的功能包和修改launch文件来建立机器人模型的功能包.

1.2 Moveit!功能包配置

在建立机器人模型的基础上,需要进行Moveit!功能包的配置才能实现机器人的轨迹规划.在Moveit！功能包中,move_group节点是控制核心,可以调用三种接口,包括C++接口、Python接口和GUI可视化接口.move_group节点会从ROS参数服务器获取URDF信息、SRDF信息和Moveit!的相关配置信息.在控制机器人的过程中,move_group节点与机器人控制器交互机器人关节轨迹的数据信息,机器人传感器也会不断给move_group节点反馈关节状态信息,以便move_group节点运用运动规划算法来进行正逆运动学求解和运动轨迹生成.Moveit!功能包的控制架构图如图1所示.

图1 Moveit!功能包的控制架构图

Moveit!功能包的配置是通过打开Moveit! Setup Assistant工具界面来实现.在Setup Assistant工具界面上,首先需要导入之前所生成的URDF文件,配置自碰撞检测矩阵,使机器人的各个连杆在运动过程中不会相互重合;然后将机器人的基座位置设置为虚拟关节,创建机器人的规划群,以便确定哪些连杆和关节需要进行规划;最后设置机器人的零点位置,生成配置文件.Moveit!功能包的配置流程如图2所示.

图2 Moveit!功能包的配置流程图

2 机器人的轨迹规划

在ROS构建完成后,利用Moveit!功能包所提供的Python相关API来对机器人进行空间直线轨迹规划和空间圆弧轨迹规划.在轨迹规划后,所规划的只是空间轨迹的位置信息,需利用Moveit!功能包加载URDF模型,用其所包含的开放式运动规划库(open motion planning library,OMPL)为这条空间运动轨迹赋予速度与加速度约束,进行运动学求解,生成包含机器人6个关节的角度信息和对应的时间信息,这些信息统称为路径点信息.将这些路径点信息发送给关节轨迹控制器,通过关节映射和主题发布的方式发布给Rviz界面的虚拟关节,控制虚拟机器人运动.与此同时,这些路径点信息也可以经过三次样条插值,将路径点信息细分,通过EtherCAT总线下发给伺服驱动器,从而控制实际机器人运动.其控制流程图如图3所示.

图3 控制流程图

2.1 空间直线轨迹规划

在笛卡尔空间中,当机器人末端执行器沿直线从点P0(x0,y0,z0)运动到点Pe(xe,ye,ze),需要计算这两点之间的距离L,其值为：

(1)

假设机器人末端执行器以速度v从点P0运动到点Pe,则相邻插补点的距离为ΔL=vΔt,其中Δt为插补时间间隔.由此可以计算出空间直线轨迹规划的插补次数为：

(2)

式中,Ent表示向下取整函数.

由此可以得出空间直线轨迹的中间插补点坐标(xi+1,yi+1,zi+1)为：

(3)

2.2 空间圆弧轨迹规划

在笛卡尔空间内,当机器人末端执行器作空间圆弧轨迹规划时,首先在空间内确定不共线的3个点,其坐标分别为P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3).由这不共线的三点可以确定一个平面,作P1P2和P2P3的垂直平分线交与点o1,则点o1是这个圆弧的圆心坐标(如图4所示),其坐标为o1(x4,y4,z4),圆弧的半径R为：

(4)

图4 笛卡尔空间的圆弧轨迹

已知圆心坐标和空间3个点坐标,易求出弧P1P2和弧P2P3所对应的圆心角为θ1和θ2.以o1为坐标圆点,建立新的坐标系o1x1y1z1,x1轴、y1轴、z1轴所对应的单位向量为n、o、a,则由o0坐标系到o1坐标系的变换矩阵为：

(5)

式中,p=[x4y4z4]T.

设机器人末端执行器进行圆弧插补的角度位移量为Δθ,则空间圆弧插补的次数为：

(6)

空间圆弧在x1z1平面坐标系中的插补为：

(7)

式中,i=1,2,3,…,N.

对于x1z1平面上任意一点P,都可以通过式(5)将其转化到基座标系o0x0y0z0下.对新坐标系下的圆弧轨迹进行计算,借助变换矩阵,可以得到在笛卡尔空间的空间圆弧轨迹.

2.3 OMPL运动规划

在空间直线或空间圆弧轨迹规划后,利用OMPL对生成的空间直线或空间圆弧进行采点,基于快速扩展随机树(rapidly-exploring random trees,RRT)算法[9]和KDL正逆解求解器算出对应的空间直线或空间圆弧轨迹.这条空间轨迹只包含位置信息,不包含速度信息和加速度信息.为此,需要将这条轨迹带入AddTimeParameterization模块,该模块的作用是为空间轨迹设置速度和加速约束,计算出机器人运动到每个路径点的时间信息,从而得出机器人的路径点信息.

2.4 三次样条插值

在ROS上,关节轨迹控制器可以通过关节映射和主题发布的形式将路径点信息发布给虚拟关节,控制Rviz界面上的虚拟机器人,也可以将路径点信息下发给伺服驱动器,控制实际机器人.但是这些路径点信息是不连续的,且时间间隔较大,不能直接下发给伺服驱动器,否则会导致机器人末端执行器出现较大误差.为了解决这一问题,本文采用三次样条插值算法将机器人各关节的角度值进行细分,这样不仅使实际生成的轨迹更加光滑,保持了位置和速度的连续性,而且其加速度也不会产生突变.

在三次样条插值算法中,假设在区间[a,b]内有n+1个结点,表示为：

a=t0

(8)

在[a,b]中的一个函数f(t)如果满足以下两个条件,则称它为三次样条函数[10]：

1) 在每个小区间[ti-1,ti]内,其中i=1,2,…,n,f(t)都表示为三次多项式函数：

fi(x)=ai+bi(t-ti)+ci(t-ti)2+di(t-ti)3

(9)

2)f(t)、f′(t)和f″(t)都在区间[a,b]上连续,并满足：

f(t0)=y0,…,f(tn+1)=yn+1

(10)

f-(ti)=f+(ti)=yi,i=1,2,…,n

(11)

(12)

(13)

式(10)表示f(t)满足插值条件,且有n+2个条件;式(11)表示f(t)在插值点连续且有n个条件;式(12)和式(13)表示f′(t)和f″(t)在插值点连续,共有2n个条件.这4个公式一共提供了4n+2个条件.在区间[a,b]上有n+1个区间,每个区间上的三次多项式有4个待定系数,即共有4n+4个待定系数,需再补充2个条件就可确定三次样条函数f(t).为了使机器人运动更加平滑,本文指定机器人的初始加速度和终止加速度为0.

3 试验验证

3.1 仿真试验验证

在ROS上,为了验证利用Moveit!功能包来控制虚拟机器人做空间直线和空间圆弧运动的功能,采用Moveit!所提供的Python相关API来编写控制程序,在Rviz界面上显示虚拟机器人运动的空间直线和空间圆弧轨迹.

3.2 实物试验验证

为了验证利用ROS来控制实际机器人做运动规划的功能,本文采用一台工控机和一台六自由度机器人搭建试验平台,如图5所示.

图5 试验平台图

基于该试验平台,本文的机器人控制系统总体框图如图6所示.工控机首先装入Linux系统;然后,为了提高系统的实时性,给其打上Motorcortex实时核,并将其配置成IGH EtherCAT主站,以便于工控机直接通过网口与工业机器人进行EtherCAT通信;最后,工控机装入ROS,并将IGH EtherCAT主站改造成ROS下的一个节点模块,以便通过ROS硬件接口将机器人的状态参数下发给IGH EtherCAT主站,并在主站的实时核中实现过程数据的下发与接收.工业机器人选用的是六自由度的机器人,其主要包含六个伺服驱动器和相对应的伺服电机.

图6 机器人控制系统的总体框图

整个控制的流程是工控机通过EtherCAT总线将经三次样条插值后的控制数据下发给伺服驱动器,伺服驱动器驱动伺服电机,实现机器人的运动.此外,伺服驱动器也会通过EtherCAT总线将机器人各关节实际运动的位置、速度等信息实时反馈给工控机,形成闭环控制.以机器人空间圆弧轨迹规划为例,机器人各关节角度的设定值与实际值的对比如图7(a)所示,机器人末端空间轨迹的设定值与实际值的对比如图7(b)所示.由图7可见,在实际机器人运动过程中,机器人各关节运动平滑,机器人跟踪空间圆弧与理论空间圆弧的距离误差不超过5 mm,保证了机器人轨迹运动的高精度,验证了基于ROS的机器人运动规划效果.

(a) 机器人各关节角度

(b) 机器人末端空间轨迹

4 结语

本文基于ROS对机器人的运动规划进行仿真和试验研究,试验结果表明机器人在关节坐标系下运动平稳,精度较高.同时,本文基于ROS系统的机器人控制方法使虚拟机器人和实际机器人同步运动,为研究机器人的数字孪生功能提供了参考.