CN115046557A

CN115046557A - 一种结合b样条曲线和a星算法的agv路径规划方法

Info

Publication number: CN115046557A
Application number: CN202210961697.3A
Authority: CN
Inventors: 向泽波; 姚志坚
Original assignee: Hefei Jingsong Intelligent Technology Co ltd
Current assignee: Hefei Jingsong Intelligent Technology Co ltd
Priority date: 2022-08-11
Filing date: 2022-08-11
Publication date: 2022-09-13
Anticipated expiration: 2042-08-11
Also published as: CN115046557B

Abstract

本发明公开了一种结合B样条曲线和A星算法的AGV路径规划方法，从上位机获取待规划路径的起点信息、终点信息和约束条件，根据起点信息和起点控制约束距离生成起点方向控制节点，根据终点信息和终点控制约束距离生成终点方向控制节点；基于起、终点方向控制节点生成若干层候选节点，利用这些候选节点作为A星算法的“邻居”，进行迭代计算，可以在保持起终点方向控制的同时，更快速生成远离障碍物的样条曲线；灵活度高，适应更多应用场景。

Description

一种结合B样条曲线和A星算法的AGV路径规划方法

技术领域

本发明涉及AGV路径规划技术领域，尤其是一种结合B样条曲线和A星算法的AGV路径规划方法。

背景技术

随着社会的进步与发展，智能算法不断推陈出新，计算机功能日益强大，AGV小车与社会各行业的联系也越来越紧密，智能化、无人化、高效化逐步成为现代物流系统的主流发展方向。近年来各个行业的快速发展，对自动化、智能化的要求不断增加，越来越多的智能化配送系统被广泛应用，也相应促进了AGV的快速发展。AGV的独特优势已逐渐显现，例如在物流系统中，它可以依据任务分配原则很快地将物料准确地运输到指定位置，运输路径容易调节，在一定程度上降低了劳动力的使用，从而使得生产效率和便捷性得到提高。

在智能AGV技术领域，路径规划是其核心技术，路径规划近些年蓬勃发展，在地图信息已知的静态环境下的路径规划的发展已经日趋成熟。路径规划的评价指标一般包括路径是否最短、规划效率是否最高、路径平滑度是否良好等方面。在日常应用中，AGV小车往往要执行在特定地形、环境复杂地图下的任务，因此，不仅要注意在复杂环境下的路径规划，还要从全局路径规划的角度考虑规划路径的质量。

例如，仓储环境较为复杂，AGV视觉检测托盘得到的目标点与其当前所处方位之间的距离可能存在无法满足其自旋转所需空间的情况，这就要求在路径规划时，对AGV起点方向进行约束；与此同时，为了消除安全隐患，到达托盘目标点的整个路径需要尽可能平滑且符合AGV运动特性。目前，符合此要求的路径规划方法主要有两种，分别为五次多项式曲线拟合法和Beizer曲线法。

五次多项式曲线拟合法的主要思想是Jerk能量最小化，通过六个边界条件构造五次多项式，生成满足条件的轨迹，再拟合成样条曲线；此方法生成的路径无法让起终点的方向有更好的约束，并且拟合过程中存在精度损失。

Bezier曲线法，是通过设置一定的距离阈值，迭代起终点的方向控制节点，以曲率为代价计算最优曲线；由于Bezier曲线通常为4个控制点，其中仅有两个可以变动的方向控制节点，无法很好地表达需要生成路径的曲线，导致路径上的很多地方曲率较大的问题。

A*搜寻算法俗称A星算法，是比较流行的启发式搜索算法之一，被广泛应用于路径优化领域。A星算法的独特之处在于，检查最短路径中每个可能的节点时引入了全局信息，对当前节点距终点的距离做出估计，并作为评价该节点处于最短路线上可能性的量度。

A星算法需要用栅格地图对搜寻区域进行划分，这是寻路的第一步，将搜索区域简化为 2 维数组，数组的每一项代表一个方格，它的状态分为可走和不可走，通过计算从起点到终点需要走过哪些方格，找到路径。然而，对于当前所处方格，都需要搜索八个邻居方格，计算移动到该方格的移动代价G以及从该方格移动到终点的估算成本H，计算量较大。

发明内容

针对现有AGV路径规划方法存在的技术问题，本发明提出一种结合B样条曲线和A星算法的AGV路径规划方法。

一种结合B样条曲线和A星算法的AGV路径规划方法，包括以下步骤：

步骤1，从上位机获取待规划路径的起点信息、终点信息和约束条件，根据起点信息和起点控制约束距离生成起点方向控制节点，根据终点信息和终点控制约束距离生成终点方向控制节点；

步骤2，根据约束条件，生成从起点方向控制节点到终点方向控制节点路径上的若干层候选节点，每层包含候选节点若干个；

步骤3，初始化每一个候选节点，更新其level值、G值和H值，其中level值为该候选节点所处的层级，G值为从起点到该候选节点的损耗代价，通过起点方向控制节点到该候选节点的欧氏距离计算，H值为从该候选节点到终点的启发代价，通过该候选节点到终点方向控制节点的欧氏距离计算；

步骤4，为每一个节点添加一个潜在邻居列表，并且其中潜在邻居的level值大于该节点的level值；

步骤5，将起点方向控制节点列入Openlist；

步骤6，遍历Openlist，查找F值最小的节点，作为当前节点，其中F=G+H；

步骤7，判断是否第一次搜索该节点的潜在邻居，若是则顺序执行步骤8，若否则跳转步骤9；

步骤8，遍历level值大于当前节点level值的所有候选节点，将满足条件的候选节点添加至该节点的潜在邻居列表中；

步骤9，提取当前节点的潜在邻居列表中的F值最小的邻居节点，并将其从潜在邻居列表中删除；

步骤10，判断提取的邻居节点是否为终点方向控制节点，若是则顺序执行步骤11，若否则跳转步骤12；

步骤11，判断当前节点与终点方向候选节点和终点是否满足连接约束条件，若否则顺序执行步骤12，若否则跳转步骤13；

步骤12，更新当前节点的F值并存入Openlist，跳转步骤6；

步骤13，通过父节点回溯生成节点路径，根据节点路径生成样条路径。

进一步的，所述步骤2具体包括以下步骤：

步骤2.1，以起点方向控制节点为圆心生成n1层起点方向候选节点，以终点方向控制节点为圆心生成n2层终点方向候选节点；

步骤2.2，连接起点方向控制节点和终点方向控制节点，以连接线作为中间路径，在该中间路径上等间距生成n3层中间约束候选节点；

步骤2.3，删除靠近障碍物的候选节点。

进一步的，起点控制约束距离、终点控制约束距离均为采用最保守距离safeDist。

进一步的，n1层起点方向候选节点的半径分别为(DirectionDist – safeDist)/ni，i=1,2,...,n1；n2层终点方向候选节点的半径分别为(DirectionDist – safeDist)/nj，j=1,2,...,n2；其中DirectionDist为起点方向控制节点与起点以及终点方向控制节点与终点之间的距离且DirectionDist≤DistSG/3，DistSG为起点与终点之间的欧式距离。

进一步的，所述步骤11中的连接约束条件为：假设三个连续采样点分别为O、P、Q，转向角度γ小于车轮最大转动角度并且PQ之间的距离大于车体最小转向距离L。

本发明将B样条曲线与A星算法相结合，基于起、终点方向控制节点生成若干层候选节点，利用这些候选节点作为A星算法的“邻居”，可以在保持起终点方向控制的同时，更快速生成远离障碍物的样条曲线；灵活度高，适应更多应用场景。

附图说明

图1为基于B样条曲线的AGV路径规划方法流程图；

图2为起、终点信息示意图；

图3为起、终点方向候选节点示意图；

图4为起点方向和终点方向同向而位置相反的情况示意图；

图5为中间约束候选节点示意图；

图6为删除节点示意图；

图7为当前节点与父亲节点、潜在邻居节点关系示意图；

图8为连续三点之间的连接约束示意图；

图9为根据节点路径直接生成的样条示意图；

图10为图9所示样条插值处理后的样条示意图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步详细的说明。本发明的实施例是为了示例和描述起见而给出的，而并不是无遗漏的或者将本发明限于所公开的形式。很多修改和变化对于本领域的普通技术人员而言是显而易见的。选择和描述实施例是为了更好说明本发明的原理和实际应用，并且使本领域的普通技术人员能够理解本发明从而设计适于特定用途的带有各种修改的各种实施例。

实施例1

一种结合B样条曲线和A星算法的AGV路径规划方法，如图1所示，包括以下步骤：

1、生成起、终点方向控制节点

从上位机获取待规划路径的起点信息、终点信息和约束条件，根据起点信息和起点控制约束距离生成起点方向控制节点，根据终点信息和终点控制约束距离生成终点方向控制节点。

起点信息包括但不限于起点Start的位置坐标、出发角度startAngle、出发速度Vs，终点信息包括但不限于终点Goal的位置坐标、到达角度goalAngle、到达速度Vg，如图2所示。约束条件包括但不限于起点控制约束距离、终点控制约束距离、AGV最小转弯半径、车轮最大转动角度。

本实施例中，起点控制约束距离、终点控制约束距离均为采用最保守距离safeDist。最保守距离需要保证AGV能够从起点正常到达起点方向控制节点，同样能够正常从终点方向控制节点到达终点。该值为经验值，可以与AGV轴距关联，但最大不超过DistSG/3，其中DistSG为起点Start和终点Goal之间欧式距离。

2、生成若干层候选节点

根据约束条件，生成从起点方向控制节点到终点方向控制节点路径上的若干层候选节点，每层包含候选节点若干个，具体包括以下步骤：

⑴以起点方向控制节点为圆心生成n1层起点方向候选节点，以终点方向控制节点为圆心生成n2层终点方向候选节点，如图3所示，此处n1、n2、n3均为抽象数量。

n1层起点方向候选节点的半径分别为(DirectionDist – safeDist)/ni，i=1,2,...,n1；

n2层终点方向候选节点的半径分别为(DirectionDist – safeDist)/nj，j=1,2,...,n2；

其中DirectionDist为起点方向控制节点与起点以及终点方向控制节点与终点之间的距离且DirectionDist≤DistSG/3，DistSG为起点与终点之间的欧式距离。

然而，单纯依靠起、终点方向控制节点，很多时候并不能很好地解决方向约束的问题，例如图4所示情况，起点方向和终点方向同向而位置相反，因此还需要进行方向调节。

⑵连接起点方向控制节点和终点方向控制节点，以连接线作为中间路径，在该中间路径上等间距生成n3层中间约束候选节点，如图5所示。

⑶删除靠近障碍物的候选节点，如图6所示，图6中打×的圆圈为删除的方向调节点和横向采样点。

3、初始化每一个候选节点，更新其level值、G值和H值

level值即为该候选节点所处的层级；G值为从起点到该候选节点的损耗代价，通过起点方向控制节点到该候选节点的欧氏距离计算；H值为从该候选节点到终点的启发代价，通过该候选节点到终点方向控制节点的欧氏距离计算。

4、为每一个节点添加一个潜在邻居列表，并且其中潜在邻居的level值大于该节点的level值。潜在邻居列表包含该节点可以连接的所有候选节点，参照图7所示。

5、将起点方向控制节点列入Openlist。

6、遍历Openlist，查找F值（G值+H值）最小的节点，作为当前节点。

7、判断是否第一次搜索该节点的潜在邻居，若是则顺序执行步骤8，若否则跳转步骤9。

8、遍历level值大于当前节点level值的所有候选节点，将满足条件的候选节点添加至该节点的潜在邻居列表中。

9、提取当前节点的潜在邻居列表中的F值最小的邻居节点，并将其从潜在邻居列表中删除。

10、判断提取的邻居节点是否为终点方向控制节点，若是则顺序执行步骤11，若否则跳转步骤12。

11、判断当前节点与终点方向候选节点和终点是否满足连接约束条件，若否则顺序执行步骤12，若否则跳转步骤13。

下面结合图8，对连接约束条件进行展开介绍。

已知连续三点坐标O、P、Q，可以得知方向角为α、β；其中，α=arctan[(Y_P-Y_O)/(X_P-X_O)]，β=arctan[(Y_Q-Y_P)/(X_Q-X_P)]，X_O、X_P、X_Q分别为点O、P、Q的横坐标，Y_O、Y_P、Y_Q分别为点O、P、Q的纵坐标。

当AGV由OP方向转向PQ方向时，是否能够完成转向取决于以下两点：第一，PQ之间的距离；第二，转向角度γ。

在图8所示情景中，转向角度γ=α+β。实际代码计算中，α为第一象限角，β位于第四象限角，通过向量计算直接取差值即可，此处是为了更直观地表述，描述为α+β。

AGV存在最小转弯半径minRadius，也就是说，当AGV进行极限转向（以最小转弯半径转向）时，最起码要满足的车体最小转向距离L=minRadius*sqrt(2*(1-cosγ))。

因此，连接约束条件需要同时满足转向角度γ小于车轮最大转动角度并且PQ之间的距离大于车体最小转向距离L。

12、更新当前节点的F值并存入Openlist，跳转步骤6；

13、通过父节点回溯生成节点路径。

由于靠近障碍物的候选节点已删除，因此生成的节点路径为4个节点连接。若直接将这4个节点当作控制点生成样条（参照图9），则生成的样条会随着起终点距离的增大，而失去方向控制作用。由于没有中间控制点，起点方向的控制作用会直接受终点方向影响，终点方向会直接受起点方向控制节点影响。

为此，本实施例在起、终点方向控制节点之间进行一定距离的插值，插值处理后的样条如图10所示。

显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域及相关领域的普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。

Claims

1.一种结合B样条曲线和A星算法的AGV路径规划方法，其特征在于，包括以下步骤：

步骤3，初始化每一个候选节点，更新其level值、G值和H值，其中level值为该候选节点所处的层级，G值为从起点到该候选节点的损耗代价，H值为从该候选节点到终点的启发代价；

步骤5，将起点方向控制节点列入Openlist；

步骤12，更新当前节点的F值并存入Openlist，跳转步骤6；

2.根据权利要求1所述的AGV路径规划方法，其特征在于，所述步骤2具体包括以下步骤：

步骤2.1，以起点方向控制节点为圆心生成n1层起点方向候选节点，以终点方向控制节点为圆心生成n2层终点方向候选节点，其中n1、n2均为抽象数量；

步骤2.2，连接起点方向控制节点和终点方向控制节点，以连接线作为中间路径，在该中间路径上等间距生成n3层中间约束候选节点，n3均为抽象数量；

步骤2.3，删除靠近障碍物的候选节点。

3.根据权利要求1所述的AGV路径规划方法，其特征在于，起点控制约束距离、终点控制约束距离均为采用最保守距离safeDist。

4.根据权利要求3所述的AGV路径规划方法，其特征在于，n1层起点方向候选节点的半径分别为(DirectionDist – safeDist)/ni，i=1,2,...,n1；n2层终点方向候选节点的半径分别为(DirectionDist – safeDist)/nj，j=1,2,...,n2；其中DirectionDist为起点方向控制节点与起点以及终点方向控制节点与终点之间的距离且DirectionDist≤DistSG/3，DistSG为起点与终点之间的欧式距离。

5.根据权利要求1所述的AGV路径规划方法，其特征在于，所述步骤11中的连接约束条件为：假设三个连续采样点分别为O、P、Q，转向角度γ小于车轮最大转动角度并且PQ之间的距离大于车体最小转向距离L。

6.根据权利要求1所述的AGV路径规划方法，其特征在于，损耗代价G值通过起点方向控制节点到该候选节点的欧氏距离计算。

7.根据权利要求1所述的AGV路径规划方法，其特征在于，启发代价H值通过该候选节点到终点方向控制节点的欧氏距离计算。