基于ROS的UR5机器人控制系统设计

赵丕洋,洪荣晶,方成刚

(1.南京工业大学 机械与动力工程学院,江苏 南京 211800; 2.江苏省工业装备数字制造及控制技术重点实验室,江苏 南京 211800)

目前,人力加工仍然是国内机械制造行业的主流方式,但其生产效率低,机械加工精度较差,工人熟练度对产品质量影响较大[1],且工厂的恶劣环境极大地损伤工人健康。工业机器人集电控、机械和自动化等多种学科特点于一体,工件装夹方便、工作空间大、经济效益高,可以独立完成机械生产中的各个阶段,提高了加工设备的柔性。开发者们着力于提高机器人的控制精度和柔性,以提升产品质量、经济效益和解放人力[2]。

机器人硬件不断升级,而控制系统和软件也亟需发展以适应其要求,因此实现加工代码的复用性、系统的模块化和通用性已成为热点问题,机器人开发者们不断尝试,以实现仿真模型和软件系统的功能共享。开源机器人操作系统(ROS)具有独特的优点,它集成了多种软件工具包,兼容主流编程语言,为机器人开发提供了更广阔的空间,越来越多的国内外专家对其开展了研究。ROS 具有分布式的系统架构[3], 具有支持程序的管理与信息的传递等功能[4],使机器人控制的加工任务和运动更加简化[5-6]。ROS能通过编写底层程序,将多种不同的传感器(力觉传感器、相机等)的通信集成在一起,极大地推动了系统的集成化和模块化开发。

1 机械臂模型建立及运动学分析

1.1 机器人Denavit-Hartenberg(D-H)参数

以丹麦公司的经典机械臂UR5为例,机械臂各个关节的坐标系可以通过D-H参数法确定,根据Pieper法则,将基座、肩部和手肘3个关节建立到同一个轴线上,以简化D-H参数[7]。查阅UR5相关使用手册,得到UR5的相关D-H参数(表1)。搭建机器人控制平台,对UR5机械臂进行运动学分析[8],建立实体模型及运动结构,如图1所示。

表1 UR5机械臂的D-H参数

图1 UR5机械臂运动模型Fig.1 Motion model of UR5 robotic arm

1.2 正运动学分析

(1)

将各关节参数分别代入式(1),计算得到各坐标系之间的变换矩阵,如式(2)所示。

(2)

末端坐标变换矩阵T如式(3)所示。

(3)

在式(3)中,通过输入6个关节角度θi(i=1～6),经过计算可得到末端坐标变换矩阵T为

(4)

推导出T中的元素值,如式(5)所示。

(5)

式中:ci=cosθi,si=sinθi,c23=cos(θ2+θ3),s23=sin(θ2+θ3),s234=sin(θ2+θ3+θ4),c234=cos(θ2+θ3+θ4)。

1.3 逆运动学分析

根据逆运动学理论,推算θi,为了得到全部逆解,使用数学解析法进行公式推导[10],由式(4)可得

(6)

其中,中间矩阵为

(8)

(9)

(10)

将式(6)—(9)代入式(5)中,解得

(11)

接着,由式(4)推导得

(12)

与前面推导过程类似,经过计算求出

(13)

通过分析上述逆解公式结果,关节1、3、5都分别有正负两组解,因此该机器人一共存在2×2×2=8组解。在实际机械臂的关节指令发布时,应以逆运动学的解为限制依据,计算机器人处于奇异位姿时的数据,避免机器人运行到奇异位姿而损害机械结构。

2 基于ROS的仿真系统设计

图2为ROS系统的架构,按照功能分为OS层、中间层和应用层。由图2可知：OS层安装了ROS依附性最好的Ubuntu操作系统;中间层基于TCPROS/UDPROS通信协议,实现数据传输和相应模型的构造;应用层中运行Master节点进行管理,集成了多种功能应用包,提高了系统开发效率[11]。

图2 ROS架构Fig.2 ROS architecture

2.1 创建机械臂模型

以D-H参数为依据,建立UR5模型,添加约束条件。为导出统一机器人描述格式(URDF)文件,安装sw_urdf_exporter插件,设置好相关参数,生成功能包后导入到ROS系统中[12]。

2.2 模型可视化

成功导入机械臂URDF文件后,配置软件相关参数,例如机器人坐标、转动关节等,在rviz可视化软件中完成机器人模型的运动驱动设置,并规划了随机点。仿真中观察到UR5模型稳定无异常抖动,规划目标点后可以准确运动。

2.3 MoveIt规划仿真分析

MoveIt被广泛应用在多种轨迹规划中,其控制结构如图3所示。由图3可知：针对move_group进行控制,使机器人的轨迹控制更加简便、高效,通过话题和服务通讯与其他独立的功能组件交互,为开发者提供ROS中的运动指令和服务[13]。

move_group节点兼容主流编程语言,可进一步改进机器人运动控制系统的开发。将URDF文件导入rviz软件中,简单设置后,生成SRDF控制文件后，再创建一个包含MoveIt配置的功能包[14]。配置完成后,以点对点的方式测试机械臂轨迹规划效果,如图4所示。由图4可知：机械臂始末位姿之间的控制规划成功且路径平滑,表明所推导的UR5运动学正逆解公式正确。

图3 move_group架构Fig.3 Architecture of move_group

图4 rviz机械臂运动仿真测试Fig.4 Motion simulation test of rviz robotic arm

3 基于快速拓展随机树(RRT)算法的运动规划分析及改进

MoveIt提供开放用户接口,可编写和改进算法作为运动规划器,并可根据自身需求对机器人进行合理运动规划,同时满足最大速度限制、关节限位和最大加速度限制等要求。

3.1 RRT算法基本原理

RRT算法的基本搜寻策略:从初始位置开始,随机在可行空间中采样。以树的形式进行探索,对路径进行随机拓展,直到覆盖整个目标区域,最后在状态空间内选取最短的一条路径作为结果输出[15]。与其他运动规划算法相比,RRT算法的实时性和可扩展性更好,且不需要对碰撞模型进行精确建模[16]。路径寻优时快速有效,可应用于机械臂的复杂任务规划[17]。

RRT规划目标点的原理如图5所示[18]。图1所示是一个二维空间,假设生成一个含有k个节点的随机树。由图5可知：该随机树的根节点用qinit表示，新随机搜索的节点用qrand表示，qnear表示生成随机树中最接近qrand的节点，d是搜索步长，L是节点qrand和qnear的连线距离。算法运行后,在状态空间内生成随机采样点qrand,接着将qrand与qnear连线,作为节点生成方向,然后将两者连接起来,若连线与障碍物产生了干涉,则该节点不会成为随机树的节点,而是在空间内重新随机搜寻,进行下一次扩展;若连线与障碍物无干涉，则判断L与d的大小关系,若L>d, 则在qnear和qrand连线上选择距离qnear长度为d的点作为新节点qnew,若L≤d,则该qrand成为新节点qnew。

在树节点扩展到目标点之后,RRT算法从终点开始反向搜索,依次查询父节点,如果最终查询到起始点,那么路径规划结束,并连接起始点和目标点生成一条最优路径。

图5 RRT算法拓展策略Fig.5 RRT algorithm expansion strategy

3.2 RRT算法改进

RRT是概率完备的算法,由于其具有随机性特征,对于相同的始末位姿,采用RRT进行规划得到的路径不一定相同,因此具有很差的路径可重复性。新节点是在工作空间中随机搜索生成的,各节点的连线可能会出现剧烈的转折,生成的规划路径圆滑度较低,因此RRT算法的规划结果并不最优，因此不能适应实际加工的需要[19]。

在六自由度机器人工作空间的运动规划中,若关节运动轨迹夹角过小,则路径曲率半径很小,机器人就会发生“急转弯”，进而引起机械臂加速度和速度的突变,不能保证机器人的平稳运动,且极易对机械结构产生损害[20]。为提高机器人在避障过程中轨迹的圆滑性,并保证机器人在运行过程中加速度产生的突变较小,缩短搜索生成路径的时间,提出判定函数F。F将相邻节点间的转角考虑在内,拓展方式如图6所示,表达式[20]如式(14)所示。

(14)

式中:si为节点qi与其父节点的连线位姿；sk为节点qi与新的随机节点的连线位姿；H(si,sk)为两种位姿的夹角，夹角越小表示机器人运动时转动弧度大且平缓,运动就越平稳;D(qi,qk)为机械臂执行装置的位置qi到目标位置qk的距离代价,距离越短表示路径规划越优;Nd和Nh为归一化函数,将距离和角度统一量纲以进行比较;Wd和Wh分别为节点间距离和节点间角度在综合因素考虑中所占权重。

由图6可知：子节点到q2的距离(d2)小于到q1的距离(d1),但转角θ2远大于θ1,综合考虑后，算法选择q1作为回溯父节点并生成轨迹。

图6 改进RRT算法Fig.6 Improved RRT algorithm

在MATLAB软件中编写M文件,对比分析改进RRT算法相较于基本RRT算法的优越性,在仿真中设置参数:生成一个100×100的搜索空间,起始点为(0,0),目标点为(95,95),搜索步长为5,障碍物是一个20×20的方块,设定随机生成种子数为1 000,在改进RRT算法中为了综合考虑角度和距离因素,设置Wd=Wh=0.5,运行程序并绘制路径图,如图7所示。

图7 基本RRT及改进RRT的搜索路径Fig.7 Basic RRT and improved RRT search path maps

由图7可知：基本RRT算法生成的轨迹转角较大,在机器人加工中会导致加速度和速度突变,造成机械结构的损伤;改进RRT算法综合考虑了相邻节点的角度和距离因素,明显提高了轨迹的圆滑度,并缩短了路径总长度,证明了理论的可行性。

4 试验结果分析

运行机械臂配置文件,配置机械臂路径规划环境,通过MoveIt配置UR5运动的相关驱动功能包,导入ROS中并进行编译。启动rviz仿真软件,对规划过程中的参数进行配置,利用改进RRT算法程序规划机械臂路径,目标偏差参数若设置太大则精度不够,过小则会导致运动器循环求解,因此设置目标偏差为0.01。

在rviz中导入正方体模型作为障碍物,设置起始位姿进行运动规划仿真,具体运动路径如图8所示,规划路径完美避过了正方体障碍,且轨迹平滑,路径距离也较优。

图8 机械臂避障路径规划路线Fig.8 Robotic arm obstacle avoidance path planning route

选择PC端作为上位机,通过TCP协议作为通讯方式,连接机器人控制柜,设置PC网址为192.168.1.101,机器人网址为192.168.1.103,输入命令行:

＄ roslaunch ur_modern_driver ur5_ros_control.launch robot_ip:=192.168.1.103

＄ roslaunch ur5_moveit_config ur5_moveit_planning_execution.launch

＄ roslaunch ur5_moveit_config moveit_rviz.launch config:=true。

在rvzi仿真可视化界面中设置起始位姿进行路径规划,规划成功后,控制器执行路径驱动机器人运动(图9),蓝色部分为机器人当前位姿,黄色部分为目标位姿,可以观测到运行过程中实体机器人与rviz模型运行同步,机械臂平稳无抖动,位置控制精度较高。

图9 PC机控制UR5实体运动Fig.9 PC control UR5 physical movement

ROS中集成了应用程序开发框架qt工具包,其具有人性化、操作简便和集成度高的优点。启动软件的界面设计(UI)功能,可以实时监测机械臂在运行过程中的角速度、角加速度等信息。输入命令行:

＄ rosrun rqt_plot rqt_plot。

选择绘制机器人关节角速度反馈(joint_velocity)的数据点线图,导出采集的数据点,经过处理得到角速度曲线，如图10所示。

图10 机械臂运行各关节角速度Fig.10 Angular velocity of each joint of the robotic arm

监测机械臂从起始点到目标点一个来回的数据,可以看出：6个关节角速度在起始时向着正负方向平稳变大,到达2.5 s时角速度恒定,匀速运动期间角速度波动很小,在接近停止点，即在4.8 s时角速度平稳减少。分析曲线斜率可知:在机械臂加速到匀速过程和开始减速过程中,角加速度有轻微突变,但是机械臂无明显抖动,整个过程运行比较平稳,能够满足机械加工需要。

5 结论

1)根据机器人学相关知识,推导UR5机器人正逆解公式,代入D-H参数并得到奇异位姿。在ROS中进行仿真测试,观测到机器人轨迹规划正确,表明计算公式和机器人模型的正确性。

2)分析了基本RRT算法,并综合考虑角度和距离两个因素进行算法改进,试验结果证明，相较于基本RRT算法,改进RRT算法缩短了寻优时间,提高了轨迹规划路径的平滑度,证明了该算法的理论可行性。

3)通过实体机器人控制试验,监测到运动过程中6个关节角速度平稳,在2.5到4.8 s的匀速运动过程中观测到的角速度波动较小,机械臂运行平稳无抖动,表明设计的ROS机器人系统控制性能较好,能够满足机械加工的位置精度和稳定性要求。