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