CN109596136B - 基于Hermite插值路径生成方法、插入路径生成方法及存储装置 - Google Patents

Abstract

本发明提供了一种基于Hermite插值的AGV路径生成方法、插入路径生成方法及存储装置.该路径生成方法使得当AGV的启动位置不在地图上，或偏离地图路径时，从没有路径的区域自动生成一条最优路径。

Description

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

技术领域

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

背景技术

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

发明内容

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

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

基于Hermite插值路径生成方法，涉及AGV偏离地图路径所在的实际位置点P₀，坐标位置为[x₀,y₀,α]；以及选取在地图路径中的目标插入点P₁，坐标位置为[x₁,y₁,β]；所述路径生成方法包括以下步骤：

a.以P₀为原点建立直角坐标系X-Y，则P₁点在该坐标系下的坐标为[Δx,Δy,β]，其中Δx＝x₁-x₀，Δy＝y₁-y₀；设V₀、V₁分别为P₀、P₁点的单位切矢，其中α、β分别为P₀、P₁点的方位角，θ、

γ分别为

与V₀之间的夹角、

与V₁之间的夹角、

的方位角，建立以P₀和P₁为两端点的三次几何Hermite插值曲线，设m₀、n₀和m₁、n₁分别为插值曲线在P₀、P₁点处的假设切矢模长和实际切矢模长。

b.根据

和

代入目标插入点P₁的坐标位置，得出假设切矢模长m₀和n₀；以及，根据

和

代入目标插入点P₁的坐标位置和假设切矢模长m₀和n₀，得出R₀和R₁，其中，R₀和R₁为生成的曲线在P₀与P₁两端点处的曲率半径。

c.比较R₀和R₁是否满足端点曲率半径约束条件，即|R₀|≥R_s，|R₁|≥R_s；其中R_s为允许最小转弯半径，若R₀和R₁满足允许最小转弯半径R_s，则假设切矢模长m₀和n₀为实际切矢模长m₁和n₁；

否则，根据

得出实际切矢模长m₁和n₁，并且，比较当前位置点P₀与目标点P₁的坐标位置是否满足切向条件：

若满足切向条件，则得出三次几何Hermite插值曲线路径：

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

进一步的，还包括：d.比较当前位置点P₀与目标点P₁的坐标位置是否满足几何光顺条件，即

若P₀和P₁同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何Hermite插值曲线路径为一级优先路径，若P₀和P₁仅满足切向条件、几何光顺条件，则判定三次几何Hermite插值曲线路径为二级优先路径，若P₀和P₁仅满足切向条件，则判定三次几何Hermite插值曲线路径为三级优先路径；通过这样判别当前选择的目标点P₁生成的路径平直度进行等级划分，便于用户根据使用需求是否需要变更目标点P₁，使用方便。

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

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

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

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

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

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

进一步的，若存在多个一级候选点时，通过

和

计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径。

进一步的，若存在多个二级候选点时，通过

和

计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径。

进一步的，若存在多个三级候选点时，通过

和

计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

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

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

a.以P₀为原点建立直角坐标系X-Y，则P₁点在该坐标系下的坐标为[Δx,Δy,β]，其中Δx＝x₁-x₀，Δy＝y₁-y₀；设V₀、V₁分别为P₀、P₁点的单位切矢，其中α、β分别为P₀、P₁点的方位角，θ、

γ分别为

与V₀之间的夹角、

与V₁之间的夹角、

的方位角，建立以P₀和P₁为两端点的三次几何Hermite插值曲线，设m₀、n₀和m₁、n₁分别为插值曲线在P₀、P₁点处的假设切矢模长和实际切矢模长。

b.根据

和

代入目标插入点P₁的坐标位置，得出假设切矢模长m₀和n₀；以及，根据

和

代入目标插入点P₁的坐标位置和假设切矢模长m₀和n₀，得出R₀和R₁，其中，R₀和R₁为生成的曲线在P₀与P₁两端点处的曲率半径。

c.比较R₀和R₁是否满足端点曲率半径约束条件，即|R₀|≥R_s，|R₁|≥R_s；其中R_s为允许最小转弯半径，若R₀和R₁满足允许最小转弯半径R_s，则假设切矢模长m₀和n₀为实际切矢模长m₁和n₁；否则，根据

得出实际切矢模长m₁和n₁，并且，比较当前位置点P₀与目标点P₁的坐标位置是否满足切向条件：

若满足切向条件，则得出三次几何Hermite插值曲线路径：

d.比较当前位置点P₀与目标点P₁的坐标位置是否满足几何光顺条件，即

若P₀和P₁同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何Hermite插值曲线路径为一级优先路径，若P₀和P₁仅满足切向条件、几何光顺条件，则判定三次几何Hermite插值曲线路径为二级优先路径，若P₀和P₁仅满足切向条件，则判定三次几何Hermite插值曲线路径为三级优先路径。

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

本发明的存储装置包括如下多条用于中控中心加载并执行的执行指令：

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

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

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

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

e.基于

和

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

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

附图说明

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

图2为实施例一中X-Y直角坐标系的示意图

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

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

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

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

具体实施方式

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

实施例一：

参见图1和图4至图6，本发明的基于Hermite插值路径生成方法，涉及AGV偏离地图路径所在的实际位置点P₀，坐标位置为[x₀,y₀,α]；以及选取在地图路径中的目标插入点P₁，坐标位置为[x₁,y₁,β]；所述路径生成方法包括以下步骤：

a.以P₀为原点建立直角坐标系X-Y，其中X-Y轴方向与导航模块的全局坐标系相同，则P₁点在该坐标系下的坐标为[Δx,Δy,β]，其中Δx＝x₁-x₀，Δy＝y₁-y₀；设V₀、V₁分别为P₀、P₁点的单位切矢，其中α、β分别为P₀、P₁点的方位角，θ、

γ分别为

与V₀之间的夹角、

与V₁之间的夹角、

的方位角，即

建立以P₀和P₁为两端点的三次几何Hermite插值曲线，设m₀、n₀和m₁、n₁分别为插值曲线在P₀、P₁点处的假设切矢模长和实际切矢模长。

b.根据

和

代入目标插入点P₁的坐标位置，得出假设切矢模长m₀和n₀；以及，根据

和

代入目标插入点P₁的坐标位置和假设切矢模长m₀和n₀，得出R₀和R₁，其中，R₀和R₁为生成的曲线在P₀与P₁两端点处的曲率半径。

c.比较R₀和R₁是否满足端点曲率半径约束条件，即|R₀|≥R_s，|R₁|≥R_s；其中R_s为允许最小转弯半径，若R₀和R₁满足允许最小转弯半径R_s，则假设切矢模长m₀和n₀为实际切矢模长m₁和n₁；

否则，根据

得出实际切矢模长m₁和n₁，并且，比较当前位置点P₀与目标点P₁的坐标位置是否满足切向条件：

若满足切向条件，则得出三次几何Hermite插值曲线路径：

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

作为优选方案，还包括：d.比较当前位置点P₀与目标点P₁的坐标位置是否满足几何光顺条件，即

若P₀和P₁同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何Hermite插值曲线路径为一级优先路径，若P₀和P₁仅满足切向条件、几何光顺条件，则判定三次几何Hermite插值曲线路径为二级优先路径，若P₀和P₁仅满足切向条件，则判定三次几何Hermite插值曲线路径为三级优先路径。

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

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

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

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

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

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

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

令：

解方程组(4)得：

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

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

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

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

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

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

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

|R₀|≥R_s,|R₁|≥R_s (9)

其中R_s为允许最小转弯半径，R₀，R₁分别为生成的曲线在P₀与P₁两端点处的曲率半径。

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

R_s＝kR_t,k≥1 (10)

令：

根据参数曲线的曲率计算公式得到三次Hermite插值曲线在端点P₀与P₁处的曲率半径R₀，R₁如下：

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

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

综合式(6)与式(13)，实际切矢模长m₁、n₁的最终取值可表达如下：

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

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

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

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

实施例二：

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

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

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

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

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

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

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

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

作为优选方案，若存在多个一级候选点时，通过

和

计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径。

若存在多个二级候选点时，通过

和

计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径。

若存在多个三级候选点时，通过

和

计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

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

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

实施例三：

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

a.以P₀为原点建立直角坐标系X-Y，则P₁点在该坐标系下的坐标为[Δx,Δy,β]，其中Δx＝x₁-x₀，Δy＝y₁-y₀；设V₀、V₁分别为P₀、P₁点的单位切矢，其中α、β分别为P₀、P₁点的方位角，θ、

γ分别为

与V₀之间的夹角、

与V₁之间的夹角、

的方位角，建立以P₀和P₁为两端点的三次几何Hermite插值曲线，设m₀、n₀和m₁、n₁分别为插值曲线在P₀、P₁点处的假设切矢模长和实际切矢模长。

b.根据

和

代入目标插入点P₁的坐标位置，得出假设切矢模长m₀和n₀；以及，根据

和

代入目标插入点P₁的坐标位置和假设切矢模长m₀和n₀，得出R₀和R₁，其中，R₀和R₁为生成的曲线在P₀与P₁两端点处的曲率半径。

c.比较R₀和R₁是否满足端点曲率半径约束条件，即|R₀|≥R_s，|R₁|≥R_s；其中R_s为允许最小转弯半径，若R₀和R₁满足允许最小转弯半径R_s，则假设切矢模长m₀和n₀为实际切矢模长m₁和n₁；否则，根据

得出实际切矢模长m₁和n₁，并且，比较当前位置点P₀与目标点P₁的坐标位置是否满足切向条件：

若满足切向条件，则得出三次几何Hermite插值曲线路径：

d.比较当前位置点P₀与目标点P₁的坐标位置是否满足几何光顺条件，即

若P₀和P₁同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何Hermite插值曲线路径为一级优先路径，若P₀和P₁仅满足切向条件、几何光顺条件，则判定三次几何Hermite插值曲线路径为二级优先路径，若P₀和P₁仅满足切向条件，则判定三次几何Hermite插值曲线路径为三级优先路径。

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

实施例四：

本实施例的的存储装置，该存储装置包括实施例三中所述的存储装置的多条执行指令，该存储装置用于中控中心加载并执行如下操作的多条执行指令：

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

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

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

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

e.基于

和

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

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

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

Claims (8)

1.基于Hermite插值路径生成方法，涉及AGV偏离地图路径所在的实际位置点P₀，坐标位置为[x₀,y₀,α]；以及选取在地图路径中的目标插入点P₁，坐标位置为[x₁,y₁,β]；其特征在于：所述路径生成方法包括以下步骤：

a.以P₀为原点建立直角坐标系X-Y，则P₁点在该坐标系下的坐标为[Δx,Δy,β]，其中Δx＝x₁-x₀，Δy＝y₁-y₀；设V₀、V₁分别为P₀、P₁点的单位切矢，其中α、β分别为P₀、P₁点的方位角，θ、

γ分别为

与V₀之间的夹角、

与V₁之间的夹角、

的方位角，建立以P₀和P₁为两端点的三次几何Hermite插值曲线，设m₀、n₀和m₁、n₁分别为插值曲线在P₀、P₁点处的假设切矢模长和实际切矢模长；

b.根据

和

代入目标插入点P₁的坐标位置，得出假设切矢模长m₀和n₀；

以及，根据

和

代入目标插入点P₁的坐标位置和假设切矢模长m₀和n₀，得出R₀和R₁，其中，R₀和R₁为生成的曲线在P₀与P₁两端点处的曲率半径；

c.比较R₀和R₁是否满足端点曲率半径约束条件，即|R₀|≥R_s，|R₁|≥R_s；其中R_s为允许最小转弯半径，若R₀和R₁满足允许最小转弯半径R_s，则假设切矢模长m₀和n₀为实际切矢模长m₁和n₁；

否则，根据

得出实际切矢模长m₁和n₁，并且，比较当前位置点P₀与目标点P₁的坐标位置是否满足切向条件：

若满足切向条件，则得出三次几何Hermite插值曲线路径：

2.根据权利要求1所述的基于Hermite插值路径生成方法，其特征在于：还包括：

d.比较当前位置点P₀与目标点P₁的坐标位置是否满足几何光顺条件，即

若P₀和P₁同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何Hermite插值曲线路径为一级优先路径，若P₀和P₁仅满足切向条件、几何光顺条件，则判定三次几何Hermite插值曲线路径为二级优先路径，若P₀和P₁仅满足切向条件，则判定三次几何Hermite插值曲线路径为三级优先路径。

3.一种应用权利要求2所述的基于Hermite插值路径生成方法的插入路径生成方法，其特征在于：包括以下步骤：

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

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

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

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

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

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

4.根据权利要求3所述的插入路径生成方法，其特征在于：若存在多个一级候选点时，通过

和

计算当前位置点与各一级候选点生成一级路径的应变能，选择应变能最小的一级路径作为优选路径。

5.根据权利要求3所述的插入路径生成方法，其特征在于：若存在多个二级候选点时，通过

和

计算当前位置点与各二级候选点生成二级路径的应变能，选择应变能最小的二级路径作为优选路径。

6.根据权利要求3所述的插入路径生成方法，其特征在于：若存在多个三级候选点时，通过

和

计算当前位置点与各三级候选点生成三级路径的应变能，选择应变能最小的三级路径作为优选路径。

7.一种存储装置，该存储装置存储多条执行指令，其特征在于：当AGV偏离地图路径时所在的实际位置点P₀，坐标位置为[x₀,y₀,α]，处理模块选取在地图路径中的目标插入点P₁，坐标位置为[x₁,y₁,β]；所述执行指令用于中控中心加载并执行以下操作：

a.以P₀为原点建立直角坐标系X-Y，则P₁点在该坐标系下的坐标为[Δx,Δy,β]，其中Δx＝x₁-x₀，Δy＝y₁-y₀；设V₀、V₁分别为P₀、P₁点的单位切矢，其中α、β分别为P₀、P₁点的方位角，θ、

γ分别为

与V₀之间的夹角、

与V₁之间的夹角、

的方位角，建立以P₀和P₁为两端点的三次几何Hermite插值曲线，设m₀、n₀和m₁、n₁分别为插值曲线在P₀、P₁点处的假设切矢模长和实际切矢模长；

b.根据

和

代入目标插入点P₁的坐标位置，得出假设切矢模长m₀和n₀；

以及，根据

和

代入目标插入点P₁的坐标位置和假设切矢模长m₀和n₀，得出R₀和R₁，其中，R₀和R₁为生成的曲线在P₀与P₁两端点处的曲率半径；

c.比较R₀和R₁是否满足端点曲率半径约束条件，即|R₀|≥R_s，|R₁|≥R_s；其中R_s为允许最小转弯半径，若R₀和R₁满足允许最小转弯半径R_s，则假设切矢模长m₀和n₀为实际切矢模长m₁和n₁；

否则，根据

得出实际切矢模长m₁和n₁，并且，比较当前位置点P₀与目标点P₁的坐标位置是否满足切向条件：

若满足切向条件，则得出三次几何Hermite插值曲线路径：

d.比较当前位置点P₀与目标点P₁的坐标位置是否满足几何光顺条件，即

若P₀和P₁同时满足切向条件、几何光顺条件、端点曲率半径约束条件，则判定三次几何Hermite插值曲线路径为一级优先路径，若P₀和P₁仅满足切向条件、几何光顺条件，则判定三次几何Hermite插值曲线路径为二级优先路径，若P₀和P₁仅满足切向条件，则判定三次几何Hermite插值曲线路径为三级优先路径。

8.一种存储装置，其特征在于：该存储装置包括权利要求7所述的存储装置的多条执行指令,该存储装置用于中控中心加载并执行如下操作的多条执行指令：

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

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

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

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

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

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

e.基于

和

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

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

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

Images