王永鵬，鄒 濤，崔永亮，孔永超，李博洋，尋 明

（1.寶雞石油機(jī)械有限責(zé)任公司，陜西寶雞 721002；2.中油國家油氣鉆井裝備工程技術(shù)研究中心有限公司，陜西寶雞 721002；3.中國石油西部鉆探工程有限公司，新疆烏魯木齊 830013）

0 引言

石油鉆機(jī)鉆臺(tái)面管柱的扶持、排放、對(duì)扣等工作具有工作量大、勞動(dòng)強(qiáng)度高、作業(yè)環(huán)境惡劣、安全風(fēng)險(xiǎn)大等特點(diǎn)，以往這些工作均由人工完成。近年來，隨著石油行業(yè)管柱自動(dòng)化設(shè)備的不斷發(fā)展，已逐步由機(jī)械代替人工，但以液壓控制為主，自動(dòng)化程度較低。電動(dòng)伺服控制相比液壓控制具有控制精度高、響應(yīng)速度快、清潔環(huán)保等優(yōu)點(diǎn)，為降低操作難度和提高設(shè)備的自動(dòng)化和智能化水平，電動(dòng)化是必由之路。

通過工藝對(duì)象創(chuàng)建運(yùn)動(dòng)機(jī)構(gòu)模型，依托運(yùn)動(dòng)機(jī)構(gòu)模型開展多軸同步控制技術(shù)研究，結(jié)合油田作業(yè)工況和工藝流程，實(shí)現(xiàn)電動(dòng)扶管機(jī)器人的復(fù)雜運(yùn)動(dòng)控制。

1 控制系統(tǒng)構(gòu)成

電動(dòng)扶管機(jī)器人的伺服控制系統(tǒng)包含運(yùn)動(dòng)控制器、伺服控制器、電源模塊、電機(jī)模塊、防爆伺服電機(jī)、管理型交換機(jī)和電纜附件等組成（圖1）。

圖1 電動(dòng)扶管機(jī)器人控制系統(tǒng)

1.1 控制部分

本系統(tǒng)的控制核心是S7-1500T 運(yùn)動(dòng)控制PLC，能夠?qū)崿F(xiàn)多軸同步控制所需的圓弧插補(bǔ)、直線插補(bǔ)、同步點(diǎn)對(duì)點(diǎn)、空間路徑規(guī)劃和軌跡仿真等功能。

管理型交換機(jī)支持IRT 等時(shí)同步功能，擁有4 個(gè)10/100 Mbps的RJ45 接口，響應(yīng)時(shí)間1 ms，抖動(dòng)時(shí)間小于1 μs，在運(yùn)動(dòng)控制器和伺服控制器之間搭建等時(shí)同步網(wǎng)絡(luò)，實(shí)現(xiàn)數(shù)據(jù)的同步傳輸。

伺服控制器為CU320 控制單元，支持PROFINET 通信和等時(shí)同步功能，用于控制各伺服單元按指定指令運(yùn)行，并將伺服單元的運(yùn)行狀態(tài)信息反饋給運(yùn)動(dòng)控制器。

1.2 供電部分

電源模塊為基本型電源模塊，實(shí)現(xiàn)整流功能，將交流電整流成直流，供給共直流母排，實(shí)現(xiàn)電能的分配?；拘碗娫茨K適用于電能無需回饋到電網(wǎng)或者電動(dòng)軸與再生軸之間的能量交換在直流母線中進(jìn)行的應(yīng)用，基本型電源模塊只能將電能從電網(wǎng)饋入直流母線，但無法反饋回到電網(wǎng)中。電源模塊內(nèi)部集成有制動(dòng)斬波器，可連接外部制動(dòng)電阻實(shí)現(xiàn)能耗制動(dòng)。

電機(jī)模塊為逆變模塊，把直流電逆變成指定電壓/頻率的交流電，從而控制電機(jī)的運(yùn)轉(zhuǎn)，并將伺服電機(jī)的編碼器信號(hào)反饋給伺服控制器，本文中電機(jī)模塊1、電機(jī)模塊2 和電機(jī)模塊3 為單軸電機(jī)模塊，每個(gè)模塊只能連接1 臺(tái)伺服電機(jī)，電機(jī)模塊4 和電機(jī)模塊5 為雙軸電機(jī)模塊，每個(gè)電機(jī)模塊可以連接2 臺(tái)伺服電機(jī)。電源模塊和電機(jī)模塊之間采用共直流母線控制技術(shù)，通過共直流母線實(shí)現(xiàn)電能的分配，系統(tǒng)集成度高，能量可以在不同的電機(jī)模塊之間交互，減少能量浪費(fèi)，達(dá)到節(jié)能效果。

1.3 傳動(dòng)部分

防爆伺服電機(jī)包括J1 行走電機(jī)、J2 回轉(zhuǎn)電機(jī)、J3 主臂電機(jī)、J4副臂電機(jī)、J5 調(diào)平電機(jī)和J6 鉗爪電機(jī)，電機(jī)功率1～5 kW，6 臺(tái)電機(jī)均為高精度防爆伺服電機(jī)，滿足石油行業(yè)防爆要求，內(nèi)置高精度編碼器，能夠?qū)崟r(shí)反饋各電機(jī)的位置、速度、扭矩等信息，同時(shí)為各定位軸提供動(dòng)力源。伺服電機(jī)驅(qū)動(dòng)連接的電缸、蝸輪蝸桿減速機(jī)和齒輪齒條等機(jī)械結(jié)構(gòu)，按指定的速度、加速度和目標(biāo)位置運(yùn)行。

2 多軸同步控制原理

電動(dòng)扶管機(jī)器人伺服控制系統(tǒng)擁有6 個(gè)執(zhí)行軸，通過工藝對(duì)象實(shí)現(xiàn)運(yùn)動(dòng)控制器和各執(zhí)行軸之間的快速連接，基于工藝對(duì)象創(chuàng)建運(yùn)動(dòng)機(jī)構(gòu)模型，依托運(yùn)動(dòng)控制模型開展多軸同步控制技術(shù)研究。

2.1 工藝對(duì)象配置方法

工藝對(duì)象配置方法相比傳統(tǒng)的報(bào)文控制和以太網(wǎng)站點(diǎn)控制方法，能夠大大降低配置的難度并簡化配置步驟，將原本在伺服控制器中進(jìn)行的復(fù)雜運(yùn)算移至運(yùn)動(dòng)控制器中，發(fā)揮運(yùn)動(dòng)控制器運(yùn)算能力強(qiáng)的優(yōu)勢。

在博圖軟件的工藝對(duì)象組里，根據(jù)提示進(jìn)行定位軸工藝對(duì)象的組態(tài)，主要包括驅(qū)動(dòng)裝置、編碼器、機(jī)械、位置限制和動(dòng)態(tài)限值等，軸1 的工藝配置界面如圖2 所示。

圖2 工藝對(duì)象配置畫面

（1）驅(qū)動(dòng)裝置：在驅(qū)動(dòng)裝置配置窗口，主要完成驅(qū)動(dòng)裝置類型、數(shù)據(jù)連接和驅(qū)動(dòng)裝置的配置。驅(qū)動(dòng)裝置類型用于選擇是PROFIdrive 驅(qū)動(dòng)裝置，還是帶模擬量接口的驅(qū)動(dòng)裝置。數(shù)據(jù)連接用于選擇是直連驅(qū)動(dòng)裝置，還是通過用戶程序中的可編輯數(shù)據(jù)塊連接，驅(qū)動(dòng)裝置用于選擇一個(gè)已經(jīng)組態(tài)的PROFIdrive 驅(qū)動(dòng)裝置。

（2）編碼器：閉環(huán)位置控制的定位軸需要編碼器反饋位置的實(shí)際值。編碼器信號(hào)通過PROFIdrive 報(bào)文發(fā)送至控制器。

（3）機(jī)械：在機(jī)械組態(tài)窗口中組態(tài)編碼器安裝類型和機(jī)械結(jié)構(gòu)減速比。

（4）位置限制：在位置限制組態(tài)窗口中組態(tài)軸的硬件限位開關(guān)和軟限位開關(guān)。

（5）動(dòng)態(tài)限值：在動(dòng)態(tài)限值組態(tài)窗口中，為定位軸組態(tài)速度、加速度、減速度和加加速度等參數(shù)的最大值。

完成軸1 的工藝對(duì)象配置后，按照相同步驟分別完成其他5 個(gè)定位軸的配置（圖3）。

圖3 工藝對(duì)象配置效果

2.2 運(yùn)動(dòng)機(jī)構(gòu)模型

運(yùn)動(dòng)機(jī)構(gòu)模型支持符合PLCopen 標(biāo)準(zhǔn)的Motion control 運(yùn)動(dòng)控制指令，通過創(chuàng)建鉸接臂式運(yùn)動(dòng)機(jī)構(gòu)模型，完成運(yùn)動(dòng)機(jī)構(gòu)各執(zhí)行軸與已創(chuàng)建工藝對(duì)象的互聯(lián)（圖4）。

圖4 運(yùn)動(dòng)機(jī)構(gòu)模型

完成運(yùn)動(dòng)機(jī)構(gòu)各執(zhí)行軸互聯(lián)后，根據(jù)電動(dòng)扶管機(jī)器人的機(jī)械結(jié)構(gòu)和控制要求，完成幾何結(jié)構(gòu)參數(shù)、動(dòng)力學(xué)參數(shù)和運(yùn)動(dòng)機(jī)構(gòu)坐標(biāo)系的配置（圖5）。

圖5 運(yùn)動(dòng)機(jī)構(gòu)坐標(biāo)系

世界坐標(biāo)系（WCS）：WCS 是運(yùn)動(dòng)機(jī)構(gòu)環(huán)境或工作空間的固定坐標(biāo)系，WCS 的零點(diǎn)是各個(gè)對(duì)象以及運(yùn)動(dòng)機(jī)構(gòu)工藝對(duì)象上的各個(gè)運(yùn)動(dòng)的基準(zhǔn)點(diǎn)。

運(yùn)動(dòng)坐標(biāo)系（KCS）：KCS連接至運(yùn)動(dòng)機(jī)構(gòu)，KCS 在運(yùn)動(dòng)機(jī)構(gòu)內(nèi)的位置針對(duì)預(yù)定義的運(yùn)動(dòng)機(jī)構(gòu)類型指定：①位置X：定義KCS 在WCS 的X 方向的平移；②位置Y：定義KCS 在WCS 的Y 方向的平移；③位置Z：定義KCS 在WCS 的Z 方向的平移；④旋轉(zhuǎn)A：定義KCS 圍繞Z 軸的旋轉(zhuǎn)；⑤旋轉(zhuǎn)B：定義KCS 圍繞Y 軸的旋轉(zhuǎn)；⑥旋轉(zhuǎn)C：定義KCS 圍繞X 軸的旋轉(zhuǎn)。

2.3 多軸同步控制

電動(dòng)扶管機(jī)器人的多軸同步控制主要包括的運(yùn)動(dòng)類型有線性運(yùn)動(dòng)、圓周運(yùn)動(dòng)和同步“點(diǎn)對(duì)點(diǎn)”運(yùn)動(dòng)。

2.3.1 線性運(yùn)動(dòng)

運(yùn)動(dòng)控制指令MC_MoveLinearAbsolute 和MC_MoveLinear－Relative 用于定義線性運(yùn)動(dòng)。MC_MoveLinearAbsolute 作業(yè)將運(yùn)動(dòng)機(jī)構(gòu)移動(dòng)到絕對(duì)位置時(shí)，會(huì)通過MC_MoveLinearRelative 作業(yè)相對(duì)于當(dāng)前位置進(jìn)行移動(dòng)。

2.3.2 圓周指令

運(yùn)動(dòng)控制指令MC_MoveCircularAbsolute 和MC_MoveCircularRelative 用于定義圓周運(yùn)動(dòng)。圓周軌跡的方向：使用圓心和角度計(jì)算圓周軌跡時(shí)，通過參數(shù)PathChoice=0 或1 定義圓周軌跡旋轉(zhuǎn)方向。

2.3.3 同步“點(diǎn)對(duì)點(diǎn)”運(yùn)動(dòng)

可采用同步“點(diǎn)對(duì)點(diǎn)”運(yùn)動(dòng)（sPTP 運(yùn)動(dòng)）的方式移動(dòng)運(yùn)動(dòng)系統(tǒng)。借助sPTP 運(yùn)動(dòng)，可以在優(yōu)化時(shí)間和運(yùn)動(dòng)的同時(shí)移動(dòng)運(yùn)動(dòng)系統(tǒng)，繞過單個(gè)位置或更改連接位置空間。運(yùn)動(dòng)系統(tǒng)沒有遵循指定的路徑，而是通過最短距離到達(dá)指定的終點(diǎn)。

可以預(yù)先確定sPTP 運(yùn)動(dòng)的軸位置或笛卡爾坐標(biāo)。單軸運(yùn)動(dòng)由同步運(yùn)動(dòng)的起始位置和目標(biāo)位置計(jì)算得出。所有運(yùn)動(dòng)系統(tǒng)軸同時(shí)移動(dòng)，并同時(shí)到達(dá)給定的目標(biāo)位置。行程時(shí)間最長的運(yùn)動(dòng)系統(tǒng)軸用于確定sPTP 運(yùn)動(dòng)的行程時(shí)間，同步“點(diǎn)對(duì)點(diǎn)”時(shí)序如圖7 所示。

圖7 同步“點(diǎn)對(duì)點(diǎn)”運(yùn)動(dòng)時(shí)序

圖8 路徑規(guī)劃

通過作業(yè)MC_MoveDirectAbsolute（A1）移動(dòng)運(yùn)動(dòng)系統(tǒng)，在時(shí)間點(diǎn)①，啟動(dòng)額外的MC_MoveDirectAbsolute 作業(yè)（A2），在時(shí)間點(diǎn)②，通過Done1 發(fā)送作業(yè)A1 完成的信號(hào)，并啟動(dòng)作業(yè)A2，Axis_1 和Axis_2 同時(shí)運(yùn)行到目標(biāo)位置。

2.4 路徑規(guī)劃

路徑規(guī)劃是在笛卡爾坐標(biāo)下完成的，路徑由點(diǎn)、點(diǎn)與點(diǎn)之間的直線或圓弧，還有過渡區(qū)域等標(biāo)準(zhǔn)元素組成。

通過上位機(jī)控制界面，將路徑上過程點(diǎn)的世界坐標(biāo)系（WCS）或機(jī)械坐標(biāo)系（MCS）坐標(biāo)輸入到控制系統(tǒng)，并預(yù)設(shè)各段路徑的速度、加速度、減速度和加加速度值（圖9）。

圖9 路徑坐標(biāo)規(guī)劃界面

2.5 運(yùn)動(dòng)機(jī)構(gòu)仿真和追蹤

通過運(yùn)動(dòng)機(jī)構(gòu)軌跡追蹤功能，能夠?qū)⑦\(yùn)動(dòng)機(jī)構(gòu)3D 可視化，實(shí)時(shí)記錄運(yùn)動(dòng)軌跡并實(shí)時(shí)顯示工具的坐標(biāo)值（圖10）。通過運(yùn)動(dòng)軌跡追蹤功能，能夠?qū)⒎抡鏈y試結(jié)果可視化，第一時(shí)間發(fā)現(xiàn)仿真測試過程中出現(xiàn)的問題，提高設(shè)計(jì)開發(fā)效率。

圖10 運(yùn)動(dòng)機(jī)構(gòu)追蹤

3 應(yīng)用

在完成電動(dòng)扶管機(jī)器人多軸同步控制系統(tǒng)開發(fā)和仿真測試后，在工程樣機(jī)上開展調(diào)試工作。經(jīng)過數(shù)個(gè)月的調(diào)試、優(yōu)化和試驗(yàn)，電動(dòng)扶管機(jī)器人的功能和可靠性得到初步驗(yàn)證。工程樣機(jī)已配套國內(nèi)首臺(tái)7000 m“一鍵式”人機(jī)交互自動(dòng)化鉆機(jī)，完成三口井的鉆井作業(yè)，應(yīng)用效果良好，設(shè)備高度的自動(dòng)化和智能化水平，大大降低現(xiàn)場作業(yè)人員的操作難度和勞動(dòng)強(qiáng)度。

4 結(jié)論

電動(dòng)扶管機(jī)器人伺服控制系統(tǒng)采用全電動(dòng)伺服控制，應(yīng)用共直流母線技術(shù)，實(shí)現(xiàn)能量在各控制模塊之間的分配，達(dá)到節(jié)能效果。采用工藝對(duì)象控制方法，搭建電動(dòng)扶管機(jī)器人運(yùn)動(dòng)機(jī)構(gòu)模型，結(jié)合現(xiàn)場作業(yè)工況和作業(yè)流程，開發(fā)出多軸同步算法模型，突破了電動(dòng)扶管機(jī)器人多軸同步控制難題，同時(shí)借助仿真平臺(tái)，提高仿真測試效率，縮短開發(fā)周期。

目前已經(jīng)完成樣機(jī)制造和油田工業(yè)化應(yīng)用試驗(yàn)，產(chǎn)品功能和可靠性得到驗(yàn)證，通過電動(dòng)扶管機(jī)器人的研制，大大提升提高公司產(chǎn)品競爭力，提高石油行業(yè)管柱自動(dòng)化和智能化水平，為后續(xù)更加復(fù)雜的多軸同步控制打好基礎(chǔ)。