一种基于类三维地图的移动机器人全局路径规划方法

摘要

本发明公开了一种基于类三维地图的移动机器人全局路径规划方法，步骤1：将普通栅格地图改造为基于等高线原理的类三维地图；步骤2：初始规划：将移动机器人的起点和终点连成一条直线作为当前路径；步骤3：扫描当前路径穿过的栅格，若未扫描到障碍，则转至步骤6；若扫描到障碍，则根据障碍类型转至步骤4或步骤5；步骤4：处理与边界接触的障碍，更新当前路径，返回步骤3；步骤5：处理不与边界接触的障碍，更新当前路径，返回步骤3；步骤6：输出当前路径，全局路径规划完成。本发明充分发掘障碍物信息，提高程序执行效率、易于实现，满足移动机器人全局路径规划的实时性要求。

说明书

技术领域

本发明属于机器人及其智能控制领域，涉及一种基于类三维地图的移动机器人全局路径规划方法。

技术背景

移动机器人路径规划的任务是在有障碍物的工作环境中寻找一条从给定起点到终点的适当的运动路径，使机器人在运动过程中能安全无碰的绕过所有障碍物。根据工作环境的不同，路径规划可以分为两种：工作环境的信息全部已知的全局路径规划；工作环境的信息全部未知或部分未知的局部路径规划。

目前应用较多的全局路径规划算法包括：拓扑法、可视图法和栅格法。拓扑法建立拓扑网络的过程相当复杂，特别在增加障碍物时如何有效的修正已经存在的拓扑网络及如何提高图形速度是有待解决的问题；可视图法在搜索最短距离路径时对所有定点的价值不加以区别，仅仅只是将所有可行路径列举出来然后在其中选出一条最优路径，导致耗时较长，特别是当机器人的尺寸大小很小时搜索时间很长；栅格法中栅格粒度太大，规划的路径很不准确；栅格粒度太小，算法的搜索范围将按指数增加，计算量大，占用大量的存储空间，实时性不理想。

这些方法均侧重于路径的生成过程，只利用障碍物的边界信息，而不能根据障碍物信息来进行决策。因此充分发掘障碍物信息，设计一种简单高效且易于编程实现的全局路径规划算法，是提高程序执行效率、准确规划路径所必须考虑的问题。

发明内容

本发明的目的是克服上述现有全局路径规划算法的不足，提出一种基于类三维地图的移动机器人全局路径规划方法，充分发掘障碍物信息，提高路径规划效率，易于实现，满足移动机器人全局路径规划的实时性要求。

本发明的技术方案如下：

一种基于类三维地图的移动机器人全局路径规划方法，其特征在于，包括以下步骤：

步骤1：将普通栅格地图改造为基于等高线原理的类三维地图；

步骤2：初始规划：将移动机器人的起点和终点连成一条直线作为当前路径；

步骤3：扫描当前路径穿过的栅格，若未扫描到障碍，则转至步骤6；若扫描到障碍，则首先确定障碍的类型，障碍的类型包括与边界接触的障碍和不与边界接触的障碍；如果是与边界接触的障碍，转至步骤4，否则转至步骤5；所述的与边界接触的障碍包括只与一侧边界接触的障碍以及与两侧边界接触的障碍；

步骤4：处理与边界接触的障碍，更新当前路径，返回步骤3；

步骤5：处理不与边界接触的障碍，更新当前路径，返回步骤3；

步骤6：输出当前路径，全局路径规划完成；

所述的步骤1的具体步骤为：

给栅格地图建立I、J坐标系，栅格地图中的栅格用数组ZG表示；数组ZG的元素ZG[I][J]表示高度值；

依据等高线原理建立类三维地图，就是给地图中的每一个栅格点都赋予高度值，ZG[I][J]＝H，H∈{0、1、2、3……}，H值越大表示高度越高；

栅格地图分为两种区域：自由区域与障碍区域，自由区域的高度值H＝0；障碍区域的高度值H＞0，障碍区域分为三种：不与边界接触的障碍区域，只与一侧边界接触的障碍区域，与两侧边界接触的障碍区域；只与一侧边界接触的障碍区域以及与两侧边界接触的障碍区域对应的障碍统称为障碍；以下分三种情况说明障碍区域的类三维地图的创建方法：

(I)对于不与边界接触的障碍区域：

iii  找出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin；

iv  给该区域的所有I＝Imax和I＝Imin的两列栅格的高度值赋予1，给该区域的所有I＝(Imax-1)和I＝(Imin+1)的两列栅格的高度值赋予2，给该区域的所有I＝(Imax-2)和I＝(Imin+2)的两列栅格的高度值赋予3，依此类推，直到该区域所有的栅格都被赋予一个高度值；

v  给该区域的所有J＝Jmax和J＝Jmin的两行栅格的高度值加上1，给该区域的所有J＝(Jmax-1)和J＝(Jmin+1)的两行栅格的高度值加上2，给该区域的所有J＝(Jmax-2)和J＝(Jmin+2)的两行栅格的高度值加上3，依此类推，直到该区域所有的栅格都被加上一个高度值；

(II)对于只与一侧边界接触的障碍区域：

i、出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin；

ii、判断Imax、Imin、Jmax、Jmin四个值中哪个为边界值，即：判断障碍区域与栅格地图的上、下、左、右四个边界中的哪个接触，从与该边界相对的一侧开始给障碍区域赋予高度值；给障碍区域中离该边界最远的一行或一列栅格赋予高度值1，给离该边界第二远的一行或一列栅格赋予高度值2，给离该边界第三远的一行或一列栅格赋予高度值3，依此类推，直到离该边界最近的一行或一列栅格也被赋予高度值；

iii、然后从另一个方向开始给障碍区域的栅格加上高度值，即：不妨设上一步中是按I坐标方向给栅格赋值，则本步骤中按J坐标方向给栅格加上高度值；给该区域的所有J＝Jmax和J＝Jmin的两行栅格的高度值加上1，给该区域的所有J＝(Jmax-1)和J＝(Jmin+1)的两行栅格的高度值加上2，给该区域的所有J＝(Jmax-2)和J＝(Jmin+2)的两行栅格的高度值加上3，依此类推，直到该区域所有的栅格都被加上一个高度值；

(II)对于与两侧边界接触的障碍区域：

a)找出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin；

b)判断Imax、Imin、Jmax、Jmin四个值中哪两个为边界值，即：判断障碍区域与地图的上、下、左、右四个边界中的哪两个边界接触，然后从两个被接触的边界中任选一个，从与该边界相对的一侧开始给障碍区域赋予高度值；不妨设选择行对应的边界为参照，给离该边界最远的一行栅格赋予高度值1，给离该边界第二远的一行栅格赋予高度值2，给离该边界第三远的一行栅格赋予高度值3，依此类推，直到离该边界最近的一行栅格也被赋予高度值；

然后从与另一边界相对的一侧开始给障碍区域加上高度值；给离另一边界最远的一列栅格的高度值加上1，给离另一边界第二远的一列栅格的高度值加上2，给离另一边界第三远的一列栅格的高度值加上3，依此类推，直到离另一边界最近的一列栅格也被加上高度值。

所述的步骤4中的处理与边界接触的障碍的步骤以及所述的步骤5中的处理不与边界接触的障碍的步骤为：当前路径上每段位于与边界接触的障碍区域的路径段按照下降方向进行基本规划；

定义下降方向：若路径穿过了某个障碍区域，则以位于障碍区域内的路径段中高度最大的点为起点，垂直于该路径段且所经过的栅格的高度值降低的方向就是下降方向；

定义基本规划：若两点之间的初始规划路径穿过了障碍区域，则找到每段位于障碍区域的路径的下降方向，然后将每段位于障碍区域的路径用沿着障碍区域从下降方向绕过障碍区域的新路径段代替，基本规划就是对初始规划进行修正，基本规划后的路径段称为修正路径段；

具体步骤分为以下四步：

I、找出当前路径上每段位于与边界接触的障碍区域的路径段的起点与终点，

修正路径段上的点(Iy，Jy)需满足以下三个条件：

第一个条件：位于下降方向一侧，即，满足下式：

式中：$k=\frac{\mathrm{Im}-\mathrm{In}}{\mathrm{Jm}-\mathrm{Jn}},$(Im，Jm)为该段穿过了障碍区域的路径段的起点坐标，(In，Jn)为该段穿过了障碍区域的路径段的终点坐标，(Iam，Jam)为该障碍区域的高度值最大的点的坐标；

第二个条件：高度值为0，ZG[Iy][Jy]＝0；

第三个条件：与对应的障碍区域直接相邻；【相邻，即路径所在栅格与障碍区域直接相邻。】

II、找出被当前路径穿过的每一块与边界接触的障碍区域的犄角点；

定义犄角点：在基本规划中，修正路径段上的点中与被修正路径段的距离最大的点就是犄角点；

III、找出与当前路径的每一段直线路径段对应的中转点；

在所有的位于当前路径同一侧的犄角点中离当前路径最远的点，称为与当前路径对应的中转点；若当前路径为折线，则要找出与折线的每一段直线路径段对应的中转点；

IV、将起点、所有中转点以及终点沿当前路径的方向用折线顺次连接起来，将新得到的折线路径作为当前路径；转到步骤3。

所述移动机器人为轮式移动机器人，其移动装置采用轮式结构，具备沿弧线与直线及折线运动的能力。

所述障碍的轮廓为块状。

所述的移动机器人设有传感器，能确定机器人在栅格地图中的位置，所述的传感器有GPS装置、激光测距仪、红外测距仪、或者超声波测距仪。所述的移动机器人所配置的传感器能感知一定范围内的障碍物信息，包括与机器人的距离和障碍物的大小。

有益效果：

与现有的技术相比，本发明用一种简单的方法对障碍区域进行处理，并且当环境中的障碍区域发生变化时处理算法也不会变复杂，克服了拓扑图法建立拓扑图很复杂的缺点；本发明创建类三维地图后，提出了下降方向的概念，当规划过程中遇到障碍时，下降方向就像路标一样及时指出最佳避障方向，克服了可视图法和普通栅格法要通过比较若干条不同路径才能得出最佳避障路径的缺点；本发明的规划时间可以预期，最坏情况下需进行的规划次数为地图中障碍区域个数，克服了可视图法和普通栅格法要进行地图中障碍区域总数目的若干倍次规划的缺点，也克服了可视图法和普通栅格法在地图中障碍区域增加时规划次数呈指数增加的缺点；本发明在规划过程中只需搜索起点与终点之间的直连线周围一定范围内的障碍区域，并不对地图中的所有障碍区域进行搜索，大大提高了规划效率，提高了规划的时效性。

附图说明

图1为全局路径规划算法主要实现过程的程序流程图；

图2为类三维地图的效果图；

图3为从一个坐标方向为每个栅格赋予高度值的一个障碍区域；

图4为从两个坐标方向为每个栅格赋予高度值的一个障碍区域；

图5为下降方向示意图；

图6为修正路径段示意图；

图7为犄角点示意图；

图8为当前路径为直线时中转点示意图；

图9为当前路径为折线时每段直线的中转点示意图；

图10为基于图8中的栅格地图输出的最终全局规划路径。

图中标号说明：1，2，3均表示障碍区域，

f表示下降方向，

A，B表示路径起点和终点，

图7中C，D，E表示犄角点，

图8中E点为中转点，

图9中折线ADEB为当前路径，C为犄角点和与AD段对应的中转点。

具体实施方式

以下将结合图和具体实施过程对本发明做进一步详细说明：

实施方式1

图1为全局路径规划算法主要实现过程的程序流程图，具体步骤如下：(1)在普通二值栅格地图的基础上依据等高线原理建立类三维地图，给地图中的每一个栅格点都赋予高度值。给地图建立I、J坐标系，地图中的栅格用数组ZG[I][J]表示。栅格的高度值用H表示，即，ZG[I][J]＝H，H∈{0、1、2、3……}，H值越大表示高度越高。

在本发明中将地图分为两种区域：自由区域与障碍区域。自由区域的高度值H＝0；障碍区域的高度值H＞0，不与边界接触的障碍区域中位于重心的栅格点高度值最大，与边界接触的障碍区域中与边界接触的栅格点高度值最大，以最高点为中心向四周辐射高度值逐渐减小。障碍区域分为三种：不与边界接触的障碍区域，只与一侧边界接触的障碍区域，与两侧边界接触的障碍区域。以下分三种情况介绍障碍区域的类三维地图的创建方法。

a)对于不与边界接触的障碍区域：

先找出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin。

然后给该区域的所有I＝Imax和I＝Imin的两列栅格的高度值赋予1，给该区域的所有I＝(Imax-1)和I＝(Imin+1)的两列栅格的高度值赋予2，给该区域的所有I＝(Imax-2)和I＝(Imin+2)的两列栅格的高度值赋予3，依此类推，直到该区域所有的栅格都被赋予一个高度值。

最后给该区域的所有J＝Jmax和J＝Jmin的两行栅格的高度值加上1，给该区域的所有J＝(Jmax-1)和J＝(Jmin+1)的两行栅格的高度值加上2，给该区域的所有J＝(Jmax-2)和J＝(Jmin+2)的两行栅格的高度值加上3，依此类推，直到该区域所有的栅格都被加上一个高度值。

b)对于只与一侧边界接触的障碍区域：

先找出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin。

判断Imax、Imin、Jmax、Jmin四个值中哪个为边界值，即：判断障碍区域与地图的上、下、左、右四个边界中的哪个接触，从与该边界相对的一侧开始给障碍区域赋予高度值。给障碍区域中离该边界最远的一行或一列栅格赋予高度值1，给离该边界第二远的一行或一列栅格赋予高度值2，给离该边界第三远的一行或一列栅格赋予高度值3，依此类推，直到离该边界最近的一行或一列栅格也被赋予高度值。

然后从另一个方向开始给障碍区域的栅格加上高度值，即：若上一步中是按I(或J)坐标方向给栅格赋值，则本步骤中按J(或I)坐标方向给栅格加上高度值。给该区域的所有J＝Jmax和J＝Jmin(或I＝Imax和I＝Imin)的两行(或两列)栅格的高度值加上1，给该区域的所有J＝(Jmax-1)和J＝(Jmin+1)(或I＝(Imax-1)和I＝(Imin+1))的两行(或两列)栅格的高度值加上2，给该区域的所有J＝(Jmax-2)和J＝(Jmin+2)(或I＝(Imax-2)和I＝(Imin+2))的两行(或两列)栅格的高度值加上3，依此类推，直到该区域所有的栅格都被加上一个高度值。

c)对于与两侧边界接触的障碍区域：

先找出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin。

判断Imax、Imin、Jmax、Jmin四个值中哪两个为边界值，即：判断障碍区域与地图的上、下、左、右四个边界中的哪两个接触，然后从两个被接触的边界中任选一个，从与该边界相对的一侧开始给障碍区域赋予高度值。给离该边界最远的一行(或一列)栅格赋予高度值1，给离该边界第二远的一行(或一列)栅格赋予高度值2，给离该边界第三远的一行(或一列)栅格赋予高度值3，依此类推，直到离该边界最近的一行(或一列)栅格也被赋予高度值。

然后从与另一边界相对的一侧开始给障碍区域加上高度值。给离另一边界最远的一列(或一行)栅格的高度值加上1，给离另一边界第二远的一列(或一行)栅格的高度值加上2，给离另一边界第三远的一列(或一行)栅格的高度值加上3，依此类推，直到离另一边界最近的一列(或一行)栅格也被加上高度值。

类三维地图的效果图如图2所示。图2中的数字表示该数字所在栅格的高度值，没有写数字的栅格的高度值为0(因为高度值H＝0的栅格的数目较大，所以未在图中标出)。

下面以图2中的2号障碍区域为例说明类三维地图的创建方法：

2号障碍区域为不与边界接触的障碍区域。

先找出该区域I坐标的最大值Imax、I坐标的最小值Imin、J坐标的最大值Jmax、J坐标的最小值Jmin。

然后给该区域的所有I＝Imax和I＝Imin的两列栅格的高度值赋予I，给该区域的所有I＝(Imax-1)和I＝(Imin+1)的两列栅格的高度值赋予2，给该区域的所有I＝(Imax-2)和I＝(Imin+2)的两列栅格的高度值赋予3，依此类推，直到该区域所有的栅格(也即：(Imax-n)＝(Imin+n)或(Imax-n)＝(Imin+n)+1)都被赋予一个高度值。如图3所示。

然后给该区域的所有J＝Jmax和J＝Jmin的两行栅格的高度值加上1，给该区域的所有J＝(Jmax-1)和J＝(Jmin+1)的两行栅格的高度值加上2，给该区域的所有J＝(Jmax-2)和J＝(Jmin+2)的两行栅格的高度值加上3，依此类推，直到该区域所有的栅格(也即：(Jmax-m)＝(Jmin+m)或(Jmax-m)＝(Jmin+m)+1)都被加上一个高度值。如图4所示。

(2)对起点与终点进行初始规划，所得到的路径称为起点与终点间的初始规划路径，并将初始规划路径作为当前路径。

初始规划就是找到两点之间的直线路径。

初始规划的实现方法为：先确定给定的起点的坐标(Im，Jm)和终点的坐标(In，Jn)，设起点和终点之间直线连线上任意一点的坐标为(Ix，Jx)，因为这三点在一条直线上，则这三点的坐标须满足下面的式子：

$\frac{\mathrm{Ix}-\mathrm{In}}{\mathrm{Jx}-\mathrm{Jn}}=\frac{\mathrm{Im}-\mathrm{In}}{\mathrm{Jm}-\mathrm{Jn}}...(.1)$

令：$\frac{\mathrm{Im}-\mathrm{In}}{\mathrm{Jm}-\mathrm{Jn}}=k,$代入(1)式得：

$\mathrm{Jx}=\frac{\mathrm{Ix}-\mathrm{In}}{k}+\mathrm{Jn},$Im＜Ix＜In  ….…………………………………(2)

Ix在Im与In之间取整数值，对于每一个Ix的取值，由2式可求得一个Jx，但Jx可能不为整数，故定义运算[。]为双边取整运算，即：若对整数进行[。]运算，则结果为该整数本身；若对非整数进行[。]运算，则结果为最接近该整数的两个整数，例如：[5]＝5，[5.4]＝5、6。

对2式中的Jx进行[。]运算即得：

$\mathrm{Jx}=[\frac{\mathrm{Ix}-\mathrm{In}}{k}+\mathrm{Jn}],$Im＜Ix＜In  ….…………………………………(3)

所以点(Ix，Jx)的坐标为：

所以起点和终点之间初始规划路径上任意一点(Ix，Jx)的坐标由(4)式确定。

(3)若当前路径穿过了任何一块与边界接触的障碍区域，则转到步骤

(4)去处理与边界接触的障碍区域；若当前路径穿过了任何一块不与边界接触的障碍区域，则转到步骤(5)去处理不与边界接触的障碍区域；否则，转到步骤(6)。

(4)处理与边界接触的障碍区域。

步骤(4)分为以下四小步：

I 找出当前路径上每段位于与边界接触的障碍区域的路径段的起点与终点，然后对找出的每一对起点与终点进行基本规划。步骤(2)中的初始规划不具备避障能力，基本规划就是对初始规划进行修正，使其具有避障能力。

a 定义下降方向：若路径穿过了障碍区域，则以位于障碍区域内的路径段中高度最大的点为起点，垂直于该路径段且所经过的栅格的高度值降低的方向就是下降方向。

位于某段穿过了障碍区域的路径的下降方向一侧的点(Iy，Jy)需满足(5)式：

式中：$k=\frac{\mathrm{Im}-\mathrm{In}}{\mathrm{Jm}-\mathrm{Jn}},$(Im，Jm)为该段穿过了障碍区域的路径段的起点坐标，(In，Jn)为该段穿过了障碍区域的路径段的终点坐标，(Iam，Jam)为该障碍区域的高度值最大的点的坐标。

如图5所示，图中的箭头f表示下降方向。

下降方向的意义：对于与边界接触的障碍区域，下降方向指出了可以成功绕过障碍的方向，使得规划程序不会陷入死胡同；对于不与边界接触的障碍区域，下降方向指出了障碍区域的劣弧方向，使得规划程序可以以较短的路径绕过障碍。

b  定义基本规划：若两点之间的初始规划路径穿过了障碍区域，则找到每段位于障碍区域的路径的下降方向，然后将每段位于障碍区域的路径用沿着障碍区域从下降方向绕过障碍区域的新路径段(称为修正路径段)代替。

基本规划的实现方法：先用初始规划方法得到初始规划路径。比较初始规划路径所经过的的所有栅格点的高度值H，即：比较由1式确定的所有栅格点的高度值H，找出H＝0的连续栅格点段和H＞0的连续栅格点段。对于H＝0的连续栅格点段，其对应的路径段不用作修改；对于H＞0的连续栅格点段，其对应的路径段需要修正，修正后的路径段称为修正路径段，修正方法为：找出该障碍区域的下降方向，然后将沿着障碍区域从下降方向绕过障碍区域的折线路径段作为修正路径段。

基本规划的具体实现方法分为以下两步：

a)先找到修正路径段上的点须满足的三个条件。

设修正路径段上任意一点的坐标为(Iy，Jy)。

第一个条件：位于下降方向。

设：初始规划路径上某段H＞0的连续栅格点段中H值最大的栅格点的坐标为(Ia，Ja)；找出点(Ia，Ja)所在障碍区域的高度值最大的点，设其坐标为(Iam，Jam)；被修正路径段的起点坐标为(Im，Jm)，被修正路径段的终点坐标为(In，Jn)。

易知：若点(Iy，Jy)与点(Iam，Jam)位于被修正路径段的不同侧，则点(Iy，Jy)位于障碍区域的下降方向一侧。

令：$\frac{\mathrm{Im}-\mathrm{In}}{\mathrm{Jm}-\mathrm{Jn}}=k$

令$D=\mathrm{Jam}-\frac{1}{k}\mathrm{Iam}+\frac{\mathrm{In}}{k}-\mathrm{Jn};$

易知：若$(\mathrm{Jy}-\frac{1}{k}\mathrm{Iy}+\frac{\mathrm{In}}{k}-\mathrm{Jn})\times D\le 0,$则点(Iy，Jy)与点(Iam，Jam)位于被修正路径段的不同侧。

由此得到修正路径段上的点(Iy，Jy)须满足的第一个条件：

第二个条件：高度值为0。

即：

ZG[Iy][Jy]＝0…………………..………………..……………(7)

第三个条件：点(Iy，Jy)与对应的障碍区域直接相邻。

此条件在程序中用简单的扫描算法实现，故略去其数学表述。

b)用修正路径段上的点(Iy，Jy)须满足的三个条件对每一段经过障碍区域的路径加以修正就得到基本规划路径。

如图6所示，对A点与B点进行基本规划，图中AB间的直线连线上经过障碍区域的路径段都由一段绕过障碍区域的曲线段代替，所得到的路径就是基本规划路径。

基本规划的作用：进行基本规划是为了找犄角点，从而优化当前路径；基本规划路径并不被更新为当前路径。

II  找出被当前路径穿过的每一块与边界接触的障碍区域的犄角点。

定义犄角点：在基本规划中，用初始规划得到的路径中位于障碍区域的路径段要进行修正，修正路径段上的点中与被修正路径段的距离最大的点就是犄角点。

找犄角点的具体实现方法：

在每一次基本规划之后，计算修正路径段上的所有点与被修正路径的距离，取距离最大的点为犄角点。

计算修正路径段上的点与被修正路径的距离的方法：

设：被修正路径段的起点坐标为(Im，Jm)，记为点A；被修正路径段的终点坐标为(In，Jn)，记为点B；修正路径段中的点的坐标为(Iy，Jy)，记为点C。

令：$a=\sqrt{{(\mathrm{Iy}-\mathrm{Im})}^2+{(\mathrm{Jy}-\mathrm{Jm})}^2},$

$b=\sqrt{{(\mathrm{Iy}-\mathrm{In})}^2+{(\mathrm{Jy}-\mathrm{Jn})}^2},$

$c=\sqrt{{(\mathrm{Im}-\mathrm{In})}^2+{(\mathrm{Jm}-\mathrm{Jn})}^2},$

p＝(a+b+c)/2，

根据海伦公式，以A、B、C三点为顶点的三角形的面积为：$s=\sqrt{p(p-a)(p-b)(p-c)},$则由几何知识知，点C与A、B之间连线的距离为：

h＝s/2c………………………………………………………(8)

(8)式即为修正路径段上的点与被修正路径的距离的计算公式。

图7中的C点、D点与E点就是犄角点。

图7中C、E两点是当前路径穿过与边界接触的障碍区域而得到的犄角点；D点是当前路径穿过不与边界接触的障碍区域而得到的犄角点。本步骤中要找的是被当前路径穿过的每一块与边界接触的障碍区域的犄角点，故要找的是C、E两点。

III 找出与当前路径的每一段直线段对应的中转点

找出前一步骤中找出的位于当前路径同一侧的犄角点中离当前路径最远的点，此点称为与当前路径对应的中转点；若当前路径为折线，则要找出与当前路径的每一段直线段对应的中转点。

定义中转点：若一段直线路径穿过了若干个障碍区域，则会产生与这段路径相对应的若干个犄角点，对位于这段路径不同侧的犄角点要分开处理，位于这段路径同一侧的犄角点中离这段路径最远的点，称为中转点。

按中转点定义可知，与一段穿过了障碍区域的路径对应的中转点可能有一个或两个。

a  以图8为例具体说明何谓中转点，直线AB为当前路径，图中有C点、D点、E点三个犄角点，C点与D点位于直线AB的同一侧，且D点比C点离直线AB远，故D点是与当前路径对应的中转点；而直线AB的另一侧只有E点一个犄角点，所以E点也是与当前路径对应的中转点。

b  若当前路径为折线，则折线的每一段直线段由于穿过障碍区域而产生的犄角点应分开处理，也就是说，假设折线由a1、a2两段直线段组成，则由直线段a1穿过障碍区域而产生的犄角点不计算其与直线段a2的距离，由直线段a2穿过障碍区域而产生的犄角点不计算其与直线段a1的距离。

如图9所示，图中的当前路径ADEB为折线，当前路径中的AD段穿过了与边界接触的障碍区域，只得到犄角点C点，则C点是与AD段对应的中转点，而不是与DE段或EB段对应的中转点。

IV 将起点、终点与前面所有步骤中找出的所有中转点沿当前路径的方向用折线顺次连接起来，将新得到的折线路径作为当前路径。转到步骤(3)。

本步骤在实际实现时，是将起点、终点与前面所有步骤中找出的所有中转点(设总数为N个)沿当前路径的方向依次排列，然后对这个排列中所有相邻的两点进行初始规划，则共进行(N-1)次初始规划，得到(N-1)条初始规划路径，这些路径首尾相连而形成一条新路径。更新当前路径，转到步骤(3)。

(5)处理不与边界接触的障碍区域。

首先找出当前路径上每段位于不与边界接触的障碍区域的直线路径段的起点与终点，然后对找出的每一对起点与终点进行基本规划。再找出被当前路径穿过的每一块不与边界接触的障碍区域的犄角点。找出与当前路径的每一段直线段对应的中转点。将起点、终点与前面步骤中找出的所有中转点沿当前路径的方向用折线顺次连接起来。将新得到的折线路径作为当前路径。转到步骤(3)。

步骤(5)各小步的实现方法与步骤(4)各小步相同，只将步骤(4)各小步实现方法中的“与边界接触的障碍区域”替换为“不与边界接触的障碍区域”。

(6)输出当前路径，规划结束。

当前路径经过步骤(3)判断，若没穿过任何障碍区域，则找到了最终路径，中断步骤(3)，转到步骤(6)，输出当前路径，规划结束。

由上述步骤可以看出，本发明全局路径规划方法的实现很简单，基本上只依靠简单的计算就可以完成完整的规划计算过程，规划可以在很短的时间内完成，从而满足移动机器人的规划实时性要求；对于障碍物很多的环境，在步骤(4)和步骤(5)中并不遍历所有的障碍，而是只对当前路径经过的障碍作处理，更新当前路径后再重新扫描当前路径是否经过了障碍，从而极大的提高了程序的执行效率；整个规划算法流程简单明晰，易于理解，很方便编程实现。