CN115077534A - 一种基于b样条曲线的agv路径规划方法 - Google Patents

Abstract

本发明公开了一种基于B样条曲线的AGV路径规划方法，从上位机获取起点信息、终点信息以及车体硬件参数信息，依次生成起、终点方向控制点，起、终点方向调节点，中间路径上的横向采样点，在删除部分起、终点方向调节点和横向采样点后，依次连接层与层之间的采样点形成连接路径，并实时判断是否满足连接约束条件，若满足则保存该条连接路径对应样条曲线的曲率代价，若不满足则再试探其他未计算过的连接路径；当找寻到低于预设曲率代价的样条路径，或者迭代次数超过预设次数时，返回代价最小的样条路径。本发明最大限度地保持了起、终点的方向控制，使得起点和终点突变曲率更小，基于采样点删除和连接约束更快速地生成远离障碍物的样条路径。

Description

一种基于B样条曲线的AGV路径规划方法

技术领域

本发明涉及AGV路径规划技术领域，尤其是一种基于B样条曲线的AGV路径规划方法。

背景技术

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

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

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

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

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

发明内容

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

一种基于B样条曲线的AGV路径规划方法，包括以下步骤：

步骤1，从上位机获取待规划路径的起点信息、终点信息，以及车体硬件参数信息，起点信息包括但不限于起点的位置坐标、出发角度、出发速度，终点信息包括但不限于终点的位置坐标、到达角度、到达速度，车体硬件参数信息包括但不限于AGV最小转弯半径、车轮最大转动角度；

步骤2，根据起点信息生成包含a个起点方向控制点的起点方向控制采样层，根据终点信息生成包含b个终点方向控制点的终点方向控制采样层；

步骤3，根据起点方向控制点和车体硬件参数信息生成包含c个起点方向调节点的起点方向调节采样层，根据终点方向控制点和车体硬件参数信息生成包含d个终点方向调节点的终点方向调节采样层；

步骤4，连接任意一个起点方向控制点和任意一个终点方向控制点，以连接线作为中间路径，在该中间路径上等间距生成n层纵向采样层，每层纵向采样层包含e个等间距横向采样点；

步骤5，删除方向控制点，不合理的起点方向调节点、终点方向调节点，以及靠近障碍物的横向采样点；

步骤6，从起点到终点，依序选择每一采样层上的任一采样点进行连接，并判断每条连接路径是否满足连接约束条件，若满足则保存该条连接路径对应样条曲线的曲率代价，若不满足则放弃该连接路径（可以予以标记保留），试探其他未计算过的连接路径；

步骤7，当找寻到低于预设曲率代价的样条路径，或者迭代次数超过预设次数时，返回代价最小的样条路径。

进一步的，所述步骤2的具体操作为：根据起点的位置坐标、出发角度和距离约束生成起点方向控制采样层，根据终点的位置坐标、到达角度和距离约束生成终点方向控制采样层，起点的距离约束与出发速度关联，终点的距离约束与到达速度关联。

进一步的，所述步骤3的具体操作为：以起点方向控制点为圆心、AGV最小转弯半径为半径，等间隔生成c个起点方向调节点；以终点方向控制点为圆心、AGV最小转弯半径为半径，等间隔生成d个终点方向调节点。

所述步骤6具体包括以下步骤：

步骤6.1，从起点到终点，依序编号每一采样层的每一采样点；

步骤6.2，按照二叉树先序遍历原则依序选择每一采样层上的任一采样点进行连接，并实时判断是否满足连接约束条件，若满足则继续往下一采样层连接，若不满足则放弃连接当前采样层的当前采样点，选取当前采样层的下一采样点进行连接并进行连接约束判断，若当前采样点为当前采样层的最后采样点，则结束当前连接路径当前采样层的连接操作；

步骤6.3，从起点连接到终点的路径称之为连通路径，计算每条连通路径对应的样条曲线的曲率代价并保存。

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

进一步的，所述步骤6中的样条曲线的曲率代价由样条曲线上的每个采样点的曲率值直接求和得到；或者，由样条曲线上的每个采样点的曲率值加权求和得到，其中靠近起点方向控制点f个采样点，以及靠近终点方向控制点的g个采样点的权重系数大于1，其余采样点的权重系数小于1。

本发明最大限度地保持了起、终点的方向控制，使得起点和终点突变曲率更小，动态设置起、终点方向控制点数量增加搜寻更优样条路径的可能性，基于采样点删除和连接约束更快速地生成远离障碍物的样条路径。

附图说明

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

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

图3为起、终点方向控制点范围示意图；

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

图5为起、终点方向调节点示意图；

图6为横向采样点示意图；

图7为方向调节点、横向采样点删除示意图；

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

图9为不可连接采样点示意图。

具体实施方式

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

实施例1

一种基于B样条曲线的AGV路径规划方法，如图1所示，包括以下步骤：

1、获取待规划路径的相关信息

从上位机获取待规划路径的起点信息、终点信息，以及车体硬件参数信息。

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

2、生成起点、终点方向控制采样层

根据起点信息生成包含a个起点方向控制点的起点方向控制采样层，根据终点信息生成包含b个终点方向控制点的终点方向控制采样层，此处a、b均为抽象数量。

起点的位置坐标、出发角度确定，则起点方向控制点startDirectionCP必定在以起点对应出发角度的延长线上，终点方向控制点goalDirectionCP必定在以终点对应到达角度的反向延长线上，因此只要再分别限定二者与起点和终点的距离，就可以得到确定的起点方向控制点和终点方向控制点。

起点方向控制点startDirectionCP与起点Start的距离，以及终点方向控制点goalDirectionCP与终点Goal的最大距离DirectionDist为起点Start和终点Goal之间欧式距离DistSG的1/3。

本实施例分别采用startK、goalK这两个参数调节起点方向控制点startDirectionCP与起点Start、终点方向控制点goalDirectionCP与终点Goal之间的距离。而startK、goalK的参数值分别根据起点速度约束、终点速度约束动态控制，进而动态改变起点方向控制点和终点方向控制点可以生成的范围，如图3所示。

由此形成路径上的第一层采样点startDirectionCPS，为与起点在一定距离范围内的等间距采样点；以及最后一层采样点goalDirectionCPS，为与终点在一定距离范围内的等间距采样点。此两层采样点给予路径上起点和终点的方向控制。

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

3、生成起点、终点方向调节采样层

根据起点方向控制点和车体硬件参数信息生成包含c个起点方向调节点的起点方向调节采样层，根据终点方向控制点和车体硬件参数信息生成包含d个终点方向调节点的终点方向调节采样层，此处c、d也均为抽象数量。

此处涉及到的车体硬件参数信息为AGV最小转弯半径，具体为：以起点方向控制点为圆心、AGV最小转弯半径为半径，等间隔生成c个起点方向调节点；以终点方向控制点为圆心、AGV最小转弯半径为半径，等间隔生成d个终点方向调节点，如图5所示。

4、生成纵向采样层

连接任意一个起点方向控制点和任意一个终点方向控制点，以连接线作为中间路径，在该中间路径上等间距生成n层纵向采样层，每层纵向采样层包含e个等间距横向采样点，如图6所示，此处n、e也均为抽象数量。从图6中可以看出“纵向”、“横向”的含义所在，每层横向采样点的生成数量可以人为设定，或者由其他约束条件确定。

5、删除方向控制点、不合理的方向调节点和靠近障碍物的横向采样点

首先，删除第一层、最后一层的方向控制点，起、终点固定且起、终点方向控制点的方向固定，因此连接第二层和倒数第二层的方向调节点便已经具有足够的约束，当然此步骤不必须；

其次，按照起、终点方向，删除一定角度范围内的方向调节点；

最后，删除一些靠近障碍物的横向采样点，如图7所示，图7中打×的圆圈为删除的方向调节点和横向采样点。

6、连接层与层之间的采样点形成连接路径

从起点到终点，依序选择每一采样层上的任一采样点进行连接，并判断每条连接路径是否满足连接约束条件，若满足则保存该条连接路径对应样条曲线的曲率代价，若不满足则放弃该连接路径，试探其他未运算过的连接路径。

此步骤可以形成从起到到终点的连通路径后，再对该连通路径进行连接约束判断，但是如此操作将存在大量不必要的运算操作，继而导致运算资源的浪费。

为此，本实施例中，步骤6具体包括以下步骤：

⑴从起点到终点，依序编号每一采样层的每一采样点，例如第一层为Slice1，第一层第二个采样点为Slice1Point2，第三层第五个采样点为Slice3Point5。

⑵按照二叉树先序遍历原则（根左右）依序选择每一采样层上的任一采样点进行连接，并实时判断是否满足连接约束条件，若满足则继续往下一采样层连接，若不满足则放弃连接当前采样层的当前采样点，选取当前采样层的下一采样点进行连接并进行连接约束判断，若当前采样点为当前采样层的最后采样点，则结束当前连接路径当前采样层的连接操作。

下面结合图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。

通过判断连接约束条件，可以在进行层间连接时快速排除不可连接的情况。对于每次不可连接的情况可以予以标记保留，这样在重新规划路径时，可以更快速地筛选出可连接的采样点，如图9所示，图9中的连接虚线表示不满足连接约束条件，属于无法连接的情况。

⑶从起点连接到终点的路径称之为连通路径，计算每条连通路径对应的样条曲线的曲率代价并保存。

此处样条曲线的曲率代价由样条曲线上的每个采样点的曲率值直接求和得到；或者，由样条曲线上的每个采样点的曲率值加权求和得到，其中靠近起点方向控制点f个采样点，以及终点方向控制点的g个采样点的权重系数大于1（比如1.2），其余采样点的权重系数小于1（比如0.8），此处f、g也均为抽象数量。

采用曲率值加权求和确定样条曲线的曲率代价的思路在于，认同靠近起、终点方向控制点的采样点在路径规划中的重要性高于中间采样点。

7、返回曲率代价最小的样条路径

当找寻到低于预设曲率代价的样条路径，或者迭代次数超过预设次数时，返回曲率代价最小的样条路径。

这也非常容易理解，已经找寻到比预设条件更好的样条路径，自然直接选择该路径；但是若一直未找寻到比预设条件更好，或者与预设条件相当的样条路径，则可以认为不存在， 直接在超过预设迭代次数后，返回曲率代价最小的样条路径。

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

Claims (8)

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

步骤1，从上位机获取待规划路径的起点信息、终点信息，以及车体硬件参数信息，起点信息包括但不限于起点的位置坐标、出发角度、出发速度，终点信息包括但不限于终点的位置坐标、到达角度、到达速度，车体硬件参数信息包括但不限于AGV最小转弯半径、车轮最大转动角度；

步骤2，根据起点信息生成包含a个起点方向控制点的起点方向控制采样层，根据终点信息生成包含b个终点方向控制点的终点方向控制采样层，其中a、b均为抽象数量；

步骤3，根据起点方向控制点和车体硬件参数信息生成包含c个起点方向调节点的起点方向调节采样层，根据终点方向控制点和车体硬件参数信息生成包含d个终点方向调节点的终点方向调节采样层，其中c、d均为抽象数量；

步骤4，连接任意一个起点方向控制点和任意一个终点方向控制点，以连接线作为中间路径，在该中间路径上等间距生成n层纵向采样层，每层纵向采样层包含e个等间距横向采样点，其中n、e均为抽象数量；

步骤5，删除方向控制点，不合理的起点方向调节点、终点方向调节点，以及靠近障碍物的横向采样点；

步骤6，从起点到终点，依序选择每一采样层上的任一采样点进行连接，并判断每条连接路径是否满足连接约束条件，若满足则保存该条连接路径对应样条曲线的曲率代价，若不满足则放弃该连接路径，试探其他未计算过的连接路径；

步骤7，当找寻到低于预设曲率代价的样条路径，或者迭代次数超过预设次数时，返回代价最小的样条路径。

2.根据权利要求1所述的基于B样条曲线的AGV路径规划方法，其特征在于，所述步骤2的具体操作为：

根据起点的位置坐标、出发角度和距离约束生成起点方向控制采样层，根据终点的位置坐标、到达角度和距离约束生成终点方向控制采样层，起点的距离约束与出发速度关联，终点的距离约束与到达速度关联。

3.根据权利要求1或2所述的基于B样条曲线的AGV路径规划方法，其特征在于，所述步骤3的具体操作为：以起点方向控制点为圆心、AGV最小转弯半径为半径，等间隔生成c个起点方向调节点；以终点方向控制点为圆心、AGV最小转弯半径为半径，等间隔生成d个终点方向调节点。

4.根据权利要求1所述的基于B样条曲线的AGV路径规划方法，其特征在于，所述步骤6具体包括以下步骤：

步骤6.1，从起点到终点，依序编号每一采样层的每一采样点；

步骤6.2，按照二叉树先序遍历原则依序选择每一采样层上的任一采样点进行连接，并实时判断是否满足连接约束条件，若满足则继续往下一采样层连接，若不满足则放弃连接当前采样层的当前采样点，选取当前采样层的下一采样点进行连接并进行连接约束判断，若当前采样点为当前采样层的最后采样点，则结束当前连接路径当前采样层的连接操作；

步骤6.3，从起点连接到终点的路径称之为连通路径，计算每条连通路径对应的样条曲线的曲率代价并保存。

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

6.根据权利要求1或4所述的基于B样条曲线的AGV路径规划方法，其特征在于，所述步骤6中的样条曲线的曲率代价由样条曲线上的每个采样点的曲率值直接求和得到。

7.根据权利要求1或4所述的基于B样条曲线的AGV路径规划方法，其特征在于，所述步骤6中的样条曲线的曲率代价由样条曲线上的每个采样点的曲率值加权求和得到，其中靠近起点方向控制点f个采样点，以及靠近终点方向控制点的g个采样点的权重系数大于1，其余采样点的权重系数小于1，其中f、g均为抽象数量。

8.根据权利要求1所述的基于B样条曲线的AGV路径规划方法，其特征在于，对步骤6中放弃的连接路径予以标记保留。

Images