[发明专利]一种自动对位换油机器人的建模与控制方法

权利要求书

1.一种自动对位换油机器人的建模与控制方法，所述自动对位换油机器人包括横行桁架、水平平移轴、转动轴、装卸油鹤管组件以及密封帽；

其中，水平平移轴安装在横行桁架上并可在横行桁架上水平移动，转动轴安装在水平平移轴上，转动轴上安装卸油鹤管组件，转动轴可带动卸油鹤管组件做旋转运动，卸油鹤管组件包括鹤管内臂和鹤管外臂；鹤管外臂的末端为卸油鹤管注油口，卸油鹤管注油口位置设置有密封帽；

建模与控制方法的具体步骤如下：

【1】自动对位换油机器人建模

【1.1】定义坐标系；

定义坐标系坐标系的原点为水平平移轴与横行桁架的连接点；水平平移轴沿横向桁架向右运动的方向为X₁轴正方向，垂直向上方向定为Z₁轴正方向，右手定则确定Y₁轴；

定义坐标系坐标系的原点为转动轴中心点，Z₂轴为沿转动轴方向，X₂轴与X₁轴同向，右手定则确定Y₂轴；

定义坐标系坐标系的原点为鹤管注油口中心点，坐标系各坐标轴的方向与坐标系各坐标轴的方向一致；

建立空间基坐标系空间基坐标系的原点定为横行桁架的左侧起始点；空间基坐标系各坐标轴的方向和坐标系各坐标轴的方向一致；

【1.2】计算鹤管注油口中心点在坐标系下的坐标(X,Y,Z,A,B,C)；具体计算公式为：

其中，S表示的是坐标系沿X₀轴正方向移动的距离；

a表示坐标系原点和坐标系原点之间的距离在Y₁轴上的投影；

b表示坐标系原点和坐标系原点之间的距离在Z₁轴上的投影；

c表示坐标系原点和坐标系原点之间的距离在Z₂轴上的投影；

L₁为鹤管内臂长度，L₂为鹤管外臂长度，θ为鹤管内臂转动的角度；

【1.3】计算出水平平移轴移动的距离和转动轴转动的角度：

【2】调试阶段

【2.1】记录待机位置信息

自定义待机位置，操作转动轴使其处于垂直状态，操作水平平移轴使其运动到待机位置，根据当前水平移动的距离S₀和当前鹤管内臂转动角度θ₀，θ₀＝90°，应用步骤【1.2】中的公式，计算机器人移动到待机位置时，注油口位置的坐标(X₀,Y₀,Z₀,A₀,B₀,C₀)，并记录；

【2.2】记录扫描起点位置信息

操作水平平移轴使其运动到扫描起点位置，转动轴仍保持垂直状态，根据当前水平移动的距离S_s和当前鹤管内臂转动角度θ₀，θ₀＝90°，应用【1.2】中的公式，计算机器人移动到扫描起点位置时，注油口位置的坐标(X_s,Y_s,Z_s,A_s,B_s,C_s)，并记录；

【2.3】记录扫描终点位置信息

操作水平平移轴使其运动到扫描终点位置，转动轴仍保持垂直状态，根据当前水平移动的距离S_e和当前鹤管内臂转动角度θ₀，θ₀＝90°，应用【1.2】中的公式，计算机器人移动到扫描终点位置时，注油口位置的坐标(X_e,Y_e,Z_e,A_e,B_e,C_e)，并记录；

【2.4】记录垂直下降高度信息

设定从罐口到装卸油位置的垂直下降高度H，并记录；

【3】自动运行阶段

【3.1】读取已存储的待机位置信息(X₀,Y₀,Z₀,A₀,B₀,C₀)，机器人运动到待机位；

机器人从开机初始位置到待机位置的运动过程中需要应用【1.3】中的公式，实时计算每一时刻的S和θ，用于控制水平平移轴和转动轴运动；

【3.2】读取已存储的扫描起点位置信息(X_s,Y_s,Z_s,A_s,B_s,C_s)，机器人运动到扫描起点；

机器人从待机位置到扫描起点位置的运动过程中，转动轴一直保持垂直状态θ＝90°，水平平移轴从S₀运动到S_s；

【3.3】扫描罐口位置；

【3.3.1】运动到扫描起点后，启动激光测距仪，读取激光测距仪的输出信号，此时应为高电平；

【3.3.2】从扫描起点沿水平方向移动，当检测到激光测距仪的输出信号从高电平变为低电平时，则罐口区域被检测到，停止移动，并记录此时的水平平移轴位置S_a；

【3.3.3】继续沿水平方向运动，当检测到激光测距仪的输出信号从低电平变为高电平时，则激光测距仪已经移出罐口区域，停止移动，并记录此时的水平平移轴位置S_b；

【3.3.4】计算得到罐口中心位置S_h，

【3.3.5】根据S_h和θ＝90°，应用【1.2】中的公式，计算此时注油口位置的坐标：(X_h,Y_h,Z_h,A_h,B_h,C_h)；

【3.4】机器人运动到罐口位置，转动轴一直保持垂直状态θ＝90°，水平平移轴从S_b运动到S_h；

【3.5】根据调试阶段设定的从罐口到装卸油位置的垂直下降高度H，计算得到装卸油位置注油口的坐标(X_l,Y_l,Z_l,A_l,B_l,C_l)；

【3.6】机器人运动到装卸油位置；

机器人从罐口位置到装卸油位置的运动过程中需要应用【1.3】中的公式，实时计算每一时刻的S和θ，用于控制水平平移轴和转动轴运动，当转动轴向下转动时，水平平移轴同时向后移动，保证鹤管注油口垂直下降；

【3.7】启动装卸油的阀门，进行装卸油；

【3.8】装卸油任务完成后，机器人恢复至待机位置。

2.根据权利要求1所述的自动对位换油机器人的建模与控制方法，其特征在于：

所述自动对位换油机器人还包括感应片以及接近开关；所述感应片安装在密封帽上，所述接近开关安装在所述鹤管外臂的下部；

执行步骤【3.7】时：

当密封帽上的密封气垫和罐口之间紧密贴合时，罐口的反作用力会使感应片上移靠近接近开关，接近开关感应到感应片后输出低电平信号；

当密封气垫和罐口之间有间隙时，感应片远离接近开关，接近开关会输出高电平信号；

检测接近开关的输出信号，当低电平信号则密封气垫和罐口贴合紧密，水平平移轴和转动轴不动作，当检测为高电平信号时，则控制水平平移轴和转动轴运动，使注油口垂直下降，直到接近开关的输出信号变为低电平。