基于改进D星路径规划算法的电缆沟机器人导航避障方法与流程

本发明属于机器人导航领域，更具体地，涉及一种基于改进d星(即d^*lite)路径规划算法的电缆沟机器人导航避障方法。

背景技术：

地下电缆沟作为地下输配电网重要组成部分之一，是电网建设中必不可少的设施。沟内电力电缆长时间运行会导致电缆外绝缘老化放电，加上电缆沟中一些动植物尸体经过微生物分解产生的甲烷等可燃性气体积累，最终可能引起地下电缆沟发生火灾爆炸。由于地下电缆沟内部环境狭小复杂，工作人员巡检尤为困难。因此，使用机器人代替人工加强对地下电缆沟的巡检，对尽早发现地下电缆沟内部隐患，降低地下电缆故障率，提高电网运行的稳定性有着至关重要的意义。

但是，由于地下电缆沟内部环境狭小复杂，仅使用d星(即d^*lite)路径规划算法作为电缆沟机器人自主导航避障方法时，规划出的路径并不平滑，电缆沟机器人面对静态环境时规划的预设路径与障碍物非常容易接近，很可能会引发碰撞，且环境发生动态变化时，更是大大增加了碰撞的风险。

因此，亟需设计一种具备抗干扰强、针对电缆隧道复杂环境、稳定性好、精度较高的电缆沟机器人自动导航避障方法，以使得电缆沟机器人在电缆隧道复杂环境中自如的行走。

技术实现要素：

(一)要解决的技术问题

基于上述背景技术，本发明设计了一种基于改进d星路径规划算法的电缆沟机器人导航避障方法，该方法可以在多种障碍物多的场景中应用，解决了d星路径规划算法频繁转弯且导航路径过于接近障碍物的问题，便于使得搭载外形较大的设备和机械臂的电缆沟机器人等进行导航，且不与环境中的障碍物产生碰撞。

(二)技术方案

本发明公开了一种基于改进d星路径规划算法的电缆沟机器人导航避障方法，该方法包括以下步骤：

步骤1：通过电缆沟机器人的本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息；

步骤2：基于步骤1中采集的信息建立电缆沟机器人的数学模型，并用slam算法在自身定位的基础上建立环境地图；

步骤3：电缆沟机器人通过路径规划算法得到一条最优路径，使机器人按照最优路径移动到目标位置；所述步骤3中具体包括以下步骤：

步骤3.1：获取环境信息后，规划最优路径；在所述规划最优路径时，改进d星路径规划算法加入演化算法中的映射操作，让电缆沟机器人可以对电缆沟内规划行走区域的栅格进行安全判定，模运算映射规则见下式：

其中，为在k维度平面上第i个超出边界烟花的位置，和为在k维度平面上的上下边界值，％为模运算符号；

步骤3.2：选择重要转折点，在使用d星路径规划算法的过程中，从第二个路径点开始，如果此路径点的方向和前一个节点的父节点相同，则认为此路径点为重复，将此路径点删除并重新规划路径点组合，按这个方法规划所有路径节点，得到只有起点、转折点和终点的组合，称为重要转折点；

步骤3.3：构建烟花算法适应度函数，适应度函数的选择会影响算法的收敛速度以及是否可以找到近似最优解，考虑到机器人行走路径的长度，用下式(10)来表达路径的代价值：

其中，pij为从i节点到j节点线段的代价值；下式(11)作为个体适应度函数

其中，pmax是一个相对较大且合适的常数；

步骤3.4：使用烟花算法二次规划重要转折点之间的路径，基于适应度函数生成全局最优路径作为最短路径；

步骤3.5：电缆沟机器人做出决策，按照最短路径出发行走，移动到下一节点并指定该节点为起始点。

进一步的，所述步骤3.4中具体包括：使用两相邻的重要转折点作为路径的起点和终点，并再次栅格化两点之间的空间，通过爆炸算子使烟花爆炸，生成n个烟花，每个烟花代表一条起点与终点之间的连线，然后使用适应度函数算出所有烟花的适应度，并重复该过程直到找到终点，通过烟花算法中的烟花的爆炸、火花的选择，即可在两个相邻的重要转折点之间生成近似最佳解，所有的重要转折点之间路径优化过后，即生成全局最优路径作为最短路径。

进一步的，所述步骤2中具体包括以下步骤：

步骤2.1：进行电缆沟机器人的坐标变换；

通过建立地下电缆沟状态监测机器人全局坐标系和局部坐标系之间的关系，以描述地下电缆沟状态监测机器人在二维空间中的位姿，其中{xt,yt}为全局坐标系，原点以运动起点机器人的第一关节几何中心定义，xt为起点时机器人的正前方向，yt为起点时机器人车体第一关节垂直方向，{xr,yr}为局部坐标系，原点以机器人第一关节当前位置几何中心定义，xr为当前机器人的正前方向，yr为当前机器人车体第一关节垂直方向，在全局坐标系下，电缆沟机器人的位置为(x,y)，β为机器人的偏航角，由位姿以及全局坐标系与局部坐标系旋转角度差得出最终全局坐标系和局部坐标系映射公式如下式(2)：

步骤2.2：进行电缆沟机器人的航迹推理，并得到电缆沟机器人的数学模型；设在t时刻状态监测机器人的相对于当前机器人的正前方向的前进角度为θt，线速度为vt，角速度为ωt，规定机器人前进时vt为正，后退时vt为负；机器人逆时针转动时ωt为正，顺时针旋转时ωt为负，则可以求出全局坐标系下地下电缆沟状态监测机器人的位姿增量为

此时，电缆沟机器人在全局坐标系下做近似圆弧运动。

进一步的，所述步骤2中还包括以下步骤2.3：

步骤2.3：在使用slam算法的基础上，采用自适应蒙特卡罗定位算法对机器人自身进行自定位，并使用自适应蒙特卡洛定位算法输出定位坐标；其中，电缆沟机器人采用的自适应蒙特卡洛定位算法包括：

步骤一：进行初始化离散工作后，检测路标，生产路标的不变性表示，并测量距离和角度。

步骤二：驱动电缆沟机器人运动，并根据运动模型估计下一运动，检测路标，计算机器人的相对位置，根据预测值和观测值计算各粒子的偏差；

步骤三：对粒子进行跟踪后，判断粒子是否集中，如果是，则结束定位，如果否，则回到步骤二。

进一步的，所述步骤3.5中还包括：按照起始点行走后，机器人每次移动后判断是否达到目标点，如果达到目标点，则结束导航，如果未达到目标点，则进一步判断环境是否发生变化，如果环境发生变化，则更新节点参数并返回步骤3.1，以重新规划最短路径，如果环境未发生变化，则继续按照原有的最短路径行走到下一节点。

在另外一方面，本发明公开了一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如上述任一项所述的基于改进d星路径规划算法的电缆沟机器人导航避障方法。

(三)有益效果

1)本发明的导航避障方法在对机器人工作环境进行分析后，以机器人为核心建立自身运动模型，主要包括机器人的坐标变换和航迹推理两部分。坐标变换是描述机器人的空间位置，是把一种坐标系变换成另一种坐标系，建立两个坐标系之间的对应关系。航迹推理是根据机器人本体感知传感器计算出机器人的运动轨迹，以便控制机器人完成圆弧巡检运动的自主导航任务。与现有技术相比，本发明的基于改进d^*lite路径规划算法的电缆沟机器人自主导航避障方法，抗干扰强、实施简单、稳定性好、精度较高，可以在复杂环境下运行，通过实时采集电缆隧道内的相关环境信息，根据实时情况作出调整，使得电缆沟机器人可以适应各种复杂环境下的电缆隧道的检修工作。

2)本发明的导航避障方法通过引入烟花算法解决了d星路径规划算法频繁转弯，且导航路径过于接近障碍物的问题，便于机器人搭载外形较大的设备和机械臂而不与障碍物产生碰撞，且可以在多种场景中获得应用，主要应用如下：a)地下电缆沟的巡检，对于地下电缆沟狭小且复杂的地形，人工巡检非常困难，若巡检机器人采用基于改进的d^*lite路径规划算法进行自主导航避障方法，不仅节省了人力成本，而且降低了机器人在电缆沟内发生碰撞而引发事故的风险。b)变电站的巡检，对于地形相对开阔且环境信息的动态变化较剧烈的环境，使用机器人巡检时，同样可以采用基于改进的d^*lite路径规划算法进行自主导航避障，减少不必要的转弯并且提高巡检效率。

附图说明

为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例所需要使用的附图进行简单地介绍：

图1为本发明实施例中电缆沟机器人的自主导航系统框图；

图2为本发明实施例中电缆沟机器人导航避障控制策略流程图；

图3为本发明实施例中的电缆沟机器人做近似圆弧运动的坐标图；

图4为本发明中机器人采用amcl(自适应蒙特卡罗定位)算法的流程图；

图5为本发明中通过烟花算法改进的d^*lite算法规划路径效果对比示意图；

图6为本发明中基于改进d星路径规划算法的电缆沟机器人导航避障方法的流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1-2所示，本发明提供了一种电缆沟机器人的自主导航系统，该系统通过机器人本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息，使得电缆沟机器人能够在运动过程中使用定位与地图构建(simultaneouslocalizationandmapping，简称slam)算法在自身定位的基础上建立地图，并通过全局路径规划算法得到一条最优路径，机器人按照最优路径进行避障的同时从起始点导航移动到目标点，并实现电缆沟的巡检。

如图2-6所示，本发明中基于改进d星路径规划算法的电缆沟机器人导航避障方法，包括以下步骤：

步骤1：通过电缆沟机器人的本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息；

步骤2：基于步骤1中采集的信息建立电缆沟机器人的数学模型，并用slam算法在自身定位的基础上建立环境地图；

进一步的，步骤2包括以下步骤2.1～步骤2.3：

具体的，以电缆沟机器人为核心建立符合自身运动特点的数学模型(也可称运动模型)包括机器人的坐标变换和航迹推理两部分，坐标变换是描述机器人的空间位置，是把一种坐标系变换成另一种坐标系，建立两个坐标系之间的对应关系；航迹推理是根据机器人本体感知传感器计算出机器人的运动轨迹，以便控制机器人完成自主导航任务，。

步骤2.1：进行电缆沟机器人的坐标变换；

对于本发明控制对象的电缆沟机器人来说，因为工作的特殊性，故其与一般的机器人的数学模型不同，在建立数学模型时需把电缆机器人的第一关节看作一个刚体，忽略机器人后面几关节，作为一个刚体看待时，其运动模型可以由6个变量描述，包括三维坐标系和相对全局坐标系的俯仰角、翻滚角和偏航角，由于本发明的电缆沟机器人理论上可以看作在一个二维平面上运动，故可以将三维坐标系的z轴和俯仰角、翻滚角忽略，只用二维平面坐标和偏航角三个变量来描述电缆沟机器人的数学模型，其位姿主要描述地下电缆沟内的电缆沟器人在二维平面坐标系的位置和机器人运动方向。

如图3所示，通过建立地下电缆沟状态监测机器人全局坐标系和局部坐标系之间的关系，以描述地下电缆沟状态监测机器人在二维空间中的位姿，其中{xt,yt}为全局坐标系，原点以运动起点机器人的第一关节几何中心定义，xt为起点时机器人的正前方向，yt为起点时机器人车体第一关节垂直方向，{xr,yr}为局部坐标系，原点以机器人第一关节当前位置几何中心定义，xr为当前机器人的正前方向，yr为当前机器人车体第一关节垂直方向。

在全局坐标系下，电缆沟机器人的位置为(x,y)，β为机器人的偏航角，由全局坐标系与局部坐标系旋转角度差得出。由此，机器人的位姿由向量表示。

通常称β为机器人的航向，β＝0°时，机器人的航向为xr轴方向；β＝90°时，机器人的航向为yr轴方向，没有航向的位姿称为位置，位置关系到机器人在地下电缆沟中对四周的感知，位置用一个二维向量表征。

在建立了机器人在全局坐标系和局部坐标系中的位姿后，机器人局部坐标系与全局坐标系的映射关系为下式(1)，使用正交旋转矩阵r(β)把机器人全局坐标系中的位姿对应到局部坐标系下：

其中作为航向的旋转角度β由于直接影响全局坐标系和局部坐标系之间的映射，所以求解机器人的航向β很重要。

根据位姿的向量，最终得出全局坐标系和局部坐标系映射公式如下式(2)：

步骤2.2：进行电缆沟机器人的航迹推理，并得到电缆沟机器人的数学模型；

进行航迹推理时，因为电缆沟机器人需要围绕第一关节运动巡检，故将机器人的运动分割成一段一段的圆弧进行积分来获得机器人的运动轨迹。

如图2所示，设在t时刻状态监测机器人的线速度为vt，角速度为ωt，规定机器人前进时vt为正，后退时vt为负；机器人逆时针转动时ωt为正，顺时针旋转时ωt为负，则对机器人的控制量为

在全局坐标系中，假设机器人在(t,t+1)内的控制量ut保持不变，机器人围绕一个固定半径圆弧做运动，可知机器人的运动半径r为

如图2所示，为在全局坐标系下机器人做圆弧移动近似图，t+1时刻的位姿xt+1＝(xt+1,yt+1,θt+1)由t时刻的位姿推出，则可有图可知∠xt+1xtd为

如附图2可知，假设地下电缆沟状态监测机器人在(t,t+1)中线速度恒定，则机器人移动的距离为

xt+1xt＝vtδt(6)

则可以求出全局坐标系下地下电缆沟状态监测机器人的位姿增量为

由上式(7)得出的位姿增量，可得到电缆沟机器人在全局坐标系下做近似圆弧运动的数学模型，如下式(8)

步骤2.3：在使用slam算法的基础上，本发明采用自适应蒙特卡罗定位算法(amcl,全称adaptivemontecarlolocalization)对机器人自身进行自定位，并使用自适应蒙特卡洛定位算法输出定位坐标，以使得电缆沟机器人在狭窄的电缆沟内的定位更加准确，本发明中采用的自适应蒙特卡罗定位算法的步骤如附图4所示。

如图4所示，电缆沟机器人采用的自适应蒙特卡洛定位算法包括：步骤一：进行初始化离散工作后，检测路标，生产路标的不变性表示，并测量距离和角度；步骤二：驱动电缆沟机器人运动，并根据运动模型估计下一运动，检测路标，计算机器人的相对位置，根据预测值和观测值计算各粒子的偏差；步骤三：对粒子进行跟踪后，判断粒子是否集中，如果是，则结束定位，如果否，则回到步骤二。

步骤3：电缆沟机器人通过路径规划算法得到一条最优路径，使机器人按照最优路径移动到目标位置。

为使得电缆沟机器人能够高效的完成巡检工作，本发明提出采用烟花算法与d^*lite路径规划算法融合，通过烟花算法确保机器人采用的路径与障碍维持安全距离，并对重要转折点之间的路径进行平滑处理，使机器人能够更好的在电缆沟内部自主导航避障，从而在机器人做圆弧移动时能够保持距离以避开电缆沟内的石头等诸多小障碍。

具体的，改进d星算法实现方法的流程如附图6所示，可知，与现有技术中的d星(d^*lite)算法相比，本发明中采用的改进d星路径规划算法中包括如下步骤：

步骤3.1：获取环境信息后，规划最优路径；在所述规划最优路径时，改进d星路径规划算法加入演化算法中的映射操作，让电缆沟机器人可以对电缆沟内规划行走区域的栅格进行安全判定，模运算映射规则见下式：

其中，为在k维度平面上第i个超出边界烟花的位置，和为在k维度平面上的上下边界值，％为模运算符号。

步骤3.2：选择重要转折点，在使用d星路径规划算法的过程中，从第二个路径点开始，如果此路径点的方向和前一个节点的父节点相同，则认为此路径点为重复，将此路径点删除并重新规划路径点组合，按这个方法规划所有路径节点，得到只有起点、转折点和终点的组合，称为重要转折点。

步骤3.3：构建烟花算法适应度函数，适应度函数的选择会影响算法的收敛速度以及是否可以找到近似最优解，考虑到机器人行走路径的长度，用下式(10)来表达路径的代价值：

其中，pij为从i节点到j节点线段的代价值；下式(11)作为个体适应度函数

其中，pmax是一个相对较大且合适的常数。

步骤3.4：使用烟花算法二次规划重要转折点之间的路径，以实现重要转折点之间的路径平滑，使用两相邻的重要转折点作为路径的起点和终点，并再次栅格化两点之间的空间，通过爆炸算子使烟花爆炸，生成n个烟花，每个烟花代表一条起点与终点之间的连线，然后使用适应度函数算出所有烟花的适应度，并重复该过程直到找到终点，通过烟花算法中的烟花的爆炸、火花的选择，即可在两个相邻的重要转折点之间生成近似最佳解，所有的重要转折点之间路径优化过后，即生成全局最优路径。如图5所示，为通过烟花算法改进d星路径规划算法得到的平滑处理效果示意图，图5中左图为不加烟花算法的d星路径规划算法示意图，右图为改进d星路径规划算法平滑处理后的示意图。

步骤3.5：电缆沟机器人做出决策，按照最短路径出发行走，移动到下一节点并指定该节点为起始点。

进一步的，步骤3.5中还进一步包括，按照起始点(即sstart)行走后，机器人每次移动后判断是否达到目标点(即sgoal)，如果达到目标点，则结束导航，如果未达到目标点，则进一步判断环境是否发生变化，如果环境发生变化，则更新节点参数并回到步骤3.1重新规划最短路径，如果环境未发生变化，则继续按照原有的最短路径行走到下一节点。

需要指出的是，图5中判断的环境是否发生变化指的是电缆沟机器人前进过程中，检查路径上相关格子是否发生变化，当环境信息发生变化且这种变化对最优路径上格子的参数有影响时，才更新节点参数并重新规划最短路径，如果电缆沟内的环境信息变化，但是这种改变对已有最优路径无影响，即不会影响现有的路径规划结果，则算法不更新路径，并继续按照原有的最短路径行走。

另外，在本发明各个实施例中的各步骤对应的功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施例所述方法的部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-onlymemory，rom)、随机存取存储器(randomaccessmemory，ram)、磁碟或者光盘等各种可以存储程序代码的介质。

最后说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。