一种仓储环境下AGV路径规划方法和装置

摘要

本发明涉及一种仓储环境下AGV路径规划方法和装置，属于自动导引运输车技术领域，解决现有局部路径规划方法处在通过隘口时会出现停顿或者绕路等的问题。该方法包括：获取AGV当前状态下的运动模型和位姿；对AGV的速度进行采样，以使用采样的速度模拟AGV的运行轨迹；计算运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点的曼哈顿距离的总评分，并选择总评分最好的轨迹作为最优轨迹；判断AGV的当前点位置是否达到坐标阈值，当AGV的当前点位置没有达到坐标阈值时，判断AGV是否到达目标坐标位置；当AGV到达目标坐标位置时获取AGV的运行轨迹图。路径规划过程中引入可变终点并合理设置中转点的位置，提升了路径规划的准确性。

权利要求

1.一种仓储环境下AGV路径规划方法，其特征在于，包括：获取AGV当前状态下的运动模型和位姿；

对所述AGV的速度进行采样，以使用采样的速度模拟所述AGV的运行轨迹；

计算所述运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点的曼哈顿距离五个方面的总评分，并选择所述总评分最好的轨迹作为最优轨迹，其中，通过以下总的评价函数对速度矢量对应的运行轨迹进行评价：其中，G表示总的评价函数值，heading用于评价当前航向与目标点方向之间的偏差，distance用于评价当前轨迹上各点和障碍物之间的最小距离，velocity用于评价当前AGV速度的大小，euclidean用于评价AGV距所述起点和所述终点的距离之和，manhattan用于评价当前AGV与终点之间的曼哈顿距离，α、β、γ、δ和ε是各自加权系数，所述各自加权系数均为常数；

在选择所述最优轨迹之后，判断所述AGV的当前点位置是否达到坐标阈值，其中，将所述坐标阈值设置在所述起点与中转点之间并且将所述坐标阈值选择在隘口附近的横坐标阈值或纵坐标阈值，当所述AGV的当前点位置没有达到坐标阈值时，判断所述AGV是否到达目标坐标位置；以及

当所述AGV到达所述目标坐标位置时，获取所述AGV的运行轨迹图。

2.根据权利要求1所述的仓储环境下AGV路径规划方法，其特征在于，进一步包括：当所述AGV的当前点位置到达所述坐标阈值时，将当前终点更新为最终终点同时更新地图；以及

重复执行根据权利要求1所述的各个步骤。

3.根据权利要求1所述的仓储环境下AGV路径规划方法，其特征在于，进一步包括：当所述AGV没有到达所述目标坐标位置时，更新地图并重复执行根据权利要求1所述的各个步骤。

4.根据权利要求1所述的仓储环境下AGV路径规划方法，其特征在于，获取AGV当前状态下的运动模型和位姿进一步包括：在所述AGV的运动模型中，获取所述AGV的当前线速度v和当前角速度ω；

在所述AGV的位姿中，获取所述AGV的当前位置坐标(x,y)和当前角度θ，用于确定下一步的速度空间及所处位置，其中，基于所述AGV的线速度v和角速度w，获取所述AGV的位姿坐标及运动轨迹坐标为：其中，qp为所述AGV的当前角度，xp为所述AGV当前所处位置的横坐标，yp为所述AGV当前所处位置的纵坐标。

5.根据权利要求1所述的仓储环境下AGV路径规划方法，其特征在于，对所述AGV的速度进行采样进一步包括：

根据所述AGV的加减速性能确定第一速度空间为：其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；w表示角速度，wp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示单位时间间隔；

根据所述AGV的速度限制确定第二速度空间为：其中，vmin、vmax表示最小线速度和最大线速度，wmin、wmax表示最小角速度和最大角速度；

以及

所述速度采样空间取值范围的界限表示为：。

6.根据权利要求1所述的仓储环境下AGV路径规划方法，其特征在于，通过以下公式分别计算所述曼哈顿距离的评分及所述欧几里得距离的评分，包括：其中，所述起点的坐标为（x0,y0），所述终点的坐标为（X,Y），各个障碍物的坐标为（xi,yi），以及当前AGV所处位置的坐标为（x,y）。

7.一种仓储环境下AGV路径规划装置，其特征在于，包括：获取模块，用于获取AGV当前状态下的运动模型和位姿；

采样与模拟模块，用于对所述AGV的速度进行采样，以使用采样的速度模拟所述AGV的运行轨迹；

总评分计算模块；用于计算所述运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点的曼哈顿距离五个方面的总评分，其中，通过以下总的评价函数对速度矢量对应的运行轨迹进行评价：其中，G表示总的评价函数值，heading用于评价当前航向与目标点方向之间的偏差，distance用于评价当前轨迹上各点和障碍物之间的最小距离，velocity用于评价当前AGV速度的大小，euclidean用于评价AGV距所述起点和所述终点的距离之和，manhattan用于评价当前AGV与终点之间的曼哈顿距离，α、β、γ、δ和ε是各自加权系数，所述各自加权系数均为常数，

轨迹选择模块，用于选择所述总评分最好的轨迹作为最优轨迹；

坐标阈值判断模块，用于在选择所述最优轨迹之后，判断所述AGV的当前点位置是否达到坐标阈值，其中，将所述坐标阈值设置在所述起点与中转点之间并且将所述坐标阈值选择在隘口附近的横坐标阈值或纵坐标阈值；

目标坐标位置判断模块，用于当所述AGV的当前点位置没有达到坐标阈值时，判断所述AGV是否到达目标坐标位置，其中，当所述AGV到达所述目标坐标位置时，获取所述AGV的运行轨迹图。

8.根据权利要求7所述的仓储环境下AGV路径规划装置，其特征在于，所述获取模块进一步包括速度获取子模块、位置及角度获取子模块和位姿及轨迹获取子模块，其中，所述速度获取子模块，用于在所述AGV的运动模型中，获取所述AGV的当前线速度v和当前角速度ω；

所述位置及角度获取子模块，用于在所述AGV的位姿中，获取所述AGV的当前位置坐标(x,y)和当前角度θ，用于确定下一步的速度空间及所处位置，其中，所述位姿及轨迹获取子模块，用于基于所述AGV的线速度v和角速度w，获取所述AGV的位姿坐标及运动轨迹坐标为：

其中，qp为所述AGV的当前角度，xp为所述AGV当前所处位置的横坐标，yp为所述AGV当前所处位置的纵坐标。

9.根据权利要求7所述的仓储环境下AGV路径规划装置，其特征在于，所述采样与模拟模块进一步包括：第一速度空间子模块、第二速度空间子模块和速度采样空间子模块，其中，

所述第一速度空间子模块，用于根据所述AGV的加减速性能确定第一速度空间为：其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；w表示角速度，wp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示单位时间间隔；

所述第二速度空间子模块，用于根据所述AGV的速度限制确定第二速度空间为：其中，vmin、vmax表示最小线速度和最大线速度，wmin、wmax表示最小角速度和最大角速度；

以及

速度采样空间子模块，用于获取所述速度采样空间取值范围的界限：。

说明书

一种仓储环境下AGV路径规划方法和装置

技术领域

[0001] 本发明涉及自动导引运输车技术领域，尤其涉及一种仓储环境下AGV路径规划方法和装置。

背景技术

[0002] 随着智能技术的发展，AGV(automated guided vehicle，意即"自动导引运输车")承担仓库中货物的起重、运输等日常任务，在仓储过程中扮演重要角色，可有效提升仓库中
货品存取效率，是指装备有电磁或光学等自动导引装置，它能够沿规定的导引路径行驶，具
有安全保护以及各种移载功能的运输车。合理的路径规划能降低AGV在搬运过程中的事故
率，提升整个仓库的工作效率。路径规划技术需要综合考虑AGV行驶过程的约束条件，进行
全局或局部的规划，使仓储实现智能化、自主化。与基于已知地图状况进行全局路径规划不
同，在AGV实际操作过程中，需要实时判断自身位置和障碍物的距离，进行局部路径规划。采
用现有的局部路径规划技术，AGV在通过隘口时会出现停顿，或者是绕路等情况，导致AGV工
作效率降低。我们首先研究AGV路径不准确的原因：停顿是因为AGV不清楚也难以判断隘口
的具体位置，需要不断调整方向；绕路是因为AGV路径规划轨迹评价函数不合理，没能找到
最短的路径。如果不改变上述停顿和绕路情况，可能导致AGV路径规划失败，进而影响仓储
工作效率。

发明内容

[0003] 鉴于上述的分析，本发明实施例旨在提供一种仓储环境下AGV路径规划方法和装置，用以解决现有局部路径规划方法处在通过隘口时会出现停顿或者绕路等的问题。

[0004] 一方面，本发明实施例提供了一种仓储环境下AGV路径规划方法，包括：获取AGV当前状态下的运动模型和位姿；对所述AGV的速度进行采样，以使用采样的速度模拟所述AGV
的运行轨迹；计算所述运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、
线速度、距终点的曼哈顿距离五个方面的总评分，并选择所述总评分最好的轨迹作为最优
轨迹；在选择所述最优轨迹之后，判断所述AGV的当前点位置是否达到坐标阈值，其中，当所
述AGV的当前点位置没有达到坐标阈值时，判断所述AGV是否到达目标坐标位置；以及当所
述AGV到达所述目标坐标位置时，获取所述AGV的运行轨迹图。

[0005] 上述技术方案的有益效果如下：在进行速度采样之后，先获取相对应的轨迹，在评分环节加入欧几里得距离和曼哈顿距离，有效缩短了当前AGV距离起点和终点的距离，使得
找到的路径距离更短。利用地图中障碍物、起点、终点、AGV位置之间的关系，设计轨迹评价
函数，结合全局路径规划的结果，设置可变目标点和中转点，实现规划路径的高准确率和高
效率。因此，在路径规划过程中引入可变终点并合理设置中转点的位置，可大大提升了路径
规划的准确性。

[0006] 基于上述方法的进一步改进，仓储环境下AGV路径规划方法进一步包括：当所述AGV的当前点位置到达所述坐标阈值时，将当前终点更新为最终终点同时更新地图，将所述
坐标阈值设置在所述起点与中转点之间并且将所述坐标阈值选择在隘口附近的横坐标阈
值或纵坐标阈值；重复执行以上实施例所述的各个步骤。

[0007] 基于上述方法的进一步改进，仓储环境下AGV路径规划方法，进一步包括：当所述AGV没有到达所述目标坐标位置时，更新地图并重复执行以上实施例所述的各个步骤。

[0008] 基于上述方法的进一步改进，获取AGV当前状态下的运动模型和位姿进一步包括：在所述AGV的运动模型中，获取所述AGV的当前线速度v和当前角速度ω；

[0009] 在所述AGV的位姿中，获取所述AGV的当前位置坐标(x,y)和当前角度θ，用于确定下一步的速度空间及所处位置，其中，

[0010] 基于所述AGV的线速度v和角速度ω，获取所述AGV的位姿坐标及运动轨迹坐标为：

[0011]

[0012]

[0013]

[0014] 其中，θp为所述AGV的当前角度，xp为所述AGV当前所处位置的横坐标，yp为所述AGV当前所处位置的纵坐标。

[0015] 基于上述方法的进一步改进，对所述AGV的速度进行采样进一步包括：根据所述AGV的加减速性能确定第一速度空间为：

[0016]

[0017] 其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；ω表示角速度，ωp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示
单位时间间隔；根据所述AGV的速度限制确定第二速度空间为：

[0018]

[0019] 其中，vmin、vmax表示最小线速度和最大线速度，ωmin、ωmax表示最小角速度和最大角速度；以及所述速度采样空间取值范围的界限表示为：

[0020] 。

[0021] 基于上述方法的进一步改进，计算所述运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距所述终点的曼哈顿距离五个方面的总评分，并选择所
述总评分最好的轨迹作为最优轨迹进一步包括：通过以下总的评价函数对速度矢量对应的
运行轨迹进行评价以选择评分最好的轨迹：

[0022]

[0023] 其中，G表示总的评价函数值，heading用于评价当前航向与目标点方向之间的偏差，distance用于评价当前轨迹上各点和障碍物之间的最小距离，velocity用于评价当前
AGV速度的大小，euclidean用于评价AGV距所述起点和所述终点的距离之和，manhattan用
于评价当前AGV与终点之间的曼哈顿距离，α、β、γ、δ和ε是各自加权系数，所述各自加权系
数均为常数。

[0024] 基于上述方法的进一步改进，通过以下公式分别计算所述曼哈顿距离的评分及所述欧几里得距离的评分，包括：

[0025]

[0026]

[0027] 其中，所述起点的坐标为（x0,y0），所述终点的坐标为（X,Y），各个障碍物的坐标为（xi,yi），以及当前AGV所处位置的坐标为（x,y）。

[0028] 另一方面，本发明实施例提供了一种仓储环境下AGV路径规划装置，包括：获取模块，用于获取AGV当前状态下的运动模型和位姿；采样与模拟模块，用于对所述AGV的速度进
行采样，以使用采样的速度模拟所述AGV的运行轨迹；总评分计算模块；用于计算所述运行
轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点的曼哈顿距
离五个方面的总评分，轨迹选择模块，用于选择所述总评分最好的轨迹作为最优轨迹；坐标
阈值判断模块，用于在选择所述最优轨迹之后，判断所述AGV的当前点位置是否达到坐标阈
值；目标坐标位置判断模块，用于当所述AGV的当前点位置没有达到坐标阈值时，判断所述
AGV是否到达目标坐标位置，其中，当所述AGV到达所述目标坐标位置时，获取所述AGV的运
行轨迹图。

[0029] 基于上述装置的进一步改进，所述获取模块进一步包括速度获取子模块、位置及角度获取子模块和位姿及轨迹获取子模块，其中，所述速度获取子模块，用于在所述AGV的
运动模型中，获取所述AGV的当前线速度v和当前角速度ω；

[0030] 所述位置及角度获取子模块，用于在所述AGV的位姿中，获取所述AGV的当前位置坐标(x,y)和当前角度θ，用于确定下一步的速度空间及所处位置，其中，

[0031] 所述位姿及轨迹获取子模块，用于基于所述AGV的线速度v和角速度ω，获取所述AGV的位姿坐标及运动轨迹坐标为：

[0032]

[0033]

[0034]

[0035] 其中，θp为所述AGV的当前角度，xp为所述AGV当前所处位置的横坐标，yp为所述AGV当前所处位置的纵坐标。

[0036] 基于上述方法的进一步改进，所述采样与模拟模块进一步包括：第一速度空间子模块、第二速度空间子模块和速度采样空间子模块，其中，所述第一速度空间子模块，用于
根据所述AGV的加减速性能确定第一速度空间为：

[0037]

[0038] 其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；ω表示角速度，ωp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示
单位时间间隔；所述第二速度空间子模块，用于根据所述AGV的速度限制确定第二速度空间
为：

[0039]

[0040] 其中，vmin、vmax表示最小线速度和最大线速度，ωmin、ωmax表示最小角速度和最大角速度；以及速度采样空间子模块，用于获取所述速度采样空间取值范围的界限：

[0041] 。

[0042] 与现有技术相比，本发明至少可实现如下有益效果之一：

[0043] 1、在进行速度采样之后，先获取相对应的轨迹，在评分环节加入欧几里得距离和曼哈顿距离，有效缩短了当前AGV距离起点和终点的距离，使得找到的路径距离更短。利用
地图中障碍物、起点、终点、AGV位置之间的关系，设计轨迹评价函数，结合全局路径规划的
结果，设置可变目标点和中转点，实现规划路径的高准确率和高效率。因此，在路径规划过
程中引入可变终点并合理设置中转点的位置，可大大提升了路径规划的准确性。

[0044] 2、提升路径规划灵活度，通过可能影响最优路径的隘口位置后，立刻变换终点，避免通过关键隘口困难（隘口不是终点或中转点，隘口只是为了通过）。

[0045] 3、选择中转点需要距离隘口尽可能的远，位于全局路径规划结果图3上，且最好自起点能够沿直线到达，避免为了到达中转点而可能导致不必要的AGV位姿调整及加减速。

[0046] 本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而
易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所
特别指出的内容中来实现和获得。

附图说明

[0047] 附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件；

[0048] 图１为根据本发明实施例的仓储环境下AGV路径规划方法的流程图；

[0049] 图2为根据本发明实施例的路径规划方法的流程示意图；

[0050] 图3为基于DWA的局部路径规划方法的栅格地图传统DWA算法仿真结果示意图；

[0051] 图4为根据本发明实施例的基于深度强化学习的全局路径规划方法的栅格仿真结果示意图；

[0052] 图5为根据本发明实施例的基于DWA的局部路径规划方法的中转点选取情况图；

[0053] 图6为根据本发明实施例的基于DWA的局部路径规划方法的到达阈值点，终点位置变换示意图；

[0054] 图7为根据本发明实施例的基于DWA的局部路径规划方法的栅格地图改进DWA算法仿真结果示意图；

[0055] 图8为标记有起点、隘口位置和中转点的图4的截屏图；

[0056] 图9为标记有坐标阈值和终点位置的图5的截屏图；

[0057] 图10为根据本发明实施例的仓储环境下AGV路径规划装置的框图。

具体实施方式

[0058] 下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。

[0059] 如果能改变上述停顿或者绕路的情况，AGV就可以准确到达指定地点，高效完成仓储工作。所以本申请提出一种仓储环境下AGV路径规划方法，利用地图中障碍物、起点、终
点、AGV位置之间的关系，设计轨迹评价函数，结合全局路径规划的结果，设置可变目标点和
中转点，实现规划路径的高准确率和高效率。

[0060] 本发明的一个具体实施例，公开了一种仓储环境下AGV路径规划方法。参考图1，仓储环境下AGV路径规划方法包括：在步骤S102中，获取AGV当前状态下的运动模型和位姿；在
步骤S104中，对AGV的速度进行采样，以使用采样的速度模拟AGV的运行轨迹；在步骤S106
中，计算运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点
的曼哈顿距离五个方面的总评分，并选择总评分最好的轨迹作为最优轨迹；在步骤S108中，
在选择最优轨迹之后，判断AGV的当前点位置是否达到坐标阈值，其中，当AGV的当前点位置
没有达到坐标阈值时，判断AGV是否到达目标坐标位置；以及在步骤S110中，当AGV到达目标
坐标位置时，获取AGV的运行轨迹图。

[0061] 与现有技术相比，本实施例提供的仓储环境下AGV路径规划方法，在进行速度采样之后，先获取相对应的轨迹，在评分环节加入欧几里得距离和曼哈顿距离，有效缩短了当前
AGV距离起点和终点的距离，使得找到的路径距离更短。利用地图中障碍物、起点、终点、AGV
位置之间的关系，设计轨迹评价函数，结合全局路径规划的结果，设置可变目标点和中转
点，实现规划路径的高准确率和高效率。因此，在路径规划过程中引入可变终点并合理设置
中转点的位置，可大大提升了路径规划的准确性。

[0062] 下文中，参考图1，对根据本发明实施例的一种仓储环境下AGV路径规划方法的各个步骤进行详细描述。

[0063] 在步骤S102中，获取AGV当前状态下的运动模型和位姿。具体地，获取AGV当前状态下的运动模型和位姿进一步包括：在AGV的运动模型中，获取AGV的当前线速度v和当前角速
度ω；在AGV的位姿中，获取AGV的当前位置坐标(x,y)和当前角度θ，用于确定下一步的速度
空间及所处位置，其中，基于AGV的线速度v和角速度ω，获取AGV的位姿坐标及运动轨迹坐
标为：

[0064]

[0065]

[0066]

[0067] 其中，θp为AGV的当前角度，xp为AGV当前所处位置的横坐标，yp为AGV当前所处位置的纵坐标。

[0068] 在步骤S104中，对AGV的速度进行采样，以使用采样的速度模拟AGV的运行轨迹。对AGV的速度进行采样，以使用采样的速度模拟AGV的运行轨迹进一步包括计算当前AGV所处
的位置和角度下的速度空间，在设定的分辨率下进行采样，得到下一段单位时间内，AGV的
可能运行轨迹。具体地，对AGV的速度进行采样进一步包括：根据AGV的加减速性能确定第一
速度空间为：

[0069]

[0070] 其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；ω表示角速度，ωp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示
单位时间间隔；根据AGV的速度限制确定第二速度空间为：

[0071]

[0072] 其中，vmin、vmax表示最小线速度和最大线速度，ωmin、ωmax表示最小角速度和最大角速度；以及最终AGV硬件性能存在限制，其速度和角速度都存在界限（上述速度和加速度
有精确的分辨率，不会成为一个无穷大的空间），速度采样空间取值范围的界限表示为：

[0073] 。

[0074] 在步骤S106中，计算运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点的曼哈顿距离五个方面的总评分，并选择总评分最好的轨迹作为最优
轨迹。具体地，计算运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速
度、距终点的曼哈顿距离五个方面的总评分，并选择总评分最好的轨迹作为最优轨迹进一
步包括：通过以下总的评价函数对速度矢量对应的运行轨迹进行评价以选择评分最好的轨
迹：

[0075]

[0076] 其中，G表示总的评价函数值，heading用于评价当前航向与目标点方向之间的偏差，distance用于评价当前轨迹上各点和障碍物之间的最小距离，velocity用于评价当前
AGV速度的大小，euclidean用于评价AGV距起点和终点的距离之和，manhattan用于评价当
前AGV与终点之间的曼哈顿距离，α、β、γ、δ和ε是各自加权系数，其中，各自加权系数均为常
数。具体地，通过以下公式分别计算曼哈顿距离的评分及欧几里得距离的评分，包括：

[0077]

[0078]

[0079] 其中，起点的坐标为（x0,y0），终点的坐标为（X,Y），各个障碍物的坐标为（xi,yi），以及当前AGV所处位置的坐标为（x,y）。

[0080] 在步骤S108中，在选择最优轨迹之后，判断AGV的当前点位置是否达到坐标阈值。当AGV的当前点位置没有达到坐标阈值时，判断AGV是否到达目标坐标位置。此外，当AGV的
当前点位置到达坐标阈值时，将当前终点更新为最终终点同时更新地图，将坐标阈值设置
在起点与中转点之间并且将坐标阈值选择在隘口附近的横坐标阈值或纵坐标阈值；重复执
行上述实施例所述的各个步骤S102、S104、S106、S108和S110。

[0081] 在步骤S110中，当AGV到达目标坐标位置时，获取AGV的运行轨迹图。此外，当AGV没有到达目标坐标位置时，更新地图并重复执行上述实施例所述的各个步骤S102、S104、
S106、S108和S110。

[0082] 本发明的另一个具体实施例，公开了一种仓储环境下AGV路径规划装置。参考图10，对根据本发明实施例中的各个模块进行详细描述。

[0083] 获取模块1002用于获取AGV当前状态下的运动模型和位姿。具体地，获取模块1002进一步包括速度获取子模块、位置及角度获取子模块和位姿及轨迹获取子模块，其中，速度
获取子模块，用于在AGV的运动模型中，获取AGV的当前线速度v和当前角速度ω；位置及角
度获取子模块，用于在AGV的位姿中，获取AGV的当前位置坐标(x,y)和当前角度θ，用于确定
下一步的速度空间及所处位置，其中，位姿及轨迹获取子模块，用于基于AGV的线速度v和角
速度ω，获取AGV的位姿坐标及运动轨迹坐标为：

[0084]

[0085]

[0086]

[0087] 其中，θp为AGV的当前角度，xp为AGV当前所处位置的横坐标，yp为AGV当前所处位置的纵坐标。

[0088] 采样与模拟模块1004用于对AGV的速度进行采样，以使用采样的速度模拟AGV的运行轨迹。具体地，采样与模拟模块进一步包括：第一速度空间子模块、第二速度空间子模块
和速度采样空间子模块，其中，第一速度空间子模块，用于根据AGV的加减速性能确定第一
速度空间为：

[0089]

[0090] 其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；ω表示角速度，ωp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示
单位时间间隔；第二速度空间子模块，用于根据AGV的速度限制确定第二速度空间为：

[0091]

[0092] 其中，vmin、vmax表示最小线速度和最大线速度，ωmin、ωmax表示最小角速度和最大角速度；以及速度采样空间子模块，用于获取速度采样空间取值范围的界限：

[0093] 。

[0094] 总评分计算模块1006用于计算运行轨迹在距起点和终点的欧几里得距离、朝向角度、障碍物距离、线速度、距终点的曼哈顿距离五个方面的总评分。

[0095] 轨迹选择模块1008用于选择总评分最好的轨迹作为最优轨迹。坐标阈值判断模块1010用于在选择最优轨迹之后，判断AGV的当前点位置是否达到坐标阈值。

[0096] 目标坐标位置判断模块1012用于当AGV的当前点位置没有达到坐标阈值时，判断AGV是否到达目标坐标位置，其中，当AGV到达目标坐标位置时，获取AGV的运行轨迹图。

[0097] 本申请还提供了一种阈值选择及中途变换终点的路径规划方法，提升路径规划灵活度，通过可能影响最优路径的隘口位置后，立刻变换终点，避免通过关键隘口困难（参考
图8和图9，隘口B不是终点D或中转点C，隘口只是为了通过），隘口是指AGV可能通过困难的
区域，或通过前后使AGV路径出现抖动影响结果的区域。

[0098] 本申请还提供了一种中转点选择方法，所述选择中转点需要距离隘口尽可能的远，位于全局路径规划结果图4（左侧附图（a）的全局路径规划绕过起点和终点之间的在障
碍物和右侧附图（b）的全局路径规划穿过起点和终点之间的障碍物）上，且最好自起点能够
沿直线到达，避免为了到达中转点而可能导致不必要的AGV位姿调整及加减速，中转点满足
条件：1、距离隘口位置尽可能远；2、位于全局路径规划结果图3上；3、最好与自起点A能沿直
线到达（因为AGV初始方向不同，较为平滑的曲线也可以。只有AGV设置的初始方向与全局路
径规划结果的初始方向一致时，3才成立。具体地，可变终点在AGV到达坐标阈值之前中转点
就是当时的终点，起点A、隘口位置B、中转点C，如图5所示；可变终点在AGV到达坐标阈值之
后最终终点才是终点，阈值坐标、终点D，如图6所示。

[0099] 下文中，将参考图1至图9，以具体实例的方式对根据本发明实施例的仓储环境下AGV路径规划方法进行详细描述。

[0100] 实施例一

[0101] 请参考图2，图2为本申请所提供的一种路径规划方法的流程示意图，该路径规划方法可以包括：

[0102] S1：获取AGV的运动模型以及当前位姿。

[0103] 本步骤旨在根据获取AGV当前的运动模型以及AGV位姿，获得线速度v和当前角速度ω以及当前AGV的位置(x,y)和当前角度θ，用于确定下一步的初始速度空间及所处位置。
此过程可基于传感器信息采集设备实现，对于传感器及信息采集设备的种类和型号，本申
请不做限定，可以为雷达、专业摄像头等。需要获取AGV以下信息：

[0104] T={x,y,θ,v,ω}

[0105] 主要包括AGV所处位置的横纵坐标和朝向角{x,y,θ}，以及AGV当前的运动速度，包括线速度和角速度{v,ω}。

[0106] S2：通过对AGV进行速度采样，模拟轨迹。

[0107] 基于当前AGV所能使用的速度空间，在设定的线速度和角速度的分辨率下进行相应的速度采样，得到AGV的所有可能运动轨迹。

[0108] 本步骤旨在获得AGV的可能轨迹，其具体实现过程基于当前AGV的运动情况和位姿，以及加减速性能确定的速度空间来实现。参考图2，一般AGV运动时，如果速度过快，线加
速度或者是角加速度不够大，难以及时改变前进方向，可能会错过隘口，甚至是目标点，导
致AGV不停地绕路转圈。本申请通过设置中转点，保证无论速度AGV都能够通过关键点（或隘
口）的大方向，然后调整评价函数实现最短路径。因此确定速度空间尤为重要。根据速度空
间，以及当前AGV所处的位置和角度，在设定的分辨率下进行进行采样，能得到下一段单位
时间内，AGV的可能运行轨迹。

[0109] 根据AGV的加减速性能确定第一速度空间为：

[0110]

[0111] 其中，v表示线速度，vp表示当前线速度，amin、amax表示最小线加速度和最大线加速度；ω表示角速度，ωp表示当前角速度，βmin、βmax表示最小角加速度和最大角加速度；t表示
单位时间间隔。

[0112] 根据AGV的速度限制确定的第二速度空间为：

[0113]

[0114] 其中，vmin、vmax表示最小线速度和最大线速度，ωmin、ωmax表示最小角速度和最大角速度。

[0115] 最终AGV速度采样空间为：

[0116]

[0117] 上述速度和加速度有精确的分辨率，不会成为一个无穷大的空间。

[0118] 确定了AGV的线速度v和角速度ω，AGV位姿及运动轨迹坐标为：

[0119]

[0120]

[0121]

[0122] 其中，θp为AGV的当前角度，xp为AGV当前所处位置的横坐标，yp为AGV当前所处位置的纵坐标。

[0123] S3：对所获得轨迹进行评分，选择最优轨迹。

[0124] 基于AGV朝向角度、距离障碍物距离、行驶速度、距离起点终点的欧几里得距离以及距离终点的曼哈顿距离，对S2得到的估计轨迹进行评分，选择得分最少的估计轨迹提供
给AGV执行。

[0125] 对于AGV而言，当前所处位置和终点连线角度与行驶角度越接近，当前位置与障碍物距离越接近，自身速度越快，距离起点和终点的欧几里得距离及距离终点的曼哈顿距离
越小，之后所走的路径就可能越短。

[0126] 采用评价函数的方式对AGV每条安全可行的轨迹进行评价，评价函数为：

[0127]

[0128] 其中，G表示总的评价函数值，利用AGV的横纵坐标和朝向角{x,y,θ}，以及当前的运动速度{v,ω}进行计算。heading用于评价当前航向与目标点方向之间的偏差，distance
用于评价当前轨迹上各点和障碍物之间的最小距离，velocity用于评价当前AGV速度的大
小，euclidean用于评价AGV距所述起点和所述终点的距离之和，manhattan用于评价当前
AGV与终点之间的曼哈顿距离，α、β、γ、δ和ε是各自加权系数，所述各自加权系数均为常数。

[0129] 计算朝向得分(此处使用atan2是因为其取值范围为(‑π, π])：

[0130]

[0131] （评价当前AGV航向角度与AGV当前位置及目标点连线所得角度之间的偏差，角度偏差小则尽可能不用变换角速度）

[0132] 计算障碍物得分：

[0133]

[0134] （评价当前轨迹上各点和障碍物之间的最小距离，在仓库等障碍物密集的环境中，靠近障碍物可节省距离）

[0135] 计算速度得分：

[0136]

[0137]  （评价当前AGV线速度的情况，希望能使得AGV速度尽可能快）

[0138] 计算欧几里得距离得分：

[0139]

[0140] （评价AGV距离起点和终点的欧几里得距离之和，尽可能保证距离最短）

[0141] 计算曼哈顿距离得分：

[0142]

[0143] （评价当前AGV与终点之间的曼哈顿距离，尽可能保证靠近终点）

[0144] 其中，起点start的坐标为（x0,y0），终点goal的坐标为（X,Y），各个障碍物的坐标为（xi,yi），当前AGV所处位置current的坐标为（x,y），当前速度current_speed为v，当前
current角度为θ，设定的最大速度为max_speed。

[0145] S4：判断AGV所处位置，并更新地图。

[0146] 判断AGV是否到达设定的坐标阈值，若是，则变更终点，然后执行S1；若否，则继续进行下一个判断。

[0147] 判断AGV是否到达设定的目标位置，若是，则算法结束；若否，更新地图，然后执行S1。

[0148] 本步骤旨在使得AGV顺利到达终点，完成仓储环境下的路径规划工作。

[0149] 实施例二

[0150] 在上文中对于仓储环境下的路径规划方法进行了详细描述，AGV行驶的起点和终点如图3所示，本申请还提供一种与该方法对应的中转点及可变终点选取方法。

[0151] 通过实施例一的相关步骤，AGV的运动已经获得保障，但为了防止可能出现的通过隘口困难及绕远问题，有效设定坐标阈值，将确定和到达终点可以分为两部分：在全局路径
中寻找中转点，使AGV通过关键隘口；通过隘口之后，沿障碍物使AGV准确抵达终点。

[0152] 图3为不设置中转点及可变终点，使得AGV直接前往终点位置所可能发生的绕远路情况，在实际操作中会浪费转向时间，且连续转向可能发生AGV转向故障。

[0153] 在上述实施例的基础上，提供中转点及可变终点选取方法，该选取办法包括：

[0154] 具体地，图5、图6和图7中的左侧附图（即，（a））表示AGV一个阶段的历程，图5、图6和图7中的右侧附图（即，（b））表示AGV另一个阶段的历程。图5中给出的是起点和中转点，其
中×表示中转点。图6给出的是通过坐标阈值之后的情况，其中×表示最终的终点。图7给出
的是最后的路线图。根据全局路径规划算法，获得栅格图中宏观规划的最优路径，全局路径
规划结果如图5所示；

[0155] 根据获得的全局最优路径，获取需要AGV转弯的隘口位置并制定自起点能够沿直线到达的中转点，则AGV自起点出发地图状况如图6所示。

[0156] 找到可以满足AGV通过隘口且距离隘口相对较远的点，设定坐标阈值，达到坐标阈值则改变终点位置，将目标点由中转点转变成真正的终点，此时地图状况如图7所示；在图6
的截屏图9中标记有D。

[0157] 坐标阈值图9中的y轴坐标（左侧附图（a））小于17，图9中x轴坐标大于8（右侧附图（b））。

[0158] 到达终点周边满足AGV与终点的距离之差小于AGV自身半径就可以算作到达终点，并根据加速度安排停车，如图8所示。图8（左侧附图（a）和右侧附图（b））标记有起点A、隘口
位置B和中转点C。

[0159] 本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所
述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

[0160] 以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，
都应涵盖在本发明的保护范围之内。