基于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打印平臺進行了打印測試實驗。結果表明：零件成型時出絲量較為均勻，避免了因擠料與打印頭運動速度不匹配導致的材料堆積或缺失現象。由于通信周期問題，擠料運動動作時間較打印頭移動動作時間有一定延時，后續需提高通信的實時同步性，進一步提高成型質量。