CN113867336B - 一种适用于复杂场景下移动机器人路径导航及规划方法 - Google Patents

Abstract

本发明提供一种适用于复杂场景下移动机器人路径导航及规划方法，获取障碍物的点云数据并进行滤波处理，根据代价地图以及机器人在地图中的定位信息和终止点位置，生成由子路径点集构成的全局路径，对子路径点集进行路径优化删除掉非必须子路径点，得到最优子路径点集，构建局部离线路径库，根据最优局部路径评价函数在局部离线路径库中寻找最优局部路径；本发明方法以感知信息为基础提出一种基于滑动窗优化的全局路径规划方法和一种全局重规划判别法，在局部路径选择方面提出了新的路径组评价函数，从而提高了移动机器人的路径导航过程中的稳定性和路径规划的效率，进而提高移动机器人自主导航的效率和平稳性。

Description

一种适用于复杂场景下移动机器人路径导航及规划方法

技术领域

本发明属于移动机器人自主导航技术领域，具体涉及一种适用于复杂场景下移动机器人路径导航及规划方法。

背景技术

在移动机器人系统中，整个导航过程主要分为三个步骤：第一步，获取导航环境的先验代价地图；第二步，根据得到的先验地图进行全局路径规划；第三步，根据定位和传感器信息，以及全局路径进行局部路径规划。

移动机器人自主导航过程中，全局路径规划Dijkstra，A*等算法规划时间较长，无法在短时间内规划出一条有效路径，针对此问题，有学者提出了JPS算法，JPS算法相比于Dijkstra算法和A*算法能够快速规划出一条全局路径，并返回跳点，也称为子路径点，引导移动机器人行进，但是JPS算法规划出的子路径点集并不是最优的，需要对JPS算法产生的子路径点集进一步后端优化，并且现有的大多数导航框架中，当全局规划路径不能继续满足导航要求时，无法快速有效的针对地图信息进行重规划，同时机器人的局部规划中严格跟随全局规划，使移动机器人的自主导航灵活性大大降低，对移动机器人避障产生巨大影响。

路径规划是影响移动机器人自主导航的平稳性和效率的关键环节，目前该环节中所使用的的规划方法存在以下缺陷：

1、搜索时间较长，目前所使用的的A*或Dijkstra等算法在栅格地图搜索过程中会搜索扩展很多无用栅格，导致搜索时间较长，很难满足实时性要求；

2、对生成的全局路径缺少优化，经搜索算法生成的全局路径无法满足移动机器人动力学与运动学约束，算法在栅格地图中只能向8个方向搜索，导致生产的全局路径出现多余的转弯；

3、在全局规划过程中，没有加入重规划；

4、针对局部路径规划中关于路径的评价函数设计不合理，导致用机器人在导航过程中出现一定程度上的抖动。

发明内容

基于上述问题，本发明提出一种适用于复杂场景下移动机器人路径导航及规划方法，包括：

步骤1：获取障碍物的点云数据并进行滤波处理；

步骤2：根据代价地图以及机器人在地图中的定位信息和终止点位置，生成由子路径点集构成的全局路径；

步骤3：对子路径点集进行路径优化删除掉非必须子路径点，得到最优子路径点集；

步骤4：判断机器人前进过程中是否需要重新规划路径；

步骤5：构建局部离线路径库，根据最优局部路径评价函数在局部离线路径库中寻找最优局部路径。

所述步骤1包括：

步骤1.1：将2D激光雷达采集到的信息经过三角函数换算得到对应的数据坐标点；

步骤1.2：构建以数据点p为中心、f_R为半径的圆，计算圆内包含的其他数据点的个数n；

步骤1.3：如果n<f_n，则认为数据点p为噪声点，需要滤除掉，否则保留该数据点，其中，f_n为判断数据点是否为噪声点所设置的阈值。

所述步骤2包括：

步骤2.1：将机器人以代价地图中的当前位置点作为起始点，并将起始点加入到跳点搜索算法中的列表Openlist中；

步骤2.2：计算列表Openlist中所有点的代价值g_cost：

式中，为运行代价，/>为启发代价，cost_parent为父节点代价值，cost_toparent为父节点到节点的欧氏距离；

步骤2.3：将列表Openlist中g_cost最小的点s加入到列表closelist中；

步骤2.4：判断列表closelist中是否存在终点，如果存在，得到全局路径对应的子路径点集，否则判断点s的邻居点s′是否可通行，如果邻居点s′不可通行，则舍弃该邻居点s′，否则判断该邻居点s′是否需要被修剪掉，如果s′需要被修剪掉，则舍弃该邻居点，否则s′加入到列表neiPoint中，修剪原则是跳点搜索中对邻居节点是否需要被搜索所规定的挑选原则；

步骤2.5：计算列表neiPoint中邻居点s′的代价值，并以s′为起点，沿邻居点方向搜寻跳点，并计算每个跳点的代价值；

步骤2.6：判断每个跳点是否在列表closelist或列表Openlist中，如果是，则更新跳点的代价值，否则将该跳点加入到Openlist中；

步骤2.7：重复上述步骤2.2～步骤2.6，一直搜索到目标点。

所述步骤3包括:

步骤3.1：将子路径点集作为滑动窗优化函数的输入，判断子路径点个数是否大于2，如果子路径点个数大于2，则设置滑动窗起始位置点p_u、终止位置点p_v，其中u＝0,v＝2，u,v为起始位置点和终止位置点在子路径点集中的索引，执行步骤3.2；否则子路径点数为2，表示两个子路径点分别为路径的起点和终点，优化结束；

步骤3.2：以起始位置p_u为滑动窗起点，终止位置p_v为滑动窗终点，以代价地图栅格边长l的为步长对直线段均匀采样，采样步长/>

步骤3.3：计算直线段上各采样点在代价地图上所属的栅格：

式中，indx、indy分别为栅格的列索引、行索引，floor(.)表示向下取整，(o_x,o_y)表示代价地图的起始位置坐标，坐标(x_i,y_i)表示为第i个采样点坐标；

步骤3.4：判断第indy行、第indx列的栅格是否为自由区域内的栅格，如果是，则直线段没有经过代价地图中的障碍物层、膨胀层和未知探索层，表示移动机器人可以从p_u点出发，直接朝向p_e点前进，不再需要经过p_u、p_v之间的子路径点，将p_u、p_v之间的子路径点舍弃；否则，表示移动机器人需要经过p_u、p_v之间的子路径点，则将p_u、p_v之间的子路径点保留；

步骤3.5：如果p_u、p_v之间的子路径点被舍弃，滑动窗的起始位置不变，终止位置更新，即令v加1；如果p_u、p_v之间的子路径点被保留，滑动窗的起始位置和终止位置同时更新，即令u加1、v加1；

步骤3.6：如果v大于子路径点的总个数，优化结束，否则重复执行步骤3.2～3.6。

所述步骤4包括：

步骤4.1：判断当前机器人所在的代价地图中的障碍物层栅格和膨胀层栅格是否遮挡机器人的当前子路径点与前一时刻经过的子路径点所形成的直线段L，当未遮挡时，机器人继续运行；当遮挡时，执行步骤4.2；

步骤4.2：计算机器人在代价地图中的当前位置点到直线段L之间的距离，如果距离大于设定的阈值，则执行全局重规划，否则执行步骤4.3；

步骤4.3：当机器人从前一时刻子路径点出发时开始计时，经过预设时间段T后仍无法到达当前子路径点，则执行全局重规划，否则继续前行。

所述步骤5包括：

步骤5.1：构建局部离线路径库；

步骤5.2：以最优子路径点作为虚拟目标点，将子路径点坐标由惯性坐标系转化为移动机器人坐标系下坐标；

步骤5.3：利用公式(4)计算每个路径组的评价值，将评价值最大的路径组作为主路径用于指导机器人向目标点前进；

式中，score_j表示第j个路径组的评价值，当路径组内第m条子路径被障碍物遮挡时，c_m＝0，否则c_m＝1，Δθ_m为第m条子路径的末端与机器人当前位置点的连线所形成的直线段L’与目标点的虚拟点与机器人当前位置点的连线形成的直线段L”的夹角。

所述步骤5.1包括：

1)从原点按照一定角度间隔生成n个路径组的主路经；

2)每个主路径末端按照一定角度间隔再生n条一级子路径；

3)每个一级子路径末端按照一定角度再生成n条二级子路径，即局部离线路径库包含n个路径组，每个路径组包含n²条局部路径，每条局部路径使用三次样条进行优化。

本发明的有益效果是：

本发明提出了一种适用于复杂场景下移动机器人路径导航及规划方法，以感知信息为基础提出一种基于滑动窗优化的全局路径规划方法和一种全局重规划判别法，在局部路径选择方面提出了新的路径组评价函数，从而提高了移动机器人的路径导航过程中的稳定性和路径规划的效率，进而提高移动机器人自主导航的效率和平稳性。

附图说明

图1为本发明中适用于复杂场景下移动机器人路径导航及规划方法流程图；

图2为本发明中适用于复杂场景下移动机器人路径导航及规划方法的原理图；

图3为本发明中跳点搜索算法(简称JPS算法)的流程图；

图4为本发明中对全局路径规划生成的子路径点集进行滑动窗优化的流程图；

图5为本发明中判断机器人前进过程中是否需要重新规划路径的流程图；

图6为本发明中局部路径组评价函数示意图，其中机器人前进方向为x轴，垂直与x轴生成y轴，从而建立机器人坐标系。

具体实施方式

下面结合附图和具体实施实例对发明做进一步说明。本发明提出的一种适用于复杂场景下移动机器人路径导航及规划方法，设置至少一台计算机设备用于执行移动机器人生成代价地图、全局规划、局部规划，同时计算机设备还用于储存感知信息、定位信息和建图信息，微型计算机可以是笔记本电脑，平板电脑或者微型迷你电脑(简称NUC)，导航机器人装备一部2D激光雷达用于移动机器人周围环境信息感知。原理框架图如图2所示，主要包括三部分：感知信息、全局规划以及局部规划；其中，感知信息部分为全局规划和局部规划提供传感器数据，根据传感器数据获取周围环境的感知信息、移动机器人的定位信息，并生成代价地图；全局规划分为三部分：JPS(jump point search)跳点搜索的全局规划、路径优化和重规划判别，跳点搜索根据感知信息部分提供的代价地图、定位信息和目标点信息进行全局路径规划，路径优化将全局路径规划生成子路径点集进行优化，重规划判别是判断否进行全局路径重规划；局部规划部分分为离线路径库生成和路径库评价，在移动机器人自主导航过程之前，生成离线路径库，在移动机器人导航过程中，将优化后的全局规划子路径点作为目标点，结合接收的传感器数据和定位信息进行局部路径评价，选取最优的局部轨迹放置到底层控制中，转化为移动机器人控制量进行输出。

基于上述原理，本发明提出的一种适用于复杂场景下移动机器人路径导航及规划方法，如图1所示，包括：

步骤1：获取障碍物的点云数据并进行滤波处理；包括：

步骤1.1：通过2D、3D激光雷达或者深度相机等传感器探测障碍物到移动机器人的距离和方向，如果采用2D激光雷达需要将采集到的信息经过三角函数换算得到对应的数据坐标点；障碍物的点云数据由一系列数据坐标点构成；3D激光雷达和深度相机可直接输出障碍物的点云数据；

步骤1.2：对于传感器采集的点云数据进行噪声点滤波，激光传感器或深度相机产生的点云数据点存在噪声，对代价地图的生成、移动机器人避障产生影响，需要对传感器点云数据点进行噪声点滤波，噪声点滤波包含两个参数，滤波圆半径f_R和滤波阈值f_n，构建以数据点p为中心、f_R为半径的圆，计算圆内包含的其他数据点的个数n；

步骤1.3：如果n<f_n，则认为数据点p为噪声点，需要滤除掉，否则保留该数据点，其中，f_n为判断数据点是否为噪声点所设置的阈值；

代价地图分为为障碍物层，膨胀层，未探索层和自由空间，障碍物层表示障碍物占据的栅格，膨胀层表示障碍物层周围膨胀区域内的栅格，膨胀层大小由膨胀距离参数(简称MIP)决定。自由空间为移动机器人可通行区域的栅格。全局代价地图的障碍物层，膨胀层，未探索层用于判断JPS搜索过程中是否发生碰撞，而自由空间层用于计算全局搜索代价。

如图3所示：与传统的JPS算法基本相同，输入起始点和预设的目标点，并将起始点加入到Openlist中；判断Openlist是否为空；之后从Openlist中取出g_cost最小的点s，加入到closelist中，判断closelist中是否存在终点；如果存在，得到全局路径，否则判断点s的邻居点是否在膨胀层和障碍物层，如果邻居点在膨胀层和障碍物层，则舍弃该邻居点，否则将该邻居点加入到neiPoint中去，并计算该邻居点的g_cost值，之后以s为起点，沿邻居点方向搜寻跳点，也称为子路径点，并计算子路径点的g_cost值，判断子路径点是否在closelist或Openlist中，如果是，则更新该跳点的代价值，否则加入到Openlist中，上述过程不断重复，一直搜索到目标点或Openlist集合为空集，在计算点的g_cost值时，如果计算得到点的g_cost值是否大于当前点的g_cost值，如果大，则更新该点g_cost值，否则保持原来的g_cost值。

步骤2：根据代价地图以及机器人在地图中的定位信息和终止点位置，生成由子路径点集构成的全局路径；包括：

步骤2.1：全局搜索采用的是跳点搜索，跳点搜索生成的全局路径由跳点集构成，跳点是全局路径中的关键子路径点，因此全局路径中的跳点也可称为子路径点，将机器人以代价地图中的当前位置点作为起始点，并将起始点加入到跳点搜索算法(简称JPS算法)中的列表Openlist中；

步骤2.2：计算列表Openlist中所有点的代价值g_cost，代价值g_cost计算包含运行代价和启发代价两部分：

式中，为运行代价，/>为启发代价，cost_parent为父节点代价值，cost_toparent为父节点到节点的欧氏距离，w为正增益参数，cost_togoal为节点到目标点的欧氏距离；

步骤2.3：将列表Openlist中g_cost最小的点s加入到列表closelist中；

步骤2.4：判断列表closelist中是否存在终点，如果存在，得到全局路径对应的子路径点集，否则判断点s的邻居点s′是否可通行，如果邻居点s′不可通行，则舍弃该邻居点s′，否则判断该邻居点s′是否需要被修剪掉，如果s′需要被修剪掉，则舍弃该邻居点，否则s′加入到列表neiPoint中，修剪原则是跳点搜索中对邻居节点是否需要被搜索所规定的挑选原则；

步骤2.5：计算列表neiPoint中邻居点s′的代价值，并以s′为起点，沿邻居点方向搜寻跳点，并计算每个跳点的代价值；

步骤2.6：判断每个跳点是否在列表closelist或列表Openlist中，如果是，则更新跳点的代价值，否则将该跳点加入到Openlist中；

步骤2.7：重复上述步骤2.2～步骤2.6，一直搜索到目标点；

经过JPS算法规划之后，输出为全局路径中的跳点集合，也成为了路径点集，需要对路径点集进行优化，剔除掉非必须子路径点，如图4所示。

步骤3：对子路径点集进行路径优化删除掉非必须子路径点，得到最优子路径点集；包括:

步骤3.1：将子路径点集作为滑动窗优化函数的输入，判断子路径点个数是否大于2，如果子路径点个数大于2，则设置滑动窗起始位置点p_u、终止位置点p_v，其中u＝0,v＝2，u,v为滑动窗的起始位置点和终止位置点在子路径点集中的索引，执行步骤3.2；否则子路径点数为2，表示两个子路径点分别为路径的起点和终点，优化结束；

步骤3.2：以起始位置p_u为滑动窗起点，终止位置p_v为滑动窗终点，以代价地图栅格边长l的为步长对直线段进行均匀采样，采样步长/>

步骤3.3：获取线段采样点后，需要求取直线段采样点所属栅格，计算直线段上各采样点在代价地图上所属的栅格：

式中，indx、indy分别为栅格的列索引、行索引，floor(.)表示向下取整，(o_x,o_y)表示代价地图的起始位置坐标；

步骤3.4：判断第indy行、第indx列的栅格是否为自由区域内的栅格，如果是，则直线段没有经过代价地图中的障碍物层、膨胀层和未知探索层，表示移动机器人可以从p_u点出发，直接朝向p_v点前进，不再需要经过p_u、p_v之间的子路径点，将p_u、p_v之间的子路径点舍弃；否则，表示移动机器人需要经过p_u、p_v之间的子路径点，则将p_u、p_v之间的子路径点保留；

步骤3.5：如果p_u、p_v之间的子路径点被舍弃，滑动窗的起始位置不变，终止位置更新，即令v加1；如果p_u、p_v之间的子路径点被保留，滑动窗的起始位置和终止位置同时更新，即令u加1、v加1；

步骤3.6：如果v大于子路径点的总个数，优化结束，否则重复执行步骤3.2～3.6；

步骤4：判断机器人前进过程中是否需要重新规划路径，如图5所示；包括：

步骤4.1：判断当前机器人所在的代价地图中的障碍物层栅格和膨胀层栅格是否遮挡机器人的当前子路径点与前一时刻经过的子路径点所形成的直线段L，当未遮挡时，机器人继续运行；当遮挡时，执行步骤4.2；

步骤4.2：计算机器人在代价地图中的当前位置点到直线段L之间的距离dist，如果距离dist大于设定的阈值，则执行全局重规划，否则执行步骤4.3；

步骤4.3：当机器人从前一时刻子路径点出发时开始计时，经过预设时间段T后仍无法到达当前子路径点，则执行全局重规划，否则继续前行；

局部离线路径库的生成为从原点按照一定角度间隔生成n个路径组的主路经，每个主路径末端按照一定角度间隔再生n条一级子路径，每个一级子路径末端按照一定角度再生成n条二级子路径，即路径库包含n个路径组，每个路径组包含n²条局部路径，每条局部路径使用三次样条进行优化，n为生成局部路径参数。

步骤5：构建局部离线路径库，根据最优局部路径评价函数在局部离线路径库中寻找最优局部路径；包括：

步骤5.1：构建局部离线路径库；包括：

1)从原点按照一定角度间隔生成n个路径组的主路经；

2)每个主路径末端按照一定角度间隔再生n条一级子路径；

3)每个一级子路径末端按照一定角度再生成n条二级子路径，即局部离线路径库包含n个路径组，每个路径组包含n²条局部路径，每条局部路径使用三次样条进行优化；

步骤5.2：以最优子路径点作为虚拟目标点，将子路径点坐标由惯性坐标系转化为移动机器人坐标系下坐标；结合子路径点在机器人坐标系中的位置，以及移动机器人获取的传感器数据，对离线轨迹库中的每一个轨迹组中的轨迹进行评价，选取轨迹组评价值最大的轨迹组的主路径，评价函数原理图如图6所示。

步骤5.3：利用公式(4)计算每个路径组的评价值，将评价值最大的路径组作为主路径用于指导机器人向目标点前进；

式中，score_j表示第j个路径组的评价值，当路径组内第m条子路径被障碍物遮挡时，c_m＝0，否则c_m＝1，Δθ_m为第m条子路径的末端与机器人当前位置点的连线所形成的直线段L’与目标点的虚拟点与机器人当前位置点的连线形成的直线段L”的夹角，Δθ_m越大，路径组的评价值就会越小。

为了测试上述发明的有效性，在仿真环境和实际环境中测试了以上的路径导航和路径规划方法。

全局规划优化前后的数据对比如表1所示。加入滑动窗优化后，规划出的路径更短，消除了一些不必要的拐弯和路径点，使移动机器人能够快速的到达目标点，并且加入膨胀层之后，全局规划的路径与障碍物保持了一定距离，使移动机器人在导航过程中具有一定的避障空间。

表1全局规划优化前后的数据对比表

在测试中，全局规划优化完成之后，会将经过优化的子路径点依次发送给局部规划，做为局部规划过程中的目标点。局部规划根据目标点，移动机器人定位坐标和接收的传感器数据，使用评价函数对离线路径库进行评价，选取评价函数最高的路径组主路径发送给移动机器人底层控制端，控制移动机器人依次经过全局规划的路径点，最终到达目标点，在导航过程中，移动机器人不必严格跟随全局路径，只需依次经过全局规划路径点即可，这会提高了移动机器人灵活度，并且保证了移动机器人在持续导航过程中一定的避障空间。

移动机器人在导航过程中遇到临时障碍物，经过重规划判别器的判断，移动机器人重新规划了一条路径引导机器人继续导航到达目标点，根据传感器信息将，临时障碍物在占据栅格地图上更新。

Claims (6)

1.一种适用于复杂场景下移动机器人路径导航及规划方法，其特征在于，包括：

步骤1：获取障碍物的点云数据并进行滤波处理；

步骤2：根据代价地图以及机器人在地图中的定位信息和终止点位置，生成由子路径点集构成的全局路径；

步骤3：对子路径点集进行路径优化删除掉非必须子路径点，得到最优子路径点集；

步骤3.1：将子路径点集作为滑动窗优化函数的输入，判断子路径点个数是否大于2，如果子路径点个数大于2，则设置滑动窗起始位置点p_u、终止位置点p_v，其中u＝0,v＝2，u,v为起始位置点和终止位置点在子路径点集中的索引，执行步骤3.2；否则子路径点数为2，表示两个子路径点分别为路径的起点和终点，优化结束；

步骤3.2：以起始位置p_u为滑动窗起点，终止位置p_v为滑动窗终点，以代价地图栅格边长l的为步长对直线段均匀采样，采样步长/>

步骤3.3：计算直线段上各采样点在代价地图上所属的栅格：

式中，indx、indy分别为栅格的列索引、行索引，floor(.)表示向下取整，(o_x,o_y)表示代价地图的起始位置坐标，坐标(x_i,y_i)表示为第i个采样点坐标；

步骤3.4：判断第indy行、第indx列的栅格是否为自由区域内的栅格，如果是，则直线段没有经过代价地图中的障碍物层、膨胀层和未知探索层，表示移动机器人可以从p_u点出发，直接朝向p_e点前进，不再需要经过p_u、p_v之间的子路径点，将p_u、p_v之间的子路径点舍弃；否则，表示移动机器人需要经过p_u、p_v之间的子路径点，则将p_u、p_v之间的子路径点保留；

步骤3.5：如果p_u、p_v之间的子路径点被舍弃，滑动窗的起始位置不变，终止位置更新，即令v加1；如果p_u、p_v之间的子路径点被保留，滑动窗的起始位置和终止位置同时更新，即令u加1、v加1；

步骤3.6：如果v大于子路径点的总个数，优化结束，否则重复执行步骤3.2～3.6；

步骤4：判断机器人前进过程中是否需要重新规划路径；

步骤5：构建局部离线路径库，根据最优局部路径评价函数在局部离线路径库中寻找最优局部路径。

2.根据权利要求1所述的一种适用于复杂场景下移动机器人路径导航及规划方法，其特征在于，所述步骤1包括：

步骤1.1：将2D激光雷达采集到的信息经过三角函数换算得到对应的数据坐标点；

步骤1.2：构建以数据点p为中心、f_R为半径的圆，计算圆内包含的其他数据点的个数n；

步骤1.3：如果n<f_n，则认为数据点p为噪声点，需要滤除掉，否则保留该数据点，其中，f_n为判断数据点是否为噪声点所设置的阈值。

3.根据权利要求1所述的一种适用于复杂场景下移动机器人路径导航及规划方法，其特征在于，所述步骤2包括：

步骤2.1：将机器人以代价地图中的当前位置点作为起始点，并将起始点加入到跳点搜索算法中的列表Openlist中；

步骤2.2：计算列表Openlist中所有点的代价值g_cost：

式中，为运行代价，/>为启发代价，cost_parent为父节点代价值，cost_toparent为父节点到节点的欧氏距离；

步骤2.3：将列表Openlist中g_cost最小的点s加入到列表closelist中；

步骤2.4：判断列表closelist中是否存在终点，如果存在，得到全局路径对应的子路径点集，否则判断点s的邻居点s′是否可通行，如果邻居点s′不可通行，则舍弃该邻居点s′，否则判断该邻居点s′是否需要被修剪掉，如果s′需要被修剪掉，则舍弃该邻居点，否则s′加入到列表neiPoint中，修剪原则是跳点搜索中对邻居节点是否需要被搜索所规定的挑选原则；

步骤2.5：计算列表neiPoint中邻居点s′的代价值，并以s′为起点，沿邻居点方向搜寻跳点，并计算每个跳点的代价值；

步骤2.6：判断每个跳点是否在列表closelist或列表Openlist中，如果是，则更新跳点的代价值，否则将该跳点加入到Openlist中；

步骤2.7：重复上述步骤2.2～步骤2.6，一直搜索到目标点。

4.根据权利要求1所述的一种适用于复杂场景下移动机器人路径导航及规划方法，其特征在于，所述步骤4包括：

步骤4.1：判断当前机器人所在的代价地图中的障碍物层栅格和膨胀层栅格是否遮挡机器人的当前子路径点与前一时刻经过的子路径点所形成的直线段L，当未遮挡时，机器人继续运行；当遮挡时，执行步骤4.2；

步骤4.2：计算机器人在代价地图中的当前位置点到直线段L之间的距离，如果距离大于设定的阈值，则执行全局重规划，否则执行步骤4.3；

步骤4.3：当机器人从前一时刻子路径点出发时开始计时，经过预设时间段T后仍无法到达当前子路径点，则执行全局重规划，否则继续前行。

5.根据权利要求1所述的一种适用于复杂场景下移动机器人路径导航及规划方法，其特征在于，所述步骤5包括：

步骤5.1：构建局部离线路径库；

步骤5.2：以最优子路径点作为虚拟目标点，将子路径点坐标由惯性坐标系转化为移动机器人坐标系下坐标；

步骤5.3：利用公式(4)计算每个路径组的评价值，将评价值最大的路径组作为主路径用于指导机器人向目标点前进；

式中，score_j表示第j个路径组的评价值，当路径组内第m条子路径被障碍物遮挡时，c_m＝0，否则c_m＝1，Δθ_m为第m条子路径的末端与机器人当前位置点的连线所形成的直线段L’与目标点的虚拟点与机器人当前位置点的连线形成的直线段L”的夹角。

6.根据权利要求5所述的一种适用于复杂场景下移动机器人路径导航及规划方法，其特征在于，所述步骤5.1包括：

1)从原点按照一定角度间隔生成n个路径组的主路经；

2)每个主路径末端按照一定角度间隔再生n条一级子路径；

3)每个一级子路径末端按照一定角度再生成n条二级子路径，即局部离线路径库包含n个路径组，每个路径组包含n²条局部路径，每条局部路径使用三次样条进行优化。