閻思達 陳雁 馬振利 華衛(wèi)星 陳詩明 陳文卓

基金項目：國防科研基金“加油機器人關鍵技術與虛擬樣機設計研究”（批準號：YX216J021）資助的課題；國家自然科學基金（批準號：51505494）資助的課題。

作者簡介：閻思達（1993-），工程師，從事機器人及油料調運理論與技術研究。

通訊作者：陳雁（1972-），教授，博導，從事機器人及油料裝備理論與技術的研究，315926350@qq.com。

引用本文：閻思達，陳雁，馬振利，等.加油機器人軌跡規(guī)劃[J].化工自動化及儀表，2023，50（6）：000-000.

DOI：10.20030/j.cnki.1000-3932.202306000

摘? 要? 軌跡規(guī)劃是加油機器人研究過程中必須解決的一個關鍵問題。以五自由度加油機器人為研究對象，加油機器人末端執(zhí)行器的作業(yè)任務規(guī)劃為準備、開門、插管、退管、關門、復位共6個運動過程。軌跡生成采用五次多項式插值方法，在關節(jié)空間生成點到點的運動軌跡，在笛卡爾空間生成連續(xù)路徑運動軌跡。復位、關門和退管過程沿規(guī)劃好的準備、開門和插管3個過程軌跡進行逆向運動。仿真表明：生成的軌跡能使機器人按既定路徑運動，順利完成無碰撞加油過程，軌跡規(guī)劃方法合理可行。

關鍵詞? 加油機器人? 軌跡? 規(guī)劃? 末端執(zhí)行器? 關節(jié)

中圖分類號? TP24? ? ? 文獻標志碼? A? ? ? 文章編號? 1000-3932（2023）06-0000-00

加油機器人是未來智能化加油站的關鍵設備[1～4]。其軌跡規(guī)劃是根據(jù)加油作業(yè)需求完成末端執(zhí)行器的任務規(guī)劃和路徑規(guī)劃，計算出機器人各關節(jié)的位移、速度和加速度，生成運動軌跡[5]。軌跡規(guī)劃不合理，輕則導致無法完成加油作業(yè)，重則損壞加油機器人和車輛。加油機器人的研究涉及汽車加油、航天衛(wèi)星加油[6]、艦艇加油[7]及飛機加油[8]等應用場景。

國內外對加油機器人的研究有很多，文獻[9，10]研究了加油機器人的機構組成和運動學問題。文獻[11，12]進行了加油機器人機械設計和動力學問題研究。文獻[13，14]對加油機器人雙目視覺系統(tǒng)進行了研究。文獻[15]采用加油機器人擬動力試驗法規(guī)避了工業(yè)機器人的接觸反應速度限制。文獻[16～18]分別研發(fā)了不同的加油機器人系統(tǒng)。文獻[19，20]進行了加油機器人操作臂設計和系統(tǒng)布局研究，并用軌跡規(guī)劃的插值函數(shù)進行了簡單分析。然而，這些研究都未結合加油作業(yè)需求深入探討軌跡規(guī)劃方法。因此，筆者針對五自由度加油機器人結合加油作業(yè)需求深入研究軌跡規(guī)劃方法。

1? 任務規(guī)劃

加油機器人為五自由度關節(jié)型機器人，汽車加油口為無蓋專用加油口，采用D-H法建立的連桿坐標系如圖1所示。固定不動的基座坐標系{0}為參考坐標系，末端執(zhí)行器上的坐標系包括吸盤坐標系{s}和加油管坐標系{n}，加油管插入油箱的專用加油口加油，吸盤用于打開和關閉油箱門。

加油機器人末端執(zhí)行器作業(yè)任務可規(guī)劃為6個過程：

a. 準備，即為打開油箱門，吸盤末端由初始位姿（圖1）運動到油箱門處；

b. 開門，即吸盤打開汽車油箱門；

c. 插管，即加油管插入加油口；

d. 退管，即加油管拔出加油口；

e. 關門，即吸盤關閉汽車油箱門；

f. 復位，即加油機器人回到初始位姿。

復位、關門和退管過程沿規(guī)劃好的準備、開門和插管3個過程軌跡逆向運動。

2? 路徑規(guī)劃

2.1? 準備階段路徑

在準備過程中，吸盤末端由初始位置（圖1）運動到距油箱門L1（本研究L1取200 mm）點P1處（圖2）。此時吸盤坐標系與油箱門坐標系方向相同，軸共線。原點之間在X軸方向上距離為L1。

吸盤坐標系在基座坐標系中的位姿矩陣為：

（1）

準備階段運動過程中無約束，為點到點運動。

2.2? 開門階段路徑

2.2.1? 到達油箱門

加油機器人末端吸盤沿著軸做直線運動，由P1點（圖2）運動至油箱門D處（圖3）。此時吸盤坐標系與油箱門坐標系在基座坐標系中的位姿相同。

2.2.2? 打開油箱門

吸盤將油箱門吸住后，其末端繞油箱門旋轉軸做一個90°圓弧軌跡運動，打開油箱門，吸盤末端到達D1點（圖4）。

油箱門旋轉軸線坐標系{k}的Z軸與油箱門旋轉軸線共線，當油箱門旋轉角度為零時，坐標系{k}與油箱門坐標系方向相同，如圖5所示。

旋轉過程中油箱門坐標系在基座坐標系中的位姿和吸盤末端到達D1點時吸盤坐標系在基坐標系中的位姿分別為：

（2）

（3）

其中，為油箱門旋轉軸線坐標系在基座坐標系中的位姿矩陣。

該過程的起始和終止狀態(tài)已知，運動路徑是一個圓弧，為連續(xù)路徑運動。

2.3? 插管階段路徑

2.3.1? 脫離油箱門

首先吸盤末端沿油箱門坐標系的軸反方向做直線運動，到達距油箱門前L2（本研究中L2取200 mm）的點P2處（圖6a）。該過程已知起始和終點狀態(tài)，運動路徑為直線，為連續(xù)路徑運動。此時吸盤坐標系與油箱門坐標系方向相同，X軸共線。原點之間在X軸方向上的距離為L2。吸盤坐標系在基座坐標系的位姿為：

（4）

a

b

c

2.3.2? 到達加油準備點P3

隨后使加油管末端到達距加油口L3（本研究L3取200 mm）的點P3處（圖6b）。該過程已知起始和終點狀態(tài)，運動路徑無約束，因此為點到點運動。此時加油管坐標系和加油口坐標系方向相同，X軸共線。原點之間在X軸方向上的距離為L3。加油管坐標系在基座坐標系中的位姿為：

（5）

其中，為加油口坐標系{H}在基座坐標系中的位姿。

2.3.3? 插入加油口

加油管末端向前運動，插入加油口100 mm，到達油箱內H1處（圖6c）。此時加油管坐標系{n}在基座坐標系中的位姿為：

（6）

該過程已知起始和終點狀態(tài)，運動路徑無約束，為點到點運動。油箱加滿油后，加油機器人按照插管、開門和準備運動規(guī)劃進行逆向運動，即分別完成退管、關門和復位過程。

3? 軌跡生成

采用5次多項式插值方法，在關節(jié)空間生成點到點運動軌跡，在笛卡爾空間生成連續(xù)路徑運動軌跡。

3.1? 關節(jié)空間軌跡生成

在對路徑無障礙下的點到點運動進行軌跡規(guī)劃時，采用5次多項式插值在關節(jié)空間進行軌跡規(guī)劃。關節(jié)變量、關節(jié)速度和加速度分別為：

（7）

（8）

（9）

令和分別表示起始點和終止點的關節(jié)角，和表示起始點和終止點的關節(jié)角速度，和表示起始點和終止點的關節(jié)角加速度。代入式（7）～（9）可得6個約束條件：

（10）

為了使得關節(jié)速度和加速度連續(xù)，需滿足，，，。則求解可得：

3.2? 笛卡爾空間直線軌跡生成

加油機器人末端做直線運動時，已知直線起始坐標為（x0，y0，z0），終止坐標為（xz，yz，zz），運動時間為，末端軸線的方向保持不變。將基座原點O到機器人末端的矢徑在笛卡爾坐標系中用時間的5次多項式表示。矢徑、速度和加速度分別為：

（11）

（12）

（13）

令，；，；，分別表示起始點和終止點在基座坐標系的向量、末端速度矢量和加速度。為了使速度和加速度連續(xù)，令，；，。則各系數(shù)分別為：

設時間步長為，將運行時間分成個點，則每個時間點i（）的坐標為：

（14）

根據(jù)運動學逆解，計算得出時的各關節(jié)轉角。然后用數(shù)值微分法計算和：

（15）

（16）

3.3? 笛卡爾空間圓弧軌跡規(guī)劃

當進行開油箱門的作業(yè)任務時，吸盤末端需要繞加油口軸線做一個弧度為90°的圓弧軌跡。油箱門角度隨時間t的變化寫成5次多項式函數(shù)：

（17）

已知開油箱門起止狀態(tài)：，，為使速度和加速度連續(xù)，令，，，。則插值系數(shù)為：

設時間步長為，將運行時間分成個點。運行過程中每個時間點油箱門的角度為（）。然后由式（2）計算得出每個時間點油箱門坐標系的位姿。根據(jù)運動學逆解，計算得出時的各關節(jié)轉角。最后用數(shù)值微分法計算和：

（18）

（19）

4? 仿真

以具體的加油實例，對加油機器人進行軌跡規(guī)劃。

加油機器人關節(jié)轉角初始參數(shù)為（0，0，90°，-90°，0）。油箱門坐標系{D}、油箱門旋轉軸坐標系{k}和加油口坐標系{H}在基座坐標系中的位姿參數(shù)分別為：

4.1? 準備階段軌跡

初始狀態(tài)已知，采用關節(jié)空間軌跡生成方法用時6 s，得到該段各關節(jié)角隨時間變化規(guī)律如圖7所示。

4.2? 開門階段軌跡

P1–D段軌跡采用笛卡爾空間直線軌跡生成方法，用時2 s，得到該段各關節(jié)角隨時間的變化規(guī)律，如圖8所示。D–D1段軌跡采用笛卡爾空間圓弧軌跡生成方法，用時4 s，得到該段各關節(jié)角隨時間的變化規(guī)律，如圖9所示。

4.3? 插管階段軌跡

D1–P2段軌跡采用笛卡爾空間直線軌跡生成方法，用時1s，得到該段各關節(jié)角隨時間的變化規(guī)律，如圖10所示。P2–P3段軌跡采用關節(jié)空間軌跡生成方法，用時5 s，得到該段各關節(jié)角隨時間的變化規(guī)律，如圖11所示。P3–H1段軌跡采用笛卡爾空間直線軌跡生成方法，用時2 s該段各關節(jié)角隨時間的變化規(guī)律，如圖12所示。

4.4? 仿真實驗

在SolidWorks中進行仿真驗證，準備階段吸盤運動路徑如圖13a所示，開門階段的吸盤運動軌跡如圖13b所示，插管過程吸盤運動路徑如圖13c所示，加油管的運動路徑如圖13d所示。

仿真實驗表明，根據(jù)上述軌跡規(guī)劃方法得到的軌跡能夠使機器人按既定的路徑運動完成加油過程，無碰撞發(fā)生。

5? 結束語

針對五自由度加油機器人提出了一種結合加油作業(yè)需求的軌跡規(guī)劃方法。加油機器人末端執(zhí)行器的作業(yè)任務規(guī)劃為6個運動過程，并分別進行路徑規(guī)劃。軌跡生成采用5次多項式插值方法，在關節(jié)空間生成點到點運動的軌跡，在笛卡爾空間生成連續(xù)路徑運動的軌跡。復位、關門和退管過程沿規(guī)劃好的準備、開門和插管3個過程軌跡進行逆向運動。仿真實驗表明，加油機器人軌跡規(guī)劃方法合理可行。

參? 考? 文? 獻

[1] 陳雁，陳文卓，黎波，等.汽車加油機器人關鍵技術分析[J].后勤工程學院學報，2015，31（1）：56-61；67.

[2] 王寧.加油站自助加油的思考[J].中國石油企業(yè)，2011（7）：86-87.

[3] 田景惠.國外加油站的發(fā)展趨勢及其對我們的啟示[J].國際石油經濟，2007（2）：55-58；83.

[4] KOCH W H.Is the Long Wait for Robotic Fueling Finally Over？[EB/OL].[2014-04-05].http：//t-r-i.comgifs/xPET5-97.pdf.

[5] 陳文卓.汽車智能加油機構設計研究[D].重慶：后勤工程學院，2015.

[6] HILL C，SCHLUTZ J，F(xiàn)INK A，et al.Explore：Technology and process demonstration for orbital refueling on a sounding rocket[C]//Proceedings of 20th Symposium on European Rocket and Balloon Programmers and Related Research.Paris，F(xiàn)rance：The European Space Agency，2011：121-128.

[7] GALWAY B，HARRIS B.Unmanned sea surface vehicle（USSV）motion data and refueling equipment design[R].Virginia，US：Naval Surface War?fare Center Carderock Division，2010：1-20.

[8] MART?NEZ C，RICHARDSON T，THOMAS P，et al.A vision?based strategy for autonomous aerial refueling tasks[J].Robotics and Autonomous Systems，2013，61（8）：876-895.

[9] 劉玉芝，曾紅，李大為.加油機器人結構、運動學問題的研究[J].佳木斯大學學報（自然科學版），2004，22（3）：334-337.

[10] 祝冉，李著信，蘇毅，等.加油機器人操作臂設計及運動學研究[J].后勤工程學院學報，2013，29（2）：59-63.

[11] 劉慶明，高雪官，張月文.自動加油機器人的關鍵技術設計分析[J].機械設計與研究，2020，36（1）：17-21.

[12] 鄒子杰，高雪官.自動加油機器人的運動控制與仿真[J].機電一體化，2021，27（6）：3-10.

[13] 馬智.基于雙目視覺的汽車油箱蓋識別與檢測技術研究[D].長春：吉林大學，2022.

[14] 梁銘裕.汽車加油機器人的雙目視覺系統(tǒng)研究[D].廣州：華南理工大學，2021.

[15] BOLIEN M，IRAVANI P，BOIS J D.Towards Robotic Pseudo-Dynamic Testing for Hybrid Simulations of Air-to-Air Refueling[J].IEEE/ASME Transactions on Mechatronics，2017，22（2）：1004-1013.

[16] SCOTT G P，HENSHAW C G，WALKER I D，et al.Autonomous robotic refueling of an unmanned surface vehicle in varying sea states[C]//Proceedings of IEEE International Conference on Intelligent Robots and Systems.Hamburg，Germany：IEEE，2015：1664-1671.

[17] ROTEC ENGINEERING：Tankpitstop Refueling Robot[EB/OL].[2014-04-05].http：//rotec-engineering.nl/en/tankpitstop-refueling-robot.

[18] Automatic Refueling Systems Remove Hassles at the Gas Pump[EB/OL].[2014-04-05].http：//finance.yahoo.com/news/automatic-refueling-systems-remove-hassles-090000963.html.

[19] 石文凱，馬振利，胡道達.加油機器人系統(tǒng)布局及機械臂尺寸選取[J].自動化與儀器儀表，2016（4）：100-102.

[20] 石文凱，馬振利，劉洲.加油機器人操作臂設計、運動學研究及軌跡規(guī)劃[J].四川兵工學報，2015，36（10）：80-84.

（收稿日期：2023-01-16，修回日期：2023-04-07）