CN111273128A - 一种用于检测地下电缆故障的管道机器人 - Google Patents

Abstract

本申请公开了一种用于检测地下电缆故障的管道机器人，以履带式底盘作为承载和行走机构，在履带式底盘上方设置各结构模块，包括传感器模块、采集控制模块、图像获取模块、驱动控制模块、运动控制模块以及工控机，其中：所述传感器模块用于获取传感器数据，包括激光点云数据、温度数据和磁场数据；所述驱动控制模块用于接收姿态调整指令或运动控制指令，根据指令驱动履带式底盘向不同方向移动或转向；所述采集控制模块实现传感器模块、驱动控制模块与工控机之间的交互；所述运动控制模块用于测量机器人的位姿数据；所述图像获取模块用于获取周围环境的图像信息并发送给工控机；所述工控机用于实现机器人定位、控制、图像采集和相关算法。

Description

一种用于检测地下电缆故障的管道机器人

技术领域

本申请涉及机器人以及管道检测领域，具体涉及一种用于检测地下电缆故障的管道机器人。

背景技术

随着城市的快速发展，电缆管道覆盖面不断扩大，为保证城市供电的可靠性和稳定性，就必须保证电缆无故障。但是，由于电缆在地下深埋，比较容易导致电缆的绝缘层腐蚀，损坏，使金属导线裸露在外。一旦发生故障或者损坏，如果没有好的地下电缆定位技术及故障排查的方法，而是采用随机采挖寻找故障点的方式的话，会严重影响城市的正常运行以及人们的生活，这就要求电缆管道的巡视维修工作的水平需要不断的提高。

对于地下电缆故障检测，如今大多采用的技术是潜望镜法，就是将顶部装有探头的长杆深入电缆管道里面进行检测。这种装置虽然简单，但是操作不灵活，耗费大量的人力物力。管道机器人是一种新型的机器人，自身携带各种传感器，可从管道外，沿着管道行走，完成电缆故障点检测作业的机电一体化系统，使操作人员可以更容易、更方便远程去控制机器人在管道内移动，完成故障点检测作业。同时，操作人员还可以利用该机器人搭载的摄像头，以第一人称的视觉观察周围的环境。然而，现有的利用管道机器人的检测方法存在地图构建不准确，检测效率低等问题。

发明内容

本申请的目的是提供一种用于检测地下电缆故障的管道机器人，用以提升建图准确率和检测效率。

为了实现上述任务，本申请采用以下技术方案：

一种用于检测地下电缆故障的管道机器人，以履带式底盘作为承载和行走机构，在履带式底盘上方设置各结构模块，包括传感器模块、采集控制模块、图像获取模块、驱动控制模块、运动控制模块以及工控机，其中：

所述传感器模块用于获取传感器数据，传感器模块包括SLAM模块以及温度传感器、磁场传感器，其中，SLAM模块包括激光雷达和里程计，激光雷达用于在机器人行走时获取周围环境的激光点云数据；所述温度传感器、磁场传感器则用于获取温度和磁场数据；

所述驱动控制模块用于接收姿态调整指令或运动控制指令，根据指令驱动履带式底盘向不同方向移动或转向；

所述采集控制模块用于编码里程计、接收所述传感器模块获取的传感器数据，包括激光点云数据、温度和磁场数据并传递给工控机，同时接收工控机发送来的指令并发送给驱动控制模块；

所述运动控制模块用于测量机器人的位姿数据，以提供更加精确的机器人位置信息；

所述图像获取模块用于获取周围环境的图像信息并发送给工控机；

所述工控机利用激光SLAM系统，通过对不同时刻点云数据的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，从而完成对机器人自身的定位；同时，工控机将点云数据和位姿数据进行融合，基于ROS系统生成姿态调整指令并发送给驱动控制模块，从而对机器人的姿态进行调整；工控机上装配有WIFI模块，用以和PC机进行通讯，工控机将图像获取模块采集的图像信息通过WIFI模块发送给PC机，使检修人员在PC机上可实时观察到管道内的情况；所述PC机还通过工控机向驱动控制模块发送运动控制指令，以使驱动控制模块根据所述运动控制指令驱动机器人在管道内行进。

进一步地，所述机器人的建图算法、路径规划算法运行在工控机或PC机上，并在建立好的地图上标记故障点的位置。

进一步地，利用路由器搭建WIFI系统，实现工控机和PC机之间的无线通讯，从而使PC机提取机器人各结构模块的信息以及对机器人的运行状态进行实时监控；机器人安装好之后，通过下面方法进行配置：

在同一路由器下，分别配置PC机以及工控机在网段上的域名，然后检修人员PC机连接工控机，在PC机的系统变量上设置工控机的域名，从而完成机器人和PC机通信的配置；完成配置后，检修人员可在PC机上订阅一个话题，就可以进行PC机和工控机的数据交换。

进一步地，对于机器人的控制，检修人员通过键盘发送上下左右以及各个方向上旋转的运动控制指令，通过WIFI无线传输，工控机接收到后发送给驱动控制模块，驱动控制模块通过电机使履带式底盘带动机器人旋转或行进，从而根据实际需要在管道内移动、检测电缆故障；所述图像获取模块还包括搭载摄像头的云台，检修人员可通过键盘控制云台的旋转，以第一视角的角度，观察管道内环境。

进一步地，所述机器人进行建图时采用的算法为：在获得激光雷达每一帧点云数据后，利用扫描匹配在最优的估计位置上插入，生成子地图，待所有的子地图生成完成之后，进行全局优化。

进一步地，所述机器人的建图算法具体包括：

S2.1，获取激光雷达扫描的一帧点云数据，包含辐射状的点信息[x,y,theta]、点集H，同时获取机器人当前的位姿ξ；

S2.2，将点云数据进行降采样，以减少点集的规模的同时保持点集的特征；

S2.3，进入局部优化，迭代地将降采样后的点云数据和子地图进行匹配，以进行子地图的构建；

S2.4，每过一个短的时间段进入全局优化，将所有已构建完成的子地图和所有点云数据作为回环检测的约束进行稀疏姿势调整。

进一步地，所述进入局部优化，迭代地将降采样后的点云数据和子地图进行匹配，以进行子地图的构建，包括：

S2.3-1，对于最新采集到的一帧点云数据，通过点云数据的位姿和子地图位姿构造非线性最小二乘优化，如式1所示，最大化点云数据在子地图中的概率，对点云数据的位姿进行优化，从而得到点云数据插入子地图的位置和角度；

其中，M_smooth()函数用于将二维的坐标数据平滑为一维的概率值数据，本方案中采用双三次插值。T_ξ表示将h_k从点云数据的坐标转换成子地图中的坐标，h_k是点集中的第k个扫描点，ξ表示机器人当前的位姿；

S2.3-2，将点云数据插入子地图时的相对位姿保存在内存中；

S2.3-3，当获得新的一帧点云数据时，在其附近一定空间范围内搜索匹配帧，若该匹配帧符合要求时，则认为是一个回环；该匹配过程的描述如式2：

上式中，W表示搜索空间，M_nearest()用于计算扫描点h_k对应的栅格点的距离值；

为了在W空间中寻找匹配帧的最优解，把全部可行的解空间不断分割为越来越小的子集，并为每个子集内的解的值计算一个下界或者上界；在每次分割后，对凡是界限超出已知可行解值的那些子集，就不再做进一步分割了，这样，解的许多子集就可以不再考虑了，从而缩小了搜索范围；

通过以上步骤，每当获得一帧点云数据后，便与当前最近建立的子地图去进行扫描匹配，使这一帧的点云数据插入到子地图上最优的位置；在不断插入新数据帧的同时，该子地图也得到更新，一定量的数据组合成为一个子地图，当不再有新的点云数据插入到子地图时，便完成对子地图的构建。

进一步地，所述全局优化的过程包括：

通过式3进行全局优化，对历史所有的子地图位姿和点云数据位姿进行优化：

上式中，ρ()表示损失函数，

为子地图位姿，ξ_i ^m为第i个子地图位姿，

表示点云数据位姿，

为第j个点云数据位姿，其中参数m、s表示子地图和点云数据的助记标记符，p、n表示子地图位姿、点云数据位姿的数量；ξ_ij表示点云数据在子地图坐标系下的位姿，描述点云数据在哪一个子地图中坐标匹配；Σ_ij表示相应的协方差矩阵，E表示残差，计算公式如下：

上式中，l()表示拉普拉斯变换，上标T表示转置。

进一步地，所述的子地图通过Rviz工具构建；在建图完成之后，通过TF树变换，并利用ROS系统的话题通讯机制，订阅故障点的具体信息，然后在构建的地图上标记故障点的位置，包括：

定义两个坐标系，一个坐标系原点位于该机器人的履带式底盘中心，为另一个位于激光雷达的中心；

通过采集控制模块获取温度传感器、磁场传感器的数据，并根据这些数据判断是否检测到电缆的故障点，如果检测到电缆存在故障，则通过发布的最新坐标信息作为当前机器人的坐标，记录当前的故障点，并进行发布；

通过读取ROS系统中的TF树中机器人的激光雷达相对于世界坐标系的TF变换，来实时更新机器人当前的位置；另外把点云数据从以激光雷达中心为坐标原点的坐标系下，转换到以履带式底盘中心为坐标原点的坐标系下；

接收发布的机器人所在坐标，然后解析并封装成Rviz中的标记点Marker类，发布给Rviz；

打开Rviz，添加话题，用来接收所发布的电缆故障点信息，并显示出来，同时在PC机的终端上打印出电缆故障点的温度和磁场强度信息。

进一步地，所述机器人采用的路径规划算法为DWA算法，在速度空间中进行速度采样，并对随机采样的速度进行限制，减小采样数目，再使用代价函数进行优化，优化的步骤如下：

S4.1，速度搜索空间，根据以下三点进行速度空间降采样：

圆弧轨迹：该轨迹由采样速度(v,w)决定，这些速度构成一个速度搜索空间；其中v表示线速度，w表示角速度；允许速度：如果机器人能够在碰到最近的障碍物之前停止，那么这个采样速度将被评估；动态窗口：由于机器人加速度的限制，因此只有在加速度时间内能达到的速度才会被保留；

为了能够在碰到障碍物前停下来，因此在最大减速度条件下，速度有一个范围；设刹车时的加速度，包括线加速度和角加速度为

则V_a表示机器人不与障碍物碰撞的速度集合，如式5所示：

上式中，dist(v,w)表示采样速度(v,w)对应轨迹上离障碍物最近的距离；

考虑到机器人的加速度，搜索空间降采样到动态窗口，只保留以当前加速度可达到的速度，设t为时间间隔，(v_a,w_a)为实际的线速度和角速度，那么动态窗口的速度集合为V_d，如式6所示，该集合以外的速度将不能在该时间间隔内到达；

其中，

和

表示刹车时的线加速度和角加速度；

最终的速度搜索空间为式7：

V_r＝V_a∩V_b 式7

S4.2，求最优值，代价函数为式8：

G(v,w)＝σ(α*heading(v,w)+β*dist(v,w)+γ*vel(v,w)) 式8

其中，heading()为评价机器人与目标位置夹角大小的函数，当机器人朝着目标位置移动时，heading()的值取最大；与目标位置偏差越大，heading()的越小；dist(v,w)表示采样速度(v,w)对应轨迹上离障碍物最近的距离；vel(v,w)表示机器人在某条轨迹上向前的移动线速度和角速度；α,β,γ均为正则化系数，用以使代价函数中三个部分都被正则化[0,1]之间；σ表示平滑系数，用以使代价函数中三个部分的权重更加平滑，使得轨迹与障碍物之间保持一定的距离。

结合所述的速度搜索空间和所述代价函数，求得最优的速度，使管道机器人在导航过程中，避开障碍物，以较快的速度行驶。

本申请具有以下技术特点：

1.相对于现有的潜望镜法，工作效率更高，检测作业的危险系数大大降低，能有效代替人力完成地下电缆故障的检测工作，应用前景广阔。

2.采用了温度、磁场、IMU、激光雷达多传感器的数据融合方法来判断电缆故障点位置。

3.本申请采用回环检测和全局优化的算法，减小了构建地图时的累积误差，提升建图准确率，使得管道机器人适用于电缆管道内不平坦的地面和较大的环境。

4.本申请中的避障算法，在动态杂乱环境中可以有效避开障碍物，以较快速度移动，特别适用于管道内这种环境；计算复杂度较低，适用于树莓派这种低成本的工控机。

附图说明

图1为本申请机器人的各结构模块的示意图；

图2为本申请机器人的实体结构示意图；

图3为建图算法的流程示意图；

图4为子地图构建的示意图；

图5为点云数据和子地图的关系图；

图6为本申请在实验时正常管道和非正常管道标记图。

具体实施方式

本申请公开了一种用于检测地下电缆故障的管道机器人，考虑到对地下管道内各种复杂的地形和环境的适应性，该机器人地底盘采用履带式结构，即机器人以履带式底盘作为承载和行走机构，并在履带式底盘的上方设置各结构模块。

所述履带式底盘上，两侧的履带驱动轮采用两轮差速驱动结构。虽然履带式结构在速度上比不过轮式结构，但履带式底盘利用两轮差速可以原地转动，并且通过性强，在不平的地下管道内性能和稳定性好，也具有较长的续航能力。

1.结构模块部分

如图1所示，本方案的机器人在履带式底盘上设置有传感器模块、采集控制模块、图像获取模块、驱动控制模块、运动控制模块以及工控机，其中：

所述传感器模块用于获取传感器数据，包括SLAM模块以及温度传感器、磁场传感器，其中，SLAM模块包括激光雷达和里程计，激光雷达用于在机器人行走时获取周围环境的激光点云数据；所述温度传感器、磁场传感器则用于获取温度和磁场数据。

所述驱动控制模块用于接收姿态调整指令或运动控制指令，根据指令驱动履带式底盘向不同方向移动或转向。

所述采集控制模块采用STM32处理器，采集控制模块用于编码里程计、接收所述传感器模块获取的传感器数据，包括激光点云数据、温度和磁场数据并传递给工控机，同时接收工控机发送来的指令(姿态调整指令或运动控制指令)并发送给驱动控制模块。

所述运动控制模块采用惯性测量单元，用于测量机器人的位姿数据，以提供更加精确的机器人位置信息，从而为机器人导航提供更加精确的位姿信息。

所述图像获取模块采用摄像头，用于获取周围环境的图像信息并发送给工控机。

所述工控机利用激光SLAM系统，通过对不同时刻点云数据的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，从而完成对机器人自身的定位；同时，工控机将点云数据和位姿数据进行融合，基于ROS系统生成姿态调整指令并发送给驱动控制模块，从而对机器人的姿态进行调整。另一方面，工控机上装配有WIFI模块，用以和PC机进行通讯，工控机将图像获取模块采集的图像信息通过WIFI模块发送给PC机，使检修人员在PC机上可实时观察到管道内的情况，以便于电缆故障的定位；另外，PC机还可以通过工控机向驱动控制模块发送运动控制指令，以使驱动控制模块根据所述运动控制指令驱动机器人在管道内行进。本实施例中，工控机采用Raspberry Pi 3B+，在工控机上安装可视化工具，随时观察机器人的移动进程。

在实际应用时，利用路由器搭建WIFI系统，实现工控机和PC机之间的无线通讯，从而使PC机提取机器人各结构模块的信息以及对机器人的运行状态进行实时监控；机器人安装好之后，通过下面方法进行配置：

在同一路由器下，分别配置PC机以及工控机在网段上的域名，然后检修人员通过SSH连接工控机的Ubuntu上，在PC机的系统变量上设置工控机的域名，从而完成机器人和PC机通信的配置。完成配置后，检修人员可在PC机上订阅一个话题，就可以进行PC机和工控机的数据交换；因此，建图、路径规划等算法的运算可以在工控机上进行，也可以在PC机上进行。对于机器人的控制，检修人员通过键盘发送上下左右以及各个方向上旋转的运动控制指令，通过WIFI无线传输，工控机接收到后发送给驱动控制模块，驱动控制模块通过电机使履带式底盘带动机器人旋转或行进，从而根据实际需要在管道内移动、检测电缆故障。另外，为了便于对管道内部电缆的观察，所述图像获取模块还包括搭载摄像头的云台，检修人员可通过键盘控制云台的旋转，从而能在机器人行走时，以第一视角的角度，观察管道内环境。

除了上述通过可视化工具观察管道内环境之外，检修人员也可以在PC机的ROS系统图形化工具Rviz上监控到机器人的运动方向、运行轨迹和运行状态，同样可以查看到通过激光雷达获取的点云数据所生成的环境二维地图。根据实际需要，利用PC机可打印出机器人的运行速度、位姿等信息；本申请的实体结构示意图参见图2。

2.建图算法部分

本方案的机器人在进行建图时的思想是融合多传感器数据，在Rviz工具中进行子地图的构建，并通过回环检测的扫描匹配策略和全局优化消除建图时产生的误差，具体为：在获得激光雷达每一帧点云数据后，利用扫描匹配在最优的估计位置上插入，生成子地图，待所有的子地图生成完成之后，进行全局优化。

这里采用的算法的累计误差很小，特别适用于地下管道这样不平坦的路面以及比较大的场景，使成本较低的激光雷达也能跑出很好的效果，有利于检修人员对电缆故障点位置的分析，如图3所示，具体算法如下：

S2.1，获取激光雷达扫描的一帧点云数据，包含辐射状的点信息[x,y,theta]、点集H，同时获取机器人当前的位姿ξ。其中(x,y)表示位置的平移，theta表示旋转的角度；点集H＝{h_k}_{k＝1,2,...,K},h_k∈R²，集合中每个点h_k均表示点云中的一个扫描点，K为扫描点数量，R表示实数集。当前位姿ξ＝(ξ_x,ξ,_yξ_θ)为点云数据相对所属子地图的位姿的刚性变换，其中ξ_x,ξ,_yξ_θ分别表示x,y,θ方向的位姿。这一步即为前端检测。

S2.2，将点云数据进行降采样，以减少点集的规模的同时保持点集的特征。

S2.3，进入局部优化，迭代地将降采样后的点云数据和子地图进行匹配，以进行子地图的构建，即后端非线性优化；具体步骤包括：

S2.3-1，对于最新采集到的一帧点云数据，通过点云数据的位姿和子地图位姿构造非线性最小二乘优化，如式1所示，最大化点云数据在子地图中的概率，对点云数据的位姿进行优化，从而得到点云数据插入子地图的位置和角度。

其中，M_smooth()函数用于将二维的坐标数据平滑为一维的概率值数据，本方案中采用双三次插值。T_ξ表示将h_k从点云数据的坐标转换成子地图中的坐标，h_k是点集中的第k个扫描点。

S2.3-2，将点云数据插入子地图时的相对位姿保存在内存中，用以进行回环检测。

S2.3-3，回环检测是一种匹配过程，当获得新的一帧点云数据时，在其附近一定空间范围内搜索匹配帧，若该匹配帧符合要求时，则认为是一个回环；该匹配过程的描述如式2：

上式中，W表示搜索空间，M_nearest()用于计算扫描点h_k对应的栅格点的距离值。上式表示对于点云数据中的每一个扫描点束插在子地图上时的信度和，信度和越高，就表示越相似，因此需要在W空间中寻找该信度和最大的匹配帧。

为了在W空间中寻找匹配帧的最优解，把全部可行的解空间不断分割为越来越小的子集，并为每个子集内的解的值计算一个下界或者上界。在每次分割后，对凡是界限超出已知可行解值的那些子集，就不再做进一步分割了，这样，解的许多子集就可以不再考虑了，从而缩小了搜索范围。

通过以上步骤，每当获得一帧点云数据后，便与当前最近建立的子地图去进行扫描匹配，使这一帧的点云数据插入到子地图上最优的位置。在不断插入新数据帧的同时，该子地图也得到更新，一定量的数据组合成为一个子地图，当不再有新的点云数据插入到子地图时，便完成对子地图的构建，如图4所示。

S2.4，每过一个短的时间段(2s左右)进入全局优化，将所有已构建完成的子地图和所有点云数据作为回环检测的约束进行稀疏姿势调整，即位姿优化。

相对位姿是使得一帧点云数据和对应子地图匹配最好的位姿，即在局部上是最优的。因为该算法是通过创建大量的子地图来实现大场景的建图，子地图在短时间内的准确度是可靠的，但长时间以后，会导致累计误差越来越大，为了消除累计误差，要进行全局优化，使所有点云数据对所有子地图的匹配最优。

本方案中通过式3进行全局优化，对历史所有的子地图位姿和点云数据位姿进行优化：

上式中，ρ()表示损失函数，

为子地图位姿，ξ_i ^m为第i个子地图位姿，

表示点云数据位姿，

为第j个点云数据位姿，其中参数m、s表示子地图和点云数据的助记标记符，p、n表示子地图位姿、点云数据位姿的数量。ξ_ij表示点云数据在子地图坐标系下的位姿，描述点云数据在哪一个子地图中坐标匹配；Σ_ij表示相应的协方差矩阵。E表示残差，计算公式如下：

上式中，l()表示拉普拉斯变换，上标T表示转置。

通过以上步骤，可有效减小地图构建时累计的误差。回环检测和全局优化过程中点云数据和子地图的关系如图5所示。即优化局部下的子地图和全局下的子地图之间的位姿误差，又因为所有的子地图位姿存在有约束，也就是对所有子地图的位姿进行优化，使误差error最小。

3.故障点的检测和标记

在建图完成之后，通过TF树变换，并利用ROS系统的话题通讯机制，订阅故障点的具体信息，然后在Rviz工具中构建的地图上标记故障点的位置。

Rviz是一款三维可视化工具，很好的兼容了各种基于ROS软件框架的机器人平台。在Rviz中，可以使用XML对机器人、周围物体等任何实物进行尺寸、质量、位置、材质、关节等属性的描述，并且在界面中呈现出来。同时，Rviz还可以通过图形化的方式，实时显示机器人传感器的信息，机器人的运动状态，周围环境的变化等。

TF树变换，就是定义了不同的坐标系之间的平移和旋转的关系。本申请中机器人拥有履带式底盘、位于底盘正上方并俯视机器人中心的激光雷达，TF定义了两个坐标系，一个坐标系原点位于该机器人的履带式底盘中心，为base_link，另一个位于激光雷达的中心，为base_laser。本申请中，进行故障点的标记过程如下：

S3.1，打开points_ready2pub节点。该节点通过采集控制模块获取温度传感器、磁场传感器的数据，并根据这些数据判断是否检测到电缆的故障点，如果检测到电缆存在故障，则通过refresh_bodytf所发布的最新坐标信息作为当前机器人的坐标，记录当前的故障点，并发布给error_display节点。

S3.2，打开refresh_bodytf节点，该节点通过读取ROS系统中的TF树中机器人的激光雷达相对于世界坐标系的TF变换，来实时更新机器人当前的位置。激光雷达采集得到的数据也无法直接进行SLAM任务的操作，因为数据是基于激光雷达中心，所以还需要做另外的一步就是数据坐标变换，把点云数据从从base_laser变换到base_link。

S3.3，打开error_display节点。该节点用来接收points_ready2pub所发布的机器人所在坐标，然后解析并封装成Rviz中的标记点Marker类，发布给Rviz。

S3.4，打开Rviz，添加visualization_points话题，用来接收error_display所发布的电缆故障点信息，并显示出来，同时在PC机的终端上打印出电缆故障点的温度和磁场强度信息。

在进行实验时，可通过放置铝铁硼磁铁，来模拟电缆管道内电缆破损所产生的磁场强度和温度升高，检修人员操作PC机，控制该机器人在管道内建图，并通过摄像头观察四周围的环境，结合磁场强度传感器和温度传感器的实时数据，来判断有无故障点，然后将故障点位置在Rviz上标记出来。本申请在进行实验时标记图如图6所示，图中浅灰色点为标记的故障点。

4.机器人的路径规划

在进行故障点检测之前，设置好机器人的路径规划算法，然后再打开导航结点，使机器人在管道内自主行驶，同时探索未知环境。

本申请中路径规划算法采用DWA算法，在速度空间中进行速度采样，并对随机采样的速度进行限制，减小采样数目，再使用代价函数进行优化。优化的步骤如下：

S4.1，速度搜索空间，需要根据以下三点进行速度空间降采样：

圆弧轨迹：该轨迹由采样速度(v,w)决定，这些速度构成一个速度搜索空间；其中v表示线速度，w表示角速度；允许速度：如果机器人能够在碰到最近的障碍物之前停止，那么这个采样速度将被评估。动态窗口：由于机器人加速度的限制，因此只有在加速度时间内能达到的速度才会被保留。

为了能够在碰到障碍物前停下来，因此在最大减速度条件下，速度有一个范围。设刹车时的加速度，包括线加速度和角加速度为

则V_a表示机器人不与障碍物碰撞的速度集合，如式5所示：

上式中，dist(v,w)表示采样速度(v,w)对应轨迹上离障碍物最近的距离。

考虑到机器人的加速度，搜索空间降采样到动态窗口，只保留以当前加速度可达到的速度，设t为时间间隔，(v_a,w_a)为实际的线速度和角速度，那么动态窗口的速度集合为V_d，如式6所示，该集合以外的速度将不能在该时间间隔内到达。

其中，

和

表示刹车时的线加速度和角加速度。

最终的速度搜索空间为式7：

V_r＝V_a∩V_b 式7

S4.2，求最优值，代价函数为式8：

G(v,w)＝σ(α*heading(v,w)+β*dist(v,w)+γ*vel(v,w)) 式8

其中，heading()为评价机器人与目标位置夹角大小的函数，当机器人朝着目标位置移动时，heading()的值取最大；与目标位置偏差越大，heading()的越小。dist(v,w)表示采样速度(v,w)对应轨迹上离障碍物最近的距离。vel(v,w)表示机器人在某条轨迹上向前的移动线速度和角速度；α,β,γ均为正则化系数，用以使代价函数中三个部分都被正则化[0,1]之间；σ表示平滑系数，用以使代价函数中三个部分的权重更加平滑，使得轨迹与障碍物之间保持一定的距离。

通过将S4.1求得的速度搜索空间代入该代价函数中，便可以求得最优的速度，使管道机器人在导航过程中，避开障碍物，以较快的速度行驶。

该算法主要应用于动态环境下的导航和避障，对于无法预测的障碍物，DWA算法可以较好的解决。该算法的优点是计算复杂度较低，且可以使机器人的移动速度较快。

打开导航节点，工控机端对检修人员在PC控制机上指定的目标点进行解析，发出对电机相应的控制指令，管道机器人以最快的速度，避开障碍物，到达指定的目标点，并在Rviz上展现地图上的标记，同时配合搭载的摄像头观察周围的环境状况。

Claims (10)

1.一种用于检测地下电缆故障的管道机器人，其特征在于，所述机器人以履带式底盘作为承载和行走机构，在履带式底盘上方设置各结构模块，包括传感器模块、采集控制模块、图像获取模块、驱动控制模块、运动控制模块以及工控机，其中：

所述传感器模块用于获取传感器数据，传感器模块包括SLAM模块以及温度传感器、磁场传感器，其中，SLAM模块包括激光雷达和里程计，激光雷达用于在机器人行走时获取周围环境的激光点云数据；所述温度传感器、磁场传感器则用于获取温度和磁场数据；

所述驱动控制模块用于接收姿态调整指令或运动控制指令，根据指令驱动履带式底盘向不同方向移动或转向；

所述采集控制模块用于编码里程计、接收所述传感器模块获取的传感器数据，包括激光点云数据、温度和磁场数据并传递给工控机，同时接收工控机发送来的指令并发送给驱动控制模块；

所述运动控制模块用于测量机器人的位姿数据，以提供更加精确的机器人位置信息；

所述图像获取模块用于获取周围环境的图像信息并发送给工控机；

所述工控机利用激光SLAM系统，通过对不同时刻点云数据的匹配与比对，计算激光雷达相对运动的距离和姿态的改变，从而完成对机器人自身的定位；同时，工控机将点云数据和位姿数据进行融合，基于ROS系统生成姿态调整指令并发送给驱动控制模块，从而对机器人的姿态进行调整；工控机上装配有WIFI模块，用以和PC机进行通讯，工控机将图像获取模块采集的图像信息通过WIFI模块发送给PC机，使检修人员在PC机上可实时观察到管道内的情况；所述PC机还通过工控机向驱动控制模块发送运动控制指令，以使驱动控制模块根据所述运动控制指令驱动机器人在管道内行进。

2.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述机器人的建图算法、路径规划算法运行在工控机或PC机上，并在建立好的地图上标记故障点的位置。

3.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，利用路由器搭建WIFI系统，实现工控机和PC机之间的无线通讯，从而使PC机提取机器人各结构模块的信息以及对机器人的运行状态进行实时监控；机器人安装好之后，通过下面方法进行配置：

在同一路由器下，分别配置PC机以及工控机在网段上的域名，然后检修人员PC机连接工控机，在PC机的系统变量上设置工控机的域名，从而完成机器人和PC机通信的配置；完成配置后，检修人员可在PC机上订阅一个话题，就可以进行PC机和工控机的数据交换。

4.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，对于机器人的控制，检修人员通过键盘发送上下左右以及各个方向上旋转的运动控制指令，通过WIFI无线传输，工控机接收到后发送给驱动控制模块，驱动控制模块通过电机使履带式底盘带动机器人旋转或行进，从而根据实际需要在管道内移动、检测电缆故障；所述图像获取模块还包括搭载摄像头的云台，检修人员可通过键盘控制云台的旋转，以第一视角的角度，观察管道内环境。

5.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述机器人进行建图时采用的算法为：在获得激光雷达每一帧点云数据后，利用扫描匹配在最优的估计位置上插入，生成子地图，待所有的子地图生成完成之后，进行全局优化。

6.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述机器人的建图算法具体包括：

S2.1，获取激光雷达扫描的一帧点云数据，包含辐射状的点信息[x,y,theta]、点集H，同时获取机器人当前的位姿ξ；

S2.2，将点云数据进行降采样，以减少点集的规模的同时保持点集的特征；

S2.3，进入局部优化，迭代地将降采样后的点云数据和子地图进行匹配，以进行子地图的构建；

S2.4，每过一个短的时间段进入全局优化，将所有已构建完成的子地图和所有点云数据作为回环检测的约束进行稀疏姿势调整。

7.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述进入局部优化，迭代地将降采样后的点云数据和子地图进行匹配，以进行子地图的构建，包括：

S2.3-1，对于最新采集到的一帧点云数据，通过点云数据的位姿和子地图位姿构造非线性最小二乘优化，如式1所示，最大化点云数据在子地图中的概率，对点云数据的位姿进行优化，从而得到点云数据插入子地图的位置和角度；

其中，M_smooth()函数用于将二维的坐标数据平滑为一维的概率值数据，本方案中采用双三次插值。T_ξ表示将h_k从点云数据的坐标转换成子地图中的坐标，h_k是点集中的第k个扫描点，ξ表示机器人当前的位姿；

S2.3-2，将点云数据插入子地图时的相对位姿保存在内存中；

S2.3-3，当获得新的一帧点云数据时，在其附近一定空间范围内搜索匹配帧，若该匹配帧符合要求时，则认为是一个回环；该匹配过程的描述如式2：

上式中，W表示搜索空间，M_nearest()用于计算扫描点h_k对应的栅格点的距离值；

为了在W空间中寻找匹配帧的最优解，把全部可行的解空间不断分割为越来越小的子集，并为每个子集内的解的值计算一个下界或者上界；在每次分割后，对凡是界限超出已知可行解值的那些子集，就不再做进一步分割了，这样，解的许多子集就可以不再考虑了，从而缩小了搜索范围；

通过以上步骤，每当获得一帧点云数据后，便与当前最近建立的子地图去进行扫描匹配，使这一帧的点云数据插入到子地图上最优的位置；在不断插入新数据帧的同时，该子地图也得到更新，一定量的数据组合成为一个子地图，当不再有新的点云数据插入到子地图时，便完成对子地图的构建。

8.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述全局优化的过程包括：

通过式3进行全局优化，对历史所有的子地图位姿和点云数据位姿进行优化：

上式中，ρ()表示损失函数，

为子地图位姿，

为第i个子地图位姿，

表示点云数据位姿，

为第j个点云数据位姿，其中参数m、s表示子地图和点云数据的助记标记符，p、n表示子地图位姿、点云数据位姿的数量；ξ_ij表示点云数据在子地图坐标系下的位姿，描述点云数据在哪一个子地图中坐标匹配；Σ_ij表示相应的协方差矩阵，E表示残差，计算公式如下：

上式中，l()表示拉普拉斯变换，上标T表示转置。

9.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述的子地图通过Rviz工具构建；在建图完成之后，通过TF树变换，并利用ROS系统的话题通讯机制，订阅故障点的具体信息，然后在构建的地图上标记故障点的位置，包括：

定义两个坐标系，一个坐标系原点位于该机器人的履带式底盘中心，为另一个位于激光雷达的中心；

通过采集控制模块获取温度传感器、磁场传感器的数据，并根据这些数据判断是否检测到电缆的故障点，如果检测到电缆存在故障，则通过发布的最新坐标信息作为当前机器人的坐标，记录当前的故障点，并进行发布；

通过读取ROS系统中的TF树中机器人的激光雷达相对于世界坐标系的TF变换，来实时更新机器人当前的位置；另外把点云数据从以激光雷达中心为坐标原点的坐标系下，转换到以履带式底盘中心为坐标原点的坐标系下；

接收发布的机器人所在坐标，然后解析并封装成Rviz中的标记点Marker类，发布给Rviz；

打开Rviz，添加话题，用来接收所发布的电缆故障点信息，并显示出来，同时在PC机的终端上打印出电缆故障点的温度和磁场强度信息。

10.根据权利要求1所述的用于检测地下电缆故障的管道机器人，其特征在于，所述机器人采用的路径规划算法为DWA算法，在速度空间中进行速度采样，并对随机采样的速度进行限制，减小采样数目，再使用代价函数进行优化，优化的步骤如下：

S4.1，速度搜索空间，根据以下三点进行速度空间降采样：

圆弧轨迹：该轨迹由采样速度(v,w)决定，这些速度构成一个速度搜索空间；其中v表示线速度，w表示角速度；允许速度：如果机器人能够在碰到最近的障碍物之前停止，那么这个采样速度将被评估；动态窗口：由于机器人加速度的限制，因此只有在加速度时间内能达到的速度才会被保留；

为了能够在碰到障碍物前停下来，因此在最大减速度条件下，速度有一个范围；设刹车时的加速度，包括线加速度和角加速度为

则V_a表示机器人不与障碍物碰撞的速度集合，如式5所示：

上式中，dist(v,w)表示采样速度(v,w)对应轨迹上离障碍物最近的距离；

考虑到机器人的加速度，搜索空间降采样到动态窗口，只保留以当前加速度可达到的速度，设t为时间间隔，(v_a,w_a)为实际的线速度和角速度，那么动态窗口的速度集合为V_d，如式6所示，该集合以外的速度将不能在该时间间隔内到达；

其中，

和

表示刹车时的线加速度和角加速度；

最终的速度搜索空间为式7：

V_r＝V_a∩V_b 式7

S4.2，求最优值，代价函数为式8：

G(v,w)＝σ(α*heading(v,w)+β*dist(v,w)+γ*vel(v,w)) 式8

其中，heading()为评价机器人与目标位置夹角大小的函数，当机器人朝着目标位置移动时，heading()的值取最大；与目标位置偏差越大，heading()的越小；dist(v,w)表示采样速度(v,w)对应轨迹上离障碍物最近的距离；vel(v,w)表示机器人在某条轨迹上向前的移动线速度和角速度；α,β,γ均为正则化系数，用以使代价函数中三个部分都被正则化[0,1]之间；σ表示平滑系数，用以使代价函数中三个部分的权重更加平滑，使得轨迹与障碍物之间保持一定的距离。

结合所述的速度搜索空间和所述代价函数，求得最优的速度，使管道机器人在导航过程中，避开障碍物，以较快的速度行驶。

Images