[发明专利]利用姿态传感器校正管道爬行器运行的控制系统及方法

权利要求书

1.一种利用姿态传感器校正管道爬行器运行的控制系统，其特征在于，

管道爬行器包括上位机和主体结构，其中主体结构为双螺旋滚筒结构，该管道爬行器的运动控制系统包括设置在主体结构上微机电姿态传感器(1)、两个伺服驱动器(2)、两个伺服电机(3)和嵌入式控制器(4)；其中，两个伺服电机(3)分别设置在对应的螺旋滚筒中，伺服电机带动螺旋滚筒转动，实现管道爬行器主体结构的运行；微机电姿态传感器(1)和两个伺服驱动器(2)均与嵌入式控制器(4)电连接，每个伺服驱动器(2)与对应的一个伺服电机(3)电连接，对其进行控制；上位机通过信号电缆与嵌入式控制器(4)电连接，进行通信讯。

2.基于权利要求1中控制系统对管道爬行器运行的控制方法，其特征在于，微机电姿态传感器(1)实时检测爬行器的姿态，并将采集数传送给嵌入式控制器(4)，嵌入式控制器(4)结合上位机中所设置的爬行器工作模式信息，通过两个伺服驱动器(2)分别控制对应的伺服电机(3)，调整伺服电机(3)输出扭矩和转速，控制管道爬行器平稳移动。

3.基于权利要求1中控制系统对管道爬行器运行的控制方法，其特征在于，所述的爬行器包括低速前进、低速后退、高速前进和高速后退四种工作模式，每种工作模式对应的速度在上位机中设定。

4.根据权利要求2或3所述的控制方法，其特征在于，所述爬行器运行时，嵌入式微控制器(4)结合微机电姿态传感器(1)实时检测的爬行器姿态信息，按照如下规则通过伺服驱动器(2)对对应的伺服电机(3)进行闭环转速与扭矩控制，从而防止车身侧翻；管道爬行器主体结构水平放置在管道中，初始姿态角度为0度，管道爬行器主体结构开始向前行走：

1)、管道爬行器主体结构在低速前行阶段：

1.1)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为10°～30°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以60％额定扭矩、25％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

1.2)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为30°～60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以25％额定扭矩、25％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

1.3)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度大于60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

1.4)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为0°～10°，或者爬行器主体结构向右侧倾斜，且倾斜角度为0°～10°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、25％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

1.5)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为10°～30°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、25％额定转速进行旋转，右侧伺服电机以60％额定扭矩、25％额定转速进行旋转；

1.6)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为30°～60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、25％额定转速进行旋转，右侧伺服电机以25％额定扭矩、25％额定转速进行旋转；

1.7)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度大于60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

2)、管道爬行器主体结构在低速后退阶段：

2.1)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为10°～30°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、25％额定转速进行旋转，右侧伺服电机以60％额定扭矩、25％额定转速进行旋转；

2.2)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为30°～60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、25％额定转速进行旋转，右侧伺服电机以25％额定扭矩、25％额定转速进行旋转；

2.3)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度大于60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

2.4)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为0°～10°，或者爬行器主体结构向右侧倾斜，且倾斜角度为0°～10°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、25％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

2.5)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为10°～30°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以60％额定扭矩、25％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

2.6)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为30°～60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以25％额定扭矩、25％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

2.7)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度大于60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

3)、管道爬行器主体结构在高速前行阶段：

3.1)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为10°～30°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以60％额定扭矩、75％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

3.2)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为30°～60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以25％额定扭矩、75％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

3.3)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度大于60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

3.4)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为0°～10°，或者爬行器主体结构向右侧倾斜，且倾斜角度为0°～10°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、75％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

3.5)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为10°～30°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以80％额定扭矩、75％额定转速进行旋转，右侧伺服电机以60％额定扭矩、25％额定转速进行旋转；

3.6)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为30°～60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以80％额定扭矩、75％额定转速进行旋转，右侧伺服电机以25％额定扭矩、25％额定转速进行旋转；

3.7)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度大于60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

4)、管道爬行器主体结构在高速后退阶段：

4.1)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为10°～30°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、75％额定转速进行旋转，右侧伺服电机以60％额定扭矩、25％额定转速进行旋转；

4.2)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为30°～60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、75％额定转速进行旋转，右侧伺服电机以25％额定扭矩、25％额定转速进行旋转；

4.3)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度大于60°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预；

4.4)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向左侧倾斜，且倾斜角度为0°～10°，或者爬行器主体结构向右侧倾斜，且倾斜角度为0°～10°时，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以100％额定扭矩、75％额定转速进行旋转，右侧伺服电机以100％额定扭矩、25％额定转速进行旋转；

4.5)、当管道爬行器主体结构发生倾斜，嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为10°～30°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以60％额定扭矩、75％额定转速进行旋转，右侧伺服电机以80％额定扭矩、25％额定转速进行旋转；

4.6)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度为30°～60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，调整左侧伺服电机以25％额定扭矩、75％额定转速进行旋转，右侧伺服电机以80％额定扭矩、25％额定转速进行旋转；

4.7)、当嵌入式控制器(4)通过微机电姿态传感器(1)检测到爬行器主体结构向右侧倾斜，且倾斜角度大于60°，嵌入式控制器(4)发送控制信号给对应的伺服驱动器(2)，使左、右侧的伺服电机均停止转动，等待人工干预。