李小虎，張 華，周 穩(wěn)，姜 濤

(安徽科技學(xué)院 機(jī)械工程學(xué)院, 安徽 滁州 233100)

隨著碳達(dá)峰時(shí)代的來(lái)臨，大量的汽車、燃油公交車面臨報(bào)廢待拆解的境地。目前國(guó)內(nèi)外對(duì)報(bào)廢客車拆解的方式主要為氧切和暴力切割，但氧氣切割對(duì)人傷害極大，且暴力拆解嚴(yán)重破壞了客車部分關(guān)鍵零部件的回收及使用。由于客車體積非常龐大、因此需將其進(jìn)行切割成固定大小的矩形塊，以方便其進(jìn)行回收利用。故需對(duì)殼體進(jìn)行切割運(yùn)動(dòng)規(guī)劃，以減少在切割過(guò)程中所造成的空行程。所以現(xiàn)急需研發(fā)一種智能化綠色拆解手段，來(lái)解決當(dāng)下報(bào)廢客車的拆解難題[1-3]。

如今，隨著智能化機(jī)器人的高速發(fā)展，對(duì)機(jī)器人的控制系統(tǒng)要求也愈來(lái)愈高。而目前市場(chǎng)上的機(jī)器人廠商都在開(kāi)發(fā)各自的軟件接口，故導(dǎo)致了市場(chǎng)上的機(jī)器人可移植性差，復(fù)雜的建模及對(duì)大量冗余代碼的編寫(xiě)嚴(yán)重影響開(kāi)發(fā)者的工作效率。而目前流行的機(jī)器人操作系統(tǒng)ROS(Robot opreating system)具有跨平臺(tái)(Windows、Ubuntu)、多語(yǔ)言支持(Python、C++、Java等)、代碼復(fù)用等優(yōu)點(diǎn)，能夠很好地解決此問(wèn)題[4-5]?，F(xiàn)采用高壓水槍搭載至拆解機(jī)械臂上，使用ROS操作平臺(tái)控制拆解機(jī)械臂對(duì)報(bào)廢客車殼體進(jìn)行切割運(yùn)動(dòng)規(guī)劃，并在Moveit+rviz+gazebo進(jìn)行仿真試驗(yàn)。

1 機(jī)械臂模型分析與功能包配置

由于報(bào)廢客車體積大、車身重、含易燃物質(zhì)，在拆解時(shí)不易移動(dòng)，傳統(tǒng)的機(jī)械切割與氧氣在拆解時(shí)會(huì)產(chǎn)生火花，會(huì)引發(fā)火災(zāi)等危險(xiǎn)。故采用高壓水槍搭載至機(jī)械臂上，機(jī)械臂控制水槍并對(duì)報(bào)廢客車殼體進(jìn)行拆解，具體操作示意如圖1所示。

圖1 報(bào)廢客車拆解示意

1.1 機(jī)械臂運(yùn)動(dòng)學(xué)建模過(guò)程

對(duì)機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)建模，主要是為了后續(xù)對(duì)其進(jìn)行運(yùn)動(dòng)軌跡規(guī)劃，而對(duì)于工業(yè)機(jī)械臂來(lái)說(shuō)，其構(gòu)造就是由許多連桿之間經(jīng)過(guò)關(guān)節(jié)聯(lián)結(jié)起來(lái)的運(yùn)動(dòng)鏈條，工業(yè)機(jī)械手臂末端執(zhí)行機(jī)構(gòu)和工業(yè)機(jī)械臂基座世界坐標(biāo)系間通過(guò)各連桿上固接的一個(gè)連桿坐標(biāo)系鏈接，這些連桿之間經(jīng)過(guò)關(guān)節(jié)聯(lián)結(jié)起來(lái)后，連桿坐標(biāo)系間的相對(duì)關(guān)系就建立了。在SolidWorks三維建模軟件中指定機(jī)器人每個(gè)連桿對(duì)應(yīng)的三維模型和各關(guān)節(jié)的旋轉(zhuǎn)軸等幾何特征，如圖2所示。并為每個(gè)關(guān)節(jié)定義指定運(yùn)動(dòng)范圍及運(yùn)動(dòng)速度限制，形成鏈?zhǔn)浇Y(jié)構(gòu)。然后在ROS官網(wǎng)下載插件sw2urdf，該插件可將繪制的機(jī)器人模型轉(zhuǎn)化為URDF(Unified Robot Description Format)文件，而URDF是一款專門用于描述機(jī)器人格式的XML文件，其代碼主要定義link及joint。本文中使用的六軸開(kāi)放源代碼機(jī)械手臂Rocr6如圖3所示。

圖2 機(jī)械臂關(guān)節(jié)連桿

圖3 Rocr6開(kāi)源機(jī)械臂模型

Rviz三維可視化工具是基于MoveIt框架的插件,可以用來(lái)生成自動(dòng)化機(jī)器人工作的場(chǎng)景并實(shí)現(xiàn)人機(jī)交互。在驗(yàn)證URDF模型語(yǔ)法和構(gòu)造之后，可以啟用joint_state_publisher節(jié)點(diǎn)，發(fā)送sensor_msgs/JointState信息，調(diào)用use_gui:=true監(jiān)控機(jī)械手臂滑膜關(guān)節(jié)運(yùn)動(dòng)，檢測(cè)機(jī)械手臂之間有無(wú)相互干涉和關(guān)節(jié)限位現(xiàn)象，對(duì)URDF文件進(jìn)行設(shè)置和調(diào)整等[6-8]。

1.2 機(jī)械臂力學(xué)模型分析與功能包配置

機(jī)械臂連桿是一個(gè)非常復(fù)雜的動(dòng)力學(xué)系統(tǒng)，其動(dòng)力學(xué)方程具有非線性、強(qiáng)耦合和實(shí)變等特點(diǎn)。操作過(guò)程中，機(jī)械臂連桿的動(dòng)力響應(yīng)出現(xiàn)剛?cè)狁詈犀F(xiàn)象，即臂桿在歷經(jīng)大范圍轉(zhuǎn)動(dòng)的同時(shí)疊加了自身低頻的彈性振動(dòng)，以致于定位精度嚴(yán)重降低；因此，機(jī)械臂連桿系統(tǒng)的動(dòng)力學(xué)分析是柔性機(jī)械臂實(shí)現(xiàn)工程應(yīng)用必須要解決的關(guān)鍵技術(shù)之一。以下將從柔性系統(tǒng)中最為基礎(chǔ)的單連桿機(jī)械臂模型及最為典型的雙連桿機(jī)械臂模型進(jìn)行分析，如圖4～5所示。

(a) 單連桿位置關(guān)系

1.2.1 單連桿動(dòng)力學(xué)模型

圖4是機(jī)械臂單連桿示意圖。當(dāng)機(jī)械臂不是作單純的平動(dòng)而是作比較復(fù)雜的運(yùn)動(dòng)時(shí)，各關(guān)節(jié)的運(yùn)動(dòng)狀態(tài)(速度與加速度)不相同。但總可以把物體看成質(zhì)點(diǎn)組來(lái)分析、處理，即想象把物體分成許多的質(zhì)元，在每一質(zhì)元范圍內(nèi)，速度和加速度是相同的。于是，對(duì)于每個(gè)質(zhì)元，按牛頓第二定律可建立運(yùn)動(dòng)方程，各關(guān)節(jié)質(zhì)心及速度表達(dá)式如式(1)～(2)所示，即

m1c(0 0)，m2c(C2cosθ1C2sinθ2)；

(1)

(2)

式中：m1、m2分別為兩質(zhì)點(diǎn)的質(zhì)量；c1、c2分別為第一個(gè)質(zhì)點(diǎn)與坐標(biāo)原點(diǎn)的距離及第一個(gè)質(zhì)點(diǎn)與第二個(gè)質(zhì)點(diǎn)的距離。

根據(jù)拉格朗日公式可建立關(guān)節(jié)控制扭矩τ及拉格朗日函數(shù)L之間的關(guān)系，即

(3)

根據(jù)系統(tǒng)動(dòng)能可知

T=T1+T2，

(4)

式中：T1是關(guān)節(jié)1總動(dòng)能；T2是關(guān)節(jié)2總動(dòng)能；JZ1是關(guān)節(jié)1轉(zhuǎn)動(dòng)慣量；JZ2是連桿的轉(zhuǎn)動(dòng)慣量；m2是連桿的總質(zhì)量；C2是從關(guān)節(jié)軸到連桿質(zhì)量中心的距離；ω是其相應(yīng)角速度。

1.2.2 雙連桿動(dòng)力學(xué)模型

圖5是雙連桿機(jī)械臂示意圖，各關(guān)節(jié)質(zhì)心和速度表達(dá)式如式(5)～(6)所示，即

m1c(0 0)、m2c(C2cosθ1C2sinθ1)、m3c(C3cosθ1C3sinθ1)、m4c(C3cosθ1+C4cos(θ1+θ2)C3sinθ1+C4sin(θ1+θ2))，

(5)

(6)

(a) 雙連桿位置關(guān)系

轉(zhuǎn)動(dòng)慣量如圖6所示。

(a) 中心軸線轉(zhuǎn)動(dòng)

m1、m3視作空心圓柱體(繞其中的軸線轉(zhuǎn)動(dòng))，空心圓柱體圍繞中心軸線轉(zhuǎn)動(dòng)(見(jiàn)圖6(a))時(shí)的轉(zhuǎn)動(dòng)慣量Jz可表示為

(7)

m2、m4視作空心圓柱體，空心圓柱體的轉(zhuǎn)動(dòng)軸為中心直徑時(shí)(見(jiàn)圖6(b))，它的轉(zhuǎn)動(dòng)慣量Jz可表示為

(8)

根據(jù)拉格朗日公式建立關(guān)節(jié)控制扭矩τ及拉格朗日函數(shù)L之間的關(guān)系式，即

(9)

系統(tǒng)動(dòng)能為

(10)

1.3 機(jī)械臂功能包配置

由于機(jī)械臂目前只具備模型結(jié)構(gòu)，故在獲取URDF文件后，還需對(duì)其進(jìn)行功能包配置以實(shí)現(xiàn)其控制功能。本文在RVIZ環(huán)境下對(duì)其進(jìn)行配置，使用roslaunch moveit_setup_assistant setup_assistant.launch命令來(lái)對(duì)機(jī)械臂進(jìn)行配置，在對(duì)Self_Collisions進(jìn)行配置時(shí)，需要使用Generate Collision Matrix插件進(jìn)行碰撞檢測(cè)，如圖7a所示；生成碰撞矩陣，以確保在規(guī)劃過(guò)程中關(guān)節(jié)不會(huì)相互碰撞，若在檢測(cè)過(guò)程中發(fā)生碰撞再次進(jìn)行調(diào)試直至不發(fā)生碰撞，如圖7b所示。在RVIZ環(huán)境下配置完成之后會(huì)生成一系列文件，如圖7c所示[9]。

(a) 機(jī)械臂基本配置

2 ROS控制系統(tǒng)設(shè)計(jì)

2.1 Moveit控制架構(gòu)設(shè)計(jì)

傳統(tǒng)機(jī)械臂需要通過(guò)各個(gè)廠家定義的不同編程語(yǔ)言來(lái)控制機(jī)械臂，需要逐個(gè)動(dòng)作進(jìn)行示教編程，使用起來(lái)不方便。而Moveit是一種任務(wù)型的編程，比如讓機(jī)械臂到達(dá)某個(gè)目標(biāo)點(diǎn)，只需給機(jī)械臂一個(gè)指令，達(dá)到某個(gè)目標(biāo)點(diǎn)即可。而中間的避障環(huán)節(jié)以及外部視覺(jué)的集成是由Moveit自動(dòng)幫助使用者完成，使得機(jī)器人的交互變得更加友好[9]。

ROS機(jī)械臂控制系統(tǒng)基于Moveit架構(gòu)及其插件，move_group是Moveit的核心節(jié)點(diǎn)，相當(dāng)于一個(gè)功能集散地的中心，負(fù)責(zé)所有功能的調(diào)用。其上位機(jī)的控制可以通過(guò)C++、python這種編程式的方法，也可以通過(guò)RVIZ(三維可視化空間)進(jìn)行機(jī)械臂的拖拽以及命令行的方法進(jìn)行人機(jī)交互。move_group隨之會(huì)接受這些命令，然后告訴機(jī)械臂從當(dāng)前位置到達(dá)指定位置。一般的軌跡規(guī)劃算法包括CHOMP、OMPL、SBPL，而在ROS中默認(rèn)的算法是OMPL。對(duì)于機(jī)械臂運(yùn)動(dòng)過(guò)程中的避障問(wèn)題，一般有FCL和PCD算法來(lái)解決，如圖8所示[6-7]。

圖8 Moveit流程設(shè)計(jì)

2.2 Moveit算法封裝及功能

具體運(yùn)動(dòng)規(guī)劃算法是由Moveit提供的Moveit_core package標(biāo)準(zhǔn)協(xié)議接口planning_interface.h進(jìn)行封裝。其中包含3個(gè)重要角色PlannerConfigurationSettings、PlanningContext、PlannerMange需要實(shí)現(xiàn)：PlannerConfigurationSettings功能是配置運(yùn)動(dòng)規(guī)劃插件中算法所需要的參數(shù)，PlanningContext功能是調(diào)用具體算法的位置及規(guī)劃算法的具體實(shí)現(xiàn)，PlannerManger功能是向Moveit注冊(cè)插件并告訴Moveit此插件的功能，如圖9所示。在對(duì)PlanningContext進(jìn)行繼承時(shí)，需要對(duì)其子類功能進(jìn)行實(shí)現(xiàn)。需要實(shí)現(xiàn)的4個(gè)功能函數(shù)包含2個(gè)solve函數(shù)(MotionPlanResponse和MotionPlanDetailedResponse)，目的是解決不同輸入條件下的軌跡規(guī)劃算法調(diào)用功能，terminate函數(shù)解決規(guī)劃過(guò)程中服務(wù)停止問(wèn)題，clear函數(shù)解決軌跡規(guī)劃結(jié)束后將數(shù)據(jù)清理問(wèn)題。

圖9 move_group算法封裝

3 機(jī)械臂運(yùn)動(dòng)規(guī)劃

3.1 機(jī)械臂添加控制器插件

由于機(jī)械臂的功能要求多，自由度大，而且對(duì)運(yùn)動(dòng)精度和響應(yīng)速度的要求較高，因此會(huì)造成計(jì)算量大、周期短等特點(diǎn)。為此，ROS提供了一系列的控制器接口、硬件接口、傳動(dòng)裝置接口、控制器工具箱等，能夠高效地提高開(kāi)發(fā)者地工作效率。文中在對(duì)Rocr6機(jī)械臂進(jìn)行控制時(shí)會(huì)加入arbotix插件。先對(duì)launch啟動(dòng)文件夾加載arbotix啟動(dòng)節(jié)點(diǎn),然后在config文件夾中修改joint_limits機(jī)械臂的關(guān)節(jié)位置、速度和加速度信息,并在ros_controller.yaml文件夾中編輯配置節(jié)點(diǎn)的發(fā)布狀態(tài)和頻率信息[10]，如圖10所示。

圖10 機(jī)械臂控制器插件

3.2 機(jī)械臂運(yùn)動(dòng)規(guī)劃結(jié)果

RVIZ+GAZEBO聯(lián)合仿真如圖11所示，初始位置設(shè)為home，機(jī)械臂目標(biāo)位置設(shè)為d30，機(jī)械臂通過(guò)OMPL算法自動(dòng)到達(dá)d30點(diǎn)。通過(guò)錄制joint_states話題，生成bag文件包，該文件包包括仿真時(shí)機(jī)械臂在進(jìn)行運(yùn)動(dòng)規(guī)劃時(shí)的關(guān)節(jié)狀態(tài)。然后使用clock工具生成時(shí)間變化曲線，得到如圖12所示的關(guān)節(jié)變化曲線。結(jié)果表明，整個(gè)機(jī)械臂在運(yùn)動(dòng)過(guò)程中可以實(shí)現(xiàn)平滑的軌跡過(guò)渡，從而避免機(jī)器震動(dòng)引發(fā)故障。在對(duì)報(bào)廢客車殼體拆解時(shí)根據(jù)實(shí)際切割要求，可直接利用ROS提供的語(yǔ)言接口載入具體的直線、圓弧軌跡規(guī)劃及相關(guān)軌跡插補(bǔ)。

圖11 機(jī)械臂運(yùn)動(dòng)過(guò)程

圖12 機(jī)械臂各關(guān)節(jié)角度隨時(shí)間變化曲線

4 結(jié)語(yǔ)

通過(guò)在ROS環(huán)境下對(duì)開(kāi)源機(jī)械臂Roc6進(jìn)行動(dòng)力學(xué)建模及分析，然后使用Moveit對(duì)機(jī)械臂進(jìn)行配置啟動(dòng)并使用其運(yùn)動(dòng)規(guī)劃算法插件對(duì)其算法進(jìn)行封裝，調(diào)用OMPL算法對(duì)機(jī)械臂進(jìn)行軌跡運(yùn)動(dòng)規(guī)劃，最后使用rqt_plot插件對(duì)機(jī)械臂進(jìn)行軌跡規(guī)劃分析。試驗(yàn)中對(duì)開(kāi)源機(jī)械臂Rocr6進(jìn)行了碰撞檢測(cè)，各關(guān)節(jié)的link、運(yùn)動(dòng)學(xué)建模、功能包配置，設(shè)計(jì)了機(jī)械臂運(yùn)動(dòng)規(guī)劃程序執(zhí)行任務(wù)、rqt_plot可視化分析關(guān)節(jié)變化曲線。通過(guò)上述一系列工作流程，在ROS中實(shí)現(xiàn)了機(jī)械臂對(duì)報(bào)廢客車殼體切割運(yùn)動(dòng)規(guī)劃，全程符合運(yùn)動(dòng)規(guī)劃期望。該方法為報(bào)廢客車殼體拆解提供技術(shù)支持，可高效智能地對(duì)報(bào)廢客車殼體進(jìn)行拆解，同時(shí)也對(duì)工業(yè)機(jī)械臂在ROS下的開(kāi)發(fā)應(yīng)用有重要借鑒意義。