基于Hermite插值路径生成方法、插入路径生成方法及存储装置与流程

本发明属于agv路径规划技术领域，具体涉及基于hermite插值路径生成方法、插入路径生成方法及存储装置。

背景技术：

近年来，无轨导航技术因其具备不需要铺设引导线、系统柔性化好等优点而倍受自动化行业的青睐，采用无轨导航技术的agv设备也已在众多的自动化生产线中得到广泛应用。无轨导航agv是沿着车载控制器中所保存的地图运行，地图是由一系列点和线构成的预设路径。agv只有确认自身位于预设路径上时才会正常运行，否则需要将车辆插入到路径上之后才能继续使用。由此衍生的一个需要解决的问题是，当agv的启动位置不在地图上，或由于外界因素导致运行中的agv偏离地图路径，此时如何才能令agv快速高效地从偏离位置回到地图路径上？针对这一问题，传统的解决方式是通过人工操作在手动模式下将agv重新插入到预设路径点上，但这会造成效率低下，人力资源浪费。专利cn108180911a中针对该问题提出了一种自动生成修正路径的方法，但该方法存在以下几点不足：1、通过分段迭代构造出的路径不能避免“绕圈”现象；2、不能保证生成的路径最优，尤其当有一路径点位于agv前方且距离很近时，却不会以该点为目标点生成插入路径；3、以距离最短作为最优路径选择标准不能令agv优先选择平直的路径；4、需要大量迭代运算。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种逻辑简单、算法高效，令agv快速平稳地回到预设路径上的基于hermite插值路径生成方法、插入路径生成方法及存储装置。

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

基于hermite插值路径生成方法，涉及agv偏离地图路径所在的实际位置点p0，坐标位置为[x0,y0,α]；以及选取在地图路径中的目标插入点p1，坐标位置为[x1,y1,β]；所述路径生成方法包括以下步骤：

a.以p0为原点建立直角坐标系x-y，则p1点在该坐标系下的坐标为[δx,δy,β]，其中δx＝x1-x0，δy＝y1-y0；设v0、v1分别为p0、p1点的单位切矢，其中α、β分别为p0、p1点的方位角，θ、φ、γ分别为与v0之间的夹角、与v1之间的夹角、的方位角，建立以p0和p1为两端点的三次几何hermite插值曲线，设m、n分别为插值曲线在p0、p1点处的切矢模长；

b.根据和代入目标标插入点p1的坐标位置，得出假设切矢模长m0和n0；以及，根据和代入目标插入点p1的坐标位置和假设切矢模长m0和n0，得出r0和r1，其中，r0和r1为生成的曲线在p0与p1两端点处的曲率半径。

c.比较r0和r1是否满足端点曲率半径约束条件，即|r0|≥rs，|r1|≥rs；其中rs为允许最小转弯半径，若r0和r1满足允许最小转弯半径rs，则假设切矢模长m0和n0为实际切矢模长m1和n1；

否则，根据得出实际切矢模长m1和n1，并且，比较当前位置点p0与目标点p1的坐标位置是否满足切向条件：若满足切向条件，则可得出三次几何hermite插值曲线路径：

与现有技术相比，本发明的基于hermite插值路径生成方法，可使得当agv的启动位置不在地图上，或偏离地图路径时，从没有路径的区域自动生成一条最优路径，使agv尽可能沿初始方向快速、平稳地回到预设路径上，避免大量迭代运算；算法高效、逻辑简单。

进一步的，还包括：d.比较当前位置点p0与目标点p1的坐标位置是否满足几何光顺条件，即若p0和p1同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何hermite插值曲线路径为一级优先路径，若p0和p1仅满足切向条件、几何光顺条件，则判定三次几何hermite插值曲线路径为二级优先路径，若p0和p1仅满足切向条件，则判定三次几何hermite插值曲线路径为三级优先路径；通过这样判别当前选择的目标点p1生成的路径平直度进行等级划分，便于用户根据使用需求是否需要变更目标点p1，使用方便。

本发明的另一目的在于提供应用基于hermite插值路径生成方法的插入路径生成方法，包括以下步骤：

a.在agv的导航装置内建立路径地图，所述路径地图包括若干路径点，以及由路径点拟合得出的基础路径曲线。

b.agv偏离预设行驶路径，agv的定位模块确定agv当前的位置，以当前位置为圆心建立建立半径为rp的搜索圆，并遍历检索出所有位于搜索半径范围内的路径点，使所述路径点成为候选的目标插入点。

c.分别对当前位置点与各候选的目标插入点生成的路径进行路径等级判定，若能生成一级优先路径则归属为一级候选点，若能生成二级优先路径则归属为二级候选点，若能生成三级优先路径则归属为三级候选点。

d.若存在一级候选点，则agv以一级候选点作为目标插入点生成最优插入路径；若不存在一级候选点；则以二级候选点作为目标插入点生成最优插入路径；若不存在一级候选点和二级候选点，则以三级候选点作为目标插入点生成最优插入路径。

应用本发明的基于hermite插值路径生成方法，通过对路径地图中各候选的目标插入点进行等级分类，使agv优先选择平直的路线，使agv沿生成的插入路径运动到地图预设路径点后的朝向与起始时agv的车头朝向基本一致，避免出现不必要的“转弯”或“掉头”现象。

进一步的，若存在多个一级候选点时，通过和计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径。

进一步的，若存在多个二级候选点时，通过和计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径。

进一步的，若存在多个三级候选点时，通过和计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

通过这样设置，当agv的启动位置不在地图上时，操作者将agv移动到某一区域停下时，agv的车头朝向通常代表了操作者意图使agv继续运行的方向，以应变能最小为选择最优插入路径的标准能尽可能使agv插入成功后继续按操作者的意图方向运行，提高运行效率。而且agv总是会优先选择它前面的与其当前朝向基本一致，且距离最近的那个路径点作为目标插入点，大大提高了将agv从偏离位置回到地图路径上的速度和效率。

一种存储装置，该存储装置存储多条执行指令，当agv偏离地图路径时所在的实际位置点p0，坐标位置为[x0,y0,α]，处理模块选取在地图路径中的目标插入点p1，坐标位置为[x1,y1,β]；所述执行指令用于中控中心加载并执行以下操作：

a.以p0为原点建立直角坐标系x-y，则p1点在该坐标系下的坐标为[δx,δy,β]，其中δx＝x1-x0，δy＝y1-y0；设v0、v1分别为p0、p1点的单位切矢，其中α、β分别为p0、p1点的方位角，θ、φ、γ分别为与v0之间的夹角、与v1之间的夹角、的方位角，建立以p0和p1为两端点的三次几何hermite插值曲线，设m、n分别为插值曲线在p0、p1点处的切矢模长。

b.根据和代入目标标插入点p1的坐标位置，得出假设切矢模长m0和n0；以及，根据和代入目标插入点p1的坐标位置和假设切矢模长m0和n0，得出r0和r1，其中，r0和r1为生成的曲线在p0与p1两端点处的曲率半径。

c.比较r0和r1是否满足端点曲率半径约束条件，即|r0|≥rs，|r1|≥rs；其中rs为允许最小转弯半径，若r0和r1满足允许最小转弯半径rs，则假设切矢模长m0和n0为实际切矢模长m1和n1；否则，根据得出实际切矢模长m1和n1，并且，比较当前位置点p0与目标点p1的坐标位置是否满足切向条件：若满足切向条件，则可得出三次几何hermite插值曲线路径：

d.比较当前位置点p0与目标点p1的坐标位置是否满足几何光顺条件，即若p0和p1同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何hermite插值曲线路径为一级优先路径，若p0和p1仅满足切向条件、几何光顺条件，则判定三次几何hermite插值曲线路径为二级优先路径，若p0和p1仅满足切向条件，则判定三次几何hermite插值曲线路径为三级优先路径。

本发明的存储装置，可使得当agv的启动位置不在地图上，或偏离地图路径时，从没有路径的区域自动生成一条最优路径，使agv尽可能沿初始方向快速、平稳地回到预设路径上，避免大量迭代运算；算法高效、逻辑简单。

本发明的存储装置还包括如下多条执行指令：

a.在agv的导航装置内建立路径地图，所述路径地图包括若干路径点，以及由路径点拟合得出的基础路径曲线。

b.agv偏离预设行驶路径，agv的定位模块确定agv当前的位置，以当前位置为圆心建立建立半径为rp的搜索圆，并遍历检索出所有位于搜索半径范围内的路径点，使所述路径点成为候选的目标插入点。

c.分别对当前位置点与各候选的目标插入点生成的路径进行路径等级判定，若能生成一级优先路径则归属为一级候选点，若能生成二级优先路径则归属为二级候选点，若能生成三级优先路径则归属为三级候选点。

d.若存在一级候选点，则agv以一级候选点作为目标插入点生成最优插入路径；若不存在一级候选点；则以二级候选点作为目标插入点生成最优插入路径；若不存在一级候选点和二级候选点，则以三级候选点作为目标插入点生成最优插入路径。

e.基于和若存在多个一级候选点时，计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径；若不存在一级候选点，存在多个二级候选点时，计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径；若不存在一级候选点和个二级候选点，存在多个三级候选点时，计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

应用本发明的基于hermite插值路径生成方法的存储装置，通过对路径地图中各候选的目标插入点进行等级分类，使agv优先选择平直的路线，使agv沿生成的插入路径运动到地图预设路径点后的朝向与起始时agv的车头朝向基本一致，避免出现不必要的“转弯”或“掉头”现象。

附图说明

图1为实施例一中agv路径生成方法的流程图

图2为实施二中插入路径生成方法的流程图

图3为实施例一中x-y直角坐标系的示意图

图4为一级优先路径的示意图

图5为二级优先路径的示意图

图6为三级优先路径的示意图

图7为两个应变能不同的优先路径的示意图(实线路径应变能较小、虚线路径应变能较大)

具体实施方式

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

实施例一：

参见图1和图3至图7，本发明的基于hermite插值路径生成方法，涉及agv偏离地图路径所在的实际位置点p0，坐标位置为[x0,y0,α]；以及选取在地图路径中的目标插入点p1，坐标位置为[x1,y1,β]；所述路径生成方法包括以下步骤：

a.以p0为原点建立直角坐标系x-y，其中x-y轴方向与导航模块的全局坐标系相同，则p1点在该坐标系下的坐标为[δx,δy,β]，其中δx＝x1-x0，δy＝y1-y0；设v0、v1分别为p0、p1点的单位切矢，其中α、β分别为p0、p1点的方位角，θ、φ、γ分别为与v0之间的夹角、与v1之间的夹角、的方位角，即建立以p0和p1为两端点的三次几何hermite插值曲线，设m、n分别为插值曲线在p0、p1点处的切矢模长；

b.根据和代入目标标插入点p1的坐标位置，得出假设切矢模长m0和n0；以及，根据和代入目标插入点p1的坐标位置和假设切矢模长m0和n0，得出r0和r1，其中，r0和r1为生成的曲线在p0与p1两端点处的曲率半径。

c.比较r0和r1是否满足端点曲率半径约束条件，即|r0|≥rs，|r1|≥rs；其中rs为允许最小转弯半径，若r0和r1满足允许最小转弯半径rs，则假设切矢模长m0和n0为实际切矢模长m1和n1；

否则，根据得出实际切矢模长m1和n1，并且，比较当前位置点p0与目标点p1的坐标位置是否满足切向条件：若满足切向条件，则可得出三次几何hermite插值曲线路径：

与现有技术相比，本发明的基于hermite插值路径生成方法，可使得当agv的启动位置不在地图上，或偏离地图路径时，从没有路径的区域自动生成一条最优路径，使agv尽可能沿初始方向快速、平稳地回到预设路径上，避免大量迭代运算；算法高效、逻辑简单。

作为优选方案，还包括：d.比较当前位置点p0与目标点p1的坐标位置是否满足几何光顺条件，即若p0和p1同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何hermite插值曲线路径为一级优先路径，若p0和p1仅满足切向条件、几何光顺条件，则判定三次几何hermite插值曲线路径为二级优先路径，若p0和p1仅满足切向条件，则判定三次几何hermite插值曲线路径为三级优先路径。

本发明最优路径生成的问题是寻找一条这样的曲线，其起点为p0，终点为p1，且p0、p1点处的切矢分别与单位切矢v0、v1同向。针对这一问题，本发明采用三次几何hermite插值的方法构造满足上述要求的曲线。三次几何hermite插值曲线函数：

其中m、n分别为插值曲线在p0、p1点处的切矢模长。不同的m，n值可以构造出一段不同的hermite曲线，也即生成不同的路径。为此，需确定一个优化标准，以构造出在该标准下的最优几何路径。

另一方面，曲线曲率越小，则曲率半径越大，相应agv的转弯半径也越大。转弯半径越大越有利于agv的稳定运行，转弯半径过小则易造成agv脱轨甚至无法转弯。考虑到曲线的应变能可以一定程度上反应曲线的绝对曲率大小，因此，作为一种优选，本发明以曲线应变能最小作为优化标准。

此外，依据应变能最小准则生成的曲线不会出现交叉点，这意味着生成的路径不会使agv出现“绕圈”的现象。

包含曲率因素的三次hermite曲线的应变能能量函数为：

为使函数f取得最小值，分别令其对m，n求偏导数并取零值：

整理式(3)得如下线性方程组：

令：

解方程组(4)得：

由于m，n可能存在负值或零值导致生成的路径不正确，因此，为保证本发明方法的绝对有效性，需加上适当的限制条件以保证m，n均能取得正值。

为此，令式(6)中m>0且n>0，得到如下切向保持条件：

只要agv当前位置点p0与目标点p1满足式(7)的切向保持条件，即可基于式(6)所解得的m，n值生成一条满足要求的从p0点运动到p1点的路径。

进一步的，在切向保持条件基础上，定义以下几何光顺条件：

几何光顺条件比切向保持条件更严格，满足几何光顺条件必然满足切向保持条件，但满足切向保持条件则不一定满足几何光顺条件。

若位置点p0与目标点p1满足式(8)的几何光顺条件，则所生成的路径不会出现“折回”现象，这将使得agv在整个路径上行驶时，不会出现大幅度转向，方位角变化均匀且通常不超过90°。若位置点p0与目标点p1不满足几何光顺条件，此时切矢v0与v1的夹角通常大于90°，所生成的路径弯曲幅度大，agv行驶时呈现“掉头”趋势。

当p0与p1的位置不太理想，如切矢v0与v1的交点太靠近其中一个端点时，尽管此时满足切向保持条件或几何光顺条件，可以生成路径，但在路径两端点处的曲率半径可能过小，导致agv在起始点或终点附近出现车体大幅摆动。当曲率半径小于agv的理论最小转弯半径时，将造成agv脱轨。因此，本发明提出如下端点曲率半径约束条件：

|r0|≥rs,|r1|≥rs(9)

其中rs为允许最小转弯半径，r0，r1分别为生成的曲线在p0与p1两端点处的曲率半径。

设agv的理论最小转弯半径为rt，rt取决于agv的结构参数，且应大于零。对于理论最小转弯半径为零的agv，则可根据车型结构与应用环境设置一个较小的正值。允许最小转弯半径rs设为理论最小转弯半径rt的倍数，按下式定义：

rs＝krt,k≥1(10)

令：

根据参数曲线的曲率计算公式得到三次hermite插值曲线在端点p0与p1处的曲率半径r0，r1如下：

若位置点p0与目标点p1基于式(6)所解得的m、n值满足端点曲率半径约束条件，则可直接采用所求得的m、n值。若不满足，则对不满足曲率半径约束的一端或两端的切矢模长按下式进行调整：

采用调整后的m、n值可使生成的曲线重新满足端点曲率半径约束条件。

综合式(6)与式(13)，m、n的最终取值可表达如下：

当前位置点p0与目标点p1同时满足切向保持条件、几何光顺条件以及端点曲率半径约束条件，则可基于式(14)求得的m、n值生成一条一级优先路径。一级优先路径的切向角度变化幅度小且均匀，没有折点，保向性最佳，是一条理想的agv路径。

当前位置点p0与目标点p1满足切向保持条件、几何光顺条件，而不满足端点曲率半径约束条件，则可基于式(14)求得的m、n值生成一条二级优先路径。二级优先路径没有折点，保向性好，但在起始或终点处的曲率变化率可能较大，导致agv在起始或终点位置附近处的方位角变化幅度稍大，路径的理想程度低于一级。

当前位置点p0与目标点p1仅满足切向保持条件，则可基于式(14)求得的m、n值生成一条三级优先路径。三级优先路径的弯曲幅度较大，起始点p0与终点p1处的方向夹角一般大于90°，曲率均匀性稍差，agv沿路径行驶时呈现“调头”趋势，路径的理想程度低于二级。

当前位置点p0与目标点p1不满足切向保持条件，则不适合使用本方法生成路径，需要重选目标点p1。

实施例二：

参见图2和图4至图7，本实施例提供应用实施例一中基于hermite插值路径生成方法的插入路径生成方法，包括以下步骤：

a.在agv的导航装置内建立路径地图，所述路径地图包括若干路径点，以及由路径点拟合得出的基础路径曲线。

b.agv偏离预设行驶路径，agv的定位模块确定agv当前的位置，以当前位置为圆心建立建立半径为rp的搜索圆，并遍历检索出所有位于搜索半径范围内的路径点，使所述路径点成为候选的目标插入点。

c.分别对当前位置点与各候选的目标插入点生成的路径进行路径等级判定，若能生成一级优先路径则归属为一级候选点，若能生成二级优先路径则归属为二级候选点，若能生成三级优先路径则归属为三级候选点。

d.若存在一级候选点，则agv以一级候选点作为目标插入点生成最优插入路径；

若不存在一级候选点；则以二级候选点作为目标插入点生成最优插入路径；

若不存在一级候选点和二级候选点，则以三级候选点作为目标插入点生成最优插入路径。

应用本发明的基于hermite插值路径生成方法，通过对路径地图中各候选的目标插入点进行等级分类，使agv优先选择平直的路线，使agv沿生成的插入路径运动到地图预设路径点后的朝向与起始时agv的车头朝向基本一致，避免出现不必要的“转弯”或“掉头”现象

作为优选方案，若存在多个一级候选点时，通过和计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径。

若存在多个二级候选点时，通过和计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径。

若存在多个三级候选点时，通过和计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

通过这样设置，当agv的启动位置不在地图上时，操作者将agv移动到某一区域停下时，agv的车头朝向通常代表了操作者意图使agv继续运行的方向，以应变能最小为选择最优插入路径的标准能尽可能使agv插入成功后继续按操作者的意图方向运行，提高运行效率。而且agv总是会优先选择它前面的与其当前朝向基本一致，且距离最近的那个路径点作为目标插入点，大大提高了将agv从偏离位置回到地图路径上的速度和效率。

在一些特别情况下，比如搜索范围内的所有候选点均位于agv的后面，agv的前部区域没有路径点，此时所有候选点均不满足切向保持条件，本发明中的路径生成方法受到限制，造成三种类型的候选点均不存在。针对这一极端情况，可由人工改变agv的朝向，即可重新搜索生成插入路径。

实施例三：

本发明的存储装置，该存储装置存储多条执行指令，当agv偏离地图路径时所在的实际位置点p0，坐标位置为[x0,y0,α]，处理模块选取在地图路径中的目标插入点p1，坐标位置为[x1,y1,β]；所述执行指令用于中控中心加载并执行以下操作：

a.以p0为原点建立直角坐标系x-y，则p1点在该坐标系下的坐标为[δx,δy,β]，其中δx＝x1-x0，δy＝y1-y0；设v0、v1分别为p0、p1点的单位切矢，其中α、β分别为p0、p1点的方位角，θ、φ、γ分别为与v0之间的夹角、与v1之间的夹角、的方位角，建立以p0和p1为两端点的三次几何hermite插值曲线，设m、n分别为插值曲线在p0、p1点处的切矢模长。

b.根据和代入目标标插入点p1的坐标位置，得出假设切矢模长m0和n0；以及，根据和代入目标插入点p1的坐标位置和假设切矢模长m0和n0，得出r0和r1，其中，r0和r1为生成的曲线在p0与p1两端点处的曲率半径。

c.比较r0和r1是否满足端点曲率半径约束条件，即|r0|≥rs，|r1|≥rs；其中rs为允许最小转弯半径，若r0和r1满足允许最小转弯半径rs，则假设切矢模长m0和n0为实际切矢模长m1和n1；否则，根据得出实际切矢模长m1和n1，并且，比较当前位置点p0与目标点p1的坐标位置是否满足切向条件：若满足切向条件，则可得出三次几何hermite插值曲线路径：

d.比较当前位置点p0与目标点p1的坐标位置是否满足几何光顺条件，即若p0和p1同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何hermite插值曲线路径为一级优先路径，若p0和p1仅满足切向条件、几何光顺条件，则判定三次几何hermite插值曲线路径为二级优先路径，若p0和p1仅满足切向条件，则判定三次几何hermite插值曲线路径为三级优先路径。

本发明的存储装置，可使得当agv的启动位置不在地图上，或偏离地图路径时，从没有路径的区域自动生成一条最优路径，使agv尽可能沿初始方向快速、平稳地回到预设路径上，避免大量迭代运算；算法高效、逻辑简单。

实施例四：

本实施例的的存储装置，该存储装置包括实施例三中所述的执行指令和如下多条执行指令：

a.在agv的导航装置内建立路径地图，所述路径地图包括若干路径点，以及由路径点拟合得出的基础路径曲线。

b.agv偏离预设行驶路径，agv的定位模块确定agv当前的位置，以当前位置为圆心建立建立半径为rp的搜索圆，并遍历检索出所有位于搜索半径范围内的路径点，使所述路径点成为候选的目标插入点。

c.分别对当前位置点与各候选的目标插入点生成的路径进行路径等级判定，若能生成一级优先路径则归属为一级候选点，若能生成二级优先路径则归属为二级候选点，若能生成三级优先路径则归属为三级候选点。

d.若存在一级候选点，则agv以一级候选点作为目标插入点生成最优插入路径；若不存在一级候选点；则以二级候选点作为目标插入点生成最优插入路径；若不存在一级候选点和二级候选点，则以三级候选点作为目标插入点生成最优插入路径。

e.基于和若存在多个一级候选点时，计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径；若不存在一级候选点，存在多个二级候选点时，计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径；若不存在一级候选点和个二级候选点，存在多个三级候选点时，计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

应用本发明的基于hermite插值路径生成方法的存储装置，通过对路径地图中各候选的目标插入点进行等级分类，使agv优先选择平直的路线，使agv沿生成的插入路径运动到地图预设路径点后的朝向与起始时agv的车头朝向基本一致，避免出现不必要的“转弯”或“掉头”现象。

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