一种多传感器融合的电缆沟智能巡检机器人自动导航方法

摘要

本发明公开了一种多传感器融合的电缆沟智能巡检机器人自动导航方法，涉及机器人自动导航领域，包括基于改进的D*Lite算法的路径规划和多传感器约束，所述基于改进的D*Lite算法的路径规划是通过所述机器人当前点的上下左右方向进行障碍物约束条件，增加了所述机器人运动过程中的安全距离，并在所述机器人运动到路径的关键点之间时进行了一定平滑处理让所述机器人走弧线；所述多传感器约束可以在层次复杂的电缆沟环境下，更好地辅助所述改进的D*Lite算法。利用本发明提供的算法避免了机器人在电缆沟内部运动时过冲现象，自主导航避障变得更好。

说明书

技术领域

本发明涉及一种机器人在复杂环境电缆沟提供一种导航路径规划的算法，特别涉及到改进的D*Lite导航算法，通过陀螺仪、深度相机、水体传感器得到的信息对导航过程中进行约束的，使机器人能在电缆沟环境导航时，碰到无法识别的障碍物能做出调整，该算法基于改进D*Lite与多传感器约束算法。

背景技术

2010年华中科技大学基于栅格环境下A＊算法规划出的移动机器人路径存在折线多、转折次数多、累计转折角度大等问题。为获得较优路径,提出平滑A＊算法。在A＊算法规划的路径基础上，遍历路径中的所有节点，当某一节点前后节点连线上无障碍物时，将延长线路的这一中间节点删除，建立平滑A＊模型。仿真结果表明，平滑A＊算法优于Ant(蚁群)，Anytime D＊算法。平滑A＊算法路径长度降低约5％，累计转折次数降低约50％，累计转折角度减少30％～60％。平滑A＊算法能处理不同栅格规模下、障碍物随机分布的复杂环境下移动机器人路径规划问题。

2018年西安交通大学提出了一种改进型D*Lite车辆动态路径规划算法，首先获取车辆当前状态及目标状态，并获取当前环境地图信息，再使用形态学操作对当前环境地图中的障碍物进行膨胀。使用D*Lite算法在膨胀后的环境地图上搜索路径；即将到达终点车辆时，根据Reeds-Shepp曲线进行调整朝向角。

2019年浙江工业大学为了解决较大场景下A*寻路算法存在的内存开销大、计算时间长等问题，在A*算法的基础上，结合跳点搜索算法，提出一种改进的A*算法.该算法通过筛选跳点进行扩展，直到生成最终路径，扩展过程中使用跳点代替A*算法中大量可能被添加到OpenList和ClosedList的不必要节点，从而减少计算量。

2019年，湖南省电力公司带电作业中心和国网浙江新昌电力公司变电站为了满足电力系统运维过程中电缆沟巡检的任务要求，与长沙理工大学设计了基于烟花算法改进的算法协助机器人在电缆沟环境下进行导航通过算法平滑处理的路线比不加烟花算法路D*Lite线的关键路径转折点更少，与障碍物保持的距离更远，这样减少了机器人的判断和转向次数，降低了与障碍物发生碰撞的风险并在实际环境中进行了测试。

2020年山东理工大学由于传统的人工势场法存在局部极小点问题，导致移动机器人徘徊不前，无法到达目标点。提出了基于人工势场法的左转势场法和虚拟目标点法。左转势场法将处于局部极小点的移动机器人强制转向，跳出局部极小点；虚拟目标点法是当移动机器人处于局部极小点时在合适位置设置虚拟目标点，忽略目标点和障碍物对移动机器人的作用，使移动机器人转向，跳出局部极小点。经Matlab仿真证明，这两种方法均能绕开局部极小点，顺利到达目标点。

华中科技大学、浙江工业大学提出改进的A*算法虽然取得较好的避障效果，但对于地下电缆沟复杂的环境机器人无法将所有的障碍物一次性全部进行识别，在机器人行使A*算法的路径规划中，这种传统算法无法对突然出现的障碍物进行路径规划修正。

西安交通大学提出了一种改进型D*Lite车辆动态路径规划可实现实时的动态路径规划，但在电缆沟中的实际环境中，D*Lite算法到达了障碍物较近范围内后才进行自转调节角度并重新规划路径，导致在环境较为复杂的地下电缆沟环境所规划路径时易发生过冲现象，影响实际通行。

长沙理工大学与湖南省电力公司带电作业合作的基于烟花算法改进的D*Lite算法在电缆隧道中进行路劲规划，但只是用单一的激光雷达进行障碍物识别，对于环境较复杂的电缆沟环境导航效果效果不佳。

山东理工大所提出改进型人工势场导航算法对于电缆沟这钟狭窄，且障碍物相似较高的环境容易找不到路径，可能发生振荡，在狭窄通道中摆动。

在实际电缆沟进行导航的过程中，经常存在“沙坑”、悬挂在半空的电缆线，以及积水、积潮。机器人无法提前获取这些障碍物，导致这些导航算法无法在电缆沟环境下进行。同时在地下电缆沟道路碎石、瓦砾较多，导航算法移动方向只有0.25π的整数倍限制，由于在地下电缆沟需要一定的越障能力，速度不能过低，这样导致转弯时易过充、滑飘，影响机器人本身的定位导致影响导航。

因此，本领域的技术人员致力于设计一种算法，对于特定的复杂电缆沟环境，使得导航算法与障碍物保持一定的距离，在转弯时进行曲线化处理，使路径更加平滑，选择最好的路径而不只是最近的距离，并通过传感器的约束处理在电缆沟环境下无法被识别到的特殊障碍物情况进行对应操作。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是为实现上述目的，本发明提供了一种对于特定的复杂电缆沟环境，使得导航算法与障碍物保持一定的距离，在转弯时进行曲线化处理，使路径更加平滑，选择最好的路径而不只是最近的距离；并通过传感器的约束处理在电缆沟环境下无法被识别到的特殊障碍物情况进行对应操作。

为实现上述目的，本发明提供了一种多传感器融合的电缆沟智能巡检机器人自动导航方法，其特征在于，包括基于改进的D*Lite算法的路径规划和多传感器约束，其中，所述基于改进的D*Lite算法的路径规划是通过所述机器人当前点的上下左右方向进行障碍物约束条件，增加了所述机器人运动过程中的安全距离，并在所述机器人运动到路径的关键点之间时进行了一定平滑处理让所述机器人走弧线；所述多传感器约束可以在层次复杂的电缆沟环境下，更好地辅助所述改进的D*Lite算法。

进一步地，所述基于改进的D*Lite算法的路径规划包括障碍物约束，转折点曲线平滑。

进一步地，所述多传感器约束包括深度约束，重心约束，积水约束。进一步地，所述障碍物约束是指在路径规划之前，对所述机器人当前位置节点的上下节点和左右节点进行判断。

进一步地，如果所述上下节点为所述障碍物，则将该所述位置节点以及该所述位置节点的所述左右节点放入关闭列表中，如果所述左右节点为所述障碍物，则将该所述位置节点以及该所述位置节点的所述上下节点放入所述关闭列表中，在所述关闭列表中的所述位置节点之后将不进行扩展。

进一步地，所述转折点曲线平滑是通过对所述机器人以折线转弯时的32种情况进行对应的曲线平滑。

进一步地，所述曲线平滑的公式如下：

a

z

z

在上述公式中，(x

上述公式

G

G

上述公式G

进一步地，所述深度约束是指将深度相机得到的深度信息转换成伪激光数据与激光雷达进行融合，所述深度相机所得到的深度图的分辨率为240×320，将所述深度图转换成激光数据的格式，在转换之前需要先将所述深度图进行压缩处理，根据本发明的机器云台最高点的实际高度与所述深度图所得到的信息进行标定，取所述深度图的168行以下的像素信息，此外将本发明选取第100行到168行每列中距离成像平面最近的点，如下式：

r

以r

进一步地，所述重心约束是将陀螺仪放置所述机器人左后方位置，以机器人左后方为原点，地底面为XY平面，垂直地面上为Z轴的机器人自身坐标系，通过所述陀螺仪得到的x轴y轴z轴倾角信息来获取所述机器人在导航中重心坐标相对于所述机器人自身坐标系的变化进行所述重心约束，首先在平坦道路通过下式计算出重心坐标[g

g

g

上式中m

conx

cony

0≤g

上式中[conx

进一步地，所述积水约束将水体传感器放置所述机器人底端，当所述机器人踏过积水、积潮淹过所述水体传感器时，所述水体传感器将会发出高电平信号，为使得所述机器人避免因路径规划的原因踏过较深的积水，要求所述水体传感器发出所述高电平信号时所走过的路径要在一定的范围内，

0

上述公式中cond为所述机器人检测到经过积水、积潮时走过的最大路径，d

在本发明的较佳实施方式中，将小车放于电缆沟下，通过以太网与地面终端计算机通信，终端计算机通过发布小车目的地命令进行自动导航。

与现有技术相比，本发明具有如下的有益效果，对于地下电缆沟内部的导航，以往的案例都是引入单独激光雷达或者摄像头进行定位或者建图，但在电缆沟这种复杂情况下对于电缆沟复杂且危险的环境中无法保持准确建图。无法检测其他不在激光雷达平面的障碍物导致机器人导航时受阻不动。以及若由于电缆沟对于积水、积潮管理缺失，如果机器人的防水系统不够稳定，机器人内部结构进水，极其容易导致电路板烧引起危害，特别是在电缆沟充满电线以及有害气体的地方。一般的导航算法考虑周围环境都过于理想趋于平面化。无法实际实施。本发明针对上述情况，提出了针对地下电缆沟通道复杂环境一种基于改进D*Lite与多传感器约束算法的电缆沟自主巡检，将激光雷达的激光数据、深度相机的深度信息的伪激光数据进行融合。丰富了外部观测量的信息，解决了在导航过程中移动机器人对于高于激光雷达而无法观测悬挂的电缆线无法发觉最终导致导航时云台撞击的问题，加入陀螺仪传感器进行重力约束，保证机器人在电缆沟中进行导航时不会因无法识别沙坑，以及无法被激光雷达所扫描到的大石头、瓦砾导致机器人进行的路劲规划出现翻车。加入水体传感器进行积水约束，保证机器人在导航过程时不会经过积水而导致机器人内部进水损坏电路板。并针对D*Lite算法在路径规划中斜线0距离过障碍物，将其运动过程在时对8领域进行过滤去除了同向的多余子节点。使导航时机器人与障碍物间距更大。并在导航进行转弯时对转弯的弧度进行曲线平滑，消除了移动方向只能为0.25π整数倍的限制，使小车在电缆沟通更加平稳行走，避免过充与侧滑撞上电缆沟。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的一个较佳实施例的机器人总体设计框图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

本发明所针对的环境与一般的平面环境的情况不同，对于一般的室内导航机器人，机器人通过激光雷达或超声这种对单一的2d平面范围扫描得到的观测信息进行建图，将所建立的地图信息进行路径规划，但在电缆沟而在地下电缆沟复杂的情况下。机器人通过激光雷达或超声波所建的图来进行导航会遇到以下四个问题。

激光雷达无法检测到悬挂在电缆沟半空中的电缆线，导致机器人上方的云台摄像头遭受到碰撞。

激光雷达无法检测电缆沟内部出现的沙坑，机器人在导航的过程中出现“掉坑”现象以及电缆沟内部存在较大的石块或是低于激光雷达的障碍物，机器在导航中无法察觉，又由于机器人有一定的越障能力，容易导致机器人翻车。

由于电缆沟内部环境复杂，一般机器人选择底盘较低的履带式小车防止机器人在导航过程中翻车。但一般电缆沟内部由于缺少对内部积水的、积潮的管理，底盘较低机器人踏水前进若机器人防水系统不够，容易导致机器人内部电路板涉水导致电路板损坏，机器人整个报废。

本发明基于上述三个问题考虑，提出一种基于改进D*Lite与多传感器约束算法的电缆沟自主巡检。本发明的机器人总体框图如图1。

D*Lite路径规划算法具有很好的适用性，但在电缆沟中的实际环境中，D*Lite算法到达了障碍物较近范围内后才进行自转调节角度并重新规划路径，导致在环境较为复杂的地下电缆沟环境所规划路径时易发生过冲现象，影响实际通行。因此，本发明提出一种对D*Lite规划算法进行改进，通过将机器人当前点的上下左右方向进行障碍物约束条件，增加了机器人运动过程中的安全距离，并在机器人运动到路径的关键点之间时进行了一定平滑处理让机器人走弧线，防止原有算法出现的机器人斜线通过障碍物的情况，避免了机器人在电缆沟内部运动时过冲现象，自主导航避障变得更好。

与传统的D*Lite路径规划算法相比，改进部分如下:

(1)避开障碍物。传统的D*Lite路径规划算法在运动过程中将相邻的8个非障碍物节点进行扩展，由于受到节点搜索策略影响，D*Lite算法总是出现斜线通过障碍物的路径规划，本发明在D*Lite算法对相邻的节点进行扩展之前，对机器人当前位置的上下左右四个节点进行判断，如果上下节点为障碍物则将该节点以及该节点的左右两个节点放入关闭列表中，如果左右节点为障碍物则将该节点以及该节点的上下两个节点放入关闭列表中，在关闭列表中的节点之后将不进行扩展。这样可避免了传统的D*Lite路径在经过障碍物时斜线穿过障碍物。

(2)对转折点进行曲线平滑，由于传统的D*Lite路径规划算法节点移动方向只有0.25π整数倍的限制，机器人路径规划中的尖突的转折点不易在电缆沟满是沙地以及小石子的通道环境下无法平稳前进出现漂移，这将会影响机器人的定位产生较大误差，从而影响到整个机器人的导航。本发明通过对机器人以折线转弯时的32种情况进行对应的曲线平滑。公式如下：

a

z

z

在上述公式中，(x

上述公式

G

G

上述公式G

在实际电缆沟中，机器人行走的环境地面是凹凸不平的沙地，传统的机器人D*Lite算法路径规划中的转折点并未考虑到实际环境操控中转弯需要给与机器人一个提前预热准备，这在实际电缆沟环境下的易造成过充漂移或打滑现象从而影响实际操作中机器人的定位最终导致导航收到影响。本发明提出的曲线平滑算法使机器人在行使D*Lite算法得到转折点路径时进行转弯缓冲，防止了机器人在电缆沟环境下转弯过急而造成滑飘导致偏移规划路线撞上障碍物以及减轻了机器人在电缆沟环境下进行导航时的定位，更加适合于地下电缆沟的导航运动环境。

相对于室内平面移动机器人，在层次复杂的电缆沟环境下，激光雷达得到的单一层面的障碍物信息相对整个电缆沟三维信息极少，最终造成三种现象。本发明针对上述三种情况提出加入多传感器的约束时机器人获取更多的障碍物信息更好的辅助D*Lite算法。

针对高空悬挂电缆情况，本发明提出一种新的方案将深度相机得到的深度信息转换成伪激光数据与激光雷达进行融合，深度相机的深度图像虽然有噪音且不稳定，但能弥补激光雷达无法扫描到的高于自己的障碍物。

所述深度约束是指将深度相机得到的深度信息转换成伪激光数据与激光雷达进行融合，所述深度相机所得到的深度图的分辨率为240×320，将所述深度图转换成激光数据的格式，在转换之前需要先将所述深度图进行压缩处理，根据本发明的机器云台最高点的实际高度与所述深度图所得到的信息进行标定，取所述深度图的168行以下的像素信息，此外将本发明选取第100行到168行每列中距离成像平面最近的点，如下式：

r

以r

针对低洼翻车的情况，所述重心约束是将陀螺仪放置所述机器人左后方位置，以机器人左后方为原点，地底面为XY平面，垂直地面上为Z轴的机器人自身坐标系，通过所述陀螺仪得到的x轴y轴z轴倾角信息来获取所述机器人在导航中重心坐标相对于所述机器人自身坐标系的变化进行所述重心约束，首先在平坦道路通过下式计算出重心坐标[g

g

g

上式中m

conx

cony

0≤g

上式中[conx

针对积水情况，所述积水约束将水体传感器放置所述机器人底端，当所述机器人踏过积水、积潮淹过所述水体传感器时，所述水体传感器将会发出高电平信号，为使得所述机器人避免因路径规划的原因踏过较深的积水，要求所述水体传感器发出所述高电平信号时所走过的路径要在一定的范围内，

0

上述公式中cond为所述机器人检测到经过积水、积潮时走过的最大路径，d

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。