基于ROS的六轴机械臂3D打印运动规划方法

崔坤腾，谭跃刚，闵双飞，张帆，涂一文

(武汉理工大学机电工程学院，湖北武汉 430070)

0 前言

3D打印技术在制造复杂几何特征的零部件方面，具有无需模具、不受模型复杂度限制、制造周期短、一体成型等优势，在汽车、航空航天、医疗、教育、建筑等领域得到了广泛应用。传统熔融沉积(Fused Deposition Modeling,FDM)三轴打印采用平面分层、单向叠加、悬空部分添加支撑的方式。悬空支撑结构会产生不合理的废料。平面切片方式不能打印曲面层，限制了构件的形状复杂度,且由于阶梯式凸起导致表面质量不佳。单向叠加方式不能改变沉积方向，在成型方向上存在着天然性能缺陷。多轴机器人以其灵活的空间定位能力，能够突破简单平面单向叠加的传统3D打印工艺限制，实现无支撑打印成型，减少材料的浪费；能够实现空间曲面成型，减少阶梯效应、提高表面质量；能够支持多方向成型，提升零件力学性能。多轴机器人3D打印技术对提升3D打印控形控性能力具有重要作用。

传统的FDM 3D打印机系统由3个并联的电机组成运动链，运动链方程简单且可以与电机一一对应，通过3D打印控制固件完成相关的运动控制。六轴机械臂3D打印系统则由6个自由度串联的机构组成运动链，运动链方程较为复杂，对于不同型号的机械臂有唯一的运动链方程。为实现G代码到机械臂的运动，需要对G代码进行轨迹规划、逆运动学求解，求解每个关节运动的角度、速度、加速度等信息。此外，六轴机械臂3D打印系统还包含挤出机的运动控制，需要根据六轴机械臂末端的运动状态同步规划挤出机的运动状态，保证打印质量，故六轴机械臂3D打印运动规划方法对零件的稳定、高质量成型起关键性作用。

国内外众多学者对多轴机械臂3D打印技术展开了深入的研究。夏驰、刘灏强等构建并集成了激光烧结系统、机械臂运动控制系统、点胶系统，实现表面共形3D打印成型。陈丽开发了一套基于FDM工艺的工业机器人3D打印成型系统，并研究了机械臂运动速度、挤出机挤出速度对打印成型的质量影响。XIE等基于势场算法，综合考虑打印效率与机械臂运动学，实现在机器人多轴打印机上自动生成打印任意自由曲面的路径。SONG等建立了六轴运动的AM(增材制造)系统，并进行了硬件设计、运动规划、建模和平台语速检查、工具运动仿真和平台校准，多方位说明了六轴机械臂增材制造的能力。LUU等基于G指令重新规划打印轨迹，打印轨迹和打印过程的其他必要组件的准确和执行由机器人系统ROS控制，完成了六轴机械臂对简单零件的3D打印成型。国内外学者关于机械臂3D打印研究工作主要在机械臂本体的运动规划、运动避障、运动学求解等方面，对于机械臂3D打印工艺的研究较少，尚未查到在机械臂运动和挤出机运动的协同规划，机械臂运动与挤料运动的工艺参数之间的关系及对成型的影响等方面的深入研究。在零件成型时，存在打印系统运动不稳定、喷头运动速度与挤料速度不匹配、打印质量不佳等问题。

针对机械臂3D打印的运动规划问题，基于ROS系统，建立六轴机械臂的D-H参数模型。采用OMPL(Open Motion Planning Library)规划库、KDL(Kinematics and Dynamics Library)求解器对G代码进行运动规划。同时根据运动规划结果，提取机械臂末端运动状态(速度、加速度、时间)，对挤出机的运动进行同步规划，实现六轴机械臂和挤出机的协同控制，为六轴机械臂3D打印系统的高质量成型提供支持。

1 六轴机械臂3D打印流程

如图1所示，六轴机械臂3D打印系统主要由六轴机械臂、挤出装置、成型平台及控制系统组成。六轴机械臂主要完成末端执行器打印头的空间轨迹运动；挤出装置主要跟随空间轨迹的运动将材料挤出成型；成型平台为工件的打印平台；控制系统包含上位机、机械臂控制器、成型主控板等，对六轴机械臂3D打印所有运动进行协同控制。

图1 六轴机械臂3D打印系统构成

如图2所示，六轴机械臂3D打印流程主要包含三维模型的切片处理、成型路径规划、机械臂运动规划、挤出装置挤出规划、控制指令的集成、模拟仿真、打印成型等。将三维模型导入切片软件进行曲面分层，在三维空间规划成型路径，生成笛卡尔坐标下六轴机械臂的位姿、挤出机挤出长度、喷头温度、底床温度等信息的G_code指令文件。由于路径规划时，仅考虑了成型的工艺参数，并未将机械臂运动参数纳入规划范围中，无法直接将路径点用于机械臂的3D打印，需要将位姿信息提取出来，进行机械臂的运动规划及逆运动学求解，生成六轴机械臂可执行的运动指令集。同时，还需要根据机械臂的运动规划参数，提取机械臂末端执行器运动的实时状态，并对挤出机挤出速度、加速度进行规划，保证熔融材料挤出状态与喷头末端执行器运动状态的同步性。最终将机械臂运动指令、挤出机挤出指令及其他组件控制指令进行集成，生成六轴机械臂3D打印控制指令集文件。在成型过程中，通过打印头可改变沉积方向，则可有效避免悬空结构需要增加支撑结构的问题；打印头可在三维空间内做曲线运动，则可以减少模型表面凸起带来的阶梯效应；通过打印头可以在三维空间内作任意方向的成型，进一步增加了工件成型的力学性能。

图2 六轴机械臂3D打印流程

2 六轴机械臂3D打印运动规划方法

2.1 运动规划流程

如图3所示，六轴机械臂3D打印规划流程主要分为两大部分：六轴机械臂运动规划和挤出机运动规划。

图3 六轴机械臂3D打印规划流程

六轴机械臂运动规划：切片软件生成G_code文件后，提取喷头的路径点的位姿信息，并导入ROS系统，进行位姿插补、逆运动学求解等运动规划，最终生成六轴机械臂可执行的关节实时运动信息文件。六轴机械臂的运动规划将会对成型路径点进行位姿插值，且将根据实际的路径进行速度、加速度等参数求解。

为保证零件成型时，挤出机挤出耗材的均匀性、一致性，挤出机的运动规划采用六轴机械臂运动规划信息进行同步规划。其具体流程：提取每个位姿关节角度信息，正运动学求解计算出喷头在三维空间每个点的位置，求得打印头的进给量和挤出机耗材的路径长度，结合喷头与成型工艺参数，求解出需要耗材的用量；提取每个位姿关节运动的速度信息，并根据机械臂的雅克比矩阵，求得打印头在三维空间的线速度，即挤出耗材的出丝速度，同时结合挤出机的挤出工艺参数，求得挤出电机的进料速度。

最终，结合六轴机械臂的运动信息、挤出机的进出运动信息及3D打印其他重要组件的控制信息即可完成对机械臂3D打印控制指令的集成。

2.2 六轴机械臂关节运动规划

2.2.1 机械臂运动学分析

使用中科新松GCR5-910机械臂作为实验平台，GCR5-910是一种串联六轴机械臂。

标准D-H参数法对于树形结构或者闭链机构的机器人在建立连杆坐标系时会产生歧义，故采用改进D-H参数法进行建模。结合GCR5-910的实际尺寸以及结构分布建立D-H坐标系，将基坐标系{,,}建立在机械臂基座处，使用连杆长度、连杆扭角、连杆距离、关节转角4个参数来描述该机器人。以此建立的D-H坐标系以及获取的D-H参数分别如图4和表1所示。

图4 机器人连杆坐标系

表1 GCR5-910机械臂D-H参数

对GCR5-910机器人的正运动学求解进行了理论分析计算，得出机器人末端执行器的位姿矩阵公式，为机器人3D打印过程中进行挤料电机速度匹配时的机器人正运动学计算提供了前提条件。

(1)

(2)

2.2.2 机械臂模型建立

创建机械臂URDF模型：将机器人三维模型导入SolidWorks，并使用sw_urdf_exporter插件自动生成URDF文件，其基本流程如图5所示。

图5 URDF创建流程 图6 MoveIt配置流程

MoveIt配置机械臂：完成URDF建模后，在ROS中使用MoveIt配置助手对机械臂模型进行配置，根据机器人URDF模型生成SRDF(Semantic Robot Description Format)文件。主要配置步骤如图6所示。

MoveIt配置流程主要包含机器人URDF模型的加载、生成自碰撞矩阵、配置虚拟关节、创建规划组、定义机器人位姿、配置末端执行器、配置被动关节、生成配置文件等步骤，为运动学求解、运动规划提供基础。配置助手生成srdf、kinematic.yaml、launch、Move_group等文件，srdf 文件包含可视化配置的所有信息、urdf文件编译后的解析信息;kinematic.yaml是运动学求解器kdl插件配置的相关信息;launch文件提供了启动机械臂相关功能的一系列文件;Move_group是MoveIt架构的核心，其配置文件存放于config文件中。

2.2.3 运动规划

如图7所示，借助ROS MoveIt 平台，整体框架规划机械臂3D打印运动。MoveIt中的Move_group节点将根据机器人模型URDF文件、配置SRDF文件、机器人初始状态信息、机器人目标位姿信息、周围环境信息，调用运动规划算法、碰撞检测算法、插补算法、KDL求解逆运动学库生成所需的轨迹，此轨迹以PVT格式数组的形式表示机器人运动轨迹上的位姿、速度、加速度等信息。

图7 MoveIt框架

采用KDL正逆运动学求解器及OMPL轨迹规划器，能够快速将笛卡尔空间位姿转化为对应的关节角位姿。采用RRT(Rapidly-exploring Random Tree)作为机械臂仿真分析的规划算法。

切片软件生成的G_code文件包含了成型路径、挤出机挤出耗材、成型温度、底床温度、归零等信息，在进行运动规划前，需要对G_code文件进行信息处理，提取成型路径喷头的姿态信息作为机械臂目标位姿信息。

MoveIt通过运动规划器进行运动规划，具体流程如图8所示。设定打印头的目标姿态，发送运动规划请求。规划请求适配器对目标位姿进行预处理请求，帮助纠正关节的非法状态，同时也可添加速度、加速度约束，实现参数化运动规划。运动规划器根据设定的位置、方向、关节约束条件进行运动规划，并生成轨迹，同时规划请求适配器可对生成的轨迹添加时间参数等，最终生成运动轨迹信息发送给关节轨迹控制器。

图8 运动规划流程

2.3 挤出机运动规划

挤出机运动规划主要包含挤出速度及挤出量。挤出速度可根据机械臂实时运动的关节速度和雅克比矩阵求解，挤出量可根据机械臂正运动学求解矩阵、关节运动的角度及成型工艺参数求解。

2.3.1 挤出计量规划

由于运动规划时，所有的路径已经被直线插补，故所有的点位之间可视为一个直线段，挤出机初始长度等于2个相邻点位的位移差，故出丝长度Δ为

Δ=

假定喷头直径为、线材直径为、成型层高为、出丝长度为Δ、成型线宽为，则每个点位运动的耗材长度为

2.3.2 挤出速度规划

在ROS中通过监听者节点获得轨迹的运动参数，建立运动和加速度正运动学方程，求得末端姿态的运动速度，进而根据机料机的挤出参数，求得挤出机的挤出速度。

(3)

矩阵的第列(记为)可由下式给出:

假定除关节外的所有关节均被固定，则末端执行器的线速度可表示为=，其中

=--1

=-1(--1)

末端执行器相对于基坐标系的总线速度为

(4)

3 仿真与测试

3.1 ROS机械臂运动仿真

选用尺寸100 mm×100 mm×10 mm的三维模型进行切片处理，并生成G代码：

M104 S200

M105

M109 S200

G28 ;Home

;Prime the extruder

M83 ;relative extrusion mode

G1 F1500 E-6.5

M107

G0 F3600 X-31.23 Y-31.23 Z0.6 RX180 RY0 RZ0

G1 F1500 E6.5

G1 F1800 X-31.23 Y31.23 Z0.6 RX180 RY0 RZ0 E12.46

G1 X31.23 Y31.23 Z0.6 RX180 RY0 RZ0 E12.46

G1 X31.23 Y-31.23 Z0.6 RX180 RY0 RZ0 E12.46

G1 X-31.23 Y-31.23 Z0.6 RX180 RY0 RZ0 E12.46

G0 F3600 X-32.06 Y-32.06 Z0.6 RX180 RY0 RZ0

G1 F1800 X-32.06 Y32.06 Z0.6 RX180 RY0 RZ0 E12.79

G1 X32.06 Y32.06 Z0.6 RX180 RY0 RZ0 E12.79

运动指令G1 X Y Z RX RY RZ表示喷头位姿，E表示挤料量。

通过对六轴机械臂进行MoveIt配置，搭建ROS机器人仿真平台，使用MoveIt框架中的C++接口编写节点程序访问G_code文件提取位姿信息存入路点数组；调用Computer Cartesian Path函数，使用MoveIt运动规划库OMPL中的规划算法RRT以该路点数组为目标进行路径规划得到运动轨迹；调用execute函数执行该轨迹，机械臂开始进行仿真运动，并可在RVIZ中显示。ROS运动规划仿真路径如图9所示。

图9 ROS运动规划仿真

同时运行监听者节点进行监听，可得到机械臂的实时运动信息，包含关节的角度、速度、加速度等，具体信息如下：

positions:[-0.2367,0.3327,1.7842,-0.5307,-1.8004,-0.2281]//关节角度

velocities:[0.0901,0.0224,-0.0134,0.0684,-0.1872,0.11710]//关节速度

accelerations:[0.0611,0.1897,-0.3890,0.5079,0.0191,0.1523]//关节加速度

effort:[]//力

time_from_start://时间

secs:1884

nsecs:53443511

根据机械臂正运动学求解打印头每个路径点的位移增量及挤出装置的挤料量。根据速度正运动学雅克比矩阵对挤出速度进行规划匹配，其打印头与挤出机的速度规划结果如图10所示。

图10 打印头与挤出机速度状态

3.2 设备集成与测试

完成六轴机械臂三维空间的运动轨迹规划、挤出机速度规划后，将规划生成的信息进行集成。对六轴机械臂3D打印系统进行集成测试，包含机械臂本体、挤料装置、成型平台、控制系统等，最终将指令集文件通过上位机主站下发至各个控制单元，并打印测试。图11所示为六轴机械臂3D打印，此过程中六轴机械臂各关节角度、速度、挤出机速度状态信息如下：

图11 六轴机械臂3D打印

joint_pose joint_speed Extrusion _speed

N0[θ1 θ2 θ3 θ4 θ5 θ6][ω1 ω2 ω3 ω4 ω5 ω][v_e]

N1[-2.90,-0.22,-1.49,0.15,1.57,-1.34][0, 0，0，0，0,0][0]

N2[-2.91,-0.21,-1.51,0.17,1.57,-1.35][-0.06,0.10,-0.19,0.08,-0.00,-0.05][4.2573]

N3[-2.92,-0.20,-1.54,0.17,1.57,-1.35][-0.10,0.16,-0.30,0.12,-0.01,-0.09][6.6556]

N4[-2.93,-0.18,-1.56,0.18,1.57,-1.36][-0.10,0.16,-0.29,0.12,-0.01,-0.10][6.4604]

N5[-2.94,-0.17,-1.59,0.19,1.57,-1.37][-0.07,0.11,-0.19,0.08,-0.01,-0.06][4.1511]

N6[-2.95,-0.16,-1.61,0.20,1.57,-1.38][0.03,0.04,-0.07,0.03,-0.01，0.03][1.8590]

N7[-2.93,-0.16,-1.61,0.20,1.57,-1.36][0.16,0.01,-0.01,0.00，0.00，0.16][1.9545]

N8[-2.92,-0.16,-1.61,0.20,1.57,-1.34][0.25,0.01,-0.01,0.00，0.00，0.25][2.8329]

4 总结

针对六轴机械臂3D打印系统中机械臂关节运动规划及末端执行器打印头运动与挤出机挤料运动不匹配问题，建立了基于ROS的六轴机械3D打印运动规划仿真平台，提出机械臂与挤出机协同运动规划的方法。根据成型路径进行关节的运动学求解，提取关节运动状态信息，对挤出机的挤料速度进行匹配规划。选用基本零件，进行了三维模型切片路径规划、机械臂运动规划及仿真、挤出机挤料速度匹配，最后在六轴机械臂3D打印平台进行了打印测试实验。结果表明：零件成型时出丝量较为均匀，避免了因挤料与打印头运动速度不匹配导致的材料堆积或缺失现象。由于通信周期问题，挤料运动动作时间较打印头移动动作时间有一定延时，后续需提高通信的实时同步性，进一步提高成型质量。