CN103455034B - 一种基于最近距离向量场直方图的避障路径规划方法 - Google Patents

Abstract

本发明提供一种基于最近距离向量场直方图的避障路径规划方法，包括以下步骤。S1、将机器人当前扫描范围均分为n个扇区，若第k个扇区内的障碍物距离机器人中心点的最近距离向量为NDV_k，获取|NDV_k-1-NDV_k|≤与机器人直径比较，根据比较结果得到机器人当前位置的局部环境中障碍物个数。S2、设定最小避障阈值n_s，若NDV_k≥n_s,则扇区k的角度范围为避障区间，否则为自由行走区间。S3、确定瞬时目标点的搜索范围，并在所述搜索范围中获取瞬时目标点，机器人运动到达所述瞬时目标点后，确定新的瞬时目标点并向新的瞬时目标点运动，直至到达终点。

Description

一种基于最近距离向量场直方图的避障路径规划方法

技术领域

本发明涉及机器人的避障路径规划，尤其涉及一种基于最近距离向量场直方图的避障路径规划方法。

背景技术

机器人的避障路径规划问题可以分成两个部分：具有先验的完全障碍物信息的全局路径规划和具有部分未知或完全未知障碍物信息的局部路径规划。局部路径规划技术需要充分利用激光测距器、超声波等机器人机载传感器收集的局部环境信息来完成。由于机器人所处环境信息不可能预先完全获取，在完全未知或部分未知环境下进行避障路径规划是自主导航的主要能力。在这个领域中，国内外大量学者进行了相应的研究，其中最常使用的避障路径规划方法有栅格法、Bug系列算法、人工势场法APF（ArtificialPotentialField）、向量场直方图法VFH（VectorFieldHistogram）等。

CMU（CarnegieMellonUniversity）提出的栅格法，是用一种栅格形状的物理模型表示障碍物在该栅格中出现的可能性大小，每个栅格包括一个确定值（CV），表示一个障碍物存在于一个栅格中的可能性。通过栅格法，机器人可以在静态环境中实现准确导航，做出各种有效的避障动作，然而由于栅格法的计算量过大，不利于实时避障的实现。Bug系列算法采用简单的传感方法让机器人直接驶向目标点，直到在其行进的路径上遇到障碍物，机器人随即环绕障碍物运动，寻找脱离障碍物边界跟踪模式的逃离点，继续驶向目标点。人工势场法将目标点看作吸引点，障碍物看成是排斥点，规划的路径沿吸引点和排斥点产生的合力方向伸展。它构造一个称为势函数的标量，使得目标点对应于势函数的最小值，障碍物区域对应于一些较大的值，在环境中的其它位置，势函数都是向目标点单调递减。这样，当起始点在自由空间中的任何位置，只要路径存在，它都能通过势函数的负梯度方向找到目标点，即使是对于较大的障碍物区域，也可保证生成路径的无碰性。它的优点是可以迅速躲开突发障碍物，实时性好。然而该方法同时存在一些缺点：一是容易产生局部极点和死锁；二是在相近障碍物之间不能发现路径，三是在障碍物前面发生震荡；四是在狭窄通道中摆动；五是很难找到一种适合于凹形障碍物的势函数。

为了克服人工势场法的一些缺点，Borenstein等人重新设计了一种向量场直方图（VectorFieldHistogram,VFH）算法，在实时探测未知障碍物和避障的同时，驱动机器人转向目标点的运动。该算法的优点是具有较快的速度，比较适合于短距离的避障。然而该方法依然存在一些问题。为了解决VFH方法所存在的问题，Iwan和Borenstein等人相继提出了VFH+方法和VFH*算法，较大地提高了复杂未知环境下机器人的局部避障能力，避免机器人陷入死锁状态。但是VFH*和VFH+算法仍然是一种局部的避障算法。

传统的向量场直方图（VFH）在处理实际机器人的避障路径规划问题时主要存在以下几个方面的问题。第一，没有直接考虑到机器人的大小，也没有考虑到传感器感知环境数据的不确定性，仅仅通过经验性的低通滤波器来补偿机器人的大小，平滑极坐标直方图。使得低通滤波器的参数调整很不容易。第二，在VFH中，通过设置一个固定阈值τ来判别极坐标直方图中哪些扇区是通道，哪些扇区被阻塞。这种单阈值的判别方法在处理一些狭窄通道时会像人工势场法那样产生机器人运动方向剧烈震荡的问题。第三，在VFH中，忽略了机器人运动学和动力学约束，将机器人理想化为可以瞬时改变其运动方向，具有无限大的运动速度和加速度的理想机器人。第四，VFH方法在选择机器人的转向方向时，仅仅考虑了目标的方向，没有考虑机器人运动轨迹的平滑性和稳定性。使得机器人实际上很难完成VFH算法计算出来的预定轨迹。第五，VFH算法使用的传感信息也是相对于机器人本身的，容易受到机器人位置不确定性和传感不确定性的影响。

鉴于上述原因，需要设计一种基于最近距离向量场直方图的避障路径规划方法，将机器人的实际尺寸作为参数之一，计算出运动时的避障区间和自由行走区间，并确定行走路径中的瞬时目标点，以解决现有技术中存在的问题。

发明内容

本发明提供一种基于最近距离向量场直方图的避障路径规划方法，包括以下步骤：

S1、将机器人当前扫描范围均分为n个扇区，若第k个扇区内的障碍物距离机器人中心点的最近距离向量为NDV_k，获取|NDV_k-1-NDV_k|与机器人直径比较，根据比较结果得到机器人当前位置的局部环境中障碍物个数；

S2、设定最小避障阈值n_s，若NDV_k≥n_s,则扇区k的角度范围为避障区间，否则为自由行走区间；

S3、确定瞬时目标点的搜索范围，并在所述搜索范围中获取瞬时目标点，机器人运动到达所述瞬时目标点后，确定新的瞬时目标点并向新的瞬时目标点运动，直至到达终点。

优选的，在所述步骤S1之前，将测试区域划分为栅格，以栅格概率值表示每个栅格含有障碍物的概率，将机器人探测到的障碍物从环境坐标系中投影到栅格坐标系上，并更新包含了障碍物投影栅格的栅格概率值。

优选的，在所述步骤S1中，若扇区k与扇区k-1均包含障碍物，当|NDV_k-1-NDV_k|≤Δd_max时，扇区k与扇区k-1中包含同一个障碍物；当|NDV_k-1-NDV_k|>Δd_max时，扇区k与扇区k-1包含不同障碍物，其中，Δd_max=2R_rob为机器人直径。

优选的，所述步骤S1中的NDV_k表示为：其中L是机器人传感器接收到的障碍物点集，h_k(L)表示在扇区k内距离障碍物的最小距离，d_max表示机器人扫描的最大距离。

优选的，在所述步骤S1与S2之间，以起始角度begin及最终角度end表示障碍物大小，d表示障碍物所在扇区之内离机器人中心最近的距离，并根据所述begin、end及d分割出各个障碍物区，当机器人需要避开所述障碍物区时，则对应区间为避障区间。

优选的，在所述步骤S2中，n_s=d_max-d_s，其中d_s为机器人和需避碰的障碍物之间最小的安全间隙。

优选的，在所述步骤S3中，所述搜索范围的角度为其中P_t为机器人当前坐标点，SchFrom为开始搜索时的角度，SchEnd为结束搜索时的角度，SchDir为搜索开始时偏离SchFrom的角度。

优选的，在所述步骤S3中，如果传感器在搜索范围中的当前和最近扫描激光束的测量点之间的距离超过预设阈值，则在所述当前或最近扫描激光束上存在一个候选瞬时目标点。

优选的，若瞬时目标点在机器人第j个扫描传感方向上且距离为r_IG，则当所述扫描传感方向上存在长度为r_IG的安全路径时，或者两个相邻测量点的距离大于2R_rob+2d_s时，对应的候选瞬时目标点即为瞬时目标点。

根据本发明提供的基于最近距离向量场直方图的避障路径规划方法，采用向量映射法创建障碍物的占据栅格地图，根据机器人的实际大小和传感器的不确定性，获取障碍物个数。同时通过障碍物膨胀，建立障碍点集，并对相邻障碍物进行融合，形成障碍物边界集，从而决定机器人运动的自由行走区间和避障区间。同时在搜索范围内鸪瞬时目标点，在自由行走区间和避障区间中选择运动方向。如此，可根据环境中的最新信息做出相应决策，能够在密集复杂的环境中较好地完成避障和路径规划，出色完成从初始点到目标点的导航。

附图说明

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

图1是本发明提供的基于最近距离向量场直方图的避障路径规划方法的流程图；

图2是本发明较佳实施例提供的机器人探测到的环境的栅格模型示意图；

图3是本发明较佳实施例提供的机器人主动窗口示意图；

图4a是本发明较佳实施例提供的机器人在主动窗口内探测到的障碍物A、B、C及D的环境示意图；

图4b是本发明较佳实施例提供的障碍物A、B、C及D的相应最近距离向量极坐标直方图；

图5a是本发明较佳实施例提供的本发明较佳实施例提供的机器人在主动窗口内探测到的障碍物A1、B1及C1的环境示意图；

图5b是本发明较佳实施例提供的A1、B1及C1的相应最近距离向量极坐标直方图；

图6是本发明较佳实施例提供的在障碍物A、B、C及D环境下用n_s作为阈值得到的避障区示意图；

图7是本发明较佳实施例提供的在障碍物A1、B1及C1环境下用n_s作为阈值得到的避障区示意图；

图8是本发明较佳实施例提供的障碍物A、B、C及D环境下生成自由走动区间和避障区间示意图；

图9是根据VFH算法提供的障碍物A、B、C及D环境下生成自由走动区间和避障区间示意图；

图10是本发明较佳实施例提供的障碍物A1、B1及C1环境下生成自由走动区间和避障区间示意图；

图11是根据VFH算法提供的障碍物A1、B1及C1环境下生成自由走动区间和避障区间示意图；

图12是本发明较佳实施例提供的机器人从瞬时初始位置S_i,k向瞬时目标点G_i,k移动时的搜索范围示意图；

图13a是本发明较佳实施例提供的环境中的障碍分布示意图；

图13b～f是本发明较佳实施例提供的机器人感知的的通道穿行示意图；

图14a～f是本发明较佳实施例提供的机器人在密集障碍物A2、B2、C2、D2及E2中穿行过程示意图；

图15a～h是本发明较佳实施例提供的机器人在V形和U形障碍物中穿行过程示意图。

具体实施方式

下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

图1是本发明提供的基于最近距离向量场直方图的避障路径规划方法的流程图。如图1所示，本发明较佳实施例提供的基于最近距离向量场直方图的避障路径规划方法包括步骤S1～S3。

步骤S1：将机器人当前扫描范围均分为n个扇区，若第k个扇区内的障碍物距离机器人中心点的最近距离向量为NDV_k，获取|NDV_k-1-NDV_k|与机器人直径比较，根据比较结果得到机器人当前位置的局部环境中障碍物个数。

具体而言，本发明采用二维的笛卡尔栅格C表示机器人当前探测到的环境。如图2所示，在步骤S1前，将测试区域划分为栅格，以栅格概率值表示每个栅格含有障碍物的概率。在现有技术中，传感器每次获得新的环境信息之后，需要对传感器有效测量范围之内的全部栅格进行更新。因此处理的信息量较多，而且需要传感器的测量视角较宽,适用于装配多个声纳传感器的机器人上，并不适于激光传感器。

本发明提供的方法将机器人探测到的障碍物从环境坐标系中投影到栅格坐标系上，并更新包含了障碍物投影栅格的栅格概率值。在此过程中，对障碍物进行适当的膨胀，并且对于不包含障碍物的栅格不做任何更新。

本发明实施例中，每个栅格表示的大小是5cm×5cm，环境的大小设定为10m×10m。图3是本发明较佳实施例提供的机器人主动窗口示意图。如图3所示，当机器人移动时，以机器人为中心，半径为w_s的圆形窗口也在移动，圆内的区域为主动窗口C^*，主动窗口内的栅格为主动栅格在室内试验中，圆形窗口的大小为30×30栅格，对应于室内环境中的1.5m×1.5m。在室外实验中,环境的大小设定为50m×50m，每个栅格表示的大小是25cm×25cm。主动窗口的大小为30×30栅格，对应于室外环境中的7.5m×7.5m。

于此，对每个栅格设置一变量c_ij表示其栅格概率值：

本实施例中，将每个主动栅格表达的内容转化为障碍向量β，β的方向由机器人中心指向该栅格。具体为其中：x₀及y₀为当前机器人的中心坐标；x_i及y_i为主动栅格的坐标；β_i,j为主动栅格到机器人中心的方向。

接下来，把主动窗口C^*内的障碍向量映射到距离向量直方图H上。直方图的构造方法如下，在主动窗口中划分出n个角度为α的扇区（n=360/α，如α=5°，n＝72），每k个扇区分别对应一个离散的角度ρ，ρ＝kα（k＝0，1，2，…，n－1），则每个主动栅格都在某个扇区内，两者之间的关系为k＝INT（β_i,j/α）。

然后定义每个扇区的矢量值：在C^*的某个扇区k内，若扫描到n个栅格概率值是0.8，即存在n个障碍物栅格，则分别记录这n个栅格的障碍物距离信息，把n个栅格距离信息的均值作为扇区的矢量值即其中d_i,j表示主动栅格（i，j）到机器人中心的距离，n表示在扇区k内主动栅格是障碍物栅格的数量，L是激光传感器接收到的障碍物点集，h_k(L)是计算在k扇区内距离障碍点的最小距离的函数。当扇区内没有障碍物时，h_k(L)=0。max(h_k(L))=d_max，d_max表示主动窗口的最大距离。

针对本实施例较小的室内环境（5米×5米），设置d_max=1.5米。在本发明提供的最近距离向量极坐标直方图中，横坐标代表各个扇区k（k＝0，1，2，…，n－1），纵坐标代表相应扇区内障碍物栅格距离机器人中心点的最近距离向量NDV_k（k＝0，1，2，…，n－1）。其中

下面结合图4及图5对不同的环境信息地图以及相应的最近距离向量极坐标直方图作出说明。如图4a所示，在以机器人为中心，半径为w_s的主动窗口内探测到4个障碍物A、B、C及D。如图4b所示，障碍物A、B、C及D分别对应直方图上面的四个柱状区，其中B和C在直方图上是相连的。图5a中有三个障碍物A1、B1及C1，图5b的直方图上有两个柱状区A对应同一障碍物。

考虑到实际机器人的大小和传感器的不确定性，直接将机器人的尺寸融合到障碍物的边界膨胀中。地图中的障碍单元格实际上由机器人的有效距离ρ，加上障碍点的传感不确定性u_i来膨胀。ρ=R_rob+d_s，R_rob=0.24米，表示机器人的半径，d_s=0.4米是机器人和所需避碰的障碍物之间最小的安全间隙（通常由机器人的运动速度决定，机器人的运动速度越大，d_s越大，速度越小，d_s越小）。

在最近距离直方图中，若两个相邻的扇区(i,j)的最近距离值满足|NDV_i-NDV_j|>2R_rob，则扇区i及j之间存在一个不连续区，称为间隙。若NDV_i>NDV_j，则两个扇区(i,j)间的不连续区被称为从j到i的增量不连续区，或者是从i到j的减量不连续区。此外，两个连续的间隙形成一个区间，称为“山谷”。假定S={0,1,...,n-1}是所有扇区的集合，则“山谷”V={l,...,r}是扇区集S中满足下面条件的非空扇区：V中任意两个相邻扇区间不存在不连续点（即“山谷”中扇区间都是连续的）；或在V的两个边缘扇区处存在两个不连续点（即“间隙”），即|NDV_l-1-NDV_l|>2R_rob且|NDV_r+1-NDV_r|>2R_rob。

从最近距离向量直方图中可以发现，当某个区间各个扇区中的最近距离向量值相差不大时，对应的是同一障碍物。在图5中，与障碍物A1对应的两个柱状区内相邻扇区（0度和360度扇区）之间的矢量值较为接近，所以直方图上的这些区间实际上都属于环境里的同一个障碍物，故判断这些区间是一个单一的障碍物。相距较远的不同障碍物如B1和C1，尽管在一个柱状区内，但和B1及C1对应扇区的交界处扇区的数值有较大的变化，通过这种较大变化可以判断B1及C1是不同的障碍物。

因此可得出以下结论：若扇区k与扇区k-1均包含障碍物，当|NDV_k-1-NDV_k|≤Δd_max时，扇区k与扇区k-1中包含同一个障碍物；当|NDV_k-1-NDV_k|>Δd_max时，扇区k与扇区k-1包含不同障碍物，其中，Δd_max=2R_rob为机器人直径。

当计算出在机器人当前位置的局部环境中存在多少个障碍物后，以起始角度begin及最终角度end表示障碍物大小，d表示障碍物所在扇区之内离机器人中心最近的距离，并根据所述begin、end及d分割出各个障碍物区，当机器人需要避开所述障碍物区时，则对应区间为避障区间。具体地，对每个障碍物用一个数据结构记录该区对应的最近距离向量直方图上的起始角度begin，最终角度end及该区域之内离机器人中心最近的距离值d。

步骤S2：设定最小避障阈值n_s，若NDV_k≥n_s,则扇区k的角度范围为避障区间，否则为自由行走区间。

具体而言，分割出各个障碍物区后，看机器人需不需要避开当前环境中各个障碍物，若需要避开称之为避障区间，不需避开的区域称为自由行走区间。在步骤S2中，n_s=d_max-d_s，其中d_s为机器人和需避碰的障碍物之间最小的安全间隙。

在最近距离向量直方图中，最近距离向量值小于n_s=d_max-d_s的那些扇区，都是自由行走区间。于本实施例中，使用最小避障距离n_s可以去掉一部分较远的障碍物区，在图4b中，有三个避障区间A，B及D，三个自由行走区间（即“山谷”）。如图6所示，因为A，B及D区域中最近距离向量值都在n_s之上，而C区的最近距离向量值小于n_s，其中C区的障碍物对机器人而言是安全的，可以不考虑，原来的山谷3得到扩展。类似地，对图5b中的两个避障区间A和C，图7表示了其对应的两个自由行走区间。

下面结合图8～图11说明本发明提供的方法与传统VFH算法的对比效果。从图8与图9、图10与图11的比较可以看出，本发明算法所生成的自由走动区间要比VFH算法生成的多，在方向选择上也具有更多的选择性。因为VFH算法的局限性在于主动窗口半径大小的选择：如果半径选择太大，可以考虑较大范围内的障碍物，但是在运动方向的选择上有了限制，甚至忽略了潜在的较好的运动方向。例如，在图9中，屏蔽了C障碍物对应区间，在图11中，屏蔽了B障碍物对应区间。机器人在运动方向上的选择范围大大减小，从而失去了一些可能是比较好的路径。另一方面，如果主动窗口的半径选择过小，运动方向上具有较多的选择，但只能考虑较小范围内的障碍物，使得当前运动方向的选择只能是次优的。因此，采用本发明的方法克服了这一问题，在考虑机器人周围较大范围内的障碍物分布情况的同时，又扩展了运动方向的选择性。

步骤S3：确定瞬时目标点的搜索范围，并在所述搜索范围中获取瞬时目标点，机器人运动到达所述瞬时目标点后，确定新的瞬时目标点并向新的瞬时目标点运动，直至到达终点。

具体而言，传统的VFH系列方法采用面向目标的选择方式，在这些候选方向中，选择最靠近目标方向的“山谷”，然后再根据“山谷”宽度选择机器人的运动方向。但这种方法只适合短距离内的局部避障算法，无法实时应用在长距离的避障过程中，尤其是在一些复杂的环境中。为了克服上述问题，本发明采用瞬时目标点完成机器人在候选方向上对“山峰”的边界跟踪。

假设瞬时目标在机器人第j个扫描传感方向上，距离为r_IG，在极坐标系中表示为图12表示了机器人从瞬时初始位置S_i,k向瞬时目标点G_i,k移动时的搜索范围。如图12所示，在“山峰”边界跟踪模式中，机器人沿着[P_tP_IG]方向运动，直到到达瞬时目标点，然后确定新的瞬时目标点并继续前进。机器人进行障碍物边界跟踪时，存在起始点S_i，其中i=1,2,...是依次碰到的“山峰”的编号；当瞬时目标点确定后，此时机器人的位置被定义为瞬时初始位置S_i,k，其中k=1,2,...是跟踪第i个障碍物时瞬时初始位置的编号。同样地，跟踪第i个障碍物时产生的一系列瞬时目标为G_i,k。

如上所述，机器人搜索范围的角度为其中P_t为机器人当前坐标点，SchFrom为开始搜索时的角度，SchEnd为结束搜索时的角度，SchDir为搜索开始时偏离SchFrom的角度。其中，机器人的跟踪方向SchDir可以任意赋值给机器人，使其左转或右转。如图12所示，第i个障碍上的最近碰撞点就是在[S_iG]连线上的可见的障碍点，目标碰撞点定义为机器人在到达第k个瞬时目标点时所期望碰到的可见障碍点。SchFrom根据机器人所处的位置来决定，如果机器人当前处在一些起始点时，可将SchFrom简单设为朝向目标的方向，否则，机器人从S_i,k朝向G_i,k运动，将SchFrom设定为朝向障碍物边界上最接近机器人的碰撞点

当瞬时目标点的搜索范围确定以后，可以通过在这个范围内对每一个扫描激光束进行检测来获得瞬时目标点。j_cur和j_old分别表示当前和最近搜索的扫描激光束，其初始值分别为j_old=j_cur=SchFrom。

在步骤S3中，如果传感器在搜索范围中的当前和最近扫描激光束的测量点之间的距离超过预设阈值，则在所述当前或最近扫描激光束上存在一个候选瞬时目标点。如果在比较预设数量（如5个）当前和最近扫描激光束的测量点之间的距离后，发现都没有超过该阈值，则在当前或最近扫描激光束上不存在候选瞬时目标IG^*，继续搜索。

如果候选瞬时目标点存在，将候选瞬时目标点IG^*的方向设定为当前和最近扫描激光束中较长的激光扫描束的方向，表示为 $j_{{\mathrm{IG}}^{*}}=\left\{\begin{array}{ccc}{j}_{\mathrm{cur}}& \mathrm{if}& {\hat{R}}_{j_{\mathrm{cur}}}>{\hat{R}}_{j_{\mathrm{old}}}\\ j_{\mathrm{old}}& & \mathrm{otherwise}\end{array}\right..$ 则候选目标点与机器人之间的距离为 $r_{{\mathrm{IG}}^{*}}=\left\{\begin{array}{ccc}{\hat{R}}_{j_{{\mathrm{IG}}^{*}}}-{\hat{R}}_{\mathrm{rob}}& \mathrm{if}& {\hat{R}}_{j_{{\mathrm{IG}}^{*}}}\≥R_d-{\mathrm{\σ}}_{R_d}\\ \max ({\hat{R}}_{j_{{\mathrm{IG}}^{*}}}-2R_{\mathrm{rob}},r_0)& & \mathrm{otherwise}\end{array}\right..$ 其中r₀是一个确保不小于某个最小值的常数。

于此，瞬时目标点在机器人第j个扫描传感方向上且距离为r_IG，当扫描传感方向上存在长度为r_IG的安全路径时，或者两个相邻测量点的距离大于2R_rob+2d_s时（即），对应的候选瞬时目标点即为瞬时目标点。当满足上述第一种条件时，直接表明发现了瞬时目标G_i,k。第二种条件下，让j_near为相对于j_old激光扫描束的集合中最近的一条，设定j_IG=j_near,如果上述两种情况都不满足，继续搜索瞬时目标。

下面结合图13～图15，在移动机器人试验平台上按照不同情况说明本发明提供方法的优点。本实施例中机器人试验平台是半径为25厘米装有2D激光测距器，以及双目立体视觉的完整性机器人。在室内环境下，设定机器人的最大平移速度为v_max=0.3m/s，最大旋转速度为w_max=1.57rad/s。以下将在未知的、非结构化的复杂室内环境中，通过三个实验来验证算法的路径规划性能。

图13b～f表示了机器人在较复杂的通道环境中的穿行过程。在通道中机器人不断更新环境中的信息，并实时决策出新的运动方向，并到达最终目标。不难发现，本发明的算法可以使机器人在通道中运动，并且在复杂通道中，具有根据新的环境信息灵活选择运动方向的能力。

图14a～f是本发明较佳实施例提供的机器人在密集障碍物A2、B2、C2、D2及E2中穿行过程示意图。如图14a～f所示，各障碍物之间的距离相对较小，机器人先后绕过了障碍物A2、B2、C2、D2及E2，在这个过程中可以看出机器人总是可以安全避开当前发现的障碍物。在避开当前障碍物的同时，在机器人的前方又发现了新的障碍物信息，根据当前环境的信息再进行决策，设定新的临时目标点。表现出较好的实时性能和避障性能。

图15a～h是本发明较佳实施例提供的机器人在V形和U形障碍物中穿行过程示意图。如图15a～h所示，共有一个V形障碍物和两个U形障碍物。图15a中机器人首先发现了V形障碍物，此时发现的只是V形障碍物的局部信息，这个局部信息让机器人向右前方运动（如图15b）。机器人向右前方运动一小段距离后，发现右前方又有新的障碍物，机器人马上做了向左前方运动的决策，最后避开V形障碍物（如图15c）。这说明了机器人在运动过程中不断更新环境信息，根据当前环境的局部信息，实时改变不好的运动方向，表现出良好的主动纠错能力。从图15e～g可以看出，机器人成功避开了两个U形障碍物，最后穿过通道。

综上所述，根据本发明较佳实施例提供的避障路径规划方法，能够自动创建较为精确的环境地图，并解决任意时刻的定位问题。同时可以在通道中自由穿行，避开一些复杂形状的障碍物，而且在密集的障碍物环境下也具有较好的路径规划能力。此外，本发明采用的算法具有灵活的方向决策能力，能不断根据环境中新的信息调整运动方向，根据新的环境做出决策，确定新的瞬时目标点。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (4)

1.一种基于最近距离向量场直方图的避障路径规划方法，其特征在于，包括以下步骤：

S1、将机器人当前扫描范围均分为n个扇区，若第k个扇区内的障碍物距离机器人中心点的最近距离向量为NDV_k，获取|NDV_k-1-NDV_k|与机器人直径比较，根据比较结果得到机器人当前位置的局部环境中障碍物个数，

所述NDV_k表示为：其中L是机器人传感器接收到的障碍物点集，h_k(L)表示在扇区k内距离障碍物的最小距离，d_max表示机器人扫描的最大距离；

S2、设定最小避障阈值n_s，n_s＝d_max-d_s，其中d_s为机器人和需避碰的障碍物之间最小的安全间隙，若NDV_k≥n_s,则扇区k的角度范围为避障区间，否则为自由行走区间；

S3、确定瞬时目标点的搜索范围，并在所述搜索范围中获取瞬时目标点，机器人运动到达所述瞬时目标点后，确定新的瞬时目标点并向新的瞬时目标点运动，直至到达终点。

2.根据权利要求1所述的方法，其特征在于，在所述步骤S3中，所述搜索范围的角度为其中P_t为机器人当前坐标点，SchFrom为开始搜索时的角度，SchEnd为结束搜索时的角度，SchDir为搜索开始时偏离SchFrom的角度。

3.根据权利要求1所述的方法，其特征在于，在所述步骤S3中，如果传感器在搜索范围中的当前和最近扫描激光束的测量点之间的距离超过预设阈值，则在所述当前或最近扫描激光束上存在一个候选瞬时目标点。

4.根据权利要求3所述的方法，其特征在于，若瞬时目标点在机器人第j个扫描传感方向上且距离为r_IG，则当所述扫描传感方向上存在长度为r_IG的安全路径时，或者两个相邻测量点的距离大于2R_rob+2d_s时，对应的候选瞬时目标点即为瞬时目标点，其中R_rob表示机器人半径。