CN117873115A - 一种基于slam的自主导航避障路径规划方法 - Google Patents

Abstract

本发明涉及无人驾驶的自主导航避障路径规划技术领域公开一种基于SLAM的自主导航避障路径规划方法，方法中引入了障碍物距离影响系数ρ，在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，判断是否开始避障路径规划；在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径，优化方式以一定的迭代次数通过梯度下降法求出每次优化N点路径中第二个路径点的位移矢量来约束N点路径的平滑度，接着去除掉每次优化N点路径中的第一个路径点，加入第N+1个路径点重新构成新的N点路径，循环遍历所有的路径点，直到构不成新的N点路径时停止。该方法能够优化路径平滑度，更符合现实环境中机器人的实际应用。

Description

一种基于SLAM的自主导航避障路径规划方法

技术领域

本发明涉及无人驾驶的自主导航避障路径规划技术领域。

背景技术

路径规划是实现无人车自主导航的核心，根据对环境信息掌握的不同可分为全局和局部路径规划，其中全局路径规划是根据已知的地图环境信息给出一个从起点到目标点的最优路经，局部路径规划是当局部环境信息发生变化时，如动态障碍物的加入，让机器人具有优良的避障性能，能够安全可靠的运行。

D*算法是现有的一种路径规划算法，是一种动态环境下的启发式路径搜索算法，能够在环境发生变化时，最大限度利用前一次的搜索结果，从而快速进行第二次规划，即D*算法能够实现动态避障，遇障碍物能够利用前一次的路径搜索信息，无需重复规划，提高搜索效率，但是现有的D*算法在动态避障时，是到障碍物面前才开始进行新的路径规划，这种方法虽然充分利用了第一阶段A*算法的快速性，保留了在遇到障碍物之前的所有路径。但这种方法也有一定的局限性，由于在动态障碍物的面前才开始新的路径规划，这种情况下可能会由于规划的不及时，出现路径增大、重复路径、耗时增长的情况，可见现有的D*算法在使用时容易出现效率低下和路径不平滑问题，其在路径平滑度上未做优化，存在大量转折，增大了路径距离也不符合现实环境中机器人的实际应用。

发明内容

本发明的目的在于提供一种能够优化路径平滑度，更符合现实环境中机器人的实际应用的一种基于SLAM的自主导航避障路径规划方法。

为实现上述目的，本发明的技术方案是：一种基于SLAM的自主导航避障路径规划方法，方法中引入了障碍物距离影响系数ρ，在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，判断是否开始避障路径规划；在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径，优化的方式以一定的迭代次数通过梯度下降法求出每次优化N点路径中第二个路径点的位移矢量来约束N点路径的平滑度，接着去除掉每次优化N点路径中的第一个路径点，加入第N+1个路径点重新构成新的N点路径，如此依次循环下去，遍历所有的路径点，直到构不成新的N点路径时停止。

所述N点路径为4个连续的路径点的路径。

所述障碍物距离影响系数ρ，表达式为：

式中λ为配置系数，D为预判距离函数，

预判距离函数是指当前搜索点距离障碍物的距离，用欧式距离表达即为：

式中X表示当前搜索点的横纵坐标x_i、y_i，O表示障碍物的初始坐标O_x、O_y；

路径规划中在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，表达式为，

式中ρ＝0，表示搜索点距离障碍物的距离D为无限大，该障碍物不在路径搜索的范围内，路径规划时不予考虑；ρ＝+∞表示障碍物与搜索点的距离D为零，障碍物已经在规划的路径上，根据给出的合理预判距离D开始避障路径规划。

在避障路径规划中的优化计算过程是这样的，定义D*算法规划后的初始栅格路径序列点为P_i＝(x_i+y_i)，i∈[1,n]，n为总路径点数；

优化约束N点路径的路径平滑度的目标函数为：

路径序列点P_i的位置发生位移时，路径平滑度f_s也会发生变化，在满足约束条件的情况下，使路径平滑度f_s取最小值，定义路径序列点P_i点的位移方向为梯度方向，采用N点路径法对P_i求导得到梯度：

将位移矢量Δp_i＝p_i-p_i-1代入得：

得到位移矢量之后新的路径点为：

其中λ为步长,取值为[0,1]，对P_i进行多次迭代同时满足对障碍物距离的约束条件：dis(min)＜D(p_i,o)＜dis(max)。

通过采用上述技术方案，本发明的有益效果是：导航的核心在于反应迅速、路径平滑高效、同时能有效避开障碍物，本发明方法在此原则的基础上，对D*算法进行改进，其对于原有的D*算法引入了障碍物距离影响系数，通过障碍物距离影响系数和多点式的梯度下降法对D*算法进行了优化改进将全局与局部路径规划算法进行融合导航，从而达到本发明的上述能够优化路径平滑度，更符合现实环境中机器人的实际应用的目的。

附图说明

图1是本发明涉及的一种基于SLAM的自主导航避障路径规划方法中4点路径的两相邻序列点之间的位移矢量示意图。

图2是本发明涉及的一种基于SLAM的自主导航避障路径规划方法的流程示意图。

图3是本发明涉及的一种基于SLAM的自主导航避障路径规划方法中D*算法的优化计算流程图。

具体实施方式

为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。

本实施例公开的一种基于SLAM的自主导航避障路径规划方法，方法中引入了障碍物距离影响系数ρ，在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，判断是否开始避障路径规划，具体如下：

所述障碍物距离影响系数ρ，表达式为：

式中λ为配置系数，D为预判距离函数，

预判距离函数是指当前搜索点距离障碍物的距离，用欧式距离表达即为：

式中X表示当前搜索点的横纵坐标x_i、y_i，O表示障碍物的初始坐标O_x、O_y；

路径规划中在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，表达式为，

式中ρ＝0，表示搜索点距离障碍物的距离D为无限大，该障碍物不在路径搜索的范围内，路径规划时不予考虑；ρ＝+∞表示障碍物与搜索点的距离D为零，障碍物已经在规划的路径上，此时才开始避障与传统的D*算法一样，不能解决上述背景技术中存在的问题。本文的目的是想搜索点距离障碍物一定距离的时候已经开始进行避障规划，留有一定的反应时间，而不是搜索到障碍物跟前才开始。因此要根据实际环境信息，给出合理预判距离D开始避障路径规划。

通过上述引入的影响系数的判断，当判断在搜索范围内遇到障碍物，导航***搜寻路径集合，去掉其中包含障碍物路径点的路径，并生成新的子集搜寻新的路径，搜寻到新的路径时进入下述的优化，即对最短路径进行平滑优化得到最优路径发布，否则***发布避障消息，如图2所示。

通过上述过程，已经可以得到初步改善的路径，但传统D*算法规划的路径节点是由各个栅格中点连接而成，这将导致规划的路径曲线存在转折点过多和曲线不平滑的问题，因此本发明对传统D*算法作出了如下改进，

在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径，如图1所示的4个连续的路径点的4点路径，优化的方式以一定的迭代次数通过梯度下降法求出每次优化4点路径中第二个路径点的位移矢量来约束4点路径的平滑度，接着去除掉每次优化4点路径中的第一个路径点，加入第5个路径点重新构成新的4点路径，如此依次循环下去，遍历所有的路径点，直到构不成新的4点路径时停止。优化计算过程是这样的，定义D*算法规划后的初始栅格路径序列点为P_i＝(x_i+y_i)，i∈[1,n]，n为总路径点数；两相邻序列点之间的位移矢量如图1所示。

优化约束N点路径的路径平滑度的目标函数为：

结合图和路径平滑度f_s可以看出路径序列点P_i的位置发生位移时，路径平滑度f_s也会发生变化，在满足约束条件的情况下，如何尽可能使路径平滑度取得最小值则是解决问题的关键，定义路径序列点P_i点的位移方向为梯度方向，采用N点路径法对P_i求导得到梯度：

将位移矢量Δp_i＝p_i-p_i-1代入得：

得到位移矢量之后新的路径点为：

其中λ为步长,取值为[0,1]，大于1将导致路径平滑失败，要得到平滑的路径曲线可以对P_i进行多次迭代同时满足对障碍物距离的约束条件：

dis(min)＜D(p_i,o)＜dis(max)。

新得到的路径点P_i要满足距离障碍物的距离在规定的范围内，其中最大距离和最小距离根据实际情况给出。上述D*算法的优化计算流程如图3所示。

综上，本发明的上述方法对于传统的D*算法，引入了障碍物距离影响系数。通过障碍物距离影响系数和四点式的梯度下降法对D*算法进行了优化改进，运用改良的算法去规划一个全局路径，从而能够优化路径平滑度，更符合现实环境中机器人的实际应用。

上述实施例和图式并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。

Claims (5)

1.一种基于SLAM的自主导航避障路径规划方法，其特征在于，方法中引入了障碍物距离影响系数ρ，在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，判断是否开始避障路径规划；在避障路径规划中基于D*算法规划的路径上每次优化N个连续的路径点的N点路径，优化的方式以一定的迭代次数通过梯度下降法求出每次优化N点路径中第二个路径点的位移矢量来约束N点路径的平滑度，接着去除掉每次优化N点路径中的第一个路径点，加入第N+1个路径点重新构成新的N点路径，如此依次循环下去，遍历所有的路径点，直到构不成新的N点路径时停止。

2.如权利要求1所述的一种基于SLAM的自主导航避障路径规划方法，其特征在于，所述N点路径为4个连续的路径点的路径。

3.如权利要求1或2所述的一种基于SLAM的自主导航避障路径规划方法，其特征在于，所述障碍物距离影响系数ρ，表达式为：

式中λ为配置系数，D为预判距离函数，

预判距离函数是指当前搜索点距离障碍物的距离，用欧式距离表达即为：

式中X表示当前搜索点的横纵坐标x_i、y_i，O表示障碍物的初始坐标O_x、O_y；

路径规划中在搜索路径遍历栅格点的时候根据ρ的大小来判断障碍物对路径规划的影响，表达式为，

式中ρ＝0，表示搜索点距离障碍物的距离D为无限大，该障碍物不在路径搜索的范围内，路径规划时不予考虑；ρ＝+∞表示障碍物与搜索点的距离D为零，障碍物已经在规划的路径上，根据给出的合理预判距离D开始避障路径规划。

4.如权利要求1或2所述的一种基于SLAM的自主导航避障路径规划方法，其特征在于，在避障路径规划中的优化计算过程是这样的，定义D*算法规划后的初始栅格路径序列点为P_i＝(x_i+y_i)，i∈[1,n]，n为总路径点数；

优化约束N点路径的路径平滑度的目标函数为：

路径序列点P_i的位置发生位移时，路径平滑度f_s也会发生变化，在满足约束条件的情况下，使路径平滑度f_s取最小值，定义路径序列点P_i点的位移方向为梯度方向，采用N点路径法对P_i求导得到梯度：

将位移矢量Δp_i＝p_i-p_i-1代入得：

得到位移矢量之后新的路径点为：

其中λ为步长,取值为[0,1]，对P_i进行多次迭代同时满足对障碍物距离的约束条件：dis(min)＜D(p_i,o)＜dis(max)。

5.如权利要求3所述的一种基于SLAM的自主导航避障路径规划方法，其特征在于，在避障路径规划中的优化计算过程是这样的，定义D*算法规划后的初始栅格路径序列点为P_i＝(x_i+y_i)，i∈[1,n]，n为总路径点数；

优化约束N点路径的路径平滑度的目标函数为：

路径序列点P_i的位置发生位移时，路径平滑度f_s也会发生变化，在满足约束条件的情况下，使路径平滑度f_s取最小值，定义路径序列点P_i点的位移方向为梯度方向，采用N点路径法对P_i求导得到梯度：

将位移矢量Δp_i＝p_i-p_i-1代入得：

得到位移矢量之后新的路径点为：

其中λ为步长,取值为[0,1]，对P_i进行多次迭代同时满足对障碍物距离的约束条件：dis(min)＜D(p_i,o)＜dis(max)。