[发明专利]管道机器人航向保证系统及航向修正方法

权利要求书

1.管道机器人航向保证系统的航向修正方法，其特征在于：所述管道机器人航向保证系统包括前部机身（1）、后部机身（2）、转体驱动装置（3）、行进驱动装置（4）、控制器（5）；

所述前部机身（1）上设置有前车轮（6）；

所述前部机身（1）与所述后部机身（2）之间利用转体驱动装置（3）连接，用于驱动所述前部机身（1）相对于所述后部机身（2）做径向运动；

所述后部机身（2）上设置有后车轮（7）；

所述前部机身（1）还设置有前部姿态角度检测传感器（8），用于检测前部机身（1）的航向角度信号；

所述后部机身（2）还设置有后部姿态角度检测传感器（9），用于检测后部机身（2）的航向角度信号；

所述行进驱动装置（4）设置在后部机身（2）上，用以推动机器人行进，此时所述前车轮（6）为航向调整轮，用于调整管道机器人的航向，所述后车轮（7）为辅助轮，用于辅助管道机器人的航向调整和行进；

所述控制器（5）设置在前部机身（1）上，与前部姿态角度检测传感器（8）、后部姿态角度检测传感器（9）、转体驱动装置（3）、行进驱动装置（4）进行信号连接，用以接收前部姿态角度检测传感器（8）、后部姿态角度检测传感器（9）的检测信号，并向转体驱动装置（3）、行进驱动装置（4）发送执行控制信号；

包括如下步骤；

步骤1：控制器（5）获取前部姿态角度检测传感器（8）、后部姿态角度检测传感器（9）传递的姿态角度信号，并与初始给定的航向对比，判断偏航方向、偏航角度的多少；

步骤2：根据判断结果输出修正航向的扭转力矩F×L1给转体驱动装置（3），F为转体扭力，L1为转体扭力的力臂长度，使前部机身（1）相对于后部机身（2）进行设定方向的径向相对转动；

步骤3：因为步骤2的相对转动，航向调整轮中的一侧轮会离地抬起，另一侧轮维持落地状态，此时扭转力矩使前部机身（1）有一个指向落地侧轮的偏转力f1及其对应的偏转力矩，令管道机器人行进方向偏向落地侧轮方向，在行进驱动装置（3）的驱动下行进一个时间段，实现一次行进航向修正；

步骤4：通过转体驱动装置（3）与步骤1中旋转方向进行反向旋转，使得两个侧轮都恢复到落地状态，并再次将当前的航向角与初始的航向角进行比对；

步骤5：重复上述步骤，直到行进航向与期望航向的差值小于可接受的误差值，此时偏航方向、偏航角度修正完毕。

2.根据权利要求1所述的管道机器人航向保证系统的航向修正方法，其特征在于：所述行进驱动装置（4）还可设置在前部机身（1）上，用以带动管道机器人行进运动；

此时所述前车轮（6）为辅助轮，用于辅助管道机器人的航向调整和行进，所述后车轮（7）为航向调整轮，用于调整管道机器人的航向。

3.根据权利要求1所述的管道机器人航向保证系统的航向修正方法，其特征在于：所述前部姿态角度检测传感器（8）、后部姿态角度检测传感器（9）均具有一维姿态角度即航向角或者二维姿态角度即航向角和横滚角，三维姿态角度即航向角、横滚角和俯仰角的检测功能。

4.根据权利要求1所述的管道机器人航向保证系统的航向修正方法，其特征在于：所述行进驱动装置（4）设置有驱动轮或/和蠕动节，用以推动管道机器人在管内行进。

5.根据权利要求1所述的管道机器人航向保证系统的航向修正方法，其特征在于：所述转体驱动装置（3），为往复旋转驱动装置或连续旋转驱动装置。

6.根据权利要求1所述的管道机器人航向保证系统的航向修正方法，其特征在于：控制器（5）设置扭转力矩F×L1和行进驱动的持续时间，以确定一次航向修正过程中航向角度可修正的大小。

7.根据权利要求1所述的管道机器人航向保证系统的航向修正方法，其特征在于：判断偏航方向、偏航角度的方法为：利用前部姿态角度检测传感器（8）、后部姿态角度检测传感器（9）检测的当前航向角度值与期望的航向角度进行比较，以确定偏航方向和偏航角度。