CN109443357B - 基于全凸包扩张算法的障碍物间最优路径解算方法 - Google Patents

Abstract

本发明的基于全凸包扩张算法的障碍物间最优路径解算方法，步骤如下：首先，判断当前路径是否与障碍物相交，若否，则该路径为两点间最优路径，若是，则统计与该路径相交的障碍物；计算上述障碍物的中心点，并对各中心点与路径进行相对位置判断；对路径及与其相交障碍物进行凸包(壳)运算，同时对数量较多的一侧障碍物进行单侧凸包(壳)运算，将上述两凸包(壳)边界在折点处分割，添加至路径网，并删除原路径；重复上述方法，直至路径网中所有路径均不与障碍物相交；最后，按照一定路径规划规则，解算上述路径网中由起点至终点的最优路径。既克服了二叉树方法的算法复杂度高，运算时间长的缺陷，又解决了凸包(壳)边界算法丢失路径的问题。

Description

基于全凸包扩张算法的障碍物间最优路径解算方法

技术领域

本发明涉及航线设计、机器人路径规划、舰载机调运等领域，尤其涉及一种路径规划技术。

背景技术

障碍物间最优路径规划属于一种特殊的路径规划问题，其在航线设计、机器人路径规划、舰载机调运领域有广泛的应用。

当前，对于障碍物间最优路径规划的矢量方法主要有二叉树算法和凸包(壳)边界算法。二叉树算法类似于“拉线法”，是对距离当前点最近的障碍物进行绕过计算，并在航路二叉树中添加节点，重复上述过程，直至搜索至终点结束；然后按照“深度优先”和“试探搜索”相结合的原则，获取二叉树中所有路径，删除冗余节点后可得到最短路径。凸包(壳)边界算法首先统计路径穿过的所有障碍物并获取其顶点，根据各顶点与路径的距离，将顶点集分为上(下)点集，分别对上下点集进行凸包(壳)运算，将凸包(壳)边界作为路径，若新路径仍与障碍物相交，则重复上述步骤，直至所有路径均不予障碍物相交，最后用路径择优方法进行最优路径的规划。

上述两种路径规划方法仍存在许多不足：二叉树方法对各路径穿过的最近障碍物进行绕过，算法复杂度较高，尤其是当障碍物数量较多时，运算时间较长。凸包(壳)边界方法可抽象为对路径与其穿过的障碍物进行凸包(壳)运算，将凸包(壳)边界作为绕行方案，但遗漏了障碍物间潜在的路径，路径网不全面。

发明内容

为克服上述两种方法存在的不足，本发明提供了一种基于全凸包(壳)扩张算法的障碍物间最优路径解算方法。

本发明为实现上述目的所采用的技术方案是：一种基于全凸包扩张算法的障碍物间最优路径解算方法，包括以下步骤。

步骤一：将起点S至终点D连线的路径作为当前路径r_cur；

步骤二：判断当前路径r_cur与障碍物b_i是否相交(b_i∈B，B为障碍物集合)，若否，则当前路径r_cur为两点间最优路径，转步骤六，若是，统计与路径相交的障碍物b′_i，将其存入障碍物集B_cur中；

步骤三：依次判断障碍物与当前路径r_cur的相对位置关系，以左右侧为例，对应地将障碍物b′_j添加至左侧障碍物集B_left或右侧障碍物集B_right，并分别统计两侧障碍物数量Sum_left与Sum_right；

步骤四：对当前路径r_cur与障碍物集B_cur进行凸包(壳)运算，将凸包(壳)边界在折点处打断后添加至路径网R；

步骤五：若Sum_left≥(＜)Sum_right且Sum_right(Sum_left)≠0，对前路径r_cur与障碍物集B_left或B_right进行单侧凸包(壳)运算，将凸包(壳)边界并在折点处打断并添加至路径网R；

步骤六：在路径网R中删除当前路径r_cur；

步骤七：若路径网R不与障碍物集B相交，转步骤八，若仍有路径r′与障碍物集B相交，令r_cur＝r′，转步骤二；

步骤八：按照一定的最优规则，可解算由起点S至终点D的障碍物间最优路径。

可选地，所述步骤二中，若

满足：

则当前路径r_cur与障碍物不相交；

若

满足：

则当前路径r_cur与障碍物相交。

可选地，所述步骤三中，障碍物与当前路径的相对位置关系以障碍物中心点与当前路径的相对位置关系表示，首先计算障碍物b′_i∈B_cur的中心点A_i，而各障碍物中心点A_i与当前路径r_cur的相对位置关系包括但不限于中心点位于路径左右侧或上下侧。当中心点A_i位于当前路径r_cur上时，统一将该障碍物划为某侧。

若进行左右侧判断，以当前路径方向为准，位于以顺时针方向旋转180°范围内的点为路径右侧的点，位于以逆时针方向旋转180°范围内的点为路径左侧的点，反之亦可；若进行上下侧判断，设路径所在直线在平面直角坐标系中的方程为αx+βy+γ＝0，若p_i(x₀，y₀)满足：

αx₀+βy₀+γ＞0 (3)

则p_i(x₀，y₀)位于路径上侧，

若p_i(x₀，y₀)满足：

αx₀+βy₀+γ＜0 (4)

则p_i(x₀y₀)位于路径下侧。

可选地，所述步骤四、步骤五中，凸包(壳)是数据点的自然极限边界，为包含所有数据点的最小凸多边形，连接任意两点的线段必须完全位于该凸多边形中，同时区域的面积也达到最小值。

可选地，所述步骤四、步骤五中，若进行障碍物间最短路径规划，由当前路径r_cur将凸包(壳)边界分为两段，若距离较短的一段r_min满足：

则只保留r_min。

可选地，所述步骤四、步骤五中，对r_cur与B_cur进行凸包(壳)运算时，包括但不限于以下方法：(1)获取当前路径r_cur的起点p_s、终点p_d及障碍物集B_cur冲各障碍物的边界点p_i，计算点集

的凸包(壳)；(2)获取障碍物集B_cur中各障碍物的边界L，计算线集{L，r_cur}的凸包(壳)。

可选地，所述步骤七中，若

满足：

则路径网R不与障碍物集B相交，

若

满足：

则路径r′与障碍物集B相交。

可选地，所述步骤八中，最优规则包括但不限于：距离最短、转向次数最少、转向频率最低。

本发明的基于全凸包(壳)扩张算法的障碍物间最优路径解算方法，既克服了二叉树方法的算法复杂度高，运算时间长的缺陷，又解决了凸包(壳)边界算法丢失路径的问题。

附图说明

图1为本发明的基于全凸包(壳)扩张算法的障碍物间最优路径解算方法流程图。

图2为路径与障碍物示意图。

图3为路径与障碍物凸包(壳)边界示意图。

图4为路径与障碍物单侧凸包(壳)边界示意图。

图5为路径规划结果示意图。

图6为最短路径示意图。

具体实施方式

本发明的为使本发明的技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。以搜索最短路径为实施例，显然所描述实施例仅是本发明的一部分实施例，而不是全部的实施例。

图1为本实施例的障碍物与路径示意图，S、T分别为起点、终点，O₁、O₂、O₃为障碍物。

步骤一：连接ST，将其作为当前路径r_cur；

步骤二：判断当前路径r_cur与障碍物O₁、O₂、O₃是否相交，显然当前路径r_cur与障碍物O₁、O₂、O₃均相交，将其添加至B_cur；

步骤三：依次计算B_cur中障碍物的中心点，分别为A₁、A₂、A₃，其中A₁、A₂在当前路径r_cur左侧，A₃在右侧；

步骤四：将当前路径r_cur与障碍物集B_cur进行凸包(壳)运算，由于路径SP₈P₉T长度小于路径SP₁P₂P₃P₄T，只将路径SP₈P₉T在折点处打断后添加至路径网R，如图3所示；

步骤五：当前路径r_cur左侧障碍物数量较多，则对r_cur与B_left进行凸包(壳)边界运算，其中较短路径SP₅P₇T与障碍物相交，将需将整个凸包(壳)边界在折点出打断后添加至路径网R，并在路径网R中删除当前路径r_cur，如图4所示，已删除的路径以虚线表示；

步骤六：此时，路径网R中只有路径P₅P₇与障碍物相交，则继续对其进行凸包(壳)边界运算，由于路径P₅P₆P₇长度小于路径P₅P₈P₉P₇且不与障碍物相交，则将路径P₅P₆P₇添加至路径网R，并删除路径P₅P₇，如图5所示；

步骤7：至此，路径网R中所有路径均不与障碍物相交，用Dijkstra算法进行最短路径的解算，如图6所示。

本发明是通过实施例进行描述的，本领域技术人员知悉，在不脱离本发明的精神和范围的情况下，可以对这些特征和实施例进行各种改变或等效替换。另外，在本发明的教导下，可以对这些特征和实施例进行修改以适应具体的情况及材料而不会脱离本发明的精神和范围。因此，本发明不受此处所公开的具体实施例的限制，所有落入本申请的权利要求范围内的实施例都属于本发明的保护范围。

Claims (8)

1.基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：包括以下步骤：

步骤一：将起点S至终点D连线的路径作为当前路径r_cur；

步骤二：判断当前路径r_cur与障碍物b_i是否相交，若否，则当前路径r_cur为两点间最优路径，转步骤六，若是，统计与路径相交的障碍物b′_i，将其存入障碍物集B_cur中；

步骤三：依次判断障碍物与当前路径r_cur的相对位置关系，对应地将障碍物b′_i添加至左侧障碍物集B_left或右侧障碍物集B_right，并分别统计两侧障碍物数量Sum_left与Sum_right；

步骤四：对当前路径r_cur与障碍物集B_cur进行凸包运算，将凸包边界在折点处打断后添加至路径网R；

步骤五：若Sum_left≥Sum_right且Sum_right≠0，对前路径r_cur与左侧障碍物集B_left进行单侧凸包运算；若Sum_left＜Sum_right且Sum_left≠0，对前路径r_cur与右侧障碍物集B_right进行单侧凸包运算，将凸包边界并在折点处打断并添加至路径网R；

步骤六：在路径网R中删除当前路径r_cur；

步骤七：若路径网R不与障碍物集B_cur相交，转步骤八，若仍有路径r′与障碍物集B_cur相交，令r_cur＝r′，转步骤二；

步骤八：按照一定的最优规则，解算由起点S至终点D的障碍物间最优路径。

2.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：所述步骤二中，若

满足：

则当前路径r_cur与障碍物不相交；

若

满足：

则当前路径r_cur与障碍物相交。

3.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：所述步骤三中，障碍物与当前路径的相对位置关系以障碍物中心点A_i与当前路径的相对位置关系表示，首先计算障碍物b′_i∈B_cur的中心点A_i，而各障碍物中心点A_i与当前路径r_cur的相对位置关系包括中心点位于路径左右侧或上下侧，当中心点A_i位于当前路径r_cur上时，统一将该障碍物划为某侧，

若进行左右侧判断，以当前路径方向为准，位于以顺时针方向旋转180°范围内的点为路径右侧的点，位于以逆时针方向旋转180°范围内的点为路径左侧的点，反之亦可；若进行上下侧判断，设路径所在直线在平面直角坐标系中的方程为αx+β_y+γ＝0，若p_i(x₀，y₀)满足：

αx₀+βy₀+γ＞0 (3)

则p_i(x₀，y₀)位于路径上侧，

若p_i(x₀，y₀)满足：

αx₀+βy₀+γ＜0 (4)

则p_i(x₀，y₀)位于路径下侧。

4.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：所述步骤四、步骤五中，凸包是数据点的自然极限边界，为包含所有数据点的最小凸多边形，连接任意两点的线段必须完全位于该凸多边形中，同时区域的面积也达到最小值。

5.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：所述步骤四、步骤五中，若进行障碍物间最短路径规划，由当前路径r_cur将凸包边界分为两段，若距离较短的一段r_min满足：

则只保留r_min。

6.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：所述步骤四、步骤五中，对当前路径r_cur与障碍物集B_cur进行凸包运算时，采用以下方法：(1)获取当前路径r_cur的起点p_s、终点p_d及障碍物集B_cur中各障碍物的边界点p_i，计算点集

的凸包；(2)获取障碍物集B_cur中各障碍物的边界L，计算线集{L，r_cur}的凸包。

7.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：

所述步骤七中，若

满足：

则路径网R不与障碍物集B_cur相交，

若

满足：

则路径r′与障碍物集B_cur相交。

8.根据权利要求1所述的基于全凸包扩张算法的障碍物间最优路径解算方法，其特征在于：所述步骤八中，最优规则包括：距离最短、转向次数最少、转向频率最低。

Images