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

摘要

本发明涉及了一种自动对位换油机器人的建模与控制方法，该控制方法能够获取机器人在各个姿态下的注油口坐标值，实现了整个装卸油动作的高精度、全自动的操作。该方法的主要步骤如下：【1】自动对位换油机器人建模；【2】调试阶段；【3】自动运行阶段。

主权项

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】装卸油任务完成后，机器人恢复至待机位置。