4]步骤S1108，开始清洗方管的右壁面，此时喷杆竖直旋转轴为动力轴，水平旋转轴为阻力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩(常量）之和，这是由于在清洗右壁时，竖直旋转轴为了提供清洗动力，须先克服喷杆的重力，设定其最大速度为清洗速度V，设定水平旋转轴的力矩为清洗阻力F的力矩。设定完成后，对这两个轴发出第五控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对方管的右壁面进行清洗。
[0035]步骤S1109，判断喷嘴是否到达管道的右上角，即右壁清洗是否已经完成，判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明竖直旋转轴已经停止转动，也就是说喷嘴已经到达管道的上壁面。如果已经到达上壁面(竖直轴停止转动）则继续执行步骤S1110，否则返回继续监测。
[0036]步骤S1110，一个周期内的清洗动作已经完成，控制驱动机器人沿管道行进预设的行进距离。
[0037]步骤S1111，根据机器人已经移动的总距离判断是否已经完成设定的总长度，即器人已经移动的距离是否大于等于设定的总距离，如果是，继续执行步骤S1301，否则返回步骤S1102，开始执行下一个周期的清洗动作。
[0038]步骤S1301，终止清洗，等待用户的下一个指令。
[0039]如图3所示，本发明提供另一种实施例的基于力矩控制的管道清洗机器人的控制方法包括以下步骤：
步骤S1001，系统初始化，读取参数配置文件，恢复默认值，各个串口开启，打开摄像头开启监控，选择方形管道清洗，并设定以下参数：清洗管道总长度、每个清洗周期内的行进距离、清洗阻力F (即喷嘴对管道壁面的压力)，清洗动力S (即为使喷嘴在壁面移动的驱动力)，以及喷嘴在管道壁面上的行走速度V (即清洗速度)。如果对上述参数有尚未设定的，则采取默认值。
[0040]步骤S1002，根据监控摄像头采集的管道视频信号及人机交互界面，进入步骤S1200，用户选择是开启重油污清洁模式。
[0041]步骤S1201，控制喷嘴与喷杆的夹角为设定角度a，使喷杆与喷嘴所在的平面始终与所清洗的管道壁面垂直，喷杆水平旋转轴以及竖直旋转轴均采用力矩模式，设定所述喷杆竖直旋转轴和喷杆水平旋转轴的力矩大小为空走力矩，所述喷杆竖直旋转轴和喷杆水平旋转轴的最大速度设定为空走速度；开启对第一伺服电机和第二伺服电机的第一控制信号以带动所述机器人的喷嘴移动到管道清洗周期的运动原点；
其中，本实施例中所述运动原点优选位于方形管道上壁面上与右壁面相连接处，即右上角。
[0042]步骤S1202，准备开始清洗方管的上壁面，此时喷杆水平旋转轴为动力轴，竖直旋转轴为阻力轴。设定喷杆竖直旋转轴力矩为清洗阻F力矩与喷杆重力G力矩(常量）之和，这是由于在清洗上壁时，竖直旋转轴为了提供足够的压力，应克服喷杆固有的重力矩影响；设定水平旋转轴的力矩为清洗动力S的力矩，设定其最大速度为清洗速度V，设定完成后，对这两个轴发出第二控制信号，驱动水平旋转轴和竖直旋转轴开始运行。
[0043]步骤S1203，判断喷嘴是否到达管道的左上角，即管道上壁面清洗是否已经完成。判定的依据是喷杆水平旋转轴伺服驱动器返回的电机速度在一段时间T内为零，则说明水平轴已经停止转动，也就是说已经到达管道左壁。以一段时间T内的速度作为判定依据，是为了防止信号的扰动将导致的误判。如果已经到达管道左壁面(水平轴停止转动)则继续执行步骤S1204，否则返回继续监测。
[0044]步骤1204，此时，管道上壁面已经清洗完成，为顺利过度到管道的左壁面，控制水平旋转轴沿着喷嘴在上壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道左壁面上与上壁面相连接处。
[0045]步骤S1205，准备开始清洗方管的左壁面，此时喷杆水平旋转轴为阻力轴，竖直旋转轴为动力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩矩(常量)之差，这是由于在清洗左壁时，喷杆的重力能够提供一部分动力，设定其最大速度为清洗速度V，同时设定水平旋转轴的力矩为清洗阻F的力矩，设定完成后，对这两个轴发出第三控制信号，驱动水平旋转轴和竖直旋转轴开始运行。
[0046]步骤S1206，判断喷嘴是否到达管道的左下角，即左壁清洗是否已经完成。判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内为零，则说明竖直旋转轴已经停止转动，也就是说已经到达管道下壁面，如果已经到达下壁面(竖直轴停止转动)则继续执行步骤S1207，否则返回继续监测。
[0047]步骤S1207，此时，管道左壁面已经清洗完成，为顺利过度到管道的下壁面，控制竖直旋转轴沿着喷嘴在左壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道下壁面上与左壁面相连接处。
[0048]步骤S1208，准备开始清洗方管的下壁面，此时喷杆水平旋转轴为动力轴，竖直旋转轴为阻力轴。设定喷杆竖直旋转轴的力矩为清洗阻力F的力矩与喷杆重力G的力矩(常量)之差，这是由于在清洗下壁时，喷杆的重力已经提供了一部分压力，设定水平旋转轴的力矩为清洗动力S的力矩，设定其最大速度为清洗速度V。设定完成后，对这两个轴发出第四控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对管道下壁面进行清洗。
[0049]步骤S1209，判断喷嘴是否到达管道的右下角，即下壁面清洗是否已经完成，判定的依据是喷杆水平旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明水平旋转轴已经停止转动，也就是说喷嘴已经到达管道右壁面，如果已经到达右壁面(水平轴停止转动)则继续执行步骤S1210，否则返回继续监测。
[0050]步骤1210，此时，管道下壁面已经清洗完成，为顺利过度到管道的右壁面，控制水平旋转轴沿着喷嘴在下壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道右壁面上与下壁面相连接处。
[0051]步骤S1211，开始清洗方管的右壁面，此时喷杆竖直旋转轴为动力轴，水平旋转轴为阻力轴，设定喷杆竖直旋转轴的力矩为清洗动力S的力矩与喷杆重力G的力矩(常量)之和，这是由于在清洗右壁时，竖直旋转轴为了提供清洗动力，须先克服喷杆的重力，设定其最大速度为清洗速度V，设定水平旋转轴的力矩为清洗阻力F的力矩。设定完成后，对这两个轴发出第五控制信号，驱动水平旋转轴和竖直旋转轴开始运行，带动喷嘴对方管的右壁面进行清洗。
[0052]步骤S1212，判断喷嘴是否到达管道的右上角，即右壁清洗是否已经完成，判定的依据是喷杆竖直旋转轴伺服驱动器返回的电机速度在一段时间T内都为零，则说明竖直旋转轴已经停止转动，也就是说喷嘴已经到达管道的上壁面。如果已经到达上壁面(竖直轴停止转动)则继续执行步骤S1213，否则返回继续监测。
[0053]步骤S1213，此时，管道右壁面已经清洗完成，为顺利过度到管道的上壁面，控制水平旋转轴沿着喷嘴在右壁面行走方向的反方向回转设定距离H，并控制喷杆绕自身轴线逆时针旋转90度，并开启对第一伺服电机和第二伺服电机的使能信号以带动所述机器人的喷嘴移动到方形管道上壁面上与右壁面相连接处，并进入步骤S1214 ;
步骤S1214，一个周期内的清洗动作已经完成，控制驱动机器人沿管道行进预设的行进距离。
[0054]步骤S1215，根据机器人已经移动的总距离判断是否已经完成设定的总长度，即器人已经移动的距离是否大于等于设定的总距离，如果是，继续执行步骤S1301，否则返回步骤S1202，开始执行下一个周期的清洗动作。
[0055]步骤S1301，终止清洗，等待用户的下一个指令。
[0056]综上所述，执行以上步骤，就可以自动完成对方形管道的清洗。所有过程中只需操作人员在初始化时，对相关重要参数进行设定。在自动清洗时，操作人员通过监控视频监视机器人是否处于正常运行状态即可。
[0057]在本发明提供的管道清洗机器人的控制方法中，同时使用了位置控制与力矩控制，分别对应于步进电机控制和伺服电机控制，对于步进电机，我们通过调制向步进电机驱动器输入的PWM脉冲数，实现对步进电机的位置和运转速度控制；对于交流伺服电机，其可以分别运行在三种不同模式下:位置模式、速度模式以及转矩模式。这里，我们使用它的转矩模式，在转矩模式下，电机的转动转矩为可设定量，其设定范围为0-3倍的额定转矩，理论上，若阻力矩一直小于设定力矩，电机将会一直保持加速状态，最终导致转速过快，实际中，受电机功率限制，转速不会达到无限快，但转速也会维持