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

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

Info

Publication number
CN109596136A
CN109596136A CN201811569964.2A CN201811569964A CN109596136A CN 109596136 A CN109596136 A CN 109596136A CN 201811569964 A CN201811569964 A CN 201811569964A CN 109596136 A CN109596136 A CN 109596136A
Authority
CN
China
Prior art keywords
path
point
level
candidate
agv
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201811569964.2A
Other languages
English (en)
Other versions
CN109596136B (zh
Inventor
陶茂林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong Jaten Robot and Automation Co Ltd
Original Assignee
Guangdong Jaten Robot and Automation Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangdong Jaten Robot and Automation Co Ltd filed Critical Guangdong Jaten Robot and Automation Co Ltd
Priority to CN201811569964.2A priority Critical patent/CN109596136B/zh
Publication of CN109596136A publication Critical patent/CN109596136A/zh
Application granted granted Critical
Publication of CN109596136B publication Critical patent/CN109596136B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34Route searching; Route guidance
    • G01C21/3446Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

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偏离地图路径所在的实际位置点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的车头朝向基本一致,避免出现不必要的“转弯” 或“掉头”现象。
根据上述说明书的揭示和教导,本发明所属领域的技术人员还可以对上述实施方式进 行变更和修改。因此,本发明并不局限于上面揭示和描述的具体实施方式,对本发明的一 些修改和变更也应当落入本发明的权利要求的保护范围内。此外,尽管本说明书中使用了 一些特定的术语,但这些术语只是为了方便说明,并不对本发明构成任何限制。

Claims (8)

1.基于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插值曲线路径:
2.根据权利要求1所述的AGV路径生成方法,其特征在于:还包括:
d.比较当前位置点P0与目标点P1的坐标位置是否满足几何光顺条件,即若P0和P1同时满足切向条件、几何光顺条件、端点曲率半径约束条件,则判定三次几何Hermite插值曲线路径为一级优先路径,若P0和P1仅满足切向条件、几何光顺条件,则判定三次几何Hermite插值曲线路径为二级优先路径,若P0和P1仅满足切向条件,则判定三次几何Hermite插值曲线路径为三级优先路径。
3.一种应用权利要求2所述的AGV路径生成方法的插入路径生成方法,其特征在于:包括以下步骤:
a.在AGV的导航装置内建立路径地图,所述路径地图包括若干路径点,以及由路径点拟合得出的基础路径曲线;
b.AGV偏离预设行驶路径,AGV的定位模块确定AGV当前的位置,以当前位置为圆心建立半径为Rp的搜索圆,并遍历检索出所有位于搜索半径范围内的路径点,使所述路径点成为候选的目标插入点;
c.分别对当前位置点与各候选的目标插入点生成的路径进行路径等级判定,若能生成一级优先路径则归属为一级候选点,若能生成二级优先路径则归属为二级候选点,若能生成三级优先路径则归属为三级候选点;
d.若存在一级候选点,则AGV以一级候选点作为目标插入点生成最优插入路径;
若不存在一级候选点;则以二级候选点作为目标插入点生成最优插入路径;
若不存在一级候选点和二级候选点,则以三级候选点作为目标插入点生成最优插入路径。
4.根据权利要求3所述的插入路径生成方法,其特征在于:若存在多个一级候选点时,通过计算当前位置点与各一级候选点生成一级路径的应变能,选择应变能最小的一级路径作为优选路径。
5.根据权利要求3所述的插入路径生成方法,其特征在于:若存在多个二级候选点时,通过计算当前位置点与各二级候选点生成二级路径的应变能,选择应变能最小的二级路径作为优选路径。
6.根据权利要求3所述的插入路径生成方法,其特征在于:若存在多个三级候选点时,通过计算当前位置点与各三级候选点生成三级路径的应变能,选择应变能最小的三级路径作为优选路径。
7.一种存储装置,该存储装置存储多条执行指令,其特征在于:当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插值曲线路径为三级优先路径。
8.一种存储装置,其特征在于:该存储装置包括权利要求7所述的执行指令和如下多条执行指令:
a.在AGV的导航装置内建立路径地图,所述路径地图包括若干路径点,以及由路径点拟合得出的基础路径曲线;
b.AGV偏离预设行驶路径,AGV的定位模块确定AGV当前的位置,以当前位置为圆心建立建立半径为Rp的搜索圆,并遍历检索出所有位于搜索半径范围内的路径点,使所述路径点成为候选的目标插入点;
c.分别对当前位置点与各候选的目标插入点生成的路径进行路径等级判定,若能生成一级优先路径则归属为一级候选点,若能生成二级优先路径则归属为二级候选点,若能生成三级优先路径则归属为三级候选点;
d.若存在一级候选点,则AGV以一级候选点作为目标插入点生成最优插入路径;
若不存在一级候选点;则以二级候选点作为目标插入点生成最优插入路径;
若不存在一级候选点和二级候选点,则以三级候选点作为目标插入点生成最优插入路径;
e.基于若存在多个一级候选点时,计算当前位置点与各一级候选点生成一级路径的应变能,选择应变能最小的一级路径作为优选路径;
若不存在一级候选点,存在多个二级候选点时,计算当前位置点与各二级候选点生成二级路径的应变能,选择应变能最小的二级路径作为优选路径;
若不存在一级候选点和个二级候选点,存在多个三级候选点时,计算当前位置点与各三级候选点生成三级路径的应变能,选择应变能最小的三级路径作为优选路径。
CN201811569964.2A 2018-12-21 2018-12-21 基于Hermite插值路径生成方法、插入路径生成方法及存储装置 Active CN109596136B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811569964.2A CN109596136B (zh) 2018-12-21 2018-12-21 基于Hermite插值路径生成方法、插入路径生成方法及存储装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811569964.2A CN109596136B (zh) 2018-12-21 2018-12-21 基于Hermite插值路径生成方法、插入路径生成方法及存储装置

Publications (2)

Publication Number Publication Date
CN109596136A true CN109596136A (zh) 2019-04-09
CN109596136B CN109596136B (zh) 2020-07-07

Family

ID=65963246

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811569964.2A Active CN109596136B (zh) 2018-12-21 2018-12-21 基于Hermite插值路径生成方法、插入路径生成方法及存储装置

Country Status (1)

Country Link
CN (1) CN109596136B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111538334A (zh) * 2020-05-13 2020-08-14 合肥卓科智能技术有限公司 一种agv小车的路径跟踪系统
JP6863497B1 (ja) * 2020-03-11 2021-04-21 オムロン株式会社 コントローラシステムおよびその制御方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020065587A1 (en) * 2000-11-30 2002-05-30 Syrjaerinne Paula Method and apparatus for satellite orbit interpolation using piecewise hermite interpolating polynomials
CN103732007A (zh) * 2014-01-22 2014-04-16 哈尔滨工业大学 基于四轴联动贴片机的贴装头运动路径优化方法
CN103970139A (zh) * 2014-05-09 2014-08-06 上海交通大学 一种机器人连续点位运动规划方法及其运动控制器
CN104007705A (zh) * 2014-05-05 2014-08-27 上海交通大学 小线段路径压缩平滑的前瞻插补系统
US20140247276A1 (en) * 2013-03-01 2014-09-04 Microsoft Corporation Point Relocation for Digital Ink Curve Moderation
CN107608372A (zh) * 2017-08-14 2018-01-19 广西师范大学 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20020065587A1 (en) * 2000-11-30 2002-05-30 Syrjaerinne Paula Method and apparatus for satellite orbit interpolation using piecewise hermite interpolating polynomials
US20140247276A1 (en) * 2013-03-01 2014-09-04 Microsoft Corporation Point Relocation for Digital Ink Curve Moderation
CN103732007A (zh) * 2014-01-22 2014-04-16 哈尔滨工业大学 基于四轴联动贴片机的贴装头运动路径优化方法
CN104007705A (zh) * 2014-05-05 2014-08-27 上海交通大学 小线段路径压缩平滑的前瞻插补系统
CN103970139A (zh) * 2014-05-09 2014-08-06 上海交通大学 一种机器人连续点位运动规划方法及其运动控制器
CN107608372A (zh) * 2017-08-14 2018-01-19 广西师范大学 一种基于改进rrt算法与改进ph曲线相结合的多无人机协同航迹规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
KAWTHER OSMAN等: "Combined road following control and automatic lane keeping for automated guided vehicles", 《2016 14TH INTERNATIONAL CONFERENCE ON CONTROL, AUTOMATION, ROBOTICS AND VISION (ICARCV)》 *
彭辉等: "Hermite三次样条插值的车型机器人路径规划研究", 《计算机工程与应用》 *
朱从民: "AGV AGENT控制系统研究", 《中国博士学位论文全文数据库 信息科技辑》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6863497B1 (ja) * 2020-03-11 2021-04-21 オムロン株式会社 コントローラシステムおよびその制御方法
JP2021144437A (ja) * 2020-03-11 2021-09-24 オムロン株式会社 コントローラシステムおよびその制御方法
CN111538334A (zh) * 2020-05-13 2020-08-14 合肥卓科智能技术有限公司 一种agv小车的路径跟踪系统
CN111538334B (zh) * 2020-05-13 2023-08-29 合肥卓科智能技术有限公司 一种agv小车的路径跟踪系统

Also Published As

Publication number Publication date
CN109596136B (zh) 2020-07-07

Similar Documents

Publication Publication Date Title
CN113204236B (zh) 一种智能体路径跟踪控制方法
US20200149906A1 (en) Path planning method, system and device for autonomous driving
CN106950955B (zh) 基于改进los引导算法的船舶航迹控制方法
CN110617818A (zh) 一种无人机航迹生成方法
Li et al. Path planning based on combinaion of improved A-STAR algorithm and DWA algorithm
CN108981716B (zh) 一种适用于内陆和近海无人船的路径规划方法
CN109976321A (zh) 一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统
CN109388140B (zh) 一种改进的用于地面车辆路径跟踪的纯追踪控制方法
CN111007862B (zh) 一种多agv协同工作的路径规划方法
CN109685237B (zh) 一种基于Dubins路径和分支限界的无人机航迹实时规划方法
CN110109451B (zh) 一种考虑路径曲率的新型几何学路径跟踪算法
CN108876065B (zh) 一种基于轨迹单元的无人艇运动规划方法
CN109596136A (zh) 基于Hermite插值路径生成方法、插入路径生成方法及存储装置
CN112947494B (zh) 一种基于模糊pid的船舶自动靠泊控制方法
CN104615889B (zh) 基于回旋曲线追随的智能车辆路径跟踪方法及系统
Dolinskaya et al. Time-optimal trajectories with bounded curvature in anisotropic media
Sa et al. Research of mobile robot path planning based on improved A* algorithm
CN109931943B (zh) 无人船舶全局路径规划方法及电子设备
CN109459052B (zh) 一种扫地机全覆盖路径规划方法
CN105955195A (zh) 一种基于铣削力预测的曲面加工轨迹生成方法
CN114200931B (zh) 一种基于b样条曲线优化的移动机器人路径平滑方法
CN115167398A (zh) 一种基于改进a星算法的无人船路径规划方法
Chen et al. Path planning for autonomous vehicle based on a two-layered planning model in complex environment
CN112706760B (zh) 一种用于特殊道路场景的无人驾驶泊车路径规划方法
WO2023137938A1 (zh) 虚拟角色的路径规划方法及装置、电子设备、存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant