在局部感知能力下的势流法的避障方法与流程

本发明涉及一种在局部感知能力下的势流法的避障方法，属于避障方法技术领域。

背景技术：

机器人的实时避障是机器人自动能力的关键问题之一，近年来有大量海内外学者都投入研究。避障规划不同于路径规划，路径规划一般是已获知环境的全局信息。而避障规划通常是根据传感器实时检测到的局部信息，调整轨迹以避免发生碰撞。所以在未知环境的情况下，避障对于智能机器人的自主行动能力显得尤为重要。

目前传统的避障方法主要分为：可视图法、vfh(vectorfieldhistogram直方图法)、动态窗口法、人工势场法等。但是各些方法都有各自的优缺点。譬如，可视图法虽然算法简单，可是缺乏灵活性，对障碍物形状有要求，局限性太大。栅格法中栅格大小是影响它避障效果的主要问题，并且伴随有局部极小值的现象。人工势场法既可以用来避障也可以用来路径规划，但是伴随的问题是避障过程中容易陷入局部极小值问题，还存在着目标不可达的问题。在现有的避障算法中都或多或少存在着一些问题。

技术实现要素：

本发明提出了一种在局部感知能力下的势流法的避障方法，旨在未知环境下为有一定感知能力的机器人解决避障规划问题，有效地防止进入局部极值，可以平滑地绕开障碍物以达到目标点。

本发明为解决其技术问题采用如下技术方案：

一种在局部感知能力下的势流法的避障方法，包括以下步骤：

(1)构造在感知范围内的障碍物模型；

(2)构造机器人在势流法中受目标点影响的引力速度；

(3)构造机器人在势流法中受障碍干扰时的流体速度；

(4)在各种障碍环境中避障规划的仿真结果。

步骤(1)中构造在感知范围内的障碍物模型的具体步骤如下：

设定pobject＝(xobj，yobj)，pobject代表下标object的位置，xobj，yobj分别代表object的x轴、y轴的坐标；设机器人的当前位置为pr＝(xr，yr)，出发点ps＝(xs，ys)，目标点为pt＝(xt，yt)，其中：xr、yr为机器人的x轴、y轴的坐标，xs、ys为出发点的x轴、y轴的坐标，xt、yt为目标点的x轴、y轴的坐标；机器人的感知范围是以自身当前位置pr为中心，以rmax为感知的最大半径；rs为机器人结构最大外接圆半径，即安全距离；在检测范围r＜rmax内检测障碍物的点为pobs(i)＝(xi，yi)，xi、yi为第i个障碍物的x轴、y轴的坐标；pobs(i，j)代表检测第i，j障碍物之间的距离；若在检测范围内检测障碍物的任意两个点小于2rs，即其中i≠j，则认为两点间的连线为新添加虚拟障碍物点pobs+(n)＝(xn，yn)，xn、yn为第n的新添加虚拟障碍物的x轴、y轴的坐标。

步骤(2)中所述构造机器人在势流法中受目标点影响的引力速度的具体步骤如下：

在没有障碍物干扰的情况下，目标点对机器人的吸引单位速度向量为：其中：xt、yt为目标点的x轴、y轴的坐标，xr、yr为机器人的x轴、y轴的坐标，为目标点位置pt与机器人位置pr的方向矢量，pt为目标点的位置，pr为机器人的当前位置，表示的模；吸引速度向量为：式中c为常量，用来确定目标点对机器人吸引速度单位大小；这样就在没有障碍物影响的情况下形成了一种类似于由高地势往低地势流入的速度流场，速度流场上的速度即对机器人的期望速度。

其特征在于，步骤(3)中所述构造机器人在势流法中受障碍干扰时的流体速度的具体步骤如下：

假设r为机器人检测范围，若在r≤rmax内检测到障碍，rmax为机器人感知的最大半径，则距离机器人最近的障碍物po＝(xmin，ymin)会对机器人起干扰作用，使机器人从障碍物旁边以流水型的方式绕过障碍；机器人位置pr与最近障碍物位置po的位置关系为：即xmin，ymin分别代表距离机器人最近的障碍物的x轴、y轴上的坐标，xr、yr为机器人的x轴、y轴的坐标，xro为的x轴坐标，yro为的y轴坐标，表示距离机器人最近障碍物位置po与机器人位置pr的方向矢量；最近障碍物po使机器人pr的流体速度为代表垂直切线方向的单位速度矢量，具体方向取决于上一时刻速度的方向；代表方向的单位速度矢量；代表吸引力方向的单位速度矢量，α、β、δ分别为的参数，c为速度常量，表示距离机器人最近障碍物位置po与机器人位置pr的方向矢量，表示机器人位置pr与目标点位置pt的方向矢量。

本发明的有益效果如下：

1、本发明是在未知环境中，以机器人为中心的局部感知的条件下，利用现有的障碍检测信息去确定下一步机器人的速度，以达到避开障碍物的结果。相比较传统的避障方法，算法简单明了，易于实现，工程上的应用更合理、更广泛。解决了现有避障技术在未知环境下难以实施的问题。

2、本发明提出的一种新型的速度势流场方法。本发明相对于传统的人工势场法、栅格法、bug法在控制上更加容易实现。临时构建的速度流场。不会产生太多冗余数据。而由于算法的简易性，可以保证在工程上的处理速度快，系统响应快，使机器人的避障的实时性更加可靠。

3、本发明在速度势流场上的速度函数构造，使机器人比较合理的响应最近障碍物信息。机器人会以自己当前位置和速度为基础规划出下一步比较合理的速度，更加贴合实际。创新性的以水流绕开障碍物为模型构建速度函数。通过仿真结果可知：在未知环境下，从检测到障碍物到绕过障碍物，机器人规划的路径都相对来说比较平滑、合理。

附图说明

图1是避障方法流程图。

图2是障碍物1的形状图。

图3是障碍物1构造后的形状图。

图4是障碍物2的形状图。

图5是障碍物2构造后的形状图。

图6是避障规划情况1示意图。

图7是避障规划情况2示意图。

图8是避障规划情况3示意图。

图9是避障规划情况4示意图。

具体实现方式

下面结合附图对本发明创造做进一步详细说明。其中rmax为机器人感知的最大半径；rs为机器人结构最大外接圆半径，即安全距离；ps为出发点位置，pt为目标点位置，pr为机器人的位置。pobs为检测范围以内检测到的障碍物。

图1为本文阐述的势流法避障方法的流程图。

图2为rmax＝30；rs＝4；pr＝(45，65)时检测到的障碍物1的形状，圆心是机器人的位置，直线圆是机器人检测范围：以rmax为半径的圆。星点是在检测范围以内检测到的障碍物pobs。图3中的圆点是构造出新添加的障碍物pobs+。同理图4、图5分别为rmax＝30；rs＝4；pr＝(45，65)时检测到的障碍物2的形状和构造出新添加的障碍物。

图6为障碍物为分散点时，机器人的避障情况：rmax＝5；rs＝2时，三角形是出发点ps＝(0，0)，正方形是目标点pt＝(100，100)，虚线是机器人行驶的路线，星点是障碍物。仿真显示：机器人的路径较为合理地到达目标点。

图7当障碍物通道类型时，机器人的避障情况：rmax＝5；rs＝2时，三角形是出发点ps＝(0，0)，正方形是目标点pt＝(70，150)，虚线是机器人行驶的路线，星点是障碍物。仿真显示：机器人在经过通道时路线比较平滑，基本不会震荡。

图8表示当障碍物有小于2rs的安全距离的通道，机器人的避障情况：rmax＝5；rs＝2时，三角形是出发点ps＝(20，0)，正方形是目标点pt＝(70，150)，虚线是机器人行驶的路线，星点是障碍物。仿真显示：当障碍物有小于2rs的安全距离的通道时，机器人不能通过，则会把通道添加成新的障碍物，并绕开障碍物，防止发生碰撞。

图9表示当障碍物为凹型时，机器人的避障情况：rmax＝5；rs＝2时，三角形是出发点ps＝(40，0)，正方形是目标点pt＝(70，150)，虚线是机器人行驶的路线，星点是障碍物。仿真显示：当障碍物为凹型时，机器人比较合理的绕开障碍物，到达目标点。并不会陷入局部极值。

(1)构造在感知范围内的障碍物模型：

假设机器人的当前位置为pr＝(xr，yr)，出发点为ps＝(xs，ys)，目标点为pt＝(xt，yt)，其中：xr、yr为机器人的x轴、y轴的坐标，xs、ys为出发点的x轴、y轴的坐标，xt、yt为目标点的x轴、y轴的坐标。机器人的感知范围是以自身当前位置pr为中心，以rmax为感知的最大半径；rc为危险半径；rs为机器人结构最大外接圆半径，即安全距离。在检测范围r＜rmax内检测障碍物的点为pobs(i)＝(xi，yi)，i为检测的第i个障碍物，xi为第i个障碍物的x轴的坐标，yi为第i个障碍物的y轴的坐标。pobs(i，j)代表检测第i，j障碍物之间的距离，且i≠j。若在检测范围内检测障碍物的任意两个点小于2rs，即则认为两点间的连线点为新添加虚拟障碍物点pobs+(n)＝(xn，yn)，n为添加的第n个虚拟障碍物，xn为第n个虚拟障碍物的x轴的坐标，yn为第n个障碍物的y轴的坐标。这样的障碍物构造设定会防止机器人进入危险的通道。并且会对障碍物的边界进行一个平滑作用，避免机器人路径震荡。

(2)构造机器人在势流法中受目标点影响的引力速度：

机器人的初始速度为：其中：c为速度常数，xs为出发点x轴的坐标，ys为出发点y轴的坐标。xt为目标点x轴的坐标，yt为目标点y轴的坐标。pt为目标点位置，ps为出发点位置，代表pt和ps之间的位置向量。在没有障碍物干扰的情况下，对机器人的吸引单位速度向量：其中：xr为机器人的当前位置x轴的坐标，yr为机器人的当前位置y轴的坐标，pt为目标点的位置，pr为机器人的当前位置，代表pt和pr之间的位置向量。；吸引速度向量为：式中c为常量，用来确定目标点对机器人吸引速度单位大小。这样就在没有障碍物影响的情况下形成了一种类似于由高地势(即机器人出发点)往低地势(目标点)流入的速度流场。速度流场上的速度即对机器人的期望速度。假设机器人从当前速度到期望速度的响应时间忽略不计。机器人在此作用下会以速度匀速地到达目标点。

(3)构造机器人在势流法中受障碍干扰时的流体速度：

若在机器人检测范围rmax内检测到障碍，则距离机器人最近的障碍物po＝(xmin，ymin)会对机器人起干扰作用，使机器人从障碍物旁边以流水型的方式绕过障碍。机器人位置pr与最近障碍物位置po的位置关系为：即其中：为po和pr之间的位置向量，xro为的x轴坐标，yro为的y轴坐标，xmin为距离机器人最近的障碍物po的x轴坐标，ymin为距离机器人最近的障碍物po的y轴坐标，xr为机器人的当前位置x轴的坐标，yr为机器人的当前位置y轴的坐标；它们之间的距离为：其中：rro为po和pr之间的距离。最近障碍物po对机器人pr的流体速度为代表垂直切线方向的单位速度矢量，具体方向取决于上一时刻速度的方向；代表方向的单位速度矢量；代表吸引力方向的单位速度矢量。而α，β，δ代表各部分的加权值，且α+β+δ＝1。

其中：

障碍物po对机器人pr的流体速度主要分为两种状态：

①当rs＜rro≤rc时，主要是方向的单位速度矢量和垂直切线方向的单位速度矢量共同作用产生的速度

其中：

②当rc＜rro≤rmax时，主要是垂直切线方向的单位速度矢量和吸引力方向的单位速度矢量共同作用产生的速度

其中：

上式中rmax为机器人感知的最大半径；rc为危险半径；rs为机器人结构最大外接圆半径，即安全距离。代表rrt当前一小段时间内的导数。当且时，代表机器人朝着靠近目标点的方向前进且障碍物在机器人前方时，机器人采取绕开障碍物的策略；当且时，代表机器人朝着靠近目标点的方向前进且障碍物在机器人后方时，障碍物对机器人并没有威胁，所以速度直接以为方向驶向目标点。当时，代表机器人受障碍物影响往远离目标点的方向前进，则机器人会沿着障碍物直到绕开障碍物。

(4)在各种障碍环境中避障规划的仿真结果

经过仿真证明由于rs＜rro≤rc时，障碍物po对机器人作用的速度矢量中有排斥的速度分量，并且随着rro的减小而增大，在假设机器人响应速度的时间t可以忽略不计时，机器人不会碰撞到障碍物。

当rc＜rro≤rmax且时，代表机器人在靠近目标点，即往目标点方向前进。障碍物po对机器人作用的速度矢量中有垂直于的速度分量和平行于的速度分量当rro越小，垂直于的速度分量就越大，平行于的速度分量就越小。所以当机器人在距离障碍物较近的时候且平行于的速度分量方向满足时，它们的距离rro会收敛在rc处，即机器人会沿着垂直于的方向绕开障碍物。

而当rc＜rro≤rmax且时，代表机器人往远离目标的方向行驶，说明机器人是在障碍物的影响下才作用的。这时候机器人则只有垂直于的速度分量因为在局部感知的情况下，障碍物是机器人唯一的信息。当机器人在障碍物的作用下，往远离目标点的方向行驶，说明了机器人正在逃离凹型障碍物。则机器人在脱离这段过程中不能失去障碍物的边界信息，否则很可能陷入局部最优解。所以障碍物作用机器人的速度矢量中只有垂直于的速度分量

通过matlab仿真得到结果证明该方法是可行且理想的，势流法可以理想地使机器人受最近的障碍物影响，平滑的绕开障碍物驶向目标点。避免了很多局部最优值的情况，并且路径局部不会震荡，总体来说比较合理。因此本发明具有更广泛的应用领域。