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

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

背景技术：

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

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

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

技术实现要素：

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

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

步骤一：将起点s至终点d连线的路径作为当前路径rcur；

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

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

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

步骤五：若sumleft≥(＜)sumright且sumright(sumleft)≠0，对前路径rcur与障碍物集bleft或bright进行单侧凸包(壳)运算，将凸包(壳)边界并在折点处打断并添加至路径网r；

步骤六：在路径网r中删除当前路径rcur；

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

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

可选地，所述步骤二中，若满足：

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

若满足：

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

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

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

αx0+βy0+γ＞0(3)

则pi(x0，y0)位于路径上侧，

若pi(x0，y0)满足：

αx0+βy0+γ＜0(4)

则pi(x0y0)位于路径下侧。

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

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

则只保留rmin。

可选地，所述步骤四、步骤五中，对rcur与bcur进行凸包(壳)运算时，包括但不限于以下方法：(1)获取当前路径rcur的起点ps、终点pd及障碍物集bcur冲各障碍物的边界点pi，计算点集的凸包(壳)；(2)获取障碍物集bcur中各障碍物的边界l，计算线集{l，rcur}的凸包(壳)。

可选地，所述步骤七中，若满足：

则路径网r不与障碍物集b相交，

若满足：

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

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

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

附图说明

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

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

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

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

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

图6为最短路径示意图。

具体实施方式

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

图1为本实施例的障碍物与路径示意图，s、t分别为起点、终点，o1、o2、o3为障碍物。

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

步骤二：判断当前路径rcur与障碍物o1、o2、o3是否相交，显然当前路径rcur与障碍物o1、o2、o3均相交，将其添加至bcur；

步骤三：依次计算bcur中障碍物的中心点，分别为a1、a2、a3，其中a1、a2在当前路径rcur左侧，a3在右侧；

步骤四：将当前路径rcur与障碍物集bcur进行凸包(壳)运算，由于路径sp8p9t长度小于路径sp1p2p3p4t，只将路径sp8p9t在折点处打断后添加至路径网r，如图3所示；

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

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

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

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