CN103528585B - 一种不等距分割可通行区域的路径规划方法 - Google Patents

Abstract

本发明属于机器人及低空飞行的飞行器路径或航迹规划的技术领域，具体涉及一种不等距分割可通行区域的路径规划方法，解决了现有规划算法时间有较大的时间复杂度和空间复杂度。其步骤如下：计算每个障碍曲线的凸极值点；过每个凸极值点作水平线用以分割可通行区域；将每个分割成的小区域抽象成一个图的顶点；把所有顶点组成一个无向图；找出起点和终点所在小区域对应的顶点序号；对于无向图通过广度优先或深度优先遍历，找出所有的路径；再根据实际地图上的情形，找出运动物体实际所要行走的路径。本发明的有益效果：克服了A*等算法的在内存空间和运算时间上问题，同时也客服了蚁群算法的收敛问题；时间复杂度和空间复杂度较其他算法有较大提高。

Description

一种不等距分割可通行区域的路径规划方法

技术领域

本发明属于机器人及低空飞行的飞行器路径或航迹规划的技术领域，具体涉及一种不等距分割可通行区域的路径规划方法，用于在有障碍的地图上进行路径规划。

背景技术

由于路径或航迹规划问题一般是NP-hard问题，也就是说并没有计算最优解的多项式时间算法，同时在飞行器实际飞行过程中，随时间的变化以及威胁和障碍的变化，计算航迹的算法更加复杂。如A*算法、蚁群算法、模拟退火、禁忌搜索算法，遗传算法，粒子群优化算法，遗传算法等，它们都有各自的优缺点。A*算法是一种启发式算法，在搜索过程中无需遍历地图，使得计算复杂度相对较小，实现了快速、高效。但是，这也导致搜索的过程中排除了大量节点，而由于经验及实际问题的复杂性，引入启发信息的代价函数无法做到完全正确，这些被排除的节点可能就是最优路径的节点之一，从而导致不一定能找到最优路径。蚁群算法发现较好解的能力很强，具有分布式计算、鲁棒性强、易于与其它方法结合等优点；但也存在收敛速度慢，容易陷入局部最优的缺点。遗传算法具有自然系统的自适应特征，算法在效率和效益之间的权衡使得它能适用不同环境并取得较好的效果，但也并不能保证达到全局优化，仍然可能陷入局部极值的陷阱。粒子群优化算法运行的初始阶段，收敛速度比较快，运动轨迹呈正弦波摆动，但运行一段时间后，速度开始减慢甚至停滞（称为早熟收敛或停滞），发生该现象时粒子群高度聚集，严重缺乏多样性，粒子群会长时间或永远跳不出聚集点。

发明内容

本发明为了解决现有规划算法时间有较大的时间复杂度和空间复杂度，提出一种基于可通行区域不等距分割的路径（或航迹）规划方法。

本发明基于遍布障碍物的地图进行讨论，其中的障碍物包括阻碍飞行的高地、雷达或敌方火力威胁等。将地图中的高地、雷达、敌方火力威胁等视为障碍，每一个障碍可看成一个闭合曲线，运动物体不能进入闭合曲线内，而地图中剩余的部分为可通行区域。S点为运动物体的起点，E点为运动物体的目标点，假设运动物体要从S点运动到E点，从地图中找出一条或若干条路径（或航迹），见图1。

本发明采用如下技术方案完成，一种不等距分割可通行区域的路径规划方法，步骤如下：

（1）计算每个障碍曲线的凸极值点；

（2）过每个凸极值点作水平线用以分割可通行区域；

（3）将每个分割成的小区域抽象成一个图的顶点；

（4）把所有顶点组成一个无向图；

（5）找出起点和终点所在小区域对应的顶点序号；

（6）对于无向图通过广度优先或深度优先遍历，找出所有的路径；

（7）再根据实际地图上的情形，找出运动物体实际所要行走的路径。

本发明具体步骤如下：

1）障碍物建模

利用拟合曲线模拟障碍物的外沿，采用Bezier曲线，计算出构成曲线的所有点的横、纵坐标；

2）计算障碍曲线的凸极值点

通过循环计算每段曲线上的极大值或极小值，进而求得曲线上的极值点坐标，仅保留障碍曲线的凸极值点；

3）水平分割可通行区域

通过凸极值点做水平切线对可通行区域进行一维分割，方法如下：求出所有曲线上的最大值与最小值，从而求出曲线的外接矩形，判断极值点纵坐标是否介于一个区域的最小值与最大值之间，如果是，则得到过该极值点的水平切线与一个区域相交；如果相交，求出交点坐标，并保存在一个数组中，求曲线的交点与极值点的距离；若距离的最小值>0,说明过极值点所做的水平切线只与一个区域相交，且在所交区域的左侧；同理，若距离的最大值<0,说明过极值点所做的水平切线也只与一个区域相交，且在所交区域的右侧；若距离有正有负，则说明过极值点所做水平切线不止与一个区域相交；最后画出经过每个极值点的水平切线；

4）可通行区域的存储

在区域存储过程中，使用队列将所有的区域存储；区域存储后，根据构造的邻接表中顶点间的邻接关系，抽象出一个无向图；具体方法如下：

先存储第一个区域，若是直线边就存储两个端点，若是曲线边就将曲线上的所有的点都存储下来，对区域编号，并入队；构建邻接表，存储区域的标志位和公共边，标志位是用来标记该区域是否已访问，公共边则是为了判断区域之间的邻接关系；队头元素出队，将其相邻的区域存储，编号并入队；在入队过程中先判断，该区域是否已经访问过以及是否在队列中。若既没有被访问过且不在队列中，则入队；否则继续判断其相邻区域；每个区域抽象为图的一个顶点，若划分出的小区域有公共边，则与之对应的顶点间就有连线，构造无向图；

确定无向图中的路径，设置访问标记数组，用来标记该顶点是否已被访问；先将第一个顶点入栈；栈不为空，将栈顶元素出栈，并将其相邻的顶点入栈，设置访问标志位为已访问；若与它邻接的下一个顶点没有被访问过，将出栈元素保存到数组中；如此构成循环，找出所有的路径，并将这些路径分别存放在不同的数组中；

5）反算无向图边计算最短路径

确定路径后，根据实际情况为每条路径动态设定权值，并算出权值和，权值和最小的即为最短路径；

设定权值遵循的原理是：求两个直角三角形的斜边和，若它们的水平直角边不共线，求出起点与终点在水平方向上的中间点，连接起点与中间点，中间点与终点，此时求得的两条斜边和最短；

为每条路径设定权值的过程中，将每条路径的路径画出来，并算出每条路径的权值和；权值和为最小的即为最短路径。

本发明相对于现有技术具有如下有益效果：克服了A*等算法的在内存空间和运算时间上问题，同时也客服了蚁群算法的收敛问题。因为本算法只是在二维空间的一维上作了区域分割，达到了降维的目的和效果，实验证明本发明算法的时间复杂度和空间复杂度较其他算法有较大提高。

附图说明

图1遍布障碍的地图（其中的S点为起点，E为终点），

图2过每个障碍的凸极值点水平分割可通行区域并编号，

图3抽象每个可通行小区域而得到的无向图，

图4确定无向图中的路径，

图5无向图中边的权值设定（方法之一）示意图，

图6A*算法得到的路径，

图7蚁群算法的规划路径，

图8本发明计算的结果。

具体实施方式

结合附图对本实用新型的具体实施方式作进一步说明。

1)障碍物建模

由于障碍物的外沿是连续的光滑曲线，可利用拟合曲线来模拟，采用Bezier曲线。绘制Bezier曲线时要将控制点的位置向量分解为二维平面上的x，y方向的分量，则表达式可以表述为：

(式1)

(式2)

式1和式2为拟合曲线上的横、纵坐标。式1中的x₀，x₁，x₂，x₃是输入的四个控制点的横坐标；同理，式2中的y₀，y₁，y₂，y₃相应的控制点的纵坐标。t是一个增量步长，随着t的变化，可以计算出构成曲线的所有点的横、纵坐标。

2)计算障碍曲线的凸极值点

计算所有曲线上的凸极值点坐标，包括凸极大值和凸极小值。

设函数在点x₀附近有定义，如果对x₀附近的所有的点，都有,即是函数的一个极大值，记做：。如果对x₀附近的所有的点，都有,亦即是函数f(x)的一个极小值，记做。极值点的求法是：先对其求导；求出不可导点和导数为0的点；对不可导点左右两侧求导数的正负值；导数为0的点，求该点处二阶导（若二阶导大于0，则获得极小值；若二阶导小于0，则获得极大值；二阶导等于0是拐点，而非极值点）。

在编码过程中，通过循环计算每段曲线上的极大值或极小值，进而求得曲线上的极值点坐标。在划分区域时，由于过凹极值点水平分割可通行区域无意义，所以仅保留障碍曲线的凸极值点。

3)水平分割可通行区域

鉴于A*算法是横纵方向二维分割可通行区域造成内存耗费较大且运算速度较低，本文提出通过凸极值点做水平切线对可通行区域进行一维分割，在运算上达到了降维的目的。方法如下：求出所有曲线上的最大值与最小值，从而可以求出曲线的外接矩形。判断极值点纵坐标是否介于一个区域的最小值与最大值之间，如果是，则得到过该极值点的水平切线与一个区域相交。如果相交，求出交点坐标，并保存在一个数组中。求曲线的交点与极值点的距离。若距离的最小值>0,说明过极值点所做的水平切线只与一个区域相交，且在所交区域的左侧。同理，若距离的最大值<0,说明过极值点所做的水平切线也只与一个区域相交，且在所交区域的右侧。若距离有正有负，则说明过极值点所做水平切线不止与一个区域相交。最后可以画出经过每个极值点的水平切线。

4)可通行区域的存储

在区域存储过程中，需要用到队列和邻接表等数据结构。使用队列可方便将所有的区域存储；构建邻接表则可生成无向图。方法如下：先存储第一个区域（若是直线边就存储两个端点，若是曲线边就将曲线上的所有的点都存储下来），对区域编号，并入队。构建邻接表，存储区域的标志位和公共边（标志位是用来标记该区域是否已访问；公共边则是为了判断区域之间的邻接关系）。队头元素出队，将其相邻的区域存储，编号并入队。在入队过程中先判断，该区域是否已经访问过以及是否在队列中。若既没有被访问过且不在队列中，则入队；否则继续判断其相邻区域。抽象构造无向图确定所有路径。

对于图2中的地图通过区域抽象并为每个区域进行编号，每个区域可抽象为图的一个顶点，若划分出的小区域有公共边，则与之对应的顶点间就有连线，因此图2可以构造如图3所示的无向图。

区域存储后，根据构造的邻接表中顶点间的邻接关系，可以抽象出一个无向图。

确定无向图中的路径，主要为方便后面确定最优路径。设置访问标记数组，用来标记该顶点是否已被访问。先将第一个顶点入栈。栈不为空，将栈顶元素出栈，并将其相邻的顶点入栈，设置访问标志位为已访问。若与它邻接的下一个顶点没有被访问过，将出栈元素保存到数组中。如此构成循环，就可以找出所有的路径，并将这些路径分别存放在不同的数组中。

如图3中的无向图，对应图2中起点S位于区域2中，终点E位于区域24中，则所有路径如下：

(1)2-1-7-9-10-13-19-20-21-22-24

(2)2-1-7-9-10-13-19-20-21-23-24

(3)2-1-7-9-11-14-17-18-19-20-21-22-24

(4)2-1-7-9-11-14-17-18-19-20-21-23-24

(5)2-3-5-9-11-14-17-18-19-20-21-22-24

(6)2-3-5-9-11-14-17-18-19-20-21-23-24

(7)2-3-5-9-10-13-19-20-21-22-24

(8)2-3-5-9-10-13-19-20-21-23-24

(9)2-8-14-17-18-19-20-21-22-24

(10)2-8-14-17-18-19-20-21-23-24

(11)2-8-14-11-9-10-13-19-20-21-22-24

(12)2-8-14-11-9-10-13-19-20-21-23-24

(13)2-4-6-3-5-9-10-13-19-20-21-22-24

(14)2-4-6-3-5-9-10-13-19-20-21-23-24

(15)2-4-6-3-5-9-11-14-17-18-19-20-21-22-24

(16)2-4-6-3-5-9-11-14-17-18-19-20-21-23-24

对于路径(1)来说，起点坐标在2区域，终点坐标在24区域。从顶点2遍历到顶点24，要经过顶点1、顶点7、顶点9、顶点10、顶点13、顶点19、顶点20、顶点21、顶点22，如此构成这条路径。其余路径和这条路径类似。

5）反算无向图边计算最短路径

确定路径后，根据实际情况为每条路径动态设定权值，并算出权值和。权值和最小的即为最短路径。

设定权值遵循的原理是：求两个直角三角形的斜边和。若它们的水平直角边不共线，求出起点与终点在水平方向上的中间点，连接起点与中间点，中间点与终点，此时求得的两条斜边和最短。（见图5）

如图5所示，斜边的平方和为。若取的不是水平直角边的中点，一条边的长度设为，另一条边的长度设为，则斜边的平方和为

。由此得到取起点与终点在水平方向上的中间点时两直角三角形的斜边和最短。

设定权值时，先判断起点与终点是否在一个区域内。①若起点与终点在一个区域内。(1)起点与终点之间没有障碍物，则权值就是线段的长度。因为两点之间线段最短。(2)起点与终点之间有障碍物。先计算该障碍物最外边界的点坐标，权值为起点到该点的距离与该点到终点的距离和。②若起点与终点不在一个区域内。(1)起点与终点之间没有障碍物。先算出起点与终点的中间点的横坐标。则起点所在区域的权值和终点所在区域的权值设为三角形的斜边（其中一条直角边的长度是相等的，即为起点与终点水平距离的一半；另一条直角边的长度则是起点或终点到相邻区域的水平切线的距离）。其它顶点所在区域的权值设为区域的高度。(2)起点与终点之间有障碍物。先算出起点与终点的中间点的横坐标。根据路径的实际情况：ⅰ)若存在路径经过该中间点，先求出该路径左侧障碍物的最右边的点坐标与该路径右侧障碍物的最左边的点坐标。若中间点的横坐标介于它们之间，则起点所在区域的权值和终点所在区域的权值设为三角形的斜边（其中一条直角边的长度是相等的，即为起点与终点水平距离的一半；另一条直角边的长度则是起点或终点到相邻区域的水平切线的距离）。其它顶点所在区域的权值设为区域的高度。ⅱ)若路径在中间点的左侧，先求出该路径右侧障碍物的最左边的点坐标，设为left。第一个顶点所在区域的权值设为三角形的斜边（其中一条直角边的长度是起点到相邻区域的水平切线的距离；另一条直角边的长度是起点到left的水平距离）。计算出left与终点坐标的中间点的横坐标。找到该路径右侧障碍物的最左边的点坐标所在区域的下一个邻接区域。求出该路径接下来顶点所在区域的左侧障碍物的最右边的点坐标与该路径右侧障碍物的最左边的点坐标。若中间点坐标位于它们之间，则此顶点所在区域和最后一个顶点所在区域的权值设为三角形的斜边（其中一条直角边的长度是相等的，即为该顶点与终点水平距离的一半；另一条直角边的长度则是该顶点或终点到相邻区域的水平切线的距离），其它顶点所在区域的权值设为区域的高度。ⅲ)若路径在中间点的右侧，先求出该路径左侧障碍物的最右边的点坐标，设为right。第一个顶点所在区域的权值设为三角形的斜边（其中一条直角边的长度是起点到相邻区域的水平切线的距离；另一条直角边的长度是起点到right的水平距离）。计算出right与终点坐标的中间点的横坐标。找到该路径左侧障碍物的最右边的点坐标所在区域的下一个邻接区域。求出该路径接下来顶点所在区域的右侧障碍物的最左边的点坐标与该路径左侧障碍物的最右边的点坐标。若中间点坐标位于它们之间，则此顶点所在区域和最后一个顶点所在区域的权值设为三角形的斜边（其中一条直角边的长度是相等的，即为该顶点与终点水平距离的一半；另一条直角边的长度则是该顶点或终点到相邻区域的水平切线的距离），其它顶点所在区域的权值设为区域的高度。

在为每条路径设定权值的过程中，将每条路径的路径画出来。并算出每条路径的权值和。权值和为最小的即为最短路径。

对于图3中确定的16条路径，计算得到的权值和（路径长度）见表1。

表1各条路径的权值和

由表1得知，路径(7)，即2-3-5-9-10-13-19-20-21-22-24为图3的最短路径，权值和为248.72。对于此路径来说，起点坐标在2区域，终点坐标在24区域。从顶点2遍历到顶点24，要经过顶点3、顶点5、顶点9、顶点10、顶点13、顶点19、顶点20、顶点21、顶点22，如此构成这条路径。顶点2的权值为27.5，是过障碍物的边的一条切线的长度；顶点3的权值为21.40，顶点5的权值为20.62，根据权值设定的原理计算出的；顶点9的权值为49.99，为三角形的斜边；顶点10的权值为6，为区域的高度；顶点13的权值为61，为区域的高度；顶点19的权值为25，顶点20的权值为5，为区域的高度；顶点21的权值为12，为区域的高度；顶点22的权值为13，为区域的高度；顶点24的权值为7.21，为三角形的斜边（单位：km）。

路径(7)，即2-1-7-9-11-14-17-18-19-20-21-22-24为图3中的最长路径，权值和为460.21。对于此路径来说，起点坐标在2区域，终点坐标在24区域。从顶点2遍历到顶点24，要经过顶点1、顶点7、顶点9、顶点11、顶点14、顶点17、顶点18、顶点19、顶点20、顶点21、顶点22，如此构成这条路径。顶点2的权值为5.39，顶点1的权值为55，顶点7的权值为60，顶点9的权值为48.01，顶点11的权值为27，顶点14的权值为76.60，顶点17的权值为43，顶点18的权值为101，顶点19的权值为7，顶点20的权值为5，顶点21的权值为12，顶点22的权值为13，顶点24的权值为7.21（单位：km）。

6）对比实验结果

对比实验中地图范围是100km*100km，主要是本发明与A*算法和蚁群算法三种算法进行比较。

A*算法是基于栅格地图实现的。算法描述：从起始节点到达目标节点遍历整个地图，采用蛮力法，从起始节点开始，将与它相邻的所有正方形作为它的后继节点。移动到它的任一个后继节点并重复此过程，直到找到目标节点。问题的关键是找到合适的后继节点。A*算法使用当前位置和目标节点之间的距离，移动到距离最短的节点处。它通过结合h(n)来评估节点，h(n)是从该节点到目标节点的距离；g(n)是从开始节点到该节点的距离。计算每个后继节点和节点的总成本f(n)=h(n)+g(n)，有最小成本f(n)的作为后继节点。

蚁群算法是在完全已知的静态障碍物之间为蚂蚁寻找一条从给定的起始点到目标点的无碰撞路径。蚁群算法具有分布计算、信息正反馈和启发式搜索等特点。本实验对比算法是通过设置启发因子α和β等参数，构造启发式信息矩阵，结合蚂蚁群体反复寻食时所留下信息素的正反馈作用，实现了绕开所有障碍物找到一条最短路径。

地图的左上角为坐标原点，如图1所示，起始点坐标设置为(37.5,17.5)，终止点E坐标设置为(27.5,82.5)。其中A*算法计算得到的最短路径为141.5685km，蚁群算法计算得到的最短路径为144.4975km，本发明计算得到的最短路径为135.8017km。经比较，本发明提出路径规划算法得到的路径最短。仿真图6是A*算法得到的路径，图7是蚁群算法的规划路径，图8是本发明计算的结果。

设置不同的起始点坐标和终止点坐标，得到的最短路径，如表2所示：

表2三种算法最短路径和时间复杂度比较

由表2得出，在相同的地理环境下，A*算法的时间复杂度为，其中m是节点个数，n是任意一个节点的后继节点的个数，l是OPEN表中的节点个数，即时间复杂度为O(n³)；蚁群算法的时间复杂度为，其中m是迭代次数，n是蚂蚁个数，l是节点个数，p是蚂蚁下一步路径可选节点的个数，即时间复杂度为O(n⁴)；本发明的时间复杂度为，其中m是地图中曲线上的所有点的个数，n是凸极值点的个数，即时间复杂度为O(n²)。实验数据显示，本发明得到的路径最短，时间复杂度也是最小，为最优。

Claims (2)

1.一种不等距分割可通行区域的路径规划方法，其特征在于步骤如下：

1）计算每个障碍曲线的凸极值点；

2）过每个凸极值点作水平线用以分割可通行区域；

3）将每个分割成的小区域抽象成一个图的顶点；

4）把所有顶点组成一个无向图；

5）找出起点和终点所在小区域对应的顶点序号；

6）对于无向图通过广度优先或深度优先遍历，找出所有的路径；

7）再根据实际地图上的情形，找出运动物体实际所要行走的路径。

2.根据权利要求1所述的不等距分割可通行区域的路径规划方法，其特征在于具体步骤如下：

1）障碍物建模

利用拟合曲线模拟障碍物的外沿，采用Bezier曲线，计算出构成曲线的所有点的横、纵坐标；

2）计算障碍曲线的凸极值点

通过循环计算每段曲线上的极大值或极小值，进而求得曲线上的极值点坐标，仅保留障碍曲线的凸极值点；

3）水平分割可通行区域

通过凸极值点做水平切线对可通行区域进行一维分割，方法如下：求出所有曲线上的最大值与最小值，从而求出曲线的外接矩形，判断极值点纵坐标是否介于一个区域的最小值与最大值之间，如果是，则得到过该极值点的水平切线与一个区域相交；如果相交，求出交点坐标，并保存在一个数组中，求曲线的交点与极值点的距离；若距离的最小值>0,说明过极值点所做的水平切线只与一个区域相交，且在所交区域的左侧；同理，若距离的最大值<0,说明过极值点所做的水平切线也只与一个区域相交，且在所交区域的右侧；若距离有正有负，则说明过极值点所做水平切线不止与一个区域相交；最后画出经过每个极值点的水平切线；

4）可通行区域的存储

在区域存储过程中，使用队列将所有的区域存储；区域存储后，根据构造的邻接表中顶点间的邻接关系，抽象出一个无向图；具体方法如下：

先存储第一个区域，若是直线边就存储两个端点，若是曲线边就将曲线上的所有的点都存储下来，对区域编号，并入队；构建邻接表，存储区域的标志位和公共边，标志位是用来标记该区域是否已访问，公共边则是为了判断区域之间的邻接关系；队头元素出队，将其相邻的区域存储，编号并入队；在入队过程中先判断，该区域是否已经访问过以及是否在队列中；

若既没有被访问过且不在队列中，则入队；否则继续判断其相邻区域；每个区域抽象为图的一个顶点，若划分出的小区域有公共边，则与之对应的顶点间就有连线，构造无向图；

确定无向图中的路径，设置访问标记数组，用来标记该顶点是否已被访问；先将第一个顶点入栈；栈不为空，将栈顶元素出栈，并将其相邻的顶点入栈，设置访问标志位为已访问；若与它邻接的下一个顶点没有被访问过，将出栈元素保存到数组中；如此构成循环，找出所有的路径，并将这些路径分别存放在不同的数组中；

5）反算无向图边计算最短路径

确定路径后，根据实际情况为每条路径动态设定权值，并算出权值和，权值和最小的即为最短路径；

设定权值遵循的原理是：求两个直角三角形的斜边和，若它们的水平直角边不共线，求出起点与终点在水平方向上的中间点，连接起点与中间点，中间点与终点，此时求得的两条斜边和最短；

为每条路径设定权值的过程中，将每条路径的路径画出来，并算出每条路径的权值和；权值和为最小的即为最短路径。