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

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

0 引言

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

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

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

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

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

1.1 控制部分

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

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

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

1.2 供電部分

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

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

1.3 傳動部分

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

2 多軸同步控制原理

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

2.1 工藝對象配置方法

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

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

圖2 工藝對象配置畫面

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

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

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

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

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

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

圖3 工藝對象配置效果

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

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

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

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

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

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

運(yùn)動坐標(biāo)系（KCS）：KCS連接至運(yùn)動機(jī)構(gòu)，KCS 在運(yùn)動機(jī)構(gòu)內(nèi)的位置針對預(yù)定義的運(yùn)動機(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 多軸同步控制

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

2.3.1 線性運(yùn)動

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

2.3.2 圓周指令

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

2.3.3 同步“點(diǎn)對點(diǎn)”運(yùn)動

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

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

圖7 同步“點(diǎn)對點(diǎn)”運(yùn)動時序

圖8 路徑規(guī)劃

通過作業(yè)MC_MoveDirectAbsolute（A1）移動運(yùn)動系統(tǒng)，在時間點(diǎn)①，啟動額外的MC_MoveDirectAbsolute 作業(yè)（A2），在時間點(diǎn)②，通過Done1 發(fā)送作業(yè)A1 完成的信號，并啟動作業(yè)A2，Axis_1 和Axis_2 同時運(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)動機(jī)構(gòu)仿真和追蹤

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

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

3 應(yīng)用

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

4 結(jié)論

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

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