一种基于力矩控制的管道清洗机器人的控制方法
【技术领域】
[0001]本发明涉及管道清洗运动控制技术及其控制系统设计，具体涉及一种基于力矩控制的管道清洗机器人的控制方法。
【背景技术】
[0002]烟道是宾馆、酒楼、饭店厨房的基础设施，极易藏污纳垢，是清理的死角。同时烟道是消防、卫生、防疫部门检查的重点区域。同时，烟罩内壁易寄生老鼠、蟑螂，不符合国家卫生标准。油腻胶状物长期扒附在金属表面，还会腐蚀金属材料，缩短烟道使用寿命。油烟管道有很多油渍长时间残留在烟机上和烟道表面上形成油垢，这样就会影响油烟机、烟道的使用，使烟流不易排除。厨房排烟不畅有80%左右的原因，都是由于长时间没有对油烟净化器的内置过滤网进行清洁，油污堵塞过滤网而造成的。烟道、烟罩常年处于高温工作环境，油烟气化形成积油、积炭或油腻的胶状物，如果不及时对烟道及烟罩进行清洗，非常容易引起火灾。
[0003]现在，继中央空调风道清洗机器人的成功运用，又出现了管道清洗机器人成功解决了油烟管道清洗这一难题，使用控制系统操控的管道清洗机器人通过自身携带的摄像头，清楚掌握管道内的污染情况，可实现对管道油烟的自动清洗，但是现有的管道清洗机器人在清洗管道的过程中，一方面，因喷杆和喷嘴与管道壁的干涉会导致控制电机的堵转从而烧毁电机，从而导致管道清洗机器人中途停止清洗工作；另一方面，现有的机器人清洗喷嘴与管道壁贴合不紧，导致管道清洗效果不佳。
【发明内容】
[0004]本发明的目的旨在解决现有的管道清洗机器人清洗管道的过程中容易烧坏电机停止工作以及对管道的清洗效果不佳的技术问题，提供一种基于力矩控制的管道清洗机器人的控制方法。
[0005]本发明提供一种基于力矩控制的管道清洗机器人的控制方法，所述机器人包括喷杆及与所述喷杆相连接的喷嘴及安装在机器人上的监控摄像头，所述控制方法包括以下步骤:
读取机器人参数配置文件，并开启监控摄像头；
获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数；
输出对用于控制喷杆绕水平旋转轴水平旋转的第一伺服电机和用于控制喷杆绕竖直旋转轴竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；
输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；
判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T ;
如果是，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；
判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T ;
如果是，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；
判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T ;
如果是，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；
判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T;
如果是，所述机器人沿当前管道移动预设的行进距离；
判断机器人已经移动的距离是否大于等于设定的总距离，如果是，终止清洗工作，如果否，则进入下个周期的清洗步骤。
[0006]进一步地，所述获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数的步骤，具体为：
设定所述管道为方形管道，并设定清洗管道总长度L、每个清洗周期内的行进距离A、清洗阻力F，清洗动力S，以及喷嘴在管道壁面上的行走速度V ;
根据监控摄像头采集的管道视频信号设定清洗模式为轻油污清洗模式或重油污清洗模式；
清油污清洗模式下，控制喷嘴与喷杆的夹角为0 ;
重油污清洗模式下，控制喷嘴与喷杆的夹角为设定角度a，使喷嘴始终与所清洗的管道壁面垂直。
[0007]进一步地，所述输出对用于控制喷杆水平旋转的第一伺服电机和用于控制喷杆竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点的步骤，具体为：
设定所述喷杆竖直旋转轴和喷杆水平旋转轴的力矩大小为空走力矩，所述喷杆竖直旋转轴和喷杆水平旋转轴的最大速度设定为空走速度；
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；
其中，所述运动原点位于方形管道上壁面上与右壁面相连接处。
[0008]进一步地，所述输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤，具体为：
设定所述竖直旋转轴力矩为清洗阻力F的力矩与喷杆重力G的力矩之和，设定所述水平旋转轴的力矩为清洗动力S的力矩，水平旋转轴的最大速度为喷嘴在管道壁面上的行走速度V ;
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的上壁面。
[0009]进一步地，在所述清洗模式为重油污清洗模式下，在判断接收到喷杆水平旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括： 控制水平旋转轴沿着喷嘴在上壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道左壁面上与上壁面相连接处。
[0010]进一步地，所述输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴运行的步骤，具体为:
设定所述竖直旋转轴力矩为清洗动力S的力矩与喷杆重力G的力矩之差，设定所述水平旋转轴的力矩为清洗阻力F的力矩，竖直旋转轴的最大速度为喷嘴在管道壁面上的行走速度V ;
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的左壁面。
[0011]进一步地，在所述清洗模式为重油污清洗模式下，在判断接收到喷杆竖直旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括:
控制竖直旋转轴沿着喷嘴在左壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道下壁面上与左壁面相连接处。
[0012]进一步地，所述输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴运行的步骤，具体为:
设定所述竖直旋转轴力矩为清洗阻力F的力矩与喷杆重力G的力矩之差，设定所述水平旋转轴的力矩为清洗动力S的力矩，水平旋转轴的最大速度为喷嘴在管道壁面上的行走速度V ;
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的下壁面。
[0013]进一步地，在所述清洗模式为重油污清洗模式下，在判断接收到喷杆水平旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括:
控制水平旋转轴沿着喷嘴在下壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；
开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道右壁面上与下壁面相连接处。
[0014]进一步地，所述输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴运行的步骤，具体为:
设定所述竖直旋转轴力矩为清洗动力S的力矩与喷杆重力G的力矩之和，设定所述水平旋转轴的力矩为清洗阻力F的力矩，竖直旋转轴的最大速度为喷嘴在管道壁面上的行走速度V ; 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的右壁面。
[0015]从上述系统的方案可以看出，通过采用第一伺服电机控制喷杆绕水平旋转轴转动，并采用第二伺服电机控制喷杆绕竖直旋转轴转动，两个伺服电机分别都工作在转矩模式下，可以有效保证喷杆遇到管道侧壁面、下壁面或上壁面的干涉时，伺服电机不易被烧毁，从而有效降低了管道清洗机器人在管道中作业的故障率；同时，采用伺服电机的力矩模式，可以有效保证喷嘴始终紧贴所清洗的管道壁，有效提高了对管道油污的清洗效果。
【附图说明】
[