加油機器人軌跡規劃

閻思達 陳雁 馬振利 華衛星 陳詩明 陳文卓

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

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

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

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

DOI：10.20030/j.cnki.1000-3932.202306000

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

關鍵詞? 加油機器人? 軌跡? 規劃? 末端執行器? 關節

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

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

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

1? 任務規劃

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

加油機器人末端執行器作業任務可規劃為6個過程：

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

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

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

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

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

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

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

2? 路徑規劃

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）

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

該過程的起始和終止狀態已知，運動路徑是一個圓弧，為連續路徑運動。

2.3? 插管階段路徑

2.3.1? 脫離油箱門

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

（4）

a

b

c

2.3.2? 到達加油準備點P3

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

（5）

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

2.3.3? 插入加油口

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

（6）

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

3? 軌跡生成

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

3.1? 關節空間軌跡生成

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

（7）

（8）

（9）

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

（10）

為了使得關節速度和加速度連續，需滿足，，，。則求解可得：

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

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

（11）

（12）

（13）

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

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

（14）

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

（15）

（16）

3.3? 笛卡爾空間圓弧軌跡規劃

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

（17）

已知開油箱門起止狀態：，，為使速度和加速度連續，令，，，。則插值系數為：

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

（18）

（19）

4? 仿真

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

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

4.1? 準備階段軌跡

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

4.2? 開門階段軌跡

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

4.3? 插管階段軌跡

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

4.4? 仿真實驗

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

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

5? 結束語

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

參? 考? 文? 獻

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

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

[3] 田景惠.國外加油站的發展趨勢及其對我們的啟示[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，FINK 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，France：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] 梁銘裕.汽車加油機器人的雙目視覺系統研究[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] 石文凱，馬振利，胡道達.加油機器人系統布局及機械臂尺寸選取[J].自動化與儀器儀表，2016（4）：100-102.

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

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