CN112833904A - 一种基于自由空间与快速搜索随机树算法的无人车动态路径规划方法 - Google Patents

Abstract

本文发明公开了一种基于自由空间与快速搜索随机树算法的无人车动态路径规划方法。本文利用凹多边形凸分解的图形学方法，设计了一种在含有障碍地图上构建自由空间的方法，并应用人工蜂群算法对进行路径优化找出全局最优路径。本文通过调整随机树节点采样概率，实现了基于改进快速搜索随机树算法的快速路径规划方法。最后，本发明分别利用自由空间法和快速搜索随机树算法实现动态地图的全局路径规划和无人车行进过程中的局部快速路径规划，兼顾了动态环境下路径规划的质量和速度。

Description

一种基于自由空间与快速搜索随机树算法的无人车动态路径 规划方法

1.技术领域

本发明涉及路径规划领域，尤其涉及动态环境下无人车行进过程中地图发生变化的路径规划。

2.背景技术

自动驾驶技术是人工智能领域中的热点方向，在未来社会中，大部分车辆将会配置自动驾驶技术，使得地面交通更加通畅、交通事故率更低。作为其中关键一环的动态环境路径规划方法，须要达到以下目的和要求：1)行驶路径不与障碍物发生碰撞；2)路径须连接起点与终点区域；3)路径质量较高；4)动态规划时间短。

现有的路径规划算法很难同时满足上述要求，例如，RRT算法与A*算法容易陷入陷阱区域，且在复杂环境下规划速度较慢；人工势场法和粒子群算法易陷入局部最优解；栅格法易受栅格划分精度的影响；V图算法和切线法不适用于复杂地图；自由空间法适用于复杂地图，但由于自由空间原始构建逻辑略显复杂且计算量较大，不适用于动态环境下的局部快速规划。

3.发明内容

由于不同的路径规划算法特点各不相同，使用单一算法难以在动态环境下实现快速、高质量的响应。本发明结合自由空间法和快速搜索随机树算法，提供了一种既可以满足路径规划质量，又可以在动态变化的环境中保证路径规划实时性的算法。在无人车启动前，根据已知环境规划出一条由起点至终点且不与障碍物发生碰撞的最优路径，在无人车行进过程中，根据环境变换情况，利用局部快速规划算法实时调整路径。这样既可以保证路径质量接近最优，又可以利用局部快速路径规划算法完成快速响应。

本发明首先提出了一种基于自由空间法的全局最优路径规划算法，结合人工蜂群算法进行改进，自由空间法是一种基于图形学的路径规划算法，这种算法可以在配合优化算法的基础上得到全局最优路径。然后还提出了一种基于快速搜索随机树算法的局部路径规划方法，通过调整随机节点的采样概率，得到一种可以在局部简单地形的环境下进行快速路径规划的算法。最后，本发明结合以上两种算法，实现了动态环境下的无人车路径规划。

4.附图说明

图1为：构造单连通多边形示意图

图2为：凹凸顶点及多边形正负判断示意图

图3为：可见点判断及最优可见点选择示意图

图4为：全局搜索初始路径示意图

图5为：路径邻域搜索示意图

图6为：快速搜索随机树生成过程示意图

图7为：改进自由空间法生成全局最优路径示意图

图8为：环境动态变化示意图

图9为：改进RRT*算法生成局部路径示意图

图10为：路径调整示意图

5.具体实施方式

5.1将地图构建为单连通多边形

自由空间法的基本思想为将地图划分为若干个由凸多边形组成的自由空间，本文使用了凹多边形凸分解算法，因此首先应将带障碍物的地图转换为一个多边形，其基本思想及步骤如下。

如图1所示，当地图M₁M₂…M_n中存在一个障碍物O₁O₂…O_n时，选择障碍物的一个点O_i与地图边界的某一个顶点M_i连线，记为O_iM_i。其矢量可以有两个方向

和

假设两个矢量

与

间有距离ΔD→0，将障碍物的顶点O_i与地图的顶点M_i通过两个矢量连接，则地图变成了单连通域地图M₁M₂…M_iO_iO_i+1…O_nO₁…O_iM_i…M_n。若地图内存在多个多边形障碍，则需要满足每一个障碍物直接或间接与地图边界顶点相连即可。

其基本步骤为：若地图边界顶点序列为M_p，障碍物多边形的集合为X_obs，找出X_obs中所有障碍物每一个凸点在地图边界点上的所有可见点，并计算距离，选择距离最小的一对点连接，并将障碍物顶点序列***地图边界顶点序列形成新的地图边界M_pn，并在障碍物的集合 X_obs中删除该障碍物(其中凸点与可见点的定义参考5.2.2、5.2.3)。直至集合X_obstacle为空集，则多连通域地图至单连通域地图变换完毕，否则重复以上步骤。

5.2单连通凹多边形凸分解

按照上述方法及步骤将地图转化为一个多边形后，按照下属步骤将多边形分解为若干个凸多边形：

步骤一：首先判断多边形的正负，若多边形为正则进入下一步骤，否则将多边形顶点序列反向排序。(参照5.2.1)

步骤二：从第一个顶点开始搜索多边形顶点中的凹点，若没有凹点则多边形为凸多边形。若有凹点则进入下一步骤。(参考5.2.2)

步骤三：选择其中一个凹点，并搜索该凹点的可见点。(参考5.2.3)

步骤四：若可见点中有凹点，则在凹点中选择最优的可见点，若可见点中没有凹点，则在所有可见点中选择最优可见点。

步骤五：连接凹点与最优可见点，将多边形分解为两个子多边形，直至所有子多边形都为凸多边形。

5.2.1多边形正负的判断

多边形的正负所指的就是多边形顶点的排列顺序，若逆时针排列则多边形为正，若顺时针排列则多边形为负，其判断步骤为：找出多边形的凸点P_i(轮廓的极值点必为多边形的凸点)。与凸点P_i相邻的前后两点分别为P_i-1和P_i+1，组成的矢量为

和

若

则多边形为正。若

则多边形为负。由于三点为多边形的三个定点，所以三点不共线，不存在

的情况，具体如图2所示。

5.2.2多边形凹凸顶点的判断

对于正的多边形，任意一顶点为P_i，与顶点P_i相邻的前后两点分别为P_i-1和P_i+1，组成的矢量为

和

若

则顶点P_i为凸点；若

则顶点P_i为凹点。由于三点为多边形的三个定点，所以三点不共线，不存在

的情况。

5.2.3可见点的定义及选择

多边形的任意一顶点P_i，它与任意一点P_j的连线P_iP_j，若P_iP_j全部在多边形的内部或上面，则顶点P_j成为P_i的可见点。与凹点相邻的前后两点分别为P_i-1和P_i+1，组成的矢量为

和

与

的角平分线记为

若可见点为P_k，则可见点与凹点连线的矢量为

a为

与

的夹角，

a越小，则可见点越优。具体如图3所示。 5.3基于蜂群算法与自由空间的全局路径规划

5.3.1蜂群算法基本步骤

步骤一：取N只蜜蜂，其中N/2为侦查蜂，N/2为跟随蜂。

步骤二：侦查蜂在搜索空间中搜索蜜源，身份转变为引领蜂并在蜜源附近进行邻域搜索，选择质量较高的蜜源保留。

步骤三：引领蜂回到蜂巢共享信息，跟随蜂根据蜜源质量决定是否前往蜜源。

步骤四：前往蜜源的跟随蜂在蜜源附近进行邻域搜索，若发现质量更高的蜜源，则淘汰原蜜源，跟随蜂与引领蜂身份互换。

步骤五：若蜜源经过了T_max次邻域搜索后仍然没有找到更好的蜜源，则放弃该蜜源，引领蜂身份变为侦查蜂，返回步骤二。

步骤六：若总搜索次数达到Limit，则保留当前最优蜜源并停止搜索，否则返回步骤三。其中，蜜蜂的采蜜行为就是寻找最优路径的过程，蜜源的位置对应了可行路径，蜜源的收益对应着路径的质量，采蜜的速度对应着算法的求解速度。

在路径规划算法中，每个蜜源对应一个可行的路径，蜜源质量对应路径长度。初始蜜源的搜索与蜜源的领域搜索过程见5.3.2和5.3.4。

5.3.2全局搜索初始路径

如图4所示，全局搜索找出一组自由连线，并在自由连线中选择路径点，与起点和终点连线生成一条初始路径，其质量不一定是最优的，但此路径一定是可行的。其方法及步骤为：

步骤一：对地图进行凸多边形分解，自由连线为e₁,e₂……e_n。

步骤二：判断起点和目标点所属凸多边形

若

则进入步骤六，否则进入步骤三。

步骤三：找到属于

的所有分割线并随机选择一条e_i，存入集合E中。

步骤四：找出e_i所属的另一个凸多边形

若

则进入步骤六。否则在属于

的所有分割线中随机选择一条不属于集合E的分割线e_j。

步骤五：找出e_j所属的另一个凸多边形

若

则进入步骤六。否则在属于

的所有分割线中随机选择一条不属于集合E的分割线e_j。若e_j存在，则存入集合E中，并重复步骤五，否则清空集合E并返回步骤三。

步骤六：集合E中的每条分割线上任取一点，并按照顺序与起点和终点连接生成初始路径。

5.3.3路径代价(路径质量)

计算路径P_i＝{x_init,p_i1,p_i2,…,p_in,x_goal}的代价benefit₁，本文中路径代价为路径的欧式距离：

5.3.4邻域搜索可行路径

如图5所示，对路径进行局部调整，在本方法中，邻域搜索定义为将某一个路径点在其所处分割线上进行移动，方法及步骤为：在集合E中任选一条分割线e_p，其上的路径点为 P_ep，距离e_p两端点P_ep1，P_ep2的距离分别为d₁，d₂。对路径点P_ep的位置在一定范围内进行随机调整：将P_ep向e_p任意一端点P_epi移动随机距离d，其中0≤d≤a，若r＞d_i,则a＝d_i，若 r≤d_i,则a＝r，r为设置的微调范围，i＝1,2。

5.4快速搜索随机树算法快速生成局部路径

基于RRT*算法生成局部路径的方法如下：

1)定义树的根节点为路径的起始点。

2)更新地图当前每个点允许被采样概率，在状态空间中随机采样获取一个采样点x_rand，根据采样概率判断该点是否被采样，若被采样，则进入步骤3)，否则重复步骤2)。

3)在搜索树中寻找与该采样点距离最近的树节点x_nearest。

4)以树节点x_nearest为起点，沿x_nearest x_rand方向取长度为d的线段得到一个新的节点x_new。

5)连接x_nearest与x_new得到以条新的搜索树杈，检测这条树杈是否与障碍物或威胁区域有交集，若无交集则保留此树节点x_new，至步骤6)，否则舍弃该树节点，跳至步骤 8)。

6)在新的节点x_new附近搜索节点，并判断是否有更合适的节点作为x_new的父节点，使 x_new节点至起始点的代价小于当前情况。通常以x_new节点至起始点的欧氏距离进行比较。

7)判断x_new附近的节点如果将x_new作为父节点是否更优，即到起点的路径代价是否更小。

8)重复步骤2)至7)，直至目标点或目标区域包含在搜索树中。

9)在搜索树中沿目标点反向寻找每一个树节点的父节点，直至起始点，即可得到一条由起点至终点的可行路径。

RRT*算法树搜索过程如图6所示。

在上述步骤2)中，调整采样概率的最终目的是希望已有树节点的周围不再出现更加密集的树节点从而增加算法的搜索速度。因此每一个树节点需要对周围的状态空间被采样的概率产生影响。其中，采样概率修改方法是：给定一个状态空间

一个新的节点x_new∈X_free，其在状态空间

中的坐标为(a,b)。状态空间

中任意位置x∈X的坐标为(c,d)，则x与x_new的相对位置坐标为(x',y')，其中x'＝c-a，y'＝d-b。则x处的概率密度P_x更改为P_x'＝χP_x，其中χ＝1-af(x',y')，

f(x',y')为二维正态分布函数密度函数。根据已有的搜索树节点对状态空间中每一个点被采样的概率进行调整后，根据如下方法判断是否选择该采样点：给定一个状态空间

地图中各点允许被采样概率的集合

任意一采样点x_rand∈X，则x_rand处允许被采样的概率为P_xrand(通常取0.1～0.3)。随机取p←rand(0,1)，若p<P_xrand，则该采样点可以使用，否则重新选取采样点。

5.5动态环境下的路径规划方法

1)根据已知地图条件，利用基于自由空间法与人工蜂群算法的路径规划算法求解由起点至目标点的最优路径(如图7)。

2)无人车按照当前规划路径前进，每隔一段时间t，判断当前地图状态是否发生改变。首先判断终点是否发生改变，如发生改变则跳至步骤1)。随后判断当前路径是否被障碍物阻挡，若未被障碍物阻挡，则重新根据当前路径节点与存储的初始路径节点优化路径；若被障碍物阻挡，则跳至下一步(如图8)。

3)选择被障碍物阻挡的路径两端分别作为局部路径规划的起点和终点，利用改进RRT*算法对该段路径进行局部规划(如图9)。

4)用步骤3)中重新规划的局部路径替换被障碍物阻挡的路径。

5)路径优化：按顺序列出所的路径的起点、节点、终点，依次判断每一个节点若与其相隔的节点连线是否会与障碍物产生交集，若无交集，则舍弃两点间的其它节点(如图10)。

6)重复以上步骤直至无人车行进至目标区域。

Claims (3)

1.一种基于自由空间与快速搜索随机树算法的无人车动态路径规划方法，其特征在于分别利用自由空间法和快速搜索随机树算法实现动态地图的全局路径规划和无人车行进过程中的局部快速路径规划。

所述动态路径规划方法主要步骤：

1)根据已知地图条件，利用基于自由空间法与人工蜂群算法的路径规划算法求解由起点至目标点的最优路径；

2)无人车按照当前规划路径前进，每隔一段时间t，判断当前地图状态是否发生改变；其中，首先判断终点是否发生改变，如发生改变则跳至步骤1)；随后判断当前路径是否被障碍物阻挡，若未被障碍物阻挡，则重新根据当前路径节点与存储的初始路径节点优化路径；若被障碍物阻挡，则跳至下一步；

3)选择被障碍物阻挡的路径两端分别作为局部路径规划的起点和终点，利用快速搜索随机树算法对该段路径进行局部规划；

4)用步骤3)中重新规划的局部路径替换被障碍物阻挡的路径；

5)路径优化：按顺序列出所得路径的起点、节点、终点，依次判断每一个节点若与其相隔的节点连线是否会与障碍物产生交集，若无交集，则舍弃两点间的其它节点；

6)重复以上步骤直至无人车行进至目标区域。

2.根据权利要求1所述的一种基于自由空间与快速搜索随机树算法的动态路径规划方法，其特征在于利用凹多边形凸分解的图形学方法，设计了一种在含有障碍地图上构建自由空间的方法，并应用人工蜂群算法对进行路径优化找出全局最优路径。

所述全局路径规划方法主要步骤：

1)构建自由空间：首先将地图构建为单连通多边形，然后对单连通凹多边形进行凸分解；

2)全局搜索初始路径：全局搜索找出一组自由连线，并在自由连线中选择路径点，与起点和终点连线生成一条初始路径；

3)邻域搜索可行路径：使用人工蜂群算法对路径进行局部调整和优化。

3.根据权利要求1所述的一种基于自由空间与快速搜索随机树算法的动态路径规划方法，其特征在于通过调整随机树节点采样概率，实现了基于改进快速搜索随机树算法的局部快速路径规划方法。

所述局部路径规划方法主要步骤：

1)定义树的根节点为路径的起始点。

2)更新地图当前每个点允许被采样概率，在状态空间中随机采样获取一个采样点x_rand，根据采样概率判断该点是否被采样，若被采样，则进入步骤3)，否则重复步骤2)。

3)在搜索树中寻找与该采样点距离最近的树节点x_nearest。

4)以树节点x_nearest为起点，沿x_nearest x_rand方向取长度为d的线段得到一个新的节点x_new。

5)连接x_nearest与x_new得到以条新的搜索树杈，检测这条树杈是否与障碍物或威胁区域有交集，若无交集则保留此树节点x_new，至步骤6)，否则舍弃该树节点，跳至步骤8)。

6)在新的节点x_new附近搜索节点，并判断是否有更合适的节点作为x_new的父节点，使x_new节点至起始点的代价小于当前情况。通常以x_new节点至起始点的欧氏距离进行比较。

7)判断x_new附近的节点如果将x_new作为父节点是否更优，即到起点的路径代价是否更小。

8)重复步骤2)至7)，直至目标点或目标区域包含在搜索树中。

9)在搜索树中沿目标点反向寻找每一个树节点的父节点，直至起始点，即可得到一条由起点至终点的可行路径。

Images