一种基于力矩控制的管道清洗机器人的控制方法

摘要

本发明一种基于力矩控制的管道清洗机器人的控制方法,所述机器人包括喷杆及与所述喷杆相连接的喷嘴及安装在机器人上的监控摄像头，所述控制方法包括以下步骤：读取机器人参数配置文件，并开启监控摄像头；获取管道清洗信号；输出对第一伺服电机和第二伺服电机的控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；判断接收到喷杆水平旋转或竖直旋转轴的返回速度是否为零且持续一定时间T且是否完成一个周期的清洗；如果是，所述机器人沿当前管道移动预设的行进距离;判断机器人已经移动的距离是否大于等于设定的总距离,如果是,终止清洗工作,如果否,则进入下个周期的清洗步骤。本方案有效降低了管道清洗机器人在管道中作业中驱动电机的故障率；同时有效提高了对管道油污的清洗效果。

权利要求

1.一种基于力矩控制的管道清洗机器人的控制方法,所述机器人包括喷杆及与所述喷杆相连接的喷嘴及安装在机器人上的监控摄像头，其特征在于：所述控制方法包括以下步骤：读取机器人参数配置文件，并开启监控摄像头；

获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数；

输出对用于控制喷杆绕水平旋转轴水平旋转的第一伺服电机和用于控制喷杆绕竖直旋转轴竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T；

如果是，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T；

如果是，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T；

如果是，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T;

如果是,所述机器人沿当前管道移动预设的行进距离;

判断机器人已经移动的距离是否大于等于设定的总距离,如果是,终止清洗工作,如果否,则进入下个周期的清洗步骤。

2.如权利要求1所述的控制方法,其特征在于：所述获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数的步骤，具体为：设定所述管道为方形管道，并设定清洗管道总长度L、每个清洗周期内的行进距离A、清洗阻力F，清洗动力S，以及喷嘴在管道壁面上的行走速度V；

根据监控摄像头采集的管道视频信号设定清洗模式为轻油污清洗模式或重油污清洗模式；

轻油污清洗模式下，控制喷嘴与喷杆的夹角为0；

重油污清洗模式下，控制喷嘴与喷杆的夹角为设定角度α，使喷杆与喷嘴所在的平面始终与所清洗的管道壁面垂直。

3.如权利要求2所述的控制方法,其特征在于：所述输出对用于控制喷杆绕水平旋转轴水平旋转的第一伺服电机和用于控制喷杆绕竖直旋转轴竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点的步骤，具体为：设定所述喷杆竖直旋转轴和喷杆水平旋转轴的力矩大小为空走力矩，所述喷杆竖直旋转轴和喷杆水平旋转轴的最大速度设定为空走速度；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

其中，所述运动原点位于方形管道上壁面上与右壁面相连接处。

4.如权利要求3所述的控制方法，其特征在于：所述输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤，具体为：设定所述竖直旋转轴力矩为清洗阻力F的力矩与喷杆重力G的力矩之和，设定所述水平旋转轴的力矩为清洗动力S的力矩，水平旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的上壁面。

5.如权利要求4所述的控制方法，其特征在于：在所述清洗模式为重油污清洗模式下，在判断接收到喷杆水平旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括：控制水平旋转轴沿着喷嘴在上壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道左壁面上与上壁面相连接处。

6.如权利要求5所述的控制方法，其特征在于：所述输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤，具体为：设定所述竖直旋转轴力矩为清洗动力S的力矩与喷杆重力G的力矩之差，设定所述水平旋转轴的力矩为清洗阻力F的力矩，竖直旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的左壁面。

7.如权利要求6所述的控制方法，其特征在于：在所述清洗模式为重油污清洗模式下，在判断接收到喷杆竖直旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括：控制竖直旋转轴沿着喷嘴在左壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道下壁面上与左壁面相连接处。

8.如权利要求7所述的控制方法，其特征在于：所述输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤，具体为：设定所述竖直旋转轴力矩为清洗阻力F的力矩与喷杆重力G的力矩之差，设定所述水平旋转轴的力矩为清洗动力S的力矩，水平旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的下壁面。

9.如权利要求8所述的控制方法，其特征在于：在所述清洗模式为重油污清洗模式下，在判断接收到喷杆水平旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括：控制水平旋转轴沿着喷嘴在下壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道右壁面上与下壁面相连接处。

10.如权利要求9所述的控制方法，其特征在于：所述输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤，具体为：设定所述竖直旋转轴力矩为清洗动力S的力矩与喷杆重力G的力矩之和，设定所述水平旋转轴的力矩为清洗阻力F的力矩，竖直旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的右壁面。

说明书

一种基于力矩控制的管道清洗机器人的控制方法

技术领域

[0001] 本发明涉及管道清洗运动控制技术及其控制系统设计，具体涉及一种基于力矩控制的管道清洗机器人的控制方法。

背景技术

[0002] 烟道是宾馆、酒楼、饭店厨房的基础设施，极易藏污纳垢，是清理的死角。同时烟道是消防、卫生、防疫部门检查的重点区域。同时，烟罩内壁易寄生老鼠、蟑螂，不符合国家卫生标准。油腻胶状物长期扒附在金属表面，还会腐蚀金属材料，缩短烟道使用寿命。油烟管道有很多油渍长时间残留在烟机上和烟道表面上形成油垢，这样就会影响油烟机、烟道的使用，使烟流不易排除。厨房排烟不畅有80％左右的原因，都是由于长时间没有对油烟净化器的内置过滤网进行清洁，油污堵塞过滤网而造成的。烟道、烟罩常年处于高温工作环境，油烟气化形成积油、积炭或油腻的胶状物，如果不及时对烟道及烟罩进行清洗，非常容易引起火灾。

[0003] 现在，继中央空调风道清洗机器人的成功运用，又出现了管道清洗机器人成功解决了油烟管道清洗这一难题，使用控制系统操控的管道清洗机器人通过自身携带的摄像头，清楚掌握管道内的污染情况，可实现对管道油烟的自动清洗，但是现有的管道清洗机器人在清洗管道的过程中，一方面，因喷杆和喷嘴与管道壁的干涉会导致控制电机的堵转从而烧毁电机，从而导致管道清洗机器人中途停止清洗工作；另一方面，现有的机器人清洗喷嘴与管道壁贴合不紧，导致管道清洗效果不佳。

发明内容

[0004] 本发明的目的旨在解决现有的管道清洗机器人清洗管道的过程中容易烧坏电机停止工作以及对管道的清洗效果不佳的技术问题，提供一种基于力矩控制的管道清洗机器人的控制方法。

[0005] 本发明提供一种基于力矩控制的管道清洗机器人的控制方法,所述机器人包括喷杆及与所述喷杆相连接的喷嘴及安装在机器人上的监控摄像头，所述控制方法包括以下步骤：

[0006] 读取机器人参数配置文件，并开启监控摄像头；

[0007] 获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数；

[0008] 输出对用于控制喷杆绕水平旋转轴水平旋转的第一伺服电机和用于控制喷杆绕竖直旋转轴竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

[0009] 输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0010] 判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T；

[0011] 如果是，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0012] 判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T；

[0013] 如果是，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0014] 判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T；

[0015] 如果是，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0016] 判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T;

[0017] 如果是,所述机器人沿当前管道移动预设的行进距离;

[0018] 判断机器人已经移动的距离是否大于等于设定的总距离,如果是,终止清洗工作,如果否,则进入下个周期的清洗步骤。

[0019] 进一步地，所述获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数的步骤，具体为：

[0020] 设定所述管道为方形管道，并设定清洗管道总长度L、每个清洗周期内的行进距离A、清洗阻力F，清洗动力S，以及喷嘴在管道壁面上的行走速度V；

[0021] 根据监控摄像头采集的管道视频信号设定清洗模式为轻油污清洗模式或重油污清洗模式；

[0022] 清油污清洗模式下，控制喷嘴与喷杆的夹角为0；

[0023] 重油污清洗模式下，控制喷嘴与喷杆的夹角为设定角度α，使喷嘴始终与所清洗的管道壁面垂直。

[0024] 进一步地，所述输出对用于控制喷杆水平旋转的第一伺服电机和用于控制喷杆竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点的步骤，具体为：

[0025] 设定所述喷杆竖直旋转轴和喷杆水平旋转轴的力矩大小为空走力矩，所述喷杆竖直旋转轴和喷杆水平旋转轴的最大速度设定为空走速度；

[0026] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

[0027] 其中，所述运动原点位于方形管道上壁面上与右壁面相连接处。

[0028] 进一步地，所述输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤，具体为：

[0029] 设定所述竖直旋转轴力矩为清洗阻力F的力矩与喷杆重力G的力矩之和，设定所述水平旋转轴的力矩为清洗动力S的力矩，水平旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

[0030] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的上壁面。

[0031] 进一步地，在所述清洗模式为重油污清洗模式下，在判断接收到喷杆水平旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括：

[0032] 控制水平旋转轴沿着喷嘴在上壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；

[0033] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道左壁面上与上壁面相连接处。

[0034] 进一步地，所述输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴运行的步骤，具体为：

[0035] 设定所述竖直旋转轴力矩为清洗动力S的力矩与喷杆重力G的力矩之差，设定所述水平旋转轴的力矩为清洗阻力F的力矩，竖直旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

[0036] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的左壁面。

[0037] 进一步地，在所述清洗模式为重油污清洗模式下，在判断接收到喷杆竖直旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括：

[0038] 控制竖直旋转轴沿着喷嘴在左壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；

[0039] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道下壁面上与左壁面相连接处。

[0040] 进一步地，所述输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴运行的步骤，具体为：

[0041] 设定所述竖直旋转轴力矩为清洗阻力F的力矩与喷杆重力G的力矩之差，设定所述水平旋转轴的力矩为清洗动力S的力矩，水平旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

[0042] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的下壁面。

[0043] 进一步地，在所述清洗模式为重油污清洗模式下，在判断接收到喷杆水平旋转的返回速度为零且持续一定时间T的步骤之后，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行的步骤之前，还包括：

[0044] 控制水平旋转轴沿着喷嘴在下壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度；

[0045] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道右壁面上与下壁面相连接处。

[0046] 进一步地，所述输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴运行的步骤，具体为：

[0047] 设定所述竖直旋转轴力矩为清洗动力S的力矩与喷杆重力G的力矩之和，设定所述水平旋转轴的力矩为清洗阻力F的力矩，竖直旋转轴的最大速度为喷嘴在管道壁面上的行走速度V；

[0048] 开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴开始清洗方形管道的右壁面。

[0049] 从上述系统的方案可以看出，通过采用第一伺服电机控制喷杆绕水平旋转轴转动，并采用第二伺服电机控制喷杆绕竖直旋转轴转动，两个伺服电机分别都工作在转矩模式下，可以有效保证喷杆遇到管道侧壁面、下壁面或上壁面的干涉时，伺服电机不易被烧毁，从而有效降低了管道清洗机器人在管道中作业的故障率；同时，采用伺服电机的力矩模式，可以有效保证喷嘴始终紧贴所清洗的管道壁，有效提高了对管道油污的清洗效果。

附图说明

[0050] 图1为本发明管道清洗机器人的一种实施例的结构示意图；

[0051] 图2为本发明管道清洗机器人控制方法的第一实施例的流程图；

[0052] 图3为本发明管道清洗机器人控制方法的第二实施例的流程图。

具体实施方式

[0053] 为了使本发明所解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

[0054] 本发明提供一种实施例的基于力矩控制的管道清洗机器人的控制方法，所述管道清洗机器人，如图1所示，包括喷杆1及与喷杆相连接的喷嘴2，其上还设有水平旋转轴3及竖直旋转轴4，所述喷杆1可绕水平旋转轴3在水平面上转动，从而带动喷嘴2沿着水平面转动，所述喷杆1可绕竖直旋转轴4在竖直面上转动，从而带动喷嘴2沿着水平面转动。利用其内设置的控制装置及驱动装置（图中未示出）控制驱动所述水平旋转轴及竖直旋转轴的转动，从而带动喷杆和喷嘴完成对管道的清洗工作。本实施例中，优选采用第一伺服电机驱动所述水平旋转轴3转动，第二伺服电机驱动所述竖直旋转轴4转动。

[0055] 所述管道清洗机器人上还设有监控摄像头，用于实时监控机器人的行进情况及管道内的油污情况。

[0056] 所述基于力矩控制的管道清洗机器人的控制方法包括以下步骤：

[0057] 系统初始化，读取机器人参数配置文件，并开启监控摄像头，恢复默认值；机器人的运动控制卡初始化，串口开启，打开摄像头开启监控。

[0058] 获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数；

[0059] 输出对用于控制喷杆绕水平旋转轴水平旋转的第一伺服电机和用于控制喷杆绕竖直旋转轴竖直旋转的第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

[0060] 输出对第一伺服电机和第二伺服电机的第二控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0061] 判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T；

[0062] 如果是，输出对第一伺服电机和第二伺服电机的第三控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0063] 判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T；

[0064] 如果是，输出对第一伺服电机和第二伺服电机的第四控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0065] 判断接收到喷杆水平旋转的返回速度是否为零且持续一定时间T；

[0066] 如果是，输出对第一伺服电机和第二伺服电机的第五控制信号以控制所述喷杆的竖直旋转轴和所述喷杆的水平旋转轴的运行；

[0067] 判断接收到喷杆竖直旋转的返回速度是否为零且持续一定时间T;

[0068] 如果是,所述机器人沿当前管道移动预设的行进距离;

[0069] 判断机器人已经移动的距离是否大于等于设定的总距离,如果是,终止清洗工作,如果否,则进入下个周期的清洗步骤。

[0070] 所述获取管道清洗信号，并根据所述管道清洗信号设定所述机器人的清洗参数的步骤具体为：

[0071] 设定所述管道为方形管道，并设定清洗管道总长度L、每个清洗周期内的行进距离A、清洗阻力F，清洗动力S，以及喷嘴在管道壁面上的行走速度V；

[0072] 根据监控摄像头采集的管道视频信号设定清洗模式为轻油污清洗模式或重油污清洗模式；

[0073] 清油污清洗模式下，控制喷嘴与喷杆的夹角为0；

[0074] 重油污清洗模式下，控制喷嘴与喷杆的夹角为设定角度α，使喷嘴始终与所清洗的管道壁面垂直。

[0075] 在0-90度范围内，喷嘴与管道壁面的夹角越大，意味着喷嘴喷出的高温蒸汽射流对管道壁面的冲击力越大。因此对于较清油污，即轻油污清洗模式下，只需要使喷嘴2与喷杆1保持在同一直线上，即夹角为0度，此时喷杆的与壁面的夹角即为喷嘴与管道壁面的夹角。而对于重油污，需要转换为重油污清洗模式，因此针对重油污区，为了获得更好的清洗效果，需增大喷嘴与喷杆的夹角，此时喷嘴与壁面的实际夹角为喷杆与壁面的夹角加上喷杆与喷嘴的夹角。

[0076] 增大喷嘴与喷杆的夹角，虽然能够增强对油污的冲击力，但同时也会减小蒸汽射流的作用面积，使得清洗宽度更窄。更重要的是，当喷嘴与喷杆不处于一条直线时，喷嘴与喷杆（等效成两条直线）所构成的平面必须时刻与要清洗的管道壁面垂直，因此在诸如清洗管道上壁面切换到管道左壁面的情况时，必须使平面以喷杆为轴逆时针旋转90度，为了使旋转90度的动作顺利完成，还必须使喷杆水平向右旋转一定角度，否则喷嘴的运动将与左壁面发生干涉。

[0077] 因此，基于上述情况，本发明所述的基于力矩控制的管道清洗机器人的控制方法分为重油污清洗模式和轻油污清洗模式，以下分别对两种控制模式进行详述。

[0078] 如图2所示，本实施例中的基于力矩控制的管道清洗机器人的控制方法包括以下步骤：

[0079] 步骤S1001，系统初始化，读取参数配置文件，恢复默认值，各个串口开启，打开摄像头开启监控，选择方形管道清洗，并设定以下参数：清洗管道总长度、每个清洗周期内的行进距离、清洗阻力F（即喷嘴对管道壁面的压力），清洗动力S（即为使喷嘴在壁面移动的驱动力），以及喷嘴在管道壁面上的行走速度V（即清洗速度）。如果对上述参数有尚未设定的，则采取默认值；

[0080] 步骤S1002，根据监控摄像头采集的管道视频信号及人机交互界面，用户选择是否开启重油污清洁模式，若否进入步骤S1101；若是，则控制管道清洗机器人进入重油污清洁模式。实际应用中，判断所述管道壁上是否为重油污的方法及分界限因人而异，具体可以管道壁上是否有油滴为界限，若有油滴，则可以设定机器人的清洗模式为重油污清洗模式。当然，为了增强对任何管道的清洗力度及清洗效果，可以设定机器人一直工作在重油污模式。

[0081] 步骤S1101，喷杆水平旋转轴以及竖直旋转轴均采用力矩模式，设定所述喷杆竖直旋转轴和喷杆水平旋转轴的力矩大小为空走力矩，所述喷杆竖直旋转轴和喷杆水平旋转轴的最大速度设定为空走速度；开启对第一伺服电机和第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

[0082] 其中，本实施例中所述运动原点优选位于方形管道上壁面上与右壁面相连接处，即右上角。

[0083] 步骤S1102，准备开始清洗方管的上壁面，此时喷杆水平旋转轴为动力轴，竖直旋转轴为阻力轴。设定喷杆竖直旋转轴力矩为清洗阻F力矩与喷杆重力G力矩（常量）之和，这是由于在清洗上壁时，竖直旋转轴为了提供足够的压力，应克服喷杆固有的重力矩影响；设定水平旋转轴的力矩为清洗动力S的力矩，设定其最大速度为清洗速度V，设定完成后，对这两个轴发出第二控制信号，驱动水平旋转轴和竖直旋转轴开始运行。

[0084] 步骤S1103，判断喷嘴是否到达管道的左上角，即管道上壁面清洗是否已经完成。判定的依据是喷杆水平旋转轴伺服驱动器返回的电机速度在一段时间T内为零，则说明水平轴已经停止转动，也就是说已经到达管道左壁。以一段时间T内的速度作为判定依据，是为了防止信号的扰动将导致的误判。如果已经到达管道左壁面（水平轴停止转动）则继续执行步骤S1104，否则返回继续监测。

[0085] 步骤S1104，准备开始清洗方管的左壁面，此时喷杆水平旋转轴为阻力轴，竖直旋转轴为动力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩矩（常量）之差，这是由于在清洗左壁时，喷杆的重力能够提供一部分动力，设定其最大速度为清洗速度V，同时设定水平旋转轴的力矩为清洗阻F的力矩，设定完成后，对这两个轴发出第三控制信号，驱动水平旋转轴和竖直旋转轴开始运行。

[0086] 步骤S1105，判断喷嘴是否到达管道的左下角，即左壁清洗是否已经完成。判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内为零，则说明竖直旋转轴已经停止转动，也就是说已经到达管道下壁面，如果已经到达下壁面（竖直轴停止转动）则继续执行步骤S1106，否则返回继续监测。

[0087] 步骤S1106，准备开始清洗方管的下壁面，此时喷杆水平旋转轴为动力轴，竖直旋转轴为阻力轴。设定喷杆竖直旋转轴的力矩为清洗阻力F的力矩与喷杆重力G的力矩（常量）之差，这是由于在清洗下壁时，喷杆的重力已经提供了一部分压力，设定水平旋转轴的力矩为清洗动力S的力矩，设定其最大速度为清洗速度V。设定完成后，对这两个轴发出第四控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对管道下壁面进行清洗。

[0088] 步骤S1107，判断喷嘴是否到达管道的右下角，即下壁面清洗是否已经完成，判定的依据是喷杆水平旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明水平旋转轴已经停止转动，也就是说喷嘴已经到达管道右壁面，如果已经到达右壁面（水平轴停止转动）则继续执行步骤S1108，否则返回继续监测。

[0089] 步骤S1108，开始清洗方管的右壁面，此时喷杆竖直旋转轴为动力轴，水平旋转轴为阻力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩（常量）之和，这是由于在清洗右壁时，竖直旋转轴为了提供清洗动力，须先克服喷杆的重力，设定其最大速度为清洗速度V，设定水平旋转轴的力矩为清洗阻力F的力矩。设定完成后，对这两个轴发出第五控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对方管的右壁面进行清洗。

[0090] 步骤S1109，判断喷嘴是否到达管道的右上角，即右壁清洗是否已经完成，判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明竖直旋转轴已经停止转动，也就是说喷嘴已经到达管道的上壁面。如果已经到达上壁面（竖直轴停止转动）则继续执行步骤S1110，否则返回继续监测。

[0091] 步骤S1110，一个周期内的清洗动作已经完成，控制驱动机器人沿管道行进预设的行进距离。

[0092] 步骤S1111，根据机器人已经移动的总距离判断是否已经完成设定的总长度，即器人已经移动的距离是否大于等于设定的总距离，如果是，继续执行步骤S1301，否则返回步骤S1102，开始执行下一个周期的清洗动作。

[0093] 步骤S1301，终止清洗，等待用户的下一个指令。

[0094] 如图3所示，本发明提供另一种实施例的基于力矩控制的管道清洗机器人的控制方法包括以下步骤：

[0095] 步骤S1001，系统初始化，读取参数配置文件，恢复默认值，各个串口开启，打开摄像头开启监控，选择方形管道清洗，并设定以下参数：清洗管道总长度、每个清洗周期内的行进距离、清洗阻力F（即喷嘴对管道壁面的压力），清洗动力S（即为使喷嘴在壁面移动的驱动力），以及喷嘴在管道壁面上的行走速度V（即清洗速度）。如果对上述参数有尚未设定的，则采取默认值。

[0096] 步骤S1002，根据监控摄像头采集的管道视频信号及人机交互界面，进入步骤S1200，用户选择是开启重油污清洁模式。

[0097] 步骤S1201，控制喷嘴与喷杆的夹角为设定角度α，使喷杆与喷嘴所在的平面始终与所清洗的管道壁面垂直，喷杆水平旋转轴以及竖直旋转轴均采用力矩模式，设定所述喷杆竖直旋转轴和喷杆水平旋转轴的力矩大小为空走力矩，所述喷杆竖直旋转轴和喷杆水平旋转轴的最大速度设定为空走速度；开启对第一伺服电机和第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；

[0098] 其中，本实施例中所述运动原点优选位于方形管道上壁面上与右壁面相连接处，即右上角。

[0099] 步骤S1202，准备开始清洗方管的上壁面，此时喷杆水平旋转轴为动力轴，竖直旋转轴为阻力轴。设定喷杆竖直旋转轴力矩为清洗阻F力矩与喷杆重力G力矩（常量）之和，这是由于在清洗上壁时，竖直旋转轴为了提供足够的压力，应克服喷杆固有的重力矩影响；设定水平旋转轴的力矩为清洗动力S的力矩，设定其最大速度为清洗速度V，设定完成后，对这两个轴发出第二控制信号，驱动水平旋转轴和竖直旋转轴开始运行。

[0100] 步骤S1203，判断喷嘴是否到达管道的左上角，即管道上壁面清洗是否已经完成。判定的依据是喷杆水平旋转轴伺服驱动器返回的电机速度在一段时间T内为零，则说明水平轴已经停止转动，也就是说已经到达管道左壁。以一段时间T内的速度作为判定依据，是为了防止信号的扰动将导致的误判。如果已经到达管道左壁面（水平轴停止转动）则继续执行步骤S1204，否则返回继续监测。

[0101] 步骤1204，此时，管道上壁面已经清洗完成，为顺利过度到管道的左壁面，控制水平旋转轴沿着喷嘴在上壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道左壁面上与上壁面相连接处。

[0102] 步骤S1205，准备开始清洗方管的左壁面，此时喷杆水平旋转轴为阻力轴，竖直旋转轴为动力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩矩（常量）之差，这是由于在清洗左壁时，喷杆的重力能够提供一部分动力，设定其最大速度为清洗速度V，同时设定水平旋转轴的力矩为清洗阻F的力矩，设定完成后，对这两个轴发出第三控制信号，驱动水平旋转轴和竖直旋转轴开始运行。

[0103] 步骤S1206，判断喷嘴是否到达管道的左下角，即左壁清洗是否已经完成。判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内为零，则说明竖直旋转轴已经停止转动，也就是说已经到达管道下壁面，如果已经到达下壁面（竖直轴停止转动）则继续执行步骤S1207，否则返回继续监测。

[0104] 步骤S1207，此时，管道左壁面已经清洗完成，为顺利过度到管道的下壁面，控制竖直旋转轴沿着喷嘴在左壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道下壁面上与左壁面相连接处。

[0105] 步骤S1208，准备开始清洗方管的下壁面，此时喷杆水平旋转轴为动力轴，竖直旋转轴为阻力轴。设定喷杆竖直旋转轴的力矩为清洗阻力F的力矩与喷杆重力G的力矩（常量）之差，这是由于在清洗下壁时，喷杆的重力已经提供了一部分压力，设定水平旋转轴的力矩为清洗动力S的力矩，设定其最大速度为清洗速度V。设定完成后，对这两个轴发出第四控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对管道下壁面进行清洗。

[0106] 步骤S1209，判断喷嘴是否到达管道的右下角，即下壁面清洗是否已经完成，判定的依据是喷杆水平旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明水平旋转轴已经停止转动，也就是说喷嘴已经到达管道右壁面，如果已经到达右壁面（水平轴停止转动）则继续执行步骤S1210，否则返回继续监测。

[0107] 步骤1210，此时，管道下壁面已经清洗完成，为顺利过度到管道的右壁面，控制水平旋转轴沿着喷嘴在下壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道右壁面上与下壁面相连接处。

[0108] 步骤S1211，开始清洗方管的右壁面，此时喷杆竖直旋转轴为动力轴，水平旋转轴为阻力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩（常量）之和，这是由于在清洗右壁时，竖直旋转轴为了提供清洗动力，须先克服喷杆的重力，设定其最大速度为清洗速度V，设定水平旋转轴的力矩为清洗阻力F的力矩。设定完成后，对这两个轴发出第五控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对方管的右壁面进行清洗。

[0109] 步骤S1212，判断喷嘴是否到达管道的右上角，即右壁清洗是否已经完成，判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明竖直旋转轴已经停止转动，也就是说喷嘴已经到达管道的上壁面。如果已经到达上壁面（竖直轴停止转动）则继续执行步骤S1213，否则返回继续监测。

[0110] 步骤S1213，此时，管道右壁面已经清洗完成，为顺利过度到管道的上壁面，控制水平旋转轴沿着喷嘴在右壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道上壁面上与右壁面相连接处，并进入步骤S1214；

[0111] 步骤S1214，一个周期内的清洗动作已经完成，控制驱动机器人沿管道行进预设的行进距离。

[0112] 步骤S1215，根据机器人已经移动的总距离判断是否已经完成设定的总长度，即器人已经移动的距离是否大于等于设定的总距离，如果是，继续执行步骤S1301，否则返回步骤S1202，开始执行下一个周期的清洗动作。

[0113] 步骤S1301，终止清洗，等待用户的下一个指令。

[0114] 综上所述，执行以上步骤，就可以自动完成对方形管道的清洗。所有过程中只需操作人员在初始化时，对相关重要参数进行设定。在自动清洗时，操作人员通过监控视频监视机器人是否处于正常运行状态即可。

[0115] 在本发明提供的管道清洗机器人的控制方法中，同时使用了位置控制与力矩控制，分别对应于步进电机控制和伺服电机控制，对于步进电机，我们通过调制向步进电机驱动器输入的PWM脉冲数，实现对步进电机的位置和运转速度控制；对于交流伺服电机，其可以分别运行在三种不同模式下：位置模式、速度模式以及转矩模式。这里，我们使用它的转矩模式，在转矩模式下，电机的转动转矩为可设定量，其设定范围为0-3倍的额定转矩，理论上，若阻力矩一直小于设定力矩，电机将会一直保持加速状态，最终导致转速过快，实际中，受电机功率限制，转速不会达到无限快，但转速也会维持在一个较高的水平。因此，在伺服驱动器上，可以设定转矩模式下的最大速度，即使阻力矩一直小于设定力矩，电机转速也不会超过设定值。特别地，当电机转矩设定在额定转矩以下时，若阻力矩大于设定转矩，电机能够以零速度在一段较长的时间内输出设定的转矩，同时又不会对电机造成损伤。

[0116] 利用伺服电机在转矩模式下的这个特性，在清洗管道时，能够实现使喷嘴与管道壁贴紧，保持一定的压力（可设定大小），在管道壁平面上移动蒸汽喷嘴，保证蒸汽喷嘴与管道壁时时贴紧的同时，又不会对电机造成损伤，如果是步进电机在长时间堵转的情况下，将会烧坏。

[0117] 本发明综述了一套针对基于力矩控制的管道清洗机器人的全自动清洗方法。总的来说，本发明的实施力提出的控制方法简化了基于位置控制的控制方法的控制过程，且方法具有更强的适应性，为油烟管道及风管的清洗开辟了新的道路。不但有效降低了管道清洗机器人在管道作业中电机的故障率，同时，采用伺服电机的力矩模式，可以有效保证喷嘴始终紧贴所清洗的管道壁，有效提高了对管道油污的清洗效果。

[0118] 以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。