CN111761581B - 路径规划方法及装置、狭窄空间行进方法及装置 - Google Patents

Abstract

本发明提供了一种路径规划方法及装置、狭窄空间行进方法及装置，其中，路径规划方法包括：对狭窄空间周围的障碍物进行探测；根据探测结果得到可形成狭窄空间的线段组集；将线段组集对应的探测数据投影至基于狭窄空间所在自由空间形成的栅格地图；根据投影结果对线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；从筛选出的线段组集中选定最优线段组；根据最优线段组得到通过该狭窄空间的最优路径。其根据探测结果得到线段组集后将其投影至栅格地图，准确得到由狭窄空间形成的线段组集，帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

Description

路径规划方法及装置、狭窄空间行进方法及装置

技术领域

本发明涉及移动机器人技术领域，尤指一种路径规划方法及装置、狭窄空间行进方法及装置。

背景技术

移动机器人在自由空间中导航时，经常会出现遇到障碍物的情况，是以避障能力对移动机器人来说至关重要。另外，对于移动机器人来说，考虑躲避障碍物的同时还要保证运行的平稳性，为此通常会使用基于人工势场的控制器来实现路径的跟随。但是，这类控制器在控制移动机器人通过狭窄空间(例如门、窄通道等)时，会由于局部最优问题导致移动机器人无法顺利通过，导致移动机器人在狭窄环境中发生震荡。

发明内容

本发明的目的是提供一种路径规划方法及装置、狭窄空间行进方法及装置，有效解决现有移动机器人不能平稳通过狭窄空间的技术问题。

本发明提供的技术方案如下：

本发明提供了一种基于狭窄空间的路径规划方法，包括：

对狭窄空间周围的障碍物进行探测；

根据探测结果得到可形成狭窄空间的线段组集，所述线段组集中包括多组线段组，每组线段组中包括两条由探测数据点形成的线段，且所述两条线段互不相交；

将所述线段组集对应的探测数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

根据投影结果对所述线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；

从筛选出的线段组集中选定最优线段组；

根据所述最优线段组得到通过该狭窄空间的最优路径。

在本技术方案中，根据探测结果得到线段组集后将其投影至栅格地图，进而将不是由狭窄空间形成的线段组从线段组集中滤除，准确得到由狭窄空间形成的线段组集，进而根据该线段组集得到最优的路径，以此移动机器人能够根据该最优路径通过该狭窄空间，该方法易于实现的同时能够得到精确的结果，帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

进一步优选地，在所述根据探测结果得到可形成狭窄空间的线段组集中，包括：

获取探测数据；

依次从所述探测数据中选定预设数量的探测数据点进行直线拟合；

从直线拟合结果中选定任意两条夹角小于预设角度阈值的拟合直线对应的线段形成线段组，得到线段组集。

在本技术方案中，根据规则逐次提取探测数据中的部分探测数据点进行拟合，直到所有的探测数据拟合完成，得到可能形成狭窄空间的线段组集，这一过程中，保证了数据的完整性，不会出现错漏的情况，为后续形成最优路径提供数据基础。

进一步优选地，在所述根据探测结果得到可形成狭窄空间的线段组集之后，还包括：将包含长度小于预设长度阈值的线段的线段组滤除；和/或，将包含拟合误差大于预设误差阈值的线段的线段组滤除；和/或，将两条线段所在直线之间的距离小于预设距离阈值的线段组滤除。

在本技术方案中，根据由狭窄空间的探测数据形成的线段的特性对线段组集中的线段组进行过滤，提高后续数据处理的效率，避免造成资源浪费。

进一步优选地，在所述根据探测结果得到可形成狭窄空间的线段组集之后，还包括：连接所述线段组集中各线段组中两条线段的中心点得到中点连线；

在所述将所述线段组集对应的探测数据投影至基于所述狭窄空间所在自由空间形成的栅格地图中，包括：将所述线段组集对应的探测数据及各线段组中点连线形成的数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

在所述根据投影结果对所述线段组集中各线段组是否由狭窄空间形成进行判断中，包括：分别判断各线段组的中点连线是否与栅格地图中障碍物所在的栅格发生碰撞；若一线段组的中点连线没有与栅格地图中障碍物所在的栅格发生碰撞，判断该线段组由自由空间中的狭窄空间形成。

在本技术方案中，通过对线段组的中点连线进行碰撞检测，如果通过碰撞检测，表示该线段组大概率由狭窄空间形成，进而将明显不是由狭窄空间组成的线段组滤除，提高检测精确度。

进一步优选地，在所述从筛选出的线段组集中选定最优线段组中，包括：

分别计算线段组集中各组线段组的差值cost；

cost＝Δθ+ΔL₁+ΔL₂+ΔL₃

其中，Δθ为线段组中两条线段的朝向差，ΔL₁为线段组中两条线段的长度差，ΔL₂为由线段组中两条线段形成的四边形的对角线长度差，ΔL₃为由线段组中两条线段中心点连接得到的中点连线的长度与移动机器人的宽度差；

比较各线段组的差值，将差值最小的线段组作为最优线段组；

和/或，在所述根据所述最优线段组得到通过该狭窄空间的最优路径中，包括：

计算所述最优线段组中两条线段的平均朝向；

以由最优线段组中两条线段的中心点连接得到的中点连线的中心点为基准，沿所述平均朝向所在的方向生成预设长度的路径，作为最优路径。

在本技术方案中，通过计算各线段组差值cost的方法选出最优线段组，进而根据该最优线段组对路径进行规划，方法简单易行，结果精确度高。

本发明还提供了一种狭窄空间行进方法，包括：

对狭窄空间进行识别；

采用上述路径规划方法对通过识别的狭窄空间的路径进行规划；

跟随规划的路径前进，并判断前进过程中是否与障碍物发生碰撞；

若发生碰撞，沿路径后退，并进一步判断后退过程中是否与障碍物发生碰撞；

若发生碰撞，停止后退；否则到达后退目标停止。

在本技术方案中，该狭窄空间行进方法帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

本发明还提供了一种基于狭窄空间的路径规划装置，包括：

空间探测模块，用于对狭窄空间周围的障碍物进行探测；

线段组集形成模块，用于根据所述空间探测模块的探测结果得到可形成狭窄空间的线段组集，所述线段组集中包括多组线段组，每组线段组中包括两条由探测数据点形成的线段，且所述两条线段互不相交；

数据投影模块，用于将所述线段组集形成模块形成的线段组集对应的探测数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

线段组集筛选模块，用于根据所述数据投影模块的投影结果对所述线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；

最优线段组选定模块，用于从所述线段组集筛选模块筛选出的线段组集中选定最优线段组；

路径生成模块，用于根据所述最优线段组选定模块选定的最优线段组得到通过该狭窄空间的最优路径。

在本技术方案中，根据探测结果得到线段组集后将其投影至栅格地图，进而将不是由狭窄空间形成的线段组从线段组集中滤除，准确得到由狭窄空间形成的线段组集，进而根据该线段组集得到最优的路径，以此移动机器人能够根据该最优路径通过该狭窄空间，该方法易于实现的同时能够得到精确的结果，帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

进一步优选地，在所述线段组集形成模块中，包括：

数据获取单元，用于获取探测数据；

拟合单元，用于依次从所述数据获取单元获取的探测数据中选定预设数量的探测数据点进行直线拟合；

线段组形成单元，用于从所述拟合单元得到的直线拟合结果中选定任意两条夹角小于预设角度阈值的拟合直线对应的线段形成线段组，得到线段组集。

在本技术方案中，根据规则逐次提取探测数据中的部分探测数据点进行拟合，直到所有的探测数据拟合完成，得到可能形成狭窄空间的线段组集，这一过程中，保证了数据的完整性，不会出现错漏的情况，为后续形成最优路径提供数据基础。

进一步优选地，所述路径规划装置中还包括：中心点连接模块，用于连接所述线段组集形成模块形成的线段组集中各线段组中两条线段的中心点得到中点连线；

所述数据投影模块，用于将所述线段组集形成模块形成的线段组集对应的探测数据及所述中心点连接模块得到的中点连线的数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

所述线段组集筛选模块，用于分别判断各线段组的中点连线是否与栅格地图中障碍物所在的栅格发生碰撞；若一线段组的中点连线没有与栅格地图中障碍物所在的栅格发生碰撞，判断该线段组由自由空间中的狭窄空间形成。

在本技术方案中，通过对线段组的中点连线进行碰撞检测，如果通过碰撞检测，表示该线段组大概率由狭窄空间形成，进而将明显不是由狭窄空间组成的线段组滤除，提高检测精确度。

进一步优选地，在所述最优线段组选定模块中，包括：

第一计算单元，用于分别计算线段组集中各组线段组的差值cost；

cost＝Δθ+ΔL₁+ΔL₂+ΔL₃

其中，Δθ为线段组中两条线段的朝向差，ΔL₁为线段组中两条线段的长度差，ΔL₂为由线段组中两条线段形成的四边形的对角线长度差，ΔL₃为由线段组中两条线段中心点连接得到的中点连线的长度与移动机器人的宽度差；

比较单元，用于比较所述第一计算单元计算得到的各线段组的差值，将差值最小的线段组作为最优线段组；

和/或，在所述路径生成模块中，包括：

第二计算单元，用于计算所述最优线段组中两条线段的平均朝向；

路径形成单元，用于以由最优线段组中两条线段的中心点连接得到的中点连线的中心点为基准，沿所述第二计算单元计算得到的平均朝向所在的方向生成预设长度的路径，作为最优路径。

在本技术方案中，通过计算各线段组差值cost的方法选出最优线段组，进而根据该最优线段组对路径进行规划，方法简单易行，结果精确度高。

本发明还提供了一种狭窄空间行进装置，包括：

狭窄空间识别模块，用于对狭窄空间进行识别；

路径规划装置，用于根据所述狭窄空间识别模块识别的狭窄空间进行路径规划；

行进控制模块，用于控制移动机器人跟随所述路径规划装置规划的路径前进；及用于当判断移动机器人与障碍物发生碰撞时，控制移动机器人后退或停止；

碰撞判断模块，用于判断前进或后退过程中是否与障碍物发生碰撞。

在本技术方案中，该狭窄空间行进方法帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

本发明提供了一种移动机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器运行所述计算机程序时实现上述基于狭窄空间的路径规划方法的步骤，及运行所述计算机程序时实现上述狭窄空间行进方法的步骤。

本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述基于狭窄空间的路径规划方法的步骤，及运行所述计算机程序时实现上述狭窄空间行进方法的步骤。

附图说明

下面将以明确易懂的方式，结合附图说明优选实施例，对上述特性、技术特征、优点及其实现方式予以进一步说明。

图1为本发明中基于狭窄空间的路径规划方法一种实施例流程示意图；

图2为本发明中狭窄空间行进方法流程示意图；

图3为本发明中基于狭窄空间的路径规划装置一种实施例结构示意图；

图4为本发明中基于狭窄空间的路径规划装置另一种实施例结构示意图；

图5为本发明中移动机器人结构示意图。

附图标号说明：

100-路径规划装置，110-空间探测模块，120-线段组集形成模块，130-数据投影模块，140-线段组集筛选模块，150-最优线段组选定模块，160-路径生成模块，170-中心点连接模块。

具体实施方式

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对照附图说明本发明的具体实施例。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图，并获得其他的实施例。

本发明的第一种实施例，一种基于狭窄空间的路径规划方法，如图1所示，包括：S10对狭窄空间周围的障碍物进行探测；S20根据探测结果得到可形成狭窄空间的线段组集，线段组集中包括多组线段组，每组线段组中包括两条由探测数据点形成的线段，且两条线段互不相交；S30将线段组集对应的探测数据投影至基于狭窄空间所在的自由空间形成的栅格地图；S40根据投影结果对线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；S50从筛选出的线段组集中选定最优线段组；S60根据最优线段组得到通过该狭窄空间的最优路径。

在本实施例中，狭窄空间具体指空间宽度与移动机器人自身宽度较为接近 (如，与移动机器人自身宽度相差在10cm以内等)的空间。在常规环境中，可表现为由门框形成的空间、由墙壁形成的通道等，在现有方法中移动机器人在通过时有一定难度，可能会出现碰撞、抖动等情况。另外，应当说明，该路径规划方法适用于狭窄空间的同时也能适用于其他的需要路径规划的任意空间，这里对其适用范围这里不做具体限定，可以根据实际情况进行选定。

在步骤S10中，可以采用如激光探测的方法左右缓慢旋转对狭窄空间进行探测得到相应的探测数据，以此进一步根据该探测数据得到可能形成狭窄空间的线段组集。以激光探测为例，线段组集具体指由激光点集形成的线段集合，即其中包含的线段由一定数量的激光点形成得到。且激光点集形成的线段通常被认定为障碍物，如可形成狭窄空间的门框、墙壁等。当然，在实际应用中，还可以采用其他类似的探测数据方法对狭窄空间周围的障碍物进行探测，这里不做具体限定。

由于一般来说，由狭窄空间两侧的探测数据形成的线段长度会相当且不会发生碰撞。是以，在步骤S20根据探测数据得到线段组集中，根据该规则对探测数据进行处理得到线段组集。在根据探测数据得到线段组集之后，需要对其中包含的线段组进行筛选，以此在步骤S30中，将线段组集对应的探测数据投影至基于狭窄空间所在的自由空间形成的栅格地图中进行碰撞测试，这一过程中，被投影到探测数据的栅格视为有障碍物，未被投影的视为可通过，以此，若栅格地图中被选的栅格上都没有障碍物，则可以其对应的线段碰撞检测通过。通过碰撞测试对线段组集筛选后得到由狭窄空间形成的新的线段组集，从中选择最优线段组，即可根据该最优线段组对通过该狭窄空间的路径进行规划。

在本实施方式中，根据探测结果得到线段组集后将其投影至栅格地图，进而将不是由狭窄空间形成的线段组从线段组集中滤除，准确得到由狭窄空间形成的线段组集，进而根据该线段组集得到最优的路径，这一方法易于实现的同时能够得到精确的结果，帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

对上述实施例进行改进得到本实施例，在本实施例中，步骤S10根据探测结果得到可形成狭窄空间的线段组集中，包括：S11获取探测数据；S12依次从探测数据中选定预设数量的探测数据点进行直线拟合；S13从直线拟合结果中选定任意两条夹角小于预设角度阈值的拟合直线对应的线段形成线段组，得到线段组集。

在本实施例中，以激光探测为例，在直线拟合过程中，依次从狭窄空间一侧的激光数据中选定预设数量的激光数据点进行拟合得到拟合直线，拟合方法可以根据实际情况进行选用，如选用最小二乘法等。对于预设数量，可根据运算速度、精度等需求调整，且针对同一狭窄空间的拟合中，可以根据情况设定多个用于拟合的预设数量，如，预设数量中包括2个预设数量，即根据该2个预设数量依次选定对应数量的激光数据点进行拟合；又如预设数量中包括3个数量等，依次类推。理论上来说，该预设数量的最小值为2，最大值为狭窄空间一侧数据包含的连续激光点的数量，但是为了不浪费过多的资源，可根据情况进行适当选择。如，在一实例中，一次拟合中，针对狭窄空间一侧的激光数据(连续数据)，顺序排列后依次从中选定10个激光点数据进行直线拟合，直到所有的可能性都完成拟合后停止；二次拟合中，针对狭窄空间一侧的激光数据(连续数据)，顺序排列后依次从中选定15个激光点数据进行直线拟合，直到所有的可能性都完成拟合后停止。

拟合完成后，以狭窄空间两侧的探测数据形成的线段长度会相当且不会发生碰撞为原则，将拟合结果进行两两组合，形成线段组集(每组线段组均由狭窄空间两侧的数据组成，即包括一条狭窄空间一侧数据形成的线段和一条狭窄空间另一侧形成的线段)。另外，由于一般来说，狭窄空间的两侧是相互平行的，是以，在形成线段组集中，同时会将对应拟合直线夹角大于预设角度的线段组过滤，预设角度阈值可以为0.1°、0.2°甚至更大，可根据实际的应用环境进行调整。

在本实施方式中，根据规则逐次提取探测数据中的部分探测数据点进行拟合，直到所有的探测数据拟合完成，得到可能形成狭窄空间的线段组集，这一过程中，保证了数据的完整性，不会出现错漏的情况，为后续形成最优路径提供数据基础。

对上述实施例进行改进得到本实施例，在本实施例中，在步骤S20根据探测结果得到可形成狭窄空间的线段组集之后，还包括：将包含长度小于预设长度阈值的线段的线段组滤除；和/或，将包含拟合误差大于预设误差阈值的线段的线段组滤除；和/或，将两条线段所在直线之间的距离小于预设距离阈值的线段组滤除。

在本实施例中，为了得到更精确的线段组集，进一步对其进行过滤，如，将线段长度小于预设长度阈值(如，0.05cm、0.08cm等)的线段排除，又如，将拟合误差大于预设误差阈值(如，0.01m、0.02m等)的线段排除；再如，将两条线段所在直线之间的距离小于预设距离阈值的线段组排除，这里，可将预设距离阈值设定为移动机器人自身的宽度，即将两条线段所在直线之间的距离小于移动机器人宽度的线段组排除，当然，也可以根据实际情况设定的比移动机器人的宽度稍微大一点(如，大1cm、大2cm等)。另外，当两条线段所在直线存在夹角时，将两线段之间距离最小处(移动机器人通过由该两条线段形成的空间时出现的最小距离)与预设距离阈值进行比较。

在本实施方式中，根据由狭窄空间的探测数据形成的线段的特性对线段组集中的线段组进行过滤，提高后续数据处理的效率，避免造成资源浪费。

对上述实施例进行改进得到本实施例，在本实施例中，步骤S20根据探测结果得到可形成狭窄空间的线段组集之后，还包括：连接线段组集中各线段组中两条线段的中心点得到中点连线。步骤S30将线段组集对应的探测数据投影至基于狭窄空间所在的自由空间形成的栅格地图中，包括：将线段组集对应的探测数据及各线段组中点连线形成的数据投影至基于狭窄空间所在的自由空间形成的栅格地图。步骤S40根据投影结果对线段组集中各线段组是否由狭窄空间形成进行判断中，包括：S41分别判断各线段组的中点连线是否与栅格地图中障碍物所在的栅格发生碰撞；S42若一线段组的中点连线没有与栅格地图中障碍物所在的栅格发生碰撞，判断该线段组由自由空间中的狭窄空间形成。

在本实施例中，通过对中心连线进行碰撞检测的方式对线段组集进行过滤。具体，在根据探测数据得到线段组集之后，将线段组集中各线段组中两条线段的中心点进行连接得到中点连线；之后，将各线段组中点连线形成的数据投影至基于狭窄空间所在的自由空间形成的栅格地图，通过判断中心点连线是否与栅格地图中障碍物所在栅格发生碰撞对其进行碰撞检测，若不是由狭窄空间对应的线段组连接得到的中心连线，一般会发生碰撞，以此作为碰撞检测的基准，将中点连线与栅格地图中障碍物所在栅格发生碰撞的线段组滤除，得到新的线段组集，提高检测的精确度。

对上述实施例进行改进得到本实施例，在本实施例中，步骤S50从筛选出的线段组集中选定最优线段组中，包括：S51分别计算线段组集中各组线段组的差值cost；S52比较各线段组的差值，将差值最小的线段组作为最优线段组。

具体，根据式(1)对差值cost进行计算：

cost＝Δθ+ΔL₁+ΔL₂+ΔL₃ (1)

其中，Δθ为线段组中两条线段的朝向差，ΔL₁为线段组中两条线段的长度差，ΔL₂为由线段组中两条线段形成的四边形的对角线长度差，ΔL₃为由线段组中两条线段中心点连接得到的中点连线的长度与移动机器人的宽度差(保证移动机器人能通过)。

在一线段组中，当两条线段的朝向差越接近0说明两条线越平行，当两条线段的长度差越接近0说明两条线段的长度越接近，当两条线段形成的四边形的对角线长度差越接近0说明两条线段形成的四边形越接近矩形。也就是说，对于同一狭窄空间来说，计算得到的差值cost越小的线段组，包含的两条线段越接近于真实的狭窄空间对应的通道。另外，当计算的结果中包括多组差值cost 相同的线段组，选定线段长度较长的线段组。

得到最优线段组之后，以该最优线段组中两条线段的中心点连接得到的中点连线的中心点为基准，沿两条线段的平均朝向生成一条通过该中心点的直线，并将该直线所在的路径作为最优路径。对于该路径的长度，以通过该狭窄空间为目的，可以根据最优线段组中线段的长度进行设定(大于最优线段组中线段的长度)；也可以设定为以定长，如设定为3m等。

如图2所示，本发明还提供了一种狭窄空间行进方法，包括：S100对狭窄空间进行识别；S200采用上述路径规划方法对通过识别的狭窄空间的路径进行规划；S300跟随规划的路径前进，并判断前进过程中是否与障碍物发生碰撞；S400若发生碰撞，沿路径后退，并进一步判断后退过程中是否与障碍物发生碰撞；S500若发生碰撞，停止后退；否则到达后退目标停止。

在本实施例中，移动机器人在识别到狭窄空间之后，开始根据前述实施例中基于狭窄空间的路径规划方法对通过该狭窄空间的路径进行规划；之后，选定规划的路径中距离移动机器人最近的点跟随前进，在前进过程中，若没有发生碰撞，则在到达设定的前进目标后，结束狭窄空间的行进任务；若发生碰撞，则在判断当前并没有达到设定的后退目标(起始点)后，继续沿路径后退。且在后退过程中，对路径进行重新规划并对所在位置进行实时更新，直到到达前进目标或者退回到后退目标停止。若达到前进目标，则未发生碰撞路径即为其前进路径。

本发明还提供了一种基于狭窄空间的路径规划装置100，在一种实施例中，如图3所示，包括：空间探测模块110，用于对狭窄空间周围的障碍物进行探测；线段组集形成模块120，用于根据空间探测模块110的探测结果得到可形成狭窄空间的线段组集，线段组集中包括多组线段组，每组线段组中包括两条由探测数据点形成的线段，且两条线段互不相交；数据投影模块130，用于将线段组集形成模块120形成的线段组集对应的探测数据投影至基于狭窄空间所在的自由空间形成的栅格地图；线段组集筛选模块140，用于根据数据投影模块130的投影结果对线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；最优线段组选定模块150，用于从线段组集筛选模块140筛选出的线段组集中选定最优线段组；路径生成模块160，用于根据最优线段组选定模块150选定的最优线段组得到通过该狭窄空间的最优路径。

在本实施例中，狭窄空间具体指空间宽度与移动机器人自身宽度较为接近 (如，与移动机器人自身宽度相差在10cm以内等)的空间。在常规环境中，可表现为由门框形成的空间、由墙壁形成的通道等，在现有方法中移动机器人在通过时有一定难度，可能会出现碰撞、抖动等情况。另外，应当说明，该路径规划方法适用于狭窄空间的同时也能适用于其他的需要路径规划的任意空间，这里对其适用范围这里不做具体限定，可以根据实际情况进行选定。

在空间探测模块110中，可以采用如激光探测的方法左右缓慢旋转对狭窄空间进行探测得到相应的探测数据，以此进一步根据该探测数据得到可能形成狭窄空间的线段组集。以激光探测为例，线段组集具体指由激光点集形成的线段集合，即其中包含的线段由一定数量的激光点形成得到。且激光点集形成的线段通常被认定为障碍物，如可形成狭窄空间的门框、墙壁等。当然，在实际应用中，还可以采用其他类似的探测数据方法对狭窄空间周围的障碍物进行探测，这里不做具体限定。

由于一般来说，由狭窄空间两侧的探测数据形成的线段长度会相当且不会发生碰撞。是以，在线段组集形成模块120根据探测数据得到线段组集中，根据该规则对探测数据进行处理得到线段组集。在根据探测数据得到线段组集之后，需要对其中包含的线段组进行筛选，以此在数据投影模块130中，将线段组集对应的探测数据投影至基于狭窄空间所在的自由空间形成的栅格地图中进行碰撞测试，这一过程中，被投影到探测数据的栅格视为有障碍物，未被投影的视为可通过，以此，若栅格地图中被选的栅格上都没有障碍物，则可以其对应的线段碰撞检测通过。通过碰撞测试对线段组集筛选后得到由狭窄空间形成的新的线段组集，从中选择最优线段组，即可根据该最优线段组对通过该狭窄空间的路径进行规划。

在本实施方式中，根据探测结果得到线段组集后将其投影至栅格地图，进而将不是由狭窄空间形成的线段组从线段组集中滤除，准确得到由狭窄空间形成的线段组集，进而根据该线段组集得到最优的路径，这一方法易于实现的同时能够得到精确的结果，帮助移动机器人避开四周障碍物的同时平稳的通过狭窄空间。

对上述实施例进行改进得到本实施例，在本实施例中，线段组集形成模块 120中包括：数据获取单元，用于获取探测数据；拟合单元，用于依次从数据获取单元获取的探测数据中选定预设数量的探测数据点进行直线拟合；线段组形成单元，用于从拟合单元得到的直线拟合结果中选定任意两条夹角小于预设角度阈值的拟合直线对应的线段形成线段组，得到线段组集。

在本实施例中，以激光探测为例，在直线拟合过程中，依次从狭窄空间一侧的激光数据中选定预设数量的激光数据点进行拟合得到拟合直线，拟合方法可以根据实际情况进行选用，如选用最小二乘法等。对于预设数量，可根据运算速度、精度等需求调整，且针对同一狭窄空间的拟合中，可以根据情况设定多个用于拟合的预设数量，如，预设数量中包括2个预设数量，即根据该2个预设数量依次选定对应数量的激光数据点进行拟合；又如预设数量中包括3个数量等，依次类推。理论上来说，该预设数量的最小值为2，最大值为狭窄空间一侧数据包含的连续激光点的数量，但是为了不浪费过多的资源，可根据情况进行适当选择。

拟合完成后，线段组形成单元以狭窄空间两侧的探测数据形成的线段长度会相当且不会发生碰撞为原则，将拟合结果进行两两组合，形成线段组集(每组线段组均由狭窄空间两侧的数据组成，即包括一条狭窄空间一侧数据形成的线段和一条狭窄空间另一侧形成的线段)。另外，由于一般来说，狭窄空间的两侧是相互平行的，是以，在形成线段组集中，同时会将对应拟合直线夹角大于预设角度的线段组过滤，预设角度阈值可以为0.1°、0.2°甚至更大，可根据实际的应用环境进行调整。

在本实施方式中，根据规则逐次提取探测数据中的部分探测数据点进行拟合，直到所有的探测数据拟合完成，得到可能形成狭窄空间的线段组集，这一过程中，保证了数据的完整性，不会出现错漏的情况，为后续形成最优路径提供数据基础。

对上述实施例进行改进得到本实施例，在本实施例中，如图4所示，该路径规划装置100中还包括：中心点连接模块170，用于连接线段组集形成模块 120形成的线段组集中各线段组中两条线段的中心点得到中点连线；数据投影模块130，用于将线段组集形成模块120形成的线段组集对应的探测数据及中心点连接模块170得到的中点连线的数据投影至基于狭窄空间所在的自由空间形成的栅格地图；线段组集筛选模块140，用于分别判断各线段组的中点连线是否与栅格地图中障碍物所在的栅格发生碰撞；若一线段组的中点连线没有与栅格地图中障碍物所在的栅格发生碰撞，判断该线段组由自由空间中的狭窄空间形成。

在本实施例中，通过对中心连线进行碰撞检测的方式对线段组集进行过滤。具体，在根据探测数据得到线段组集之后，中心点连接模块170将线段组集中各线段组中两条线段的中心点进行连接得到中点连线；之后，数据投影模块130将各线段组中点连线形成的数据投影至基于狭窄空间所在的自由空间形成的栅格地图，线段组集筛选模块140通过判断中心点连线是否与栅格地图中障碍物所在栅格发生碰撞对其进行碰撞检测，若不是由狭窄空间对应的线段组连接得到的中心连线，一般会发生碰撞，以此作为碰撞检测的基准，将中点连线与栅格地图中障碍物所在栅格发生碰撞的线段组滤除，得到新的线段组集，提高检测的精确度。

在其他本实施例中，为了得到更精确的线段组集，需对数据组集进一步过滤，如，将线段长度小于预设长度阈值(如，0.05cm、0.08cm等)的线段排除，又如，将拟合误差大于预设误差阈值(如，0.01m、0.02m等)的线段排除；再如，将两条线段所在直线之间的距离小于预设距离阈值的线段组排除，这里，可将预设距离阈值设定为移动机器人自身的宽度，即将两条线段所在直线之间的距离小于移动机器人宽度的线段组排除，当然，也可以根据实际情况设定的比移动机器人的宽度稍微大一点(如，大1cm、大2cm等)。另外，当两条线段所在直线存在夹角时，将两线段之间距离最小处(移动机器人通过由该两条线段形成的空间时出现的最小距离)与预设距离阈值进行比较。

对上述实施例进行改进得到本实施例，在本实施例中，进一步优选地，在最优线段组选定模块150中，包括：第一计算单元，用于分别计算线段组集中各组线段组的差值cost；比较单元，用于比较第一计算单元计算得到的各线段组的差值，将差值最小的线段组作为最优线段组。

具体，根据式(1)对差值cost进行计算，在一线段组中，当两条线段的朝向差越接近0说明两条线越平行，当两条线段的长度差越接近0说明两条线段的长度越接近，当两条线段形成的四边形的对角线长度差越接近0说明两条线段形成的四边形越接近矩形。也就是说，对于同一狭窄空间来说，计算得到的差值cost越小的线段组，包含的两条线段越接近于真实的狭窄空间对应的通道。另外，当计算的结果中包括多组差值cost相同的线段组，选定线段长度较长的线段组。

得到最优线段组之后，路径形成单元以该最优线段组中两条线段的中心点连接得到的中点连线的中心点为基准，沿第二计算单元计算得到的两条线段的平均朝向生成一条通过该中心点的直线，并将该直线所在的路径作为最优路径。对于该路径的长度，以通过该狭窄空间为目的，可以根据最优线段组中线段的长度进行设定(大于最优线段组中线段的长度)；也可以设定为以定长，如设定为3m等。

本发明还提供了一种狭窄空间行进装置，包括：狭窄空间识别模块，用于对狭窄空间进行识别；路径规划装置，用于根据狭窄空间识别模块识别的狭窄空间进行路径规划；行进控制模块，用于控制移动机器人跟随路径规划装置规划的路径前进；及用于当判断移动机器人与障碍物发生碰撞时，控制移动机器人后退或停止；及碰撞判断模块，用于判断前进或后退过程中是否与障碍物发生碰撞。

在本实施例中，移动机器人在识别到狭窄空间之后，路径规划装置开始根据前述实施例中基于狭窄空间的路径规划方法对通过该狭窄空间的路径进行规划；之后，行进控制模块选定规划的路径中距离移动机器人最近的点控制跟随前进，在前进过程中，若碰撞判断模块判断没有发生碰撞，则在到达设定的前进目标后，结束狭窄空间的行进任务；若发生碰撞，则在判断当前并没有达到设定的后退目标(起始点)后，继续沿路径后退。且在后退过程中，通过路径规划装置对路径进行重新规划并对所在位置进行实时更新，直到到达前进目标或者退回到后退目标停止。若达到前进目标，则未发生碰撞路径即为其前进路径。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各程序模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的程序模块完成，即将装置的内部结构划分成不同的程序单元或模块，以完成以上描述的全部或者部分功能。实施例中的各程序模块可以集成在一个处理单元中，也可是各个单元单独物理存在，也可以两个或两个以上单元集成在一个处理单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件程序单元的形式实现。另外，各程序模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。

图5是本发明一个实施例中提供的移动机器人的结构示意图，如所示，该移动机器人200包括：处理器220、存储器210以及存储在存储器210中并可在处理器220上运行的计算机程序211，例如：路径规划程序和/或狭窄空间行进程序。处理器220执行计算机程序211时实现上述各个路径规划方法和/或狭窄空间行进方法实施例中的步骤，或者，处理器220执行计算机程序211时实现上述各路径规划装置和/或狭窄空间行进装置实施例中各模块的功能。

移动机器人200可以为笔记本、掌上电脑、平板型计算机、手机等设备。移动机器人200可包括，但不仅限于处理器220、存储器210。本领域技术人员可以理解，图5仅仅是移动机器人200的示例，并不构成对移动机器人200 的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如：移动机器人200还可以包括输入输出设备、显示设备、网络接入设备、总线等。

处理器220可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列 (Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器220可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器210可以是移动机器人200的内部存储单元，例如：移动机器人200 的硬盘或内存。存储器210也可以是移动机器人200的外部存储设备，例如：移动机器人200上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，存储器210 还可以既包括移动机器人200的内部存储单元也包括外部存储设备。存储器210 用于存储计算机程序211以及移动机器人200所需要的其他程序和数据。存储器210还可以用于暂时地存储已经输出或者将要输出的数据。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详细描述或记载的部分，可以参见其他实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露移动机器人和方法，可以通过其他的方式实现。例如，以上所描述的移动机器人实施例仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性、机械或其他的形式。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可能集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序211发送指令给相关的硬件完成，计算机程序211可存储于一计算机可读存储介质中，该计算机程序211在被处理器220执行时，可实现上述各个方法实施例的步骤。其中，计算机程序211包括：计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质可以包括：能够携带计算机程序211代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如：在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施例，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选实施例，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (13)

1.一种基于狭窄空间的路径规划方法，其特征在于，包括：

对狭窄空间周围的障碍物进行探测；

根据探测结果得到可形成狭窄空间的线段组集，所述线段组集中包括多组线段组，每组线段组中包括两条由探测数据点形成的线段，且所述两条线段互不相交；

将所述线段组集对应的探测数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

根据投影结果对所述线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；

从筛选出的线段组集中选定最优线段组；

根据所述最优线段组得到通过该狭窄空间的最优路径。

2.如权利要求1所述的路径规划方法，其特征在于，在所述根据探测结果得到可形成狭窄空间的线段组集中，包括：

获取探测数据；

依次从所述探测数据中选定预设数量的探测数据点进行直线拟合；

从直线拟合结果中选定任意两条夹角小于预设角度阈值的拟合直线对应的线段形成线段组，得到线段组集。

3.如权利要求1所述的路径规划方法，其特征在于，在所述根据探测结果得到可形成狭窄空间的线段组集之后，还包括：将包含长度小于预设长度阈值的线段的线段组滤除；和/或，将包含拟合误差大于预设误差阈值的线段的线段组滤除；和/或，将两条线段所在直线之间的距离小于预设距离阈值的线段组滤除。

4.如权利要求1或2或3所述的路径规划方法，其特征在于，

在所述根据探测结果得到可形成狭窄空间的线段组集之后，还包括：连接所述线段组集中各线段组中两条线段的中心点得到中点连线；

在所述将所述线段组集对应的探测数据投影至基于所述狭窄空间所在自由空间形成的栅格地图中，包括：将所述线段组集对应的探测数据及各线段组中点连线形成的数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

在所述根据投影结果对所述线段组集中各线段组是否由狭窄空间形成进行判断中，包括：分别判断各线段组的中点连线是否与栅格地图中障碍物所在的栅格发生碰撞；若一线段组的中点连线没有与栅格地图中障碍物所在的栅格发生碰撞，判断该线段组由自由空间中的狭窄空间形成。

5.如权利要求1或2或3所述的路径规划方法，其特征在于，

在所述从筛选出的线段组集中选定最优线段组中，包括：

分别计算线段组集中各组线段组的差值

；

其中，

为线段组中两条线段的朝向差，

为线段组中两条线段的长度差，

为由线段组中两条线段形成的四边形的对角线长度差，

为由线段组中两条线段中心点连接得到的中点连线的长度与移动机器人的宽度差；

比较各线段组的差值，将差值最小的线段组作为最优线段组；

和/或，在所述根据所述最优线段组得到通过该狭窄空间的最优路径中，包括：

计算所述最优线段组中两条线段的平均朝向；

以由最优线段组中两条线段的中心点连接得到的中点连线的中心点为基准，沿所述平均朝向所在的方向生成预设长度的路径，作为最优路径。

6.一种狭窄空间行进方法，其特征在于，包括：

对狭窄空间进行识别；

采用如权利要求1-5任意一项所述的路径规划方法对通过识别的狭窄空间的路径进行规划；

跟随规划的路径前进，并判断前进过程中是否与障碍物发生碰撞；

若发生碰撞，沿路径后退，并进一步判断后退过程中是否与障碍物发生碰撞；

若发生碰撞，停止后退；否则到达后退目标停止。

7.一种基于狭窄空间的路径规划装置，其特征在于，包括：

空间探测模块，用于对狭窄空间周围的障碍物进行探测；

线段组集形成模块，用于根据所述空间探测模块的探测结果得到可形成狭窄空间的线段组集，所述线段组集中包括多组线段组，每组线段组中包括两条由探测数据点形成的线段，且所述两条线段互不相交；

数据投影模块，用于将所述线段组集形成模块形成的线段组集对应的探测数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

线段组集筛选模块，用于根据所述数据投影模块的投影结果对所述线段组集中各线段组是否由狭窄空间形成进行判断，筛选出由该狭窄空间形成的线段组集；

最优线段组选定模块，用于从所述线段组集筛选模块筛选出的线段组集中选定最优线段组；

路径生成模块，用于根据所述最优线段组选定模块选定的最优线段组得到通过该狭窄空间的最优路径。

8.如权利要求7所述的路径规划装置，其特征在于，在所述线段组集形成模块中，包括：

数据获取单元，用于获取探测数据；

拟合单元，用于依次从所述数据获取单元获取的探测数据中选定预设数量的探测数据点进行直线拟合；

线段组形成单元，用于从所述拟合单元得到的直线拟合结果中选定任意两条夹角小于预设角度阈值的拟合直线对应的线段形成线段组，得到线段组集。

9.如权利要求7或8所述的路径规划装置，其特征在于，所述路径规划装置中还包括：中心点连接模块，用于连接所述线段组集形成模块形成的线段组集中各线段组中两条线段的中心点得到中点连线；

所述数据投影模块，用于将所述线段组集形成模块形成的线段组集对应的探测数据及所述中心点连接模块得到的中点连线的数据投影至基于所述狭窄空间所在自由空间形成的栅格地图；

所述线段组集筛选模块，用于分别判断各线段组的中点连线是否与栅格地图中障碍物所在的栅格发生碰撞；若一线段组的中点连线没有与栅格地图中障碍物所在的栅格发生碰撞，判断该线段组由自由空间中的狭窄空间形成。

10.如权利要求7或8所述的路径规划装置，其特征在于，

在所述最优线段组选定模块中，包括：

第一计算单元，用于分别计算线段组集中各组线段组的差值

；

其中，

为线段组中两条线段的朝向差，

为线段组中两条线段的长度差，

为由线段组中两条线段形成的四边形的对角线长度差，

为由线段组中两条线段中心点连接得到的中点连线的长度与移动机器人的宽度差；

比较单元，用于比较所述第一计算单元计算得到的各线段组的差值，将差值最小的线段组作为最优线段组；

和/或，在所述路径生成模块中，包括：

第二计算单元，用于计算所述最优线段组中两条线段的平均朝向；

路径形成单元，用于以由最优线段组中两条线段的中心点连接得到的中点连线的中心点为基准，沿所述第二计算单元计算得到的平均朝向所在的方向生成预设长度的路径，作为最优路径。

11.一种狭窄空间行进装置，其特征在于，包括：

狭窄空间识别模块，用于对狭窄空间进行识别；

如权利要求7-10任意一项所述的路径规划装置，用于根据所述狭窄空间识别模块识别的狭窄空间进行路径规划；

行进控制模块，用于控制移动机器人跟随所述路径规划装置规划的路径前进；及用于当判断移动机器人与障碍物发生碰撞时，控制移动机器人后退或停止；

碰撞判断模块，用于判断前进或后退过程中是否与障碍物发生碰撞。

12.一种移动机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器运行所述计算机程序时实现如权利要求1-5中任一项所述基于狭窄空间的路径规划方法的步骤，和/或运行所述计算机程序时实现如权利要求6中任一项所述狭窄空间行进方法的步骤。

13.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-5中任一项所述基于狭窄空间的路径规划方法的步骤，和/或运行所述计算机程序时实现如权利要求6中任一项所述狭窄空间行进方法的步骤。

Images