基于ROS的六軸機械臂3D打印運動規(guī)劃方法

崔坤騰，譚躍剛，閔雙飛，張帆，涂一文

(武漢理工大學機電工程學院，湖北武漢 430070)

0 前言

3D打印技術在制造復雜幾何特征的零部件方面，具有無需模具、不受模型復雜度限制、制造周期短、一體成型等優(yōu)勢，在汽車、航空航天、醫(yī)療、教育、建筑等領域得到了廣泛應用。傳統(tǒng)熔融沉積(Fused Deposition Modeling,FDM)三軸打印采用平面分層、單向疊加、懸空部分添加支撐的方式。懸空支撐結構會產生不合理的廢料。平面切片方式不能打印曲面層，限制了構件的形狀復雜度,且由于階梯式凸起導致表面質量不佳。單向疊加方式不能改變沉積方向，在成型方向上存在著天然性能缺陷。多軸機器人以其靈活的空間定位能力，能夠突破簡單平面單向疊加的傳統(tǒng)3D打印工藝限制，實現(xiàn)無支撐打印成型，減少材料的浪費；能夠實現(xiàn)空間曲面成型，減少階梯效應、提高表面質量；能夠支持多方向成型，提升零件力學性能。多軸機器人3D打印技術對提升3D打印控形控性能力具有重要作用。

傳統(tǒng)的FDM 3D打印機系統(tǒng)由3個并聯(lián)的電機組成運動鏈，運動鏈方程簡單且可以與電機一一對應，通過3D打印控制固件完成相關的運動控制。六軸機械臂3D打印系統(tǒng)則由6個自由度串聯(lián)的機構組成運動鏈，運動鏈方程較為復雜，對于不同型號的機械臂有唯一的運動鏈方程。為實現(xiàn)G代碼到機械臂的運動，需要對G代碼進行軌跡規(guī)劃、逆運動學求解，求解每個關節(jié)運動的角度、速度、加速度等信息。此外，六軸機械臂3D打印系統(tǒng)還包含擠出機的運動控制，需要根據(jù)六軸機械臂末端的運動狀態(tài)同步規(guī)劃擠出機的運動狀態(tài)，保證打印質量，故六軸機械臂3D打印運動規(guī)劃方法對零件的穩(wěn)定、高質量成型起關鍵性作用。

國內外眾多學者對多軸機械臂3D打印技術展開了深入的研究。夏馳、劉灝強等構建并集成了激光燒結系統(tǒng)、機械臂運動控制系統(tǒng)、點膠系統(tǒng)，實現(xiàn)表面共形3D打印成型。陳麗開發(fā)了一套基于FDM工藝的工業(yè)機器人3D打印成型系統(tǒng)，并研究了機械臂運動速度、擠出機擠出速度對打印成型的質量影響。XIE等基于勢場算法，綜合考慮打印效率與機械臂運動學，實現(xiàn)在機器人多軸打印機上自動生成打印任意自由曲面的路徑。SONG等建立了六軸運動的AM(增材制造)系統(tǒng)，并進行了硬件設計、運動規(guī)劃、建模和平臺語速檢查、工具運動仿真和平臺校準，多方位說明了六軸機械臂增材制造的能力。LUU等基于G指令重新規(guī)劃打印軌跡，打印軌跡和打印過程的其他必要組件的準確和執(zhí)行由機器人系統(tǒng)ROS控制，完成了六軸機械臂對簡單零件的3D打印成型。國內外學者關于機械臂3D打印研究工作主要在機械臂本體的運動規(guī)劃、運動避障、運動學求解等方面，對于機械臂3D打印工藝的研究較少，尚未查到在機械臂運動和擠出機運動的協(xié)同規(guī)劃，機械臂運動與擠料運動的工藝參數(shù)之間的關系及對成型的影響等方面的深入研究。在零件成型時，存在打印系統(tǒng)運動不穩(wěn)定、噴頭運動速度與擠料速度不匹配、打印質量不佳等問題。

針對機械臂3D打印的運動規(guī)劃問題，基于ROS系統(tǒng)，建立六軸機械臂的D-H參數(shù)模型。采用OMPL(Open Motion Planning Library)規(guī)劃庫、KDL(Kinematics and Dynamics Library)求解器對G代碼進行運動規(guī)劃。同時根據(jù)運動規(guī)劃結果，提取機械臂末端運動狀態(tài)(速度、加速度、時間)，對擠出機的運動進行同步規(guī)劃，實現(xiàn)六軸機械臂和擠出機的協(xié)同控制，為六軸機械臂3D打印系統(tǒng)的高質量成型提供支持。

1 六軸機械臂3D打印流程

如圖1所示，六軸機械臂3D打印系統(tǒng)主要由六軸機械臂、擠出裝置、成型平臺及控制系統(tǒng)組成。六軸機械臂主要完成末端執(zhí)行器打印頭的空間軌跡運動；擠出裝置主要跟隨空間軌跡的運動將材料擠出成型；成型平臺為工件的打印平臺；控制系統(tǒng)包含上位機、機械臂控制器、成型主控板等，對六軸機械臂3D打印所有運動進行協(xié)同控制。

圖1 六軸機械臂3D打印系統(tǒng)構成

如圖2所示，六軸機械臂3D打印流程主要包含三維模型的切片處理、成型路徑規(guī)劃、機械臂運動規(guī)劃、擠出裝置擠出規(guī)劃、控制指令的集成、模擬仿真、打印成型等。將三維模型導入切片軟件進行曲面分層，在三維空間規(guī)劃成型路徑，生成笛卡爾坐標下六軸機械臂的位姿、擠出機擠出長度、噴頭溫度、底床溫度等信息的G_code指令文件。由于路徑規(guī)劃時，僅考慮了成型的工藝參數(shù)，并未將機械臂運動參數(shù)納入規(guī)劃范圍中，無法直接將路徑點用于機械臂的3D打印，需要將位姿信息提取出來，進行機械臂的運動規(guī)劃及逆運動學求解，生成六軸機械臂可執(zhí)行的運動指令集。同時，還需要根據(jù)機械臂的運動規(guī)劃參數(shù)，提取機械臂末端執(zhí)行器運動的實時狀態(tài)，并對擠出機擠出速度、加速度進行規(guī)劃，保證熔融材料擠出狀態(tài)與噴頭末端執(zhí)行器運動狀態(tài)的同步性。最終將機械臂運動指令、擠出機擠出指令及其他組件控制指令進行集成，生成六軸機械臂3D打印控制指令集文件。在成型過程中，通過打印頭可改變沉積方向，則可有效避免懸空結構需要增加支撐結構的問題；打印頭可在三維空間內做曲線運動，則可以減少模型表面凸起帶來的階梯效應；通過打印頭可以在三維空間內作任意方向的成型，進一步增加了工件成型的力學性能。

圖2 六軸機械臂3D打印流程

2 六軸機械臂3D打印運動規(guī)劃方法

2.1 運動規(guī)劃流程

如圖3所示，六軸機械臂3D打印規(guī)劃流程主要分為兩大部分：六軸機械臂運動規(guī)劃和擠出機運動規(guī)劃。

圖3 六軸機械臂3D打印規(guī)劃流程

六軸機械臂運動規(guī)劃：切片軟件生成G_code文件后，提取噴頭的路徑點的位姿信息，并導入ROS系統(tǒng)，進行位姿插補、逆運動學求解等運動規(guī)劃，最終生成六軸機械臂可執(zhí)行的關節(jié)實時運動信息文件。六軸機械臂的運動規(guī)劃將會對成型路徑點進行位姿插值，且將根據(jù)實際的路徑進行速度、加速度等參數(shù)求解。

為保證零件成型時，擠出機擠出耗材的均勻性、一致性，擠出機的運動規(guī)劃采用六軸機械臂運動規(guī)劃信息進行同步規(guī)劃。其具體流程：提取每個位姿關節(jié)角度信息，正運動學求解計算出噴頭在三維空間每個點的位置，求得打印頭的進給量和擠出機耗材的路徑長度，結合噴頭與成型工藝參數(shù)，求解出需要耗材的用量；提取每個位姿關節(jié)運動的速度信息，并根據(jù)機械臂的雅克比矩陣，求得打印頭在三維空間的線速度，即擠出耗材的出絲速度，同時結合擠出機的擠出工藝參數(shù)，求得擠出電機的進料速度。

最終，結合六軸機械臂的運動信息、擠出機的進出運動信息及3D打印其他重要組件的控制信息即可完成對機械臂3D打印控制指令的集成。

2.2 六軸機械臂關節(jié)運動規(guī)劃

2.2.1 機械臂運動學分析

使用中科新松GCR5-910機械臂作為實驗平臺，GCR5-910是一種串聯(lián)六軸機械臂。

標準D-H參數(shù)法對于樹形結構或者閉鏈機構的機器人在建立連桿坐標系時會產生歧義，故采用改進D-H參數(shù)法進行建模。結合GCR5-910的實際尺寸以及結構分布建立D-H坐標系，將基坐標系{,,}建立在機械臂基座處，使用連桿長度、連桿扭角、連桿距離、關節(jié)轉角4個參數(shù)來描述該機器人。以此建立的D-H坐標系以及獲取的D-H參數(shù)分別如圖4和表1所示。

圖4 機器人連桿坐標系

表1 GCR5-910機械臂D-H參數(shù)

對GCR5-910機器人的正運動學求解進行了理論分析計算，得出機器人末端執(zhí)行器的位姿矩陣公式，為機器人3D打印過程中進行擠料電機速度匹配時的機器人正運動學計算提供了前提條件。

(1)

(2)

2.2.2 機械臂模型建立

創(chuàng)建機械臂URDF模型：將機器人三維模型導入SolidWorks，并使用sw_urdf_exporter插件自動生成URDF文件，其基本流程如圖5所示。

圖5 URDF創(chuàng)建流程 圖6 MoveIt配置流程

MoveIt配置機械臂：完成URDF建模后，在ROS中使用MoveIt配置助手對機械臂模型進行配置，根據(jù)機器人URDF模型生成SRDF(Semantic Robot Description Format)文件。主要配置步驟如圖6所示。

MoveIt配置流程主要包含機器人URDF模型的加載、生成自碰撞矩陣、配置虛擬關節(jié)、創(chuàng)建規(guī)劃組、定義機器人位姿、配置末端執(zhí)行器、配置被動關節(jié)、生成配置文件等步驟，為運動學求解、運動規(guī)劃提供基礎。配置助手生成srdf、kinematic.yaml、launch、Move_group等文件，srdf 文件包含可視化配置的所有信息、urdf文件編譯后的解析信息;kinematic.yaml是運動學求解器kdl插件配置的相關信息;launch文件提供了啟動機械臂相關功能的一系列文件;Move_group是MoveIt架構的核心，其配置文件存放于config文件中。

2.2.3 運動規(guī)劃

如圖7所示，借助ROS MoveIt 平臺，整體框架規(guī)劃機械臂3D打印運動。MoveIt中的Move_group節(jié)點將根據(jù)機器人模型URDF文件、配置SRDF文件、機器人初始狀態(tài)信息、機器人目標位姿信息、周圍環(huán)境信息，調用運動規(guī)劃算法、碰撞檢測算法、插補算法、KDL求解逆運動學庫生成所需的軌跡，此軌跡以PVT格式數(shù)組的形式表示機器人運動軌跡上的位姿、速度、加速度等信息。

圖7 MoveIt框架

采用KDL正逆運動學求解器及OMPL軌跡規(guī)劃器，能夠快速將笛卡爾空間位姿轉化為對應的關節(jié)角位姿。采用RRT(Rapidly-exploring Random Tree)作為機械臂仿真分析的規(guī)劃算法。

切片軟件生成的G_code文件包含了成型路徑、擠出機擠出耗材、成型溫度、底床溫度、歸零等信息，在進行運動規(guī)劃前，需要對G_code文件進行信息處理，提取成型路徑噴頭的姿態(tài)信息作為機械臂目標位姿信息。

MoveIt通過運動規(guī)劃器進行運動規(guī)劃，具體流程如圖8所示。設定打印頭的目標姿態(tài)，發(fā)送運動規(guī)劃請求。規(guī)劃請求適配器對目標位姿進行預處理請求，幫助糾正關節(jié)的非法狀態(tài)，同時也可添加速度、加速度約束，實現(xiàn)參數(shù)化運動規(guī)劃。運動規(guī)劃器根據(jù)設定的位置、方向、關節(jié)約束條件進行運動規(guī)劃，并生成軌跡，同時規(guī)劃請求適配器可對生成的軌跡添加時間參數(shù)等，最終生成運動軌跡信息發(fā)送給關節(jié)軌跡控制器。

圖8 運動規(guī)劃流程

2.3 擠出機運動規(guī)劃

擠出機運動規(guī)劃主要包含擠出速度及擠出量。擠出速度可根據(jù)機械臂實時運動的關節(jié)速度和雅克比矩陣求解，擠出量可根據(jù)機械臂正運動學求解矩陣、關節(jié)運動的角度及成型工藝參數(shù)求解。

2.3.1 擠出計量規(guī)劃

由于運動規(guī)劃時，所有的路徑已經被直線插補，故所有的點位之間可視為一個直線段，擠出機初始長度等于2個相鄰點位的位移差，故出絲長度Δ為

Δ=

假定噴頭直徑為、線材直徑為、成型層高為、出絲長度為Δ、成型線寬為，則每個點位運動的耗材長度為

2.3.2 擠出速度規(guī)劃

在ROS中通過監(jiān)聽者節(jié)點獲得軌跡的運動參數(shù)，建立運動和加速度正運動學方程，求得末端姿態(tài)的運動速度，進而根據(jù)機料機的擠出參數(shù)，求得擠出機的擠出速度。

(3)

矩陣的第列(記為)可由下式給出:

假定除關節(jié)外的所有關節(jié)均被固定，則末端執(zhí)行器的線速度可表示為=，其中

=--1

=-1(--1)

末端執(zhí)行器相對于基坐標系的總線速度為

(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++接口編寫節(jié)點程序訪問G_code文件提取位姿信息存入路點數(shù)組；調用Computer Cartesian Path函數(shù)，使用MoveIt運動規(guī)劃庫OMPL中的規(guī)劃算法RRT以該路點數(shù)組為目標進行路徑規(guī)劃得到運動軌跡；調用execute函數(shù)執(zhí)行該軌跡，機械臂開始進行仿真運動，并可在RVIZ中顯示。ROS運動規(guī)劃仿真路徑如圖9所示。

圖9 ROS運動規(guī)劃仿真

同時運行監(jiān)聽者節(jié)點進行監(jiān)聽，可得到機械臂的實時運動信息，包含關節(jié)的角度、速度、加速度等，具體信息如下：

positions:[-0.2367,0.3327,1.7842,-0.5307,-1.8004,-0.2281]//關節(jié)角度

velocities:[0.0901,0.0224,-0.0134,0.0684,-0.1872,0.11710]//關節(jié)速度

accelerations:[0.0611,0.1897,-0.3890,0.5079,0.0191,0.1523]//關節(jié)加速度

effort:[]//力

time_from_start://時間

secs:1884

nsecs:53443511

根據(jù)機械臂正運動學求解打印頭每個路徑點的位移增量及擠出裝置的擠料量。根據(jù)速度正運動學雅克比矩陣對擠出速度進行規(guī)劃匹配，其打印頭與擠出機的速度規(guī)劃結果如圖10所示。

圖10 打印頭與擠出機速度狀態(tài)

3.2 設備集成與測試

完成六軸機械臂三維空間的運動軌跡規(guī)劃、擠出機速度規(guī)劃后，將規(guī)劃生成的信息進行集成。對六軸機械臂3D打印系統(tǒng)進行集成測試，包含機械臂本體、擠料裝置、成型平臺、控制系統(tǒng)等，最終將指令集文件通過上位機主站下發(fā)至各個控制單元，并打印測試。圖11所示為六軸機械臂3D打印，此過程中六軸機械臂各關節(jié)角度、速度、擠出機速度狀態(tài)信息如下：

圖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打印系統(tǒng)中機械臂關節(jié)運動規(guī)劃及末端執(zhí)行器打印頭運動與擠出機擠料運動不匹配問題，建立了基于ROS的六軸機械3D打印運動規(guī)劃仿真平臺，提出機械臂與擠出機協(xié)同運動規(guī)劃的方法。根據(jù)成型路徑進行關節(jié)的運動學求解，提取關節(jié)運動狀態(tài)信息，對擠出機的擠料速度進行匹配規(guī)劃。選用基本零件，進行了三維模型切片路徑規(guī)劃、機械臂運動規(guī)劃及仿真、擠出機擠料速度匹配，最后在六軸機械臂3D打印平臺進行了打印測試實驗。結果表明：零件成型時出絲量較為均勻，避免了因擠料與打印頭運動速度不匹配導致的材料堆積或缺失現(xiàn)象。由于通信周期問題，擠料運動動作時間較打印頭移動動作時間有一定延時，后續(xù)需提高通信的實時同步性，進一步提高成型質量。