一种复杂管道检修机器人路径规划方法及系统

本发明属于机器人路径规划，具体地，涉及一种复杂管道检修机器人路径规划方法及系统。

背景技术：

1、目前机器人已经在内部无障碍物以及部分位置半径小幅度变化的管道中实现应用，主要用于管道内的检测、清理和修复作业；目前机器人在整体布局复杂的管道中已经开拓了多种功能，可以携带多种传感器及操作器械来完成指定任务。然而在内部具有多种障碍物、元器件和零部件的管道环境中目前机器人的应用较少，并且很难具备灵活运动和避障的基础功能。当下一些特种设备管道如电力管道及核反应管道发挥着重要作用，然而随着使用年限的提高此类管道的故障次数也逐渐增多。这些管道往往结构紧凑，内部封装了多种重要组成单元，如果一个零部件发生故障往往会带来较为复杂的排除步骤和检修作业，同时不同的故障需要相应的检修方式，会导致长时间的停止运行，例如电力管道发生故障轻则损失负荷、影响设备安全，重则造成大面积停电，对电网的安全稳定运行造成很大影响，复杂管道设备的检修和日常维护得到了高度重视。

2、传统的人工检测方法会造成较长时间的设备停运，可能危害人员人身安全，检修效率低；管道外仪器检测方法无法获取管道内图像信息，并且针对的故障种类较为局限，无法对管道内故障进行修补操作。因此，使用复杂管道检修机器人对管道进行智能化作业是行之有效的方法，不但能够提高类似管道的检修效率，而且可以减少人员伤害，降低人力资源的投入，具有很好的研究意义和实际应用价值。

3、使用上述机器人完成复杂管道内检修作业时，需要采用合理有效的方法规划出机器人执行某项任务时的一条无碰撞路径。目前机器人的路径规划方法大多应用于二维空间场景中或障碍物密度较小的场景中，无法在复杂管道内直接使用；通过结合现有的路径规划算法如a*算法、人工势场法、快速随机扩展树法和概率路标图算法等，并根据机器人的运动特性、实际应用场景和任务需求进行改进，得到机器人在复杂管道中完成任务作业的有效无碰撞路径，具有重要的发明意义。

技术实现思路

1、本发明针对现有机器人路径规划方法无法应用到复杂管道中使机器人无法参与到长距离、复杂空间管道作业的问题，结合当下路径规划先进方法和机器人系统研究成果，提出了一种复杂管道检修机器人路径规划方法及系统，本发明具有一种计算量小、搜索效率高、渐进最优、实时性强等优点。

2、本发明通过以下技术方案实现：

3、一种复杂管道检修机器人路径规划方法：

4、所述方法具体包括以下步骤：

5、步骤1：根据复杂管道检修机器人的外形特征、运动特性、运动模式以及机器人在管道内的运动自由度，确定路径规划空间的维度、信息表示的参数以及各参数具体含义；

6、步骤2：复杂管道检修机器人进入管道后，通过多传感器信息融合方法确定复杂管道检修机器人在管道中的具体位置，并根据其具体位置得到复杂管道检修机器人周围管道环境信息；

7、步骤3：根据步骤2得到的复杂管道检修机器人周围管道环境信息及其在管道中的具体位置，采用基于包围盒碰撞检测的方法，建立机器人以及周围管道环境中障碍物的包围盒模型；进行机器人路径规划过程中的碰撞检测，判断在路径搜索、扩展和连接的过程中机器人是否与管道环境中障碍物发生碰撞；

8、步骤4：根据复杂管道检修机器人所执行的任务和机器人当前所处环境信息确定机器人路径规划的起始位姿及无碰撞发生的目标位姿，并在机器人的路径规划空间中生成起始节点和目标节点，以及确定本次搜索的空间范围，进而确定路径规划空间各维度参数具体范围；

9、步骤5：根据步骤4得到的起始节点和目标节点，使用基于采样的改进双树随机扩展树方法，在路径规划空间起始节点和目标节点各生成一棵节点扩展树，在每次迭代过程中进行有概率的随机搜索并交替向对方扩展，扩展中不断对路径进行优化与改良，直到两棵树的节点相连或两棵树节点之间最近的距离在允许范围内，则得到复杂管道检修机器人从起始位姿到目标位姿的无碰撞路径规划空间轨迹，进而通过路径优化得到机器人的实际无碰撞运动轨迹。

10、进一步地，在步骤2中，

11、所述多传感器信息融合方法包括互补滤波相关方法、卡尔曼滤波相关方法和基于蒙特卡洛的相关方法。

12、进一步地，在步骤2中还包括，

13、通过预先输入的管道内部环境模型得到复杂管道检修机器人周围管道环境信息；

14、或通过复杂管道检修机器人所携带的相关传感器和计算处理器件对管道内环境的三维重建，来得到周围管道环境信息；

15、所述相关传感器包括融合惯性测量单元、轮式里程计、视觉传感器和激光传感器，需要计算处理器结合相关传感器采集的两种及以上数据信息进行三维重建；

16、所述三维重建具体为：

17、先对各个传感器采集的原始数据进行处理，然后传递给计算处理器件进行最终结果的计算与输出；

18、或者直接将传感器的原始数据实时传递给计算处理器件，进行数据处理和结果输出。

19、进一步地，在步骤3中，

20、所述建立机器人以及周围管道环境中障碍物的包围盒模型具体为：

21、对于复杂管道检修机器人采用扇形体包围盒模型进行包络，机器人周围管道内的障碍物采用球包围盒模型进行包络；

22、对于复杂管道检修机器人进行包络具体为：扇形体包围盒的前后两个端面与机器人前后端面重合，扇形体包围盒的外圆面与机器人所在的管道内壁面重合，扇形体包围盒的内圆面与机器人上端面相切，扇形体包围盒的两个侧面与机器人外形两侧面上的棱线重合，以此确定扇形体包围盒具体尺寸参数，并将机器人完全包络；

23、所述机器人周围管道内的障碍物包括内部密封管道、部分连接件、管道内功能零部件及元器件；

24、对机器人周围管道内的障碍物进行包络具体为：类球体的障碍物直接采用球包围盒模型进行包络，该球包围盒模型的球心与障碍物的几何中心重合，球包围盒模型的半径以完全包络为准；类管道状的圆柱体障碍物使用一系列球包围盒进行包络，其中球包围盒的半径为圆柱体障碍物半径的倍，使管道环境内障碍物都建立了球包围盒模型。

25、进一步地，在步骤3中还包括，

26、所述采用基于包围盒的碰撞检测算法进行碰撞检测，具体为：

27、在路径搜索、扩展和连接的过程中，搜索并得到扩展的节点后，得到该节点对应的机器人管道内具体位姿，然后对机器人在该位姿是否与管道内障碍物发生碰撞进行分析，

28、将机器人周围的管道内障碍物球包围盒与机器人扇形体包围盒逐一进行碰撞检测，

29、对每个球包围盒进行检测时需要计算球心到管道轴线的距离、球心相对管道轴线的垂线与管道轴线的交点位置、球心在管道圆周上的角度位置、扇形体包围盒前后端面与管道轴线的交点位置、扇形体的扇面在管道圆周上的角度范围，进而采用几何运算检测每个球包围盒与扇形体包围盒是否发生相交。

30、进一步地，在步骤3中对某一个球包围盒进行判断时，设机器人扇形体包围盒内圆半径为r1，障碍物球包围盒圆心与管道轴心的距离为r2，扇形体长度为l，障碍物球包围盒的半径为r；

31、第一步判断如果r+r2＜r1，则两个包围盒没有重叠部分，即不发生碰撞，否则进行第二步判断；

32、第二步如果扇形体包围盒与球包围盒的中心距离δz大于l/2+r，则两包围盒不发生碰撞，否则进行第三步判断；

33、第三步设扇形体包围盒外缘直线l1与l2在坐标系中与y轴的夹角范围为[θ1,θ2]，而障碍物包围盒与y轴夹角的范围为[θ3,θ4]；

34、当两个范围没有交集，则机器人与障碍物不发生碰撞，如果存在交集，则机器人与管道内障碍物发生碰撞。

35、进一步地，步骤5包含如下流程：

36、(1)路径搜索与扩展开始时在路径规划空间的起始节点qstart生成一棵随机扩展节点树ta，在目标节点qgoal生成另一棵随机扩展节点树tb，同时建立节点索引矩阵e，然后进入(2)中；

37、(2)首先对ta进行节点扩展，在扩展前生成一个0到1之间的随机数a与比较概率数值p进行对比，如果a小于p则在本次搜索与扩展的空间范围内生成一个随机节点qrand，否则以另一个节点树的初始节点(qstart或qgoal)为随机节点qrand，然后进入(3)中；

38、(3)遍历ta中所有节点，找到与随机节点qrand欧式距离最短的节点qnearesta，如果两节点之间的欧式距离不大于扩展距离qn＝step，则扩展的采样节点为qnewa＝qrand，其中step为扩展的步长，该步长根据搜索效果进行调整；如果qrand与qnearesta之间的欧式距离大于扩展距离qn，则从qnearesta到qrand根据该两点连线的矢量方向扩展一个采样节点qnewa＝qnearesta+qn，随后进入(4)中；

39、(4)对qnewa进行碰撞检测判断其是否有效，具体为根据qnewa中机器人的位姿参数表示得到机器人在管道内的具体位姿，然后建立此时机器人的扇形体包围盒和机器人周围管道环境障碍物的球包围盒并进行碰撞检测，如果机器人在该节点对应的位姿时，周围障碍物的球包围盒都不与机器人扇形体包围盒有重合部分，则将qnewa加入到节点树ta中，并在节点索引矩e中记录下此时qnewa对应的父节点qnearesta，检测完成后进入到(5)中；如果此时机器人扇形体包围盒与任一管道环境障碍物球包围盒有重合部分，则进入到(8)中；

40、(5)以节点qnewa为中心，在距离qnewa为r1的范围内找到随机扩展节点树ta上的一系列节点并归为待选节点，对这些节点做以下遍历判断：如果从这棵树的起始起点经过某待选节点再到qnewa比经过原先的父节点再到qnewa所需要的路径代价更小，则替换qnewa的父节点为该节点，依次判断找到该范围内qnewa的最优父节点，随后进入到(6)中；

41、(6)以节点qnewa为中心，在距离qnewa为r2的范围内找到随机扩展节点树ta上的一系列节点并归为待选节点，对这些节点做以下遍历判断：如果从这棵树的起始节点经过节点qnewa再到某待选节点比经过该待选节点的父节点再到该节点所需的路径代价更小，则修改该待选节点的父节点为qnewa，依次对所有待选节点进行判断并完成父节点的修改，随后进入到(7)中；

42、(7)首先判断如果qnewa与随机扩展节点树tb上最相近的节点qnearestab之间距离小于设定阈值d，则结束本次路径搜索并进入到(14)中；否则接下来判断如果qnewa＝qrand则进入到(8)中，如果qnewa≠qrand则使qnearesta＝qnewa并返回到(3)中；

43、(8)对随机扩展节点树tb进行扩展，找到最新加入ta中的节点qnewa并选择为qextend，如果没有则选择ta的起始节点为qextend，并且遍历tb中所有节点找到与qextend欧式距离最短的节点qnearestb，如果两节点间的欧式距离不大于扩展距离qn，则生成扩展采样节点qnewb＝qextend，否则采样节点为qnewb＝qnearestb+qn，其中qn＝step，随后进入到(9)中；

44、(9)对qnewb进行碰撞检测判断其是否有效，检测方法同流程(4)，如果机器人在该节点对应的位姿时周围障碍物的球包围盒都不与机器人扇形体包围盒有重合部分，则将qnewb加入到节点树tb中，并在节点索引矩e中记录下此时qnewb对应的父节点qnearestb，检测完成后进入到(10)中；如果此时机器人扇形体包围盒与任一管道环境障碍物球包围盒有重合部分，则进入到(13)中；

45、(10)以节点qnewb为中心，在距离qnewb为r1的范围内找到随机扩展节点树tb上的一系列节点并归为待选节点，对这些节点做以下遍历判断：如果从这棵树的起始起点经过某待选节点再到qnewb比经过原先的父节点再到qnewb所需要的路径代价更小，则替换qnewb的父节点为该节点，依次判断找到该范围内qnewa的最优父节点，随后进入到(11)中；

46、(11)以节点qnewb为中心，在距离qnewb为r2的范围内找到随机扩展节点树tb上的一系列节点并归为待选节点，对这些节点做以下遍历判断：如果从这棵树的起始节点经过节点qnewb再到某待选节点比经过该待选节点的父节点再到该节点所需的路径代价更小，则修改该待选节点的父节点为qnewb，依次对所有待选节点进行判断并完成父节点的修改，随后进入到(12)中；

47、(12)判断如果qnewb与随机扩展节点树ta上最相近的节点qnearestba之间距离小于设定阈值d，则结束本次路径搜索并进入到(14)中；否则接下来判断如果qnewb＝qextend则进入到(13)中，如果qnewb≠qextend则使qnearestb＝qnewb并返回到(8)中；

48、(13)搜索次数n增加1，并判断如果n小于最大搜索次数n则交换此时ta和tb的所有节点以及每个节点对应的父节点信息，然后返回到(2)中，如果n不小于最大搜索次数n则进入到(14)中；

49、(14)如果n不小于最大搜索次数n，则此时机器人的路径规划未在设定的搜索次数内成功完成，需要进行后续重新搜索；如果n小于最大搜索次数n，则此时机器人在复杂管道内从起始位姿到目标位姿的路径规划成功，如果从流程(7)进入到本流程，则对于ta从qnewa开始搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到ta的初始节点，对于tb从qnearestab搜索其父节点，找到父节点后继续搜索该节点的父节点，循环搜索直到搜索到tb的初始节点，如果从流程(12)进入到本流程则对于tb从qnewb开始搜索父节点，对于ta则从qnearestba搜索其父节点；搜索完所有有效路径节点后将此时ta和tb的初始节点与qstart和qgoal进行比较，如果ta的初始节点为qstart，则得到的规划路径为从qstart到其子节点一直到qnewa，再从qnearestab到其父节点一直到qgoal；如果tb的初始节点为qstart，则得到的规划路径为从qstart到其子节点一直到qnewb，再从qnearestba到其父节点一直到qgoal；得到规划路径后对路径中相邻的两个路径节点进行平滑处理，在相邻节点之间机器人运动的速度和加速度施加一定的运动规律和变化方式，处理后得到机器人在复杂管道内无碰撞的有效避障路径。

50、一种复杂管道检修机器人路径规划系统：

51、所述路径规划系统包括管道环境信息采集单元，机器人定位与几何简化单元，数据整合单元，碰撞检测单元，随机扩展节点树生成单元和无碰撞路径处理单元；

52、所述管道环境信息采集单元用于通过传感器收集的信息和预先存储的管道环境数据对管道环境进行三维重建并得到机器人实时周围管道的元器件和障碍物具体形状和位置信息；

53、所述机器人定位与几何简化单元用于通过里程计等数据结合管道环境信息采集单元收集的信息，实现复杂管道检修机器人的位姿确定，然后根据机器人几何外形和周围管道环境建立包围盒模型；

54、所述数据整合单元用于收集管道环境信息采集单元感知的信息、机器人的起始位姿和目标位姿以及上述包围盒模型，并发送给需要数据的其他单元；

55、所述碰撞检测单元用于实现路径规划中复杂管道检修机器人与管道环境元器件和障碍物的碰撞检测；

56、所述随机扩展节点树生成单元用于实现复杂管道检修机器人路径规划中建立随机扩展节点树ta和tb，在每次迭代的过程中进行有一定概率的随机搜索和交替向对方扩展直到满足结束条件；

57、所述无碰撞路径处理单元用于根据随机扩展节点树生成单元产生的随机扩展节点树ta和tb，将两棵节点树的初始节点与搜索空间起始节点进行比较，获得从起始节点到目标节点经过的所有节点作为规划路径，然后对相邻两个路径节点进行平滑处理，得到机器人在复杂管道内无碰撞的有效避障路径。

58、一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

59、一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时实现上述方法的步骤。

60、本发明有益效果

61、本发明能够使机器人在复杂管道中高效可靠地规划出避障路径，保证机器人在执行复杂管道作业任务时进行无碰撞运动，弥补了现阶段机器人在复杂管道中智能作业的不足；

62、本发明在复杂的空间环境中成功生成了扩展树，并且规划时间短，循环次数少，扩展效率高，并且渐进最优，通过父节点的唯一性从目标点到起始点生成了有效避障路径。。