CN103949446B - 一种管道清洗机器人及控制方法 - Google Patents

Abstract

本发明公开了一种管道清洗机器人及控制方法，管道清洗机器人包括底座、水平旋转盘、喷杆旋转座、喷杆、左侧步进电机、右侧步进电机、水平步进电机、曲度步进电机、竖直步进电机、喷杆旋转步进电机以及控制系统，控制系统内设有人机交互系统，人机交互系统向两轴、四轴运动控制卡发出指令，通过两轴、四轴运动控制卡发出脉冲分别控制管道清洗机器人内的各步进电机，从而调整喷头的各方位位置，完成管道清洗。手动控制方法是采用人机交互系统的操作界面，手动操作或输入的控制各步进电机的运行角度，人工控制完成管道清洗。自动控制方法是输入清洗参数，通过控制系统自动控制各步进电机运转，自动控制完成管道清洗，操作非常简单。

Description

一种管道清洗机器人及控制方法

技术领域

本发明涉及管道清洗运动控制技术及其控制系统设计，具体涉及一种管道清洗机器人及控制方法。

背景技术

随着现代餐饮业的迅猛发展，油烟管道铺设总量急剧增加。同时，油烟管道内，由于油烟净化器净化能力有限，不能完全净化油烟，油烟管道壁将会产生油垢积累。因此，油烟管道必须定期维护、检测、清洗，否则大量油垢的积累，在一定条件下，极易酿成火灾。

目前，市面上能够提供的油烟管道清洗服务，大多采用人工手持高压水枪，进入管内清洗的方法。但是，操作人员在密闭阴暗的环境中，手持高压设备，存在着极大地操作安全隐患。同时，这种清洗方法也大大地受到了管道尺寸的限制。为了提高清洗效率，个别服务商采用了一些具有腐蚀性的清洁剂，长远来讲，对油烟管道会造成极大损害。

近些年，市面上也出现了油烟管道手动清洗设备。通过摄像头采集现场信息，人工控制设备进行清洗。但摄像头视角有限，同时受环境影响较大，使操作人员难以获得较完备的实时信息。同时在可控轴较多的情况下，人工操作具有很大的不规则性，使清洗效率、效果很难得到保证。另一方面，市面现有设备采用较简易的直流控制系统，不能实现数字控制，也意味着无法实现较为精确的点位运动。

因此，如何设计出一种操作安全、清洗高效以及便捷的管道清洗机器人及控制方法是业界亟待解决的技术问题。

发明内容

为解决上述技术问题，本发明提出一种操作安全、清洗高效以及便捷的管道清洗机器人及控制方法。

本发明采用的技术方案是，设计一种管道清洗机器人，包括：底座，其两侧外部设有履带转动装置，所述底座内设有左、右侧步进电机；水平旋转盘，设于底座上面，其上安装有一喷杆旋转座，其下方安装有一水平步进电机，用于带动喷杆旋转座在水平方向转动；喷杆，所述喷杆尾端活动设于所述喷杆旋转座内，其前端通过一连杆机构连通一喷头，所述的连杆机构与设于喷杆上的曲度步进电机连接，用于调整喷头与喷杆的折弯角度；竖直步进电机，设于喷杆旋转座一侧且输出轴与喷杆旋转座固定连接，用于带动喷杆上下转动；喷杆旋转步进电机，设于喷杆与喷杆旋转座之间，用于带动喷杆沿自身轴线转动；以及控制系统，包括：人机交互系统，与人机交互系统双向通信的两轴运动控制卡与四轴运动控制卡、与人机交互系统单向通信的激光测距传感器和摄像头，所述的两轴运动卡分别与原点开关和左侧电机驱动器和曲度电机驱动器相连，所述的四轴运动控制卡分别与右侧电机驱动器、水平电机驱动器、竖直电机驱动器和喷杆旋转电机驱动器相连。

其中，连杆机构包括：一端与曲度步进电机活动端铰接的连杆、与连杆另一端铰接的直角连杆铰座，所述直角连杆铰座一端与连杆铰接、另一端与喷头固定连接，曲度步进电机活动端伸缩运动，推拉连杆带动喷头调整与喷杆的折弯角度；喷杆旋转座两端活动设于一轴承座内，竖直步进电机的输出轴穿过所述轴承座与喷杆旋转座固定连接，竖直步进电机转动带动喷杆旋转座转动；喷杆旋转步进电机横向固定于喷杆旋转座内且与喷杆平行，喷杆旋转步进电机输出轴与一主动带轮连接，喷杆上固定设有与主动带轮配合带传动的从动带轮，喷杆旋转步进电机通过带轮带动喷杆沿自身轴线转动，所述喷杆旋转步进电机每次运动固定顺时针或逆时针旋转90°。

本发明一实施例中，所述人机交互系统中包括：PC主机、显示器、鼠标、键盘以及控制手柄，所述PC主机的软件界面与所述控制手柄上设有分别单独控制六个步进电机的按键，所述PC主机的控制程序内设有按键检测模块、将按键检测模块检测到的按键信息进行翻译的翻译转换模块；所述原点开关采用机械式接近开关；所述激光测距传感器设有两个且分别设置在清洗机器人的首尾两端，所述PC主机设有接收所述激光测距传感器检测信号的测距模块。

所述PC主机与两轴运动控制卡、四轴运动控制卡分别通过PCI接口相连；原点开关的原点信号以电压开关量信号的方式反馈到两轴运动控制卡中；激光测距传感器与PC主机通过串口协议相连；摄像头的监控信号通过屏蔽线传递到视频采集卡，再通过USB接口与PC主机相连。

本发明还提供了一种管道清洗机器人的手动控制方法，包括：步骤1、管道清洗机器人通电，控制系统开机；

步骤2、硬件初始化，启动并复位运动控制卡；程序初始化，对步进电机转速赋初始值；

步骤3、选择人机交互系统中的手动控制模式，屏蔽自动控制模式；

步骤4、选择手动控制模式为手脉运动或定长运动，若是手脉运动，则执行步骤5；若是定长运动，则执行步骤11；

步骤5、对控制手柄或软件界面的按键进行操作，分别输入运动控制卡的控制指令，其中，左、右侧电机协同运动，共同配合前进、转弯动作；

步骤6、按键检测模块对按键进行检测，判断是否有按键按下，若有按键按下，则导出按键信息；若无按键按下，则继续检测是否有按键按下；

步骤7、翻译转换模块对导出的按键信息进行翻译，并分别向运动控制卡发出控制指令；

步骤8、运动控制卡分别向驱动器模块传输脉冲信号，驱动器模块驱动步进电机转动；

步骤9、按键检测模块对按键进行检测，判断是否有按键弹起，若有按键弹起，则执行步骤10；若无按键弹起，则按键检测模块继续检测；

步骤10、停止步进电机的转动，返回步骤5；

步骤11、在人机交互系统内输入各步进电机的转动角度、行进距离，以及各步进电机的转动速度；

步骤12、检测输入数据格式是否正确，若格式正确，则执行步骤13；若格式不正确，则返回步骤11；

步骤13、人机交互系统根据输入的角度、距离数据以及步进电机的转动速度，计算各驱动器所需脉冲数，对运动控制卡发出控制指令；

步骤14、运动控制卡分别向驱动器模块传输脉冲信号并计算分别对各驱动器已发射的脉冲数，驱动器模块驱动步进电机转动；

步骤15、人机交互系统实时将运动控制卡反馈的各驱动器已发射脉冲数与计算脉冲数比对，判断各步进电机是否转动到指定位置，若各驱动器已发射脉冲数与计数脉冲全部相同，则返回步骤11；若有驱动器已发射脉冲数与计数脉冲不相同，则该步进电机带动喷头继续运动。

本发明还提供了一种管道清洗机器人的全自动控制方法，包括：步骤1、管道清洗机器人通电，控制系统开机；

步骤2、硬件初始化，启动并复位运动控制卡；软件初始化，对步进电机转速赋初始值；

步骤3、选择人机交互系统中的自动控制模式，屏蔽手动控制模式；

步骤4、管道机器人自动运动到运动原点；

步骤5、设定清洗参数，所述清洗参数包括：管道截面长、宽，清洗总长度，清洗速率，清洗一周期后前进距离；

步骤6、选择是否开启自动纠偏，若开启自动纠偏，则执行步骤7；

步骤7、开启自动纠偏控制模块；

步骤8、开启自动适应控制模块；

步骤9、人机交互系统根据输入清洗参数，计算各驱动器所需脉冲数，对运动控制卡发出控制指令；

步骤10、运动控制卡分别向驱动器模块传输脉冲信号并进行计数，驱动器模块驱动步进电机转动；

步骤11、步进电机带动喷头对管道进行一周期清洗；

步骤12、人机交互系统实时将运动控制卡反馈的各驱动器已发射脉冲数与计算脉冲数比对，判断各步进电机是否将喷头带动到目的位置，若喷头已到达目的位置，则喷头停止运动；若喷头未达到目的位置，则执行步骤13；

步骤13、左、右侧步进电机带动喷头向前移动步骤5中的指定前进距离；

步骤14、自动纠偏控制模块进行实时检测纠偏，若偏差小于容差范围，则执行步骤11；若偏差大于容差范围，进行自动纠正，持续检测纠偏。

其中，所述步骤4还包括：

步骤4.1、手动设定各步进电机回原点的方向，步进电机带动喷头向原点运动；

步骤4.2、各原点开关实时检测喷头各方位是否在运动原点，若喷头到达运动原点，则各原点开关发出信号，喷头停止运动；若喷头有方位未达到原点开关，该原点开关没有信号，该步进电机继续带动喷头运动。

所述步骤11还包括：

步骤11.1、清洗管道上壁，竖直、水平、曲度步进电机带动喷头运动，使喷头对准管道上壁的右端角位置；

步骤11.2、水平步进电机向左转动，带动喷头自右向左匀速运动，直至管道上壁左端角处，完成上壁清洗；

步骤11.3、清洗管道左壁，喷杆旋转步进电机带动喷杆逆时针旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道左壁的上端角处；

步骤11.4、竖直步进电机向下转动，水平步进电机同时向右回转，带动喷头自上向下匀速运动，直至喷头到达管道左壁的下端角处，完成左壁清洗；

步骤11.5、清洗管道下壁，喷杆旋转步进电机带动喷杆逆时针旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道下壁左端角处；

步骤11.6、水平步进电机向右转动，带动喷头自左向右匀速运动，直至管道下壁右端角处，完成下壁清洗；

步骤11.7、清洗管道右壁，喷杆旋转步进电机带动喷杆旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道右壁的下端角处；

步骤11.8、竖直步进电机向上转动，水平步进电机同时向左回转，带动喷头自下向上匀速运动，直到喷头到达管道内壁的上端角处，完成右壁清洗；

步骤11.9、喷杆回到初始位置，完成一周期清洗。

所述自动纠偏控制模块包括：

步骤1，将管道清洗机器人轴线中心距离管道中轴线的距离设定为偏距d，将管道清洗机器人轴线偏离管道中轴线的角度设定为偏角α，分别定义偏距d和偏角α的最小容差和最大容差，启动管道清洗机器人，初始化管道清洗机器人两侧履带的速度；

步骤2，测距模块实时获取管道清洗机器人首尾两端的激光测距传感器的返回值，并计算管道清洗机器人的偏距d和偏角α；

步骤3，判断偏距d和偏角α是否在最小容差范围内，若小于最小容差，则返回步骤2；若大于最小容差，则执行步骤4；

步骤4，判断偏距d和偏角α是否在最大容差范围内，若小于最大容差，则执行步骤6，实施小容差纠正方案；若大于最大容差，则执行步骤5，实施大容差纠正方案；

步骤5，执行大容差纠正方案：暂停管道清洗机器人的清洗动作，将偏角α与其最小容差进行对比，若偏角α大于其最小容差，则矫正偏角，若偏角α小于其最小容差，则将偏距d与其最小容差进行对比，若偏距d大于其最小容差，则对偏距进行矫正，若偏距d小于其最小容差，则大容差纠正方案完成，继续返回步骤2；

步骤6，执行小容差纠正方案：将偏角α与其最小容差进行对比，若偏角α大于最小容差，则矫正偏角，若偏角α小于其最小容差，则初始化履带速度后，将偏距d与其最大容差进行对比，若大于，则返回步骤4，若小于，则继续将偏距d与其最小容差进行对比，若大于，则对偏距进行矫正，若小于，则完成小容差纠正方案，继续执行步骤2。

自动适应控制模块包括：

步骤1，履带暂停运动，以管道清洗机器人为原点，根据纠偏控制后的偏距d，对管道清洗机器人四个轴相对于管道的位置进行位置偏斜补偿；

步骤2，以管道清洗机器人为原点，根据步骤3中纠偏控制后的偏角α，对管道清洗机器人的电机转动范围进行角度偏斜补偿。

本发明的控制系统，使用步进脉冲控制，代替直流通断控制，实现了管道清洗机器人的数字化控制，提高操作精度。喷杆可通过六个步进电机全方位调整清洗位置，可完成更加复杂、精细的清洗动作，保证喷头与管壁的距离及夹角处在恰当的范围内，实现对管道的无死角、高精度清洗。更优的，控制系统内还加入了原点开关及激光测距传感器，使控制系统以及操作人员得到更加全面的反馈信息，以进一步修正喷杆的运动方位。并且，在管道清洗机器人系统的基础之上，提供了一种手动及全自动控制方法，尤其是全自动控制方法，只需操作人员在清洗之前设定相关参数，清洗机器人便会自动运动，对管道进行清洗，使用十分便捷。

附图说明

下面结合实施例和附图对本发明进行详细说明，其中：

图1是本发明的管道清洗机器人连接示意图；

图2是本发明的控制系统连接图；

图3是本发明的手动控制方法流程图；

图4是本发明的全自动控制方法流程图。

具体实施方式

本发明的管道清洗机器人包括：底座11、水平旋转盘10、喷杆旋转座9、喷杆8、喷头7、曲度步进电机6、喷杆旋转步进电机5、竖直步进电机4、水平步进电机3、左侧步进电机2、右侧步进电机1以及控制系统。

如图1、2所示，底座，其两侧外部设有履带转动装置，底座内设有左、右侧步进电机，左、右侧步进电机通过齿轮与履带12传动，带动机器人前后运动。水平旋转盘10，设于底座11上面，其上安装有喷杆旋转座9，其下方安装有水平步进电机3，水平步进电机3的输出轴穿过水平旋转盘10与喷杆旋转座9连接，水平步进电机3带动喷杆旋转座9在水平面内左右转动，用于带动喷杆旋转座9在水平方向转动。喷杆8，喷杆8尾端活动设于喷杆旋转座9内，其前端通过一连杆机构连通喷头7，连杆机构与设于喷杆8上的曲度步进电机6连接，用于调整喷头7与喷杆8的折弯角度，其中，连杆机构包括：一端与曲度步进电机6活动端铰接的连杆13、与连杆13另一端铰接的直角连杆铰座14，直角连杆铰座14一端与连杆13铰接、另一端与喷头7固定连接，曲度步进电机6活动端伸缩运动，推拉连杆13带动喷头7调整与喷杆8的折弯角度。竖直步进电机4，设于喷杆旋转座9一侧且输出轴与喷杆旋转座9固定连接，用于带动喷杆8上下转动，喷杆旋转座9两端活动设于一轴承座15内，竖直步进电机4的输出轴穿过所述轴承座15与喷杆旋转座9固定连接，竖直步进电机4转动带动喷杆旋转座9转动。喷杆旋转步进电机5，设于喷杆8与喷杆旋转座9之间，用于带动喷杆8沿自身轴线转动，喷杆旋转步进电机5横向固定于喷杆旋转座9内且与喷杆8平行，喷杆旋转步进电机5输出轴与一主动带轮连接，喷杆8上固定设有与主动带轮位置配合的从动带轮，主动带轮与从动带轮通过传动带16传动，喷杆旋转步进电机5通过带轮带动喷杆8沿自身轴线转动，喷杆旋转步进电机5每次运动固定顺时针或逆时针旋转90°。

喷杆8与喷杆旋转座9之间通过O性密封圈活动密封连接，喷杆旋转座9后端设有与喷杆连通的转接口17，转接口17可连接接管路，使清洗液通过转接口进入喷杆8内，从喷头7喷出，通过转接口17的设置使喷杆8的自转不影响外部管路，防止因喷杆8的自转造成管路缠绕，影响使用效果。

如图1、2所示，控制系统包括：接收操作输入指令、反馈信号及检测信号的人机交互系统，接收人机交互系统输出指令并反馈信号给人机交互系统的两轴运动控制卡与四轴运动控制卡，分别接收两轴运动控制卡输出指令的左侧电机驱动器和曲度电机驱动器，分别接收四轴运动控制卡输出指令的右侧电机驱动器、水平电机驱动器、竖直电机驱动器以及喷杆旋转电机驱动器，分别接收左侧电机驱动器、曲度电机驱动器、右侧电机驱动器、水平电机驱动器、竖直电机驱动器以及喷杆旋转电机驱动器脉冲命令的左侧步进电机2、曲度步进电机6、右侧步进电机1、水平步进电机3、竖直步进电机4以及喷杆旋转步进电机5，检测喷头各方位是否在运动原点并反馈信号给两轴运动控制卡的六个原点开关，检测机器人距离管壁距离以及行进角度并反馈信号给人机交互系统，采集机器人在管道内的实时位置信号并反馈给人机交互系统。

本发明一实施例中，人机交互系统中包括：PC主机、显示器、鼠标、键盘以及控制手柄，使用PC作为上位机，使控制代码具有较强的可移植性，对控制程序进行调试时，也更加方便，并且PC操作流程容易被操作人员掌握。PC主机的软件界面与控制手柄上设有分别单独控制六个步进电机的按键，PC主机的控制程序内设有按键检测模块、将按键检测模块检测到的按键信息进行翻译的翻译转换模块；原点开关采用机械式接近开关；所述激光测距传感器设有两个且分别设置在清洗机器人的首尾两端，所述PC主机设有接收所述激光测距传感器检测信号的测距模块。

PC主机与两轴运动控制卡、四轴运动控制卡分别通过PCI接口相连；原点开关的原点信号以电压开关量信号的方式反馈到两轴运动控制卡中；激光测距传感器与PC主机通过串口协议相连；摄像头的监控信号通过屏蔽线传递到视频采集卡，再通过USB接口与PC主机相连；控制手柄与PC主机无线连接。

如图3所示，本发明还提供了一种管道清洗机器人的手动控制方法，包括：步骤1、管道清洗机器人通电，控制系统开机；

步骤2、硬件初始化，启动并复位运动控制卡；程序初始化，对步进电机转速赋初始值；

步骤3、选择人机交互系统中的手动控制模式，屏蔽自动控制模式；

步骤4、选择手动控制模式为手脉运动或定长运动，若是手脉运动，则执行步骤5；若是定长运动，则执行步骤11；

步骤5、对控制手柄或软件界面的按键进行操作，分别输入运动控制卡的控制指令，其中，左、右侧电机协同运动，共同配合前进、转弯动作。由于管道机器人的左、右侧步进电机输出轴是相反设置的，输入到左、右侧电机驱动器内的脉冲信号必须是配合相反的，即左、右侧步进电机转动方向相反，才能使得管道机器人前后运动，当管道机器人需要转弯时，比如向左侧转弯，则减小左侧步进电机的转动速度，加大右侧电机的转动速度，两侧履带由于速度差即可向左侧转弯，同理进行右侧转弯调整；

步骤6、按键检测模块对按键进行检测，判断是否有按键按下，若有按键按下，则导出按键信息；若无按键按下，则继续检测是否有按键按下；

步骤7、翻译转换模块对导出的按钮信息进行翻译，并分别向运动控制卡发出控制指令；

步骤8、运动控制卡分别向驱动器模块传输脉冲信号，驱动器模块驱动步进电机转动；

步骤9、按键检测模块对按键进行检测，判断是否有按键弹起，若有按键弹起，则执行步骤10；若无按键弹起，则按键检测模块继续检测；

步骤10、停止步进电机的转动，返回步骤5；

步骤11、在人机交互系统内输入各步进电机的转动角度、行进距离，以及各步进电机的转动速度；

步骤12、检测输入数据格式是否正确，若格式正确，则执行步骤13；若格式不正确，则返回步骤11；

步骤13、人机交互系统根据输入的角度、距离数据以及步进电机的转动速度，计算各驱动器所需脉冲数，对运动控制卡发出控制指令；

步骤14、运动控制卡分别向驱动器模块传输脉冲信号并计算分别对各驱动器已发射的脉冲数，驱动器模块驱动步进电机转动；

步骤15、人机交互系统实时将运动控制卡反馈的各驱动器已发射脉冲数与计算脉冲数比对，判断各步进电机是否转动到指定位置，若各驱动器已发射脉冲数与计数脉冲全部相同，则返回步骤11；若有驱动器已发射脉冲数与计数脉冲不相同，则该步进电机带动喷头继续运动。

以上是管道清洗机器人系统的手动控制方法，操作时可通过无线控制手柄进行控制，使对清洗过程的控制更加灵活。

如图4所示，本发明还提供了一种管道清洗机器人的全自动控制方法，包括：

步骤1、管道清洗机器人通电，控制系统开机；

步骤2、硬件初始化，启动并复位运动控制卡；软件初始化，对步进电机转速赋初始值；

步骤3、选择人机交互系统中的自动控制模式，屏蔽手动控制模式；

步骤4、管道机器人自动运动到运动原点；

步骤5、设定清洗参数，所述清洗参数包括：管道截面长、宽，清洗总长度，清洗速率，清洗一周期后前进距离；

步骤6、选择是否开启自动纠偏，若开启自动纠偏，则执行步骤7；

步骤7、开启自动纠偏控制模块；

步骤8、开启自动适应控制模块；

步骤9、人机交互系统根据输入清洗参数，计算各驱动器所需脉冲数，对运动控制卡发出控制指令；

步骤10、运动控制卡分别向驱动器模块传输脉冲信号并进行计数，驱动器模块驱动步进电机转动；

步骤11、步进电机带动喷头对管道进行一周期清洗；

步骤12、人机交互系统实时将运动控制卡反馈的各驱动器已发射脉冲数与计算脉冲数比对，判断各步进电机是否将喷头带动到目的位置，若喷头已到达目的位置，则喷头停止运动；若喷头未达到目的位置，则执行步骤13；

步骤13、左、右侧步进电机带动喷头向前移动步骤5中的指定前进距离；

步骤14、自动纠偏控制模块进行实时检测纠偏，若偏差小于容差范围，则执行步骤11；若偏差大于容差范围，进行自动纠正，持续检测纠偏。

其中，所述步骤4还包括：

步骤4.1、手动设定各步进电机回原点的方向，步进电机带动喷头向原点运动；

步骤4.2、各原点开关实时检测喷头各方位是否在运动原点，若喷头到达方位，则该原点开关发出信号，该步进电机被锁定停止转动；若喷头未到达方位，则该原点开关没有信号，该步进电机继续带动喷头运动，直到接近原点开关。

在步骤11中，具体的一周期清洗动作包括以下步骤：

步骤11.1、清洗管道上壁，竖直、水平、曲度步进电机带动喷头运动，使喷头对准管道上壁的右端角位置；

步骤11.2、水平步进电机向左转动，带动喷头自右向左匀速运动，直至管道上壁左端角处，完成上壁清洗；

步骤11.3、清洗管道左壁，喷杆旋转步进电机带动喷杆逆时针旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道左壁的上端角处；

步骤11.4、竖直步进电机向下转动，水平步进电机同时向右回转，带动喷头自上向下匀速运动，直至喷头到达管道左壁的下端角处，完成左壁清洗；

步骤11.5、清洗管道下壁，喷杆旋转步进电机带动喷杆逆时针旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道下壁左端角处；

步骤11.6、水平步进电机向右转动，带动喷头自左向右匀速运动，直至管道下壁右端角处，完成下壁清洗；

步骤11.7、清洗管道右壁，喷杆旋转步进电机带动喷杆旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道右壁的下端角处；

步骤11.8、竖直步进电机向上转动，水平步进电机同时向左回转，带动喷头自下向上匀速运动，直到喷头到达管道内壁的上端角处，完成右壁清洗；

步骤11.9、喷杆回到初始位置，完成一周期清洗。

自动纠偏控制模块包括：

步骤1，将管道清洗机器人轴线中心距离管道中轴线的距离设定为偏距d，将管道清洗机器人轴线偏离管道中轴线的角度设定为偏角α，分别定义偏距d和偏角α的最小容差和最大容差，启动管道清洗机器人，初始化管道清洗机器人两侧履带的速度；

步骤2，测距模块实时获取管道清洗机器人首尾两端的激光测距传感器的返回值，并计算管道清洗机器人的偏距d和偏角α；

步骤3，判断偏距d和偏角α是否在最小容差范围内，若小于最小容差，则返回步骤2；若大于最小容差，则执行步骤4；

步骤4，判断偏距d和偏角α是否在最大容差范围内，若小于最大容差，则执行步骤6，实施小容差纠正方案；若大于最大容差，则执行步骤5，实施大容差纠正方案；

步骤5，执行大容差纠正方案：暂停管道清洗机器人的清洗动作，将偏角α与其最小容差进行对比，若偏角α大于其最小容差，则矫正偏角，若偏角α小于其最小容差，则将偏距d与其最小容差进行对比，若偏距d大于其最小容差，则对偏距进行矫正，若偏距d小于其最小容差，则大容差纠正方案完成，继续返回步骤2；大容差纠正方案中减缓反方向的履带的速度为履带初始速度的0.6倍；

步骤6，执行小容差纠正方案：将偏角α与其最小容差进行对比，若偏角α大于最小容差，则矫正偏角，若偏角α小于其最小容差，则初始化履带速度后，将偏距d与其最大容差进行对比，若大于，则返回步骤4，若小于，则继续将偏距d与其最小容差进行对比，若大于，则对偏距进行矫正，若小于，则完成小容差纠正方案，继续执行步骤2。小容差纠正方案中减缓反方向的履带的速度为履带初始速度的0.9倍。

自动适应控制模块包括：

步骤1，履带暂停运动，以管道清洗机器人为原点，根据纠偏控制后的偏距d，对管道清洗机器人四个轴相对于管道的位置进行位置偏斜补偿；

步骤2，以管道清洗机器人为原点，根据步骤3中纠偏控制后的偏角α，对管道清洗机器人的电机转动范围进行角度偏斜补偿。

在整个行进过程中，管道清洗机器人每完成一周期清洗动作，就向前行进指定距离，自动纠偏模块对机器人的位置进行纠偏后，进行下一周期的清洗动作，在这期间，运动控制卡对已发射的脉冲进行计数并反馈到人机交互系统，当已发射的脉冲与人机交互系统中计算好的脉冲数相同时，停止该步进电机的转动，当运动控制卡对个驱动器已发射的脉冲数与人机交互系统全部相同时，管道清洗机器人从运动原点自动行进到指定位置，即完成了整个自动清洗管道的流程。与手动清洗相比，自动清洗大大地降低了操作难度，运行时，只需一名操作人员通过摄像头采集视频监控即可。此外，全自动清洗还避免了摄像头无法观察到的死角，是整个清洗过程更加规范划，清洗效果均一。最后，自动清洗具有更高的效率，考虑到管道通常在夜间清洗的条件下，更加便捷，同时更减少了能源的耗费。

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

Claims (9)

1.一种管道清洗机器人，其特征在于包括：

底座，其两侧外部设有履带转动装置，所述底座内设有左、右侧步进电机；

水平旋转盘，设于底座上面，其上安装有一喷杆旋转座，其下方安装有一水平步进电机，用于带动喷杆旋转座在水平方向转动；

喷杆，所述喷杆尾端活动设于所述喷杆旋转座内，其前端通过一连杆机构连通一喷头，所述的连杆机构与设于喷杆上的曲度步进电机连接，用于调整喷头与喷杆的折弯角度；

竖直步进电机，设于喷杆旋转座一侧且输出轴与喷杆旋转座固定连接，用于带动喷杆上下转动；

喷杆旋转步进电机，设于喷杆与喷杆旋转座之间，用于带动喷杆沿自身轴线转动；以及

控制系统，包括：人机交互系统，与人机交互系统双向通信的两轴运动控制卡与四轴运动控制卡、与人机交互系统单向通信的激光测距传感器和摄像头，所述的两轴运动卡分别与原点开关和左侧电机驱动器和曲度电机驱动器相连，所述的四轴运动控制卡分别与右侧电机驱动器、水平电机驱动器、竖直电机驱动器和喷杆旋转电机驱动器相连；

其中，所述连杆机构包括：一端与曲度步进电机活动端铰接的连杆、与连杆另一端铰接的直角连杆铰座，所述直角连杆铰座一端与连杆铰接、另一端与喷头固定连接，曲度步进电机活动端伸缩运动，推拉连杆带动喷头调整与喷杆的折弯角度；

喷杆旋转座两端活动设于一轴承座内，竖直步进电机的输出轴穿过所述轴承座与喷杆旋转座固定连接，竖直步进电机转动带动喷杆旋转座转动；

喷杆旋转步进电机横向固定于喷杆旋转座内且与喷杆平行，喷杆旋转步进电机输出轴与一主动带轮连接，喷杆上固定设有与主动带轮配合带传动的从动带轮，喷杆旋转步进电机通过带轮带动喷杆沿自身轴线转动，所述喷杆旋转步进电机每次运动固定顺时针或逆时针旋转90°。

2.如权利要求1所述的管道清洗机器人，其特征在于，所述人机交互系统中包括：PC主机、显示器、鼠标、键盘以及控制手柄，所述PC主机的软件界面与所述控制手柄上设有分别单独控制六个步进电机的按键，所述PC主机的控制程序内设有按键检测模块、将按键检测模块检测到的按键信息进行翻译的翻译转换模块；所述原点开关设有六个，并采用机械式接近开关；所述激光测距传感器设有两个且分别设置在清洗机器人的首尾两端，所述PC主机设有接收所述激光测距传感器检测信号的测距模块。

3.如权利要求2所述的管道清洗机器人，其特征在于，所述PC主机与两轴运动控制卡、四轴运动控制卡分别通过PCI接口相连；原点开关的原点信号以电压开关量信号的方式反馈到两轴运动控制卡中；激光测距传感器与PC主机通过串口协议相连；摄像头的监控信号通过屏蔽线传递到视频采集卡，再通过USB接口与PC主机相连；控制手柄与PC主机无线连接。

4.一种管道清洗机器人的手动控制方法，其特征在于包括：步骤1、管道清洗机器人通电，控制系统开机；

步骤2、硬件初始化，启动并复位运动控制卡；程序初始化，对步进电机转速赋初始值；

步骤3、选择人机交互系统中的手动控制模式，屏蔽自动控制模式；

步骤4、选择手动控制模式为手脉运动或定长运动，若是手脉运动，则执行步骤5；若是定长运动，则执行步骤11；

步骤5、对控制手柄或软件界面的按键进行操作，分别输入运动控制卡的控制指令，其中，左、右侧电机协同运动，共同配合前进、转弯动作；

步骤6、按键检测模块对按键进行检测，判断是否有按键按下，若有按键按下，则导出按键信息；若无按键按下，则继续检测是否有按键按下；

步骤7、翻译转换模块对导出的按键信息进行翻译，并分别向运动控制卡发出控制指令；

步骤8、运动控制卡分别向驱动器模块传输脉冲信号，驱动器模块驱动步进电机转动；

步骤9、按键检测模块对按键进行检测，判断是否有按键弹起，若有按键弹起，则执行步骤10；若无按键弹起，则按键检测模块继续检测；

步骤10、停止步进电机的转动，返回步骤5；

步骤11、在人机交互系统内输入各步进电机的转动角度、行进距离，以及各步进电机的转动速度；

步骤12、检测输入数据格式是否正确，若格式正确，则执行步骤13；若格式不正确，则返回步骤11；

步骤13、人机交互系统根据输入的角度、距离数据以及步进电机的转动速度，计算各驱动器所需脉冲数，对运动控制卡发出控制指令；

步骤14、运动控制卡分别向驱动器模块传输脉冲信号并计算分别对各驱动器已发射的脉冲数，驱动器模块驱动步进电机转动；

步骤15、人机交互系统实时将运动控制卡反馈的各驱动器已发射脉冲数与计算脉冲数比对，判断各步进电机是否转动到指定位置，若各驱动器已发射脉冲数与计数脉冲全部相同，则返回步骤11；若有驱动器已发射脉冲数与计数脉冲不相同，则该步进电机带动喷头继续运动。

5.一种管道清洗机器人的全自动控制方法，其特征在于包括：步骤1、管道清洗机器人通电，控制系统开机；

步骤2、硬件初始化，启动并复位运动控制卡；软件初始化，对步进电机转速赋初始值；

步骤3、选择人机交互系统中的自动控制模式，屏蔽手动控制模式；

步骤4、管道机器人自动运动到运动原点；

步骤5、设定清洗参数，所述清洗参数包括：管道截面长、宽，清洗总长度，清洗速率，清洗一周期后前进距离；

步骤6、选择是否开启自动纠偏，若开启自动纠偏，则执行步骤7；

步骤7、开启自动纠偏控制模块；

步骤8、开启自动适应控制模块；

步骤9、人机交互系统根据输入清洗参数，计算各驱动器所需脉冲数，对运动控制卡发出控制指令；

步骤10、运动控制卡分别向驱动器模块传输脉冲信号并进行计数，驱动器模块驱动步进电机转动；

步骤11、步进电机带动喷头对管道进行一周期清洗；

步骤12、人机交互系统实时将运动控制卡反馈的各驱动器已发射脉冲数与计算脉冲数比对，判断各步进电机是否将喷头带动到目的位置，若喷头已到达目的位置，则喷头停止运动；若喷头未达到目的位置，则执行步骤13；

步骤13、左、右侧步进电机带动喷头向前移动步骤5中的指定前进距离；

步骤14、自动纠偏控制模块进行实时检测纠偏，若偏差小于容差范围，则执行步骤11；若偏差大于容差范围，进行自动纠正，持续检测纠偏。

6.如权利要求5所述的全自动控制方法，其特征在于，所述步骤4还包括：

步骤4.1、手动设定各步进电机回原点的方向，步进电机带动喷头向原点运动；

步骤4.2、各原点开关实时检测喷头各方位是否在运动原点，若喷头到达运动原点，则各原点开关发出信号，步进电机停止带动喷头运动；若喷头有方位未达到原点开关，该原点开关没有信号，该步进电机继续带动喷头运动。

7.如权利要求6所述的全自动控制方法，其特征在于，所述步骤11还包括：步骤11.1、清洗管道上壁，竖直、水平、曲度步进电机带动喷头运动，使喷头对准管道上壁的右端角位置；

步骤11.2、水平步进电机向左转动，带动喷头自右向左匀速运动，直至管道上壁左端角处，完成上壁清洗；

步骤11.3、清洗管道左壁，喷杆旋转步进电机带动喷杆逆时针旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道左壁的上端角处；

步骤11.4、竖直步进电机向下转动，水平步进电机同时向右回转，带动喷头自上向下匀速运动，直至喷头到达管道左壁的下端角处，完成左壁清洗；

步骤11.5、清洗管道下壁，喷杆旋转步进电机带动喷杆逆时针旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道下壁左端角处；

步骤11.6、水平步进电机向右转动，带动喷头自左向右匀速运动，直至管道下壁右端角处，完成下壁清洗；

步骤11.7、清洗管道右壁，喷杆旋转步进电机带动喷杆旋转90°，竖直、水平以及曲度步进电机带动喷头运动，使喷头对准管道右壁的下端角处；

步骤11.8、竖直步进电机向上转动，水平步进电机同时向左回转，带动喷头自下向上匀速运动，直到喷头到达管道内壁的上端角处，完成右壁清洗；

步骤11.9、喷杆回到初始位置，完成一周期清洗。

8.如权利要求5所述的全自动控制方法，其特征在于，所述自动纠偏控制模块包括：

步骤1，将管道清洗机器人轴线中心距离管道中轴线的距离设定为偏距d，将管道清洗机器人轴线偏离管道中轴线的角度设定为偏角α，分别定义偏距d和偏角α的最小容差和最大容差，启动管道清洗机器人，初始化管道清洗机器人两侧履带的速度；

步骤2，测距模块实时获取管道清洗机器人首尾两端的激光测距传感器的返回值，并计算管道清洗机器人的偏距d和偏角α；

步骤3，判断偏距d和偏角α是否在最小容差范围内，若小于最小容差，则返回步骤2；若大于最小容差，则执行步骤4；

步骤4，判断偏距d和偏角α是否在最大容差范围内，若小于最大容差，则执行步骤6，实施小容差纠正方案；若大于最大容差，则执行步骤5，实施大容差纠正方案；

步骤5，执行大容差纠正方案：暂停管道清洗机器人的清洗动作，将偏角α与其最小容差进行对比，若偏角α大于其最小容差，则矫正偏角，若偏角α小于其最小容差，则将偏距d与其最小容差进行对比，若偏距d大于其最小容差，则对偏距进行矫正，若偏距d小于其最小容差，则大容差纠正方案完成，继续返回步骤2；

步骤6，执行小容差纠正方案：将偏角α与其最小容差进行对比，若偏角α大于最小容差，则矫正偏角，若偏角α小于其最小容差，则初始化履带速度后，将偏距d与其最大容差进行对比，若大于，则返回步骤4，若小于，则继续将偏距d与其最小容差进行对比，若大于，则对偏距进行矫正，若小于，则完成小容差纠正方案，继续执行步骤2。

9.如权利要求8所述的全自动控制方法，其特征在于，所述自动适应控制模块包括：

步骤1，履带暂停运动，以管道清洗机器人为原点，根据纠偏控制后的偏距d，对管道清洗机器人四个轴相对于管道的位置进行位置偏斜补偿；

步骤2，以管道清洗机器人为原点，根据步骤3中纠偏控制后的偏角α，对管道清洗机器人的电机转动范围进行角度偏斜补偿。