加油机器人轨迹规划

阎思达 陈雁 马振利 华卫星 陈诗明 陈文卓

基金项目：国防科研基金“加油机器人关键技术与虚拟样机设计研究”（批准号：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）