CN115755755A - Agv小车b样条曲线路径快速生成方法、存储介质及系统 - Google Patents

Abstract

本发明的AGV小车B样条曲线路径快速生成方法、存储介质及系统，AGV小车B样条曲线路径快速生成方法，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

Description

AGV小车B样条曲线路径快速生成方法、存储介质及系统

技术领域

本发明属于AGV技术领域，具体涉及AGV小车B样条曲线路径快速生成方法、存储介质及系统。

背景技术

在AGV领域中，大部分情况路径都是预先设置好的，但是也会经常遇到无预设路径、需要自己生成路径的情况，如AGV在路径外，需要AGV自己生成路径插入路径点；或者需要AGV走到某个临时指定坐标点等。需求可简单描述为给定起始点的位姿，生成一条路径使得AGV可以从起点的位姿顺利移动到终点的位姿。业内生成路径的方法有很多，大多要么需要复杂的计算，消耗大量的计算资源，要么生成的路径并不平滑，不适合AGV行走。

发明内容

本发明的目的在于克服现有路径生成方法大多要么需要复杂和大量的计算资源或者生成的路径不平滑存在的问题，提供生成路径平滑且运输量少的AGV小车B样条曲线路径快速生成方法、存储介质及系统。

为了达到上述目的，本发明采用以下技术方案

AGV小车B样条曲线路径快速生成方法，包括以下步骤：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息。

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2。

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]。

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率。

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′。

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g。

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环。

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

与现有技术相比，本发明的AGV小车B样条曲线路径快速生成方法，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

进一步的，在步骤f中，精度阈值设为10mm。

存储介质，该存储装置存储多条执行指令，所述执行指令用于AGV的中控中心加载并执行以下操作：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息。

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2。

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]。

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率。

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′。

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g。

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环。

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

通过这样设置，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

进一步的，在步骤f中，精度阈值设为10mm。

AGV小车B样条曲线路径快速生成控制系统，其包括中控中心、AGV和用于存储多条执行指令的存储装置，所述执行指令用于中控中心加载并执行以下操作：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息。

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2。

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]。

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率。

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′。

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g。

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环。

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

进一步的，在步骤f中，精度阈值设为10mm。

通过这样设置，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

附图说明

图1为本发明的AGV小车B样条曲线路径快速生成方法的流程图

图2为本发明的实际应用的示意图

具体实施方式

以下结合附图说明本发明的技术方案：

实施例一：

参见图1和图2，本发明的AGV小车B样条曲线路径快速生成方法，包括以下步骤：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息。

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2。

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]。

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，即使用双重循环，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率。

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′。

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g。

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环。

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

与现有技术相比，本发明的AGV小车B样条曲线路径快速生成方法，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

在一种实施例中，在步骤f中，精度阈值设为10mm。

参见图2，以下列举一个试验数据说明本发明的技术方案：

步骤a：假设起点S坐标为(0，0)，终点T坐标为(300，400)，单位为mm，起点和终点的角度均为0，即起点和终点的方向均与X轴的正方向平行。

步骤c：L_ST＝500，取L1_min＝L2_min＝0.1*L_ST＝50,L1_max＝L2_max＝0.6*L_ST＝300。

步骤d：t1＝(L1_max-L1_min)/9＝(300-50)/9＝27.8,t2＝(L2_max-L2_min)/9＝27.8，双重循环，即分别枚举L1＝50+27.8*0,50+27.8*1,50+27.8*2,...,50+27.8*9，和L2＝50+27.8*0,50+27.8*1,50+27.8*2,...,50+27.8*9。

步骤e：取步骤d中B样条曲线最大曲率最小的L1、L2值(其中B样条曲线最大曲率需要具体计算得出)，记为L1’和L2’，这里假设L1’＝50+27.8*2＝105.6，L2’＝50+27.8*4＝161.2，即步骤d中的i＝2,j＝4时，得出的B样条曲线的最大曲率最小。

步骤f：t1＝t2＝27.8>10mm，进入步骤7。

步骤g：L1_min＝L1’-t1＝105.6-27.8＝77.8,L1_max＝L1’+t1＝133.4,L2_min＝L2’-t2＝161.2-27.8＝133.4,L2_max＝L2’+t2＝189，进入步骤d。

重复步骤d：t1＝(L1_max-L1_min)/9＝(133.4-77.8)/9＝6.2,t2＝(L2_max-L2_min)/9＝6.2,双重循环，即分别枚举L1＝77.8+6.2*0,77.8+6.2*1,77.8+6.2*2,...,77.8+6.2*9，和L2＝77.8+6.2*0,77.8+6.2*1,77.8+6.2*2,...,77.8+6.2*9。

重复步骤e：这里假设L1’＝77.8+6.2*0＝77.8，L2’＝77.8+6.2*5＝108.8，即步骤4中的i＝0,j＝5时，得出的B样条曲线的最大曲率最小。

重复步骤f：t1＝t2＝6.2<10mm,t1和t2均小于精度阈值，进入步骤h。

步骤f：L1’＝77.8,L2’＝108.8,可以算出P1点坐标为(77.8,0)，P2点坐标为(191.2,400)，如原文图2所示，所以三次B样条曲线4个点坐标分别为起点(0,0),控制点P1(77.8,0),控制点P2(191.2,400),终点(300,400)生成的三次clamped B样条的插入路径。

实施例二：

参见图1和图2，本实施例的主要目的一在于提供应用实施例一的AGV小车B样条曲线路径快速生成方法的存储介质，该存储装置存储多条执行指令，所述执行指令用于AGV的中控中心加载并执行以下操作：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息。

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2。

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]。

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，即使用双重循环，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率。

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′。

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g。

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环。

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

与现有技术相比，本发明的存储介质，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

在一种实施例中，在步骤f中，精度阈值设为10mm。

实施例三：

本实施例的主要目的在于提供应用实施例二的存储介质的AGV小车B样条曲线路径快速生成控制系统，其包括中控中心、AGV和用于存储多条执行指令的存储装置，所述执行指令用于中控中心加载并执行以下操作：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息。

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2。

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]。

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，即使用双重循环，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率。

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′。

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g。

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环。

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

与现有技术相比，本发明的AGV小车B样条曲线路径快速生成控制系统，可以快速生成适合AGV小车行走的用三次B样条曲线构成的路径，使用较少的算力资源和计算时间，便可使得AGV从指定起点坐标和姿态沿着生成的路径平滑地到达指定终点的坐标和姿态，使用效果好。

在一种实施例中，在步骤f中，精度阈值设为10mm。

根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对本发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对本发明构成任何限制。

Claims (6)

1.AGV小车B样条曲线路径快速生成方法，其特征在于，包括以下步骤：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息；

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2；

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]；

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率；

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′；

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g；

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环；

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

2.根据权利要求1所述AGV小车B样条曲线路径快速生成方法，其特征在于，在步骤f中，精度阈值设为10mm。

3.存储介质，该存储装置存储多条执行指令，其特征在于，所述执行指令用于AGV的中控中心加载并执行以下操作：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息；

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2；

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]；

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率；

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′；

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g；

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环；

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

4.根据权利要求3所述的存储介质，其特征在于，在步骤f中，精度阈值设为10mm。

5.AGV小车B样条曲线路径快速生成控制系统，其特征在于，其包括中控中心、AGV和用于存储多条执行指令的存储装置，所述执行指令用于中控中心加载并执行以下操作：

a.获取AGV所需生成路径的起点及终点的二维坐标及姿态角信息；

b.分别从起点S和终点T开始，往它们的姿态方向延伸，以L1为起点S延伸距离，以L2为终点T延伸距离，并且设立两个clamped B样条控制点P1及P2；

c.根据起点与终点之间的距离，计算出起点S及终点T之间的欧氏距离L_ST，计算出控制点P1与起点的最小距离L1_min、与终点的最小距离L2_min；计算出控制点P2与起点的最大距离L1_max、与终点的最大距离L2_max，令L1_min＝L2_min＝0.1*L_ST,L1_max＝L2_max＝0.6*L_ST，其中,L1∈[L1_min,L1_max],L2∈[L2_min,L2_max]；

d.设立间隔t1＝(L1_max-L1_min)/9,间隔t2＝(L2_max-L2_min)/9,使[L1_min,L1_max]和[L2_min,L2_max]区间分成10等分，分别遍历L1和L2，分别令L1＝L1_min+t1*i,L2＝L2_min+t2*j,其中i,j为0-9的整数，分别计算出每个遍历控制点P1和P2位置生成B样条曲线的最大曲率；

e.使得B样条曲线的最大曲率最小的L1和L2值，记为L1′和L2′；

f.判断t1和t2是否小于精度阈值，若t1和t2均小于精度阈值，进入步骤h，否则进入步骤g；

g.令L1_min＝L1′-t1，L1_max＝L1′+t1,L2_min＝L2′-t2，L2_max＝L2′+t2，并且进入步骤d，继续迭代循环；

h.记录L1′和L2′，根据L1′和L2′的距离计算出P1和P2的坐标，以起点S、控制点P1、控制点P2、终点T四个生成的三次clamped B样条的插入路径。

6.根据权利要求5所述的AGV小车B样条曲线路径快速生成控制系统，其特征在于，在步骤f中，精度阈值设为10mm。

Images