一种基于A*和并行TEB的AGV混合路径规划方法

一种基于a*和并行teb的agv混合路径规划方法
技术领域
1.本发明属于agv控制领域，具体涉及一种基于a*和并行teb的agv混合路径规划控制方法


背景技术：

2.现如今agv小车在物流配送、港口分拣、智能居家和工业搬运等方面发挥着越来越重要的作用，agv路径规划准确性取决于其导航精度，受成本限制，目前产业化agv多采用激光导航和视觉导航；在做到准确规划的基础上，agv的工作效率取决于路径规划控制方法，当今常用路径规划方法包括基于dijkstra算法、蚁群算法等全局路径规划，和基于动态规划窗口dwa、贝塞尔曲线等算法的局部路径规划两种方法。传统全局路径规划方法多属于广度优先搜索，虽然能确保全局路径最优，但耗费大量的计算资源，规划效率低下，而且只能规划出静态路径轨迹，无法提升agv对动态障碍物的适应能力；传统局部路径规划方法，虽然能提升agv动态适应性，但在相邻障碍物非常逼近的状况下，易陷入局部极值问题，导致agv无法规划轨迹而停驶。鉴于以上问题，开发一套融合一种高效的全局路径规划方法和可以克服局部极值问题的局部路径规划方法，将有助于提升agv运行效率、降低路径规划成本和提升车辆的动态适应能力。
3.目前现有的技术中，如2022年8月5日公开的发明专利：公开号：cn114859929a,动态环境下基于改进dwa算法的agv路径规划方法，该发明通过对局部地图内的动态障碍物根据运动速度进行分类，降低agv偏离最短路径的程度，虽然能够提升agv避障的能力但是由于dwa算法基于速度采样，多用在差分或全向agv模型，不适用于准确度较高的阿克曼模型，因此在控制精度上还有一定不足；再如2020年9月25日公开的发明专利：公开号：cn111708364a,一种基于a*算法改进的agv路径规划方法，该发明提出了一种新启发估计函数，改进了传统算法的数据结构和存储方式，虽然提升算法的运行速度，但仍属于全局路径规划，无法提升agv动态适应性。


技术实现要素：

4.为解决现有技术存在的不足，本发明提出一种基于a*和并行teb的混合路径规划方法，在由激光雷达和相机融合建立的地图中规划路径，利用含有启发代价函数的a*算法开展全局路径规划，在获取全局路径后，本发明对传统teb算法进行优化，采用基于并行teb算法开展局部路径规划，通过计算路径函数过滤同源路径，选取代价函数最小路径作为最优局部路径，本发明能够兼顾全局最优和局部最优，在提升效率的同时提升agv的动态环境适应性。
5.为实现上述目的，本发明是采用如下技术方案实现的：
6.步骤一：设定轨迹起始点、终点和障碍物位置
7.本发明在由激光雷达和相机融合确立的二维栅格地图中展开路径规划，设定agv运动轨迹起始点位置s和终点位置d，并添加若干静态障碍物o
static
，构建初始运动地图，在
实际路径规划过程中，本方法在初始地图的基础上随机添加若干动态障碍物o
dynamic
；
8.步骤二：利用a-star算法开展全局路径规划
9.建立close_list和open_list两个集合，将两个集合初始设为空集，本方法将轨迹起始点s作为算法第一个起始节点c放入close_list集合内，检测起始节点c周围是否存在附近节点n，若传感元件探测起始节点c周围均为障碍物或无附近节点n，则退出路径规划程序，输出起始点s和终点d之间不存在可行路径；若传感元件探测起始节点c周围存在附近节点n，则将所有附近节点n均移入到open_list集合内，并计算每个附近节点ni的代价函数f(n)，表达式如式1所示：
10.f(n)＝g(n)+h(n)
ꢀꢀꢀ
(1)
11.式中g(n)为实际代价，为起始节点c到附近节点ni之间的实际距离，h(n)为启发代价，代表附近节点ni到轨迹终点d之间的估计距离,采用曼哈顿距离表示附近节点ni的启发代价，计算方法如式2所示：
12.h(n)＝|x
n-x0|+|y
n-y0|
ꢀꢀꢀ
(2)
13.式中xn表示当前附近节点ni在二维栅格地图中的横坐标，yn表示当前附近节点ni在二维栅格地图中的纵坐标，x0为终点d在二维栅格地图中的横坐标，y0为终点d在二维栅格地图中的纵坐标，本方法从所有附近节点中选出代价函数最小的附近节点记作n*并判断n*是否达到了终点d，若已经达到终点d，则退出路径规划程序，若没有达到终点d，就将n
*
节点作为下一轮循环规划的初始节点c移入到close_list集合内，同时清空open_list集合中的所有节点，循环上述寻点和计算代价函数的过程，找到最优全局路径；
14.步骤三：利用基于拓扑结构的并行teb算法开展局部路径规划
15.本方法将步骤二规划的全局路径进行离散，得到整条轨迹上若干关键点，在外界环境没有发生变化的状态下，agv一直沿这些关键点运行，若突然出现动态障碍物，则将agv当前位置ps作为局部路径起点，将局部子目标点pd作为局部路径终点，检测动态障碍物o
dynamic
的上边界o1和下边界o2，构建局部拓扑地图，得到若干条可行路径，本方法将路径是否跨越障碍物作为区分不同路径的方式，对于未跨越障碍物的路径，算法设定为同源路径，具有相同的路径函数h，计算方法如式3:
16.h＝[ln(p
s-o
dynamic
)-ln(o
dynamic-pd)+i min(|arg(p
s-ε)-arg(p
d-ε)|)]
ꢀꢀꢀ
(3)
[0017]
式中(p
s-o
dynamic
)表示路径起点与障碍物边界间距离，(o
dynamic-pd)表示路径终点与障碍物边界间距离，arg(p
s-ε)-arg(p
d-ε)为由两条距离线段组成的邻边角度之差，利用不同h值，算法过滤掉同源路径，为保证agv行驶安全，本方法引入障碍物距离约束条件，如式4；
[0018]dobstacle
＜d
min
ꢀꢀꢀ
(4)
[0019]
式中d
obstacle
表示agv与障碍物之间的距离，通过传感器测距获取，初始标定agv与障碍物的最小极限距离为d
min
，距离代价函数f
obstacle
如式5，当agv与障碍物距离大于极限距离时，距离约束权重ρ
obstacle
为0，不考虑距离约束条件，
[0020][0021]
在满足安全性的基础上，本方法引入时间约束，要求agv运行时间最短，利用agv各段局部路程时间间隔构建时间代价函数f
time
，如式6：
[0022][0023]
为保证agv运行不超速，本方法引入速度约束，如式7，
[0024][0025]
(x
i+1
,y
i+1
)表示下一时刻agv所在位置坐标，(xi,yi)为当前时刻agv所在位置坐标，v
ix
为沿x方向分速度，v
iy
为沿y方向分速度，初始标定x方向和y方向速度上限分别为v
x_max
和v
y_max
，速度代价函数fv如式8：
[0026][0027]
为避免agv旋转角度过大，本方法引入角速度约束如式9，
[0028][0029]
θ
i+1
和θi分别为agv下一时刻角坐标和当前时刻角坐标，标定角速度上限为ω
max
，角速度代价函数f
ω
如式10，
[0030][0031]
为保证agv行驶平顺，本方法引入加速度约束和角加速度约束，如式11，标定加速度上限为a
max
，角加速度上限为β
max
，
[0032][0033]
加速度代价函数fa和角加速度代价函数f
β
如式12所示，
[0034][0035]
本方法利用上述所有约束构成评价路径优劣的总代价函数f，计算过滤掉同源路径后，剩余路径的代价函数，如式13，
[0036]
f＝ρ
obstaclefobstacle
＝ρ
timeftime
＝ρ
vfv
＝ρ
ωfω
+ρafa+ρ
βfβ
ꢀꢀꢀ
(13)
[0037]
式中ρ
obstacle
、ρ
time
、ρv、ρ
ω
、ρa、ρ
β
分别对应各自约束权重，可根据代价主次进行标定，选取代价函数f最小的路径作为局部最优路径；
[0038]
步骤四：循环迭代局部目标点，生成路径轨迹
[0039]
判断步骤三中局部目标点pd是否为轨迹终点d，若是，则退出规划；若不是，则循环步骤三，直至局部目标点达到轨迹终点d，路径规划结束。
[0040]
本发明与现有技术相比较，有益效果如下：
[0041]
1、本发明提出的一种基于a*和并行teb的混合路径规划方法能降低全局规划时间，压缩规划过程中的节点数和计算量，提高规划效率，在满足静态路径行驶安全的基础上，还提升了agv对动态障碍物的适应能力；
[0042]
2、本发明利用并行teb算法克服了传统teb方法易陷入局部极值的问题，能够使agv通过狭小区间，并适用于阿克曼转向模型，通过计算路径函数过滤掉众多同源路径，简化计算量、提升算法的运行效率，节约数据的存储空间。
附图说明
[0043]
下面结合附图对实施例的描述将变得容易理解，其中：
[0044]
图1为根据本发明实施例的一种基于a*和并行teb的agv混合路径规划方法流程示意图；
[0045]
图2为根据本发明实施例的全局路径规划算法流程示意图；
[0046]
图3为根据本发明实施例的局部路径规划算法流程示意图；
[0047]
图4为根据本发明实施例的建立的局部拓扑地图示意图；
具体实施方式
[0048]
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0049]
下面参考附图来描述一种考虑多目标优化的燃料电池汽车能量管理控制方法，但本发明并不限于这些实施例。
[0050]
参考附图1，发明专利的实施总共包括以下四个环节
[0051]
步骤一：设定轨迹起始点、终点和障碍物位置
[0052]
本发明在由激光雷达和相机融合确立的二维栅格地图中展开路径规划，设定agv运动轨迹起始点位置s和终点位置d，并添加若干静态障碍物o
static
，构建初始运动地图，在实际路径规划过程中，本方法在初始地图的基础上随机添加若干动态障碍物o
dynamic
；
[0053]
步骤二：利用a-star算法开展全局路径规划
[0054]
参考附图2，建立close_list和open_list两个集合，将两个集合初始设为空集，本方法将轨迹起始点s作为算法第一个起始节点c放入close_list集合内，检测起始节点c周围是否存在附近节点n，若传感元件探测起始节点c周围均为障碍物或无附近节点n，则退出路径规划程序，输出起始点s和终点d之间不存在可行路径；若传感元件探测起始节点c周围存在附近节点n，则将所有附近节点n均移入到open_list集合内，并计算每个附近节点ni的代价函数f(n)，表达式如式1所示：
[0055]
f(n)＝g(n)+h(n)
ꢀꢀꢀ
(14)
[0056]
式中g(n)为实际代价，为起始节点c到附近节点ni之间的实际距离，h(n)为启发代价，代表附近节点ni到轨迹终点d之间的估计距离,采用曼哈顿距离表示附近节点ni的启发代价，计算方法如式2所示：
[0057]
h(n)＝|x
n-x0|+|y
n-y0|
ꢀꢀꢀ
(15)
[0058]
式中xn表示当前附近节点ni在二维栅格地图中的横坐标，yn表示当前附近节点ni在二维栅格地图中的纵坐标，x0为终点d在二维栅格地图中的横坐标，y0为终点d在二维栅格地图中的纵坐标，本方法从所有附近节点中选出代价函数最小的附近节点记作n*并判断n*是否达到了终点d，若已经达到终点d，则退出路径规划程序，若没有达到终点d，就将n
*
节点作为下一轮循环规划的初始节点c移入到close_list集合内，同时清空open_list集合中的所有节点，循环上述寻点和计算代价函数的过程，找到最优全局路径；
[0059]
步骤三：利用基于拓扑结构的并行teb算法开展局部路径规划
[0060]
参考附图3，本方法将步骤二规划的全局路径进行离散，得到整条轨迹上若干关键点，在外界环境没有发生变化的状态下，agv一直沿这些关键点运行，若突然出现动态障碍物，则将agv当前位置ps作为局部路径起点，将局部子目标点pd作为局部路径终点，检测动态障碍物o
dynamic
的上边界o1和下边界o2，构建局部拓扑地图，参考附图4，得到若干条可行路径，本方法将路径是否跨越障碍物作为区分不同路径的方式，对于未跨越障碍物的路径，算法设定为同源路径，具有相同的路径函数h，计算方法如式3:
[0061]
h＝[ln(p
s-o
dynamic
)-ln(o
dynamic-pd)+i min(|arg(p
s-ε)-arg(p
d-ε)|)]
ꢀꢀꢀ
(16)
[0062]
式中(p
s-o
dynamic
)表示路径起点与障碍物边界间距离，(o
dynamic-pd)表示路径终点与障碍物边界间距离，arg(p
s-ε)-arg(p
d-ε)为由两条距离线段组成的邻边角度之差，利用不同h值，算法过滤掉同源路径，为保证agv行驶安全，本方法引入障碍物距离约束条件，如式4；
[0063]dobstacle
＜d
min
ꢀꢀꢀ
(17)
[0064]
式中d
obstacle
表示agv与障碍物之间的距离，通过传感器测距获取，初始标定agv与障碍物的最小极限距离为d
min
，距离代价函数f
obstacle
如式5，当agv与障碍物距离大于极限距离时，距离约束权重ρ
obstacle
为0，不考虑距离约束条件，
[0065][0066]
在满足安全性的基础上，本方法引入时间约束，要求agv运行时间最短，利用agv各段局部路程时间间隔构建时间代价函数f
time
，如式6：
[0067][0068]
为保证agv运行不超速，本方法引入速度约束，如式7，
[0069][0070]
(x
i+1
,y
i+1
)表示下一时刻agv所在位置坐标，(xi,yi)为当前时刻agv所在位置坐标，v
ix
为沿x方向分速度，v
iy
为沿y方向分速度，初始标定x方向和y方向速度上限分别为v
x_max
和v
y_max
，速度代价函数fv如式8：
[0071][0072]
为避免agv旋转角度过大，本方法引入角速度约束如式9，
[0073][0074]
θ
i+1
和θi分别为agv下一时刻角坐标和当前时刻角坐标，标定角速度上限为ω
max
，角速度代价函数f
ω
如式10，
[0075][0076]
为保证agv行驶平顺，本方法引入加速度约束和角加速度约束，如式11，标定加速度上限为a
max
，角加速度上限为β
max
，
[0077][0078]
加速度代价函数fa和角加速度代价函数f
β
如式12所示，
[0079][0080]
本方法利用上述所有约束构成评价路径优劣的总代价函数f，计算过滤掉同源路径后，剩余路径的代价函数，如式13，
[0081]
f＝ρ
obstaclefobstacle
+ρ
timeftime
+ρ
vfv
+ρ
ωfω
+ρafa+ρ
βfβ
ꢀꢀꢀ
(26)
[0082]
式中ρ
obstacle
、ρ
time
、ρv、ρ
ω
、ρa、ρ
β
分别对应各自约束权重，可根据代价主次进行标定，选取代价函数f最小的路径作为局部最优路径；
[0083]
步骤四：循环迭代局部目标点，生成路径轨迹
[0084]
判断步骤三中局部目标点pd是否为轨迹终点d，若是，则退出规划；若不是，则循环步骤三，直至局部目标点达到轨迹终点d，路径规划结束。

技术特征：
1.一种基于a*和并行teb的agv混合路径规划方法,其特征在于，包括以下步骤：步骤一：设定轨迹起始点、终点和障碍物位置本发明在由激光雷达和相机融合确立的二维栅格地图中展开路径规划，设定agv运动轨迹起始点位置s和终点位置d，并添加若干静态障碍物o
static
，构建初始运动地图，在实际路径规划过程中，本方法在初始地图的基础上随机添加若干动态障碍物o
dynamic
；步骤二：利用a*算法开展全局路径规划建立close_list和open_list两个集合，将两个集合初始设为空集，本方法将轨迹起始点s作为算法第一个起始节点c放入close_list集合内，检测起始节点c周围是否存在附近节点n，若传感元件探测起始节点c周围均为障碍物或无附近节点n，则退出路径规划程序，输出起始点s和终点d之间不存在可行路径；若传感元件探测起始节点c周围存在附近节点n，则将所有附近节点n均移入到open_list集合内，并计算每个附近节点n
i
的代价函数f(n)，表达式如式1所示：f(n)＝g(n)+h(n)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)式中g(n)为实际代价，为起始节点c到附近节点n
i
之间的实际距离，h(n)为启发代价，代表附近节点n
i
到轨迹终点d之间的估计距离,采用曼哈顿距离表示附近节点n
i
的启发代价，计算方法如式2所示：h(n)＝|x
n-x0|+|y
n-y0|
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)式中x
n
表示当前附近节点n
i
在二维栅格地图中的横坐标，y
n
表示当前附近节点n
i
在二维栅格地图中的纵坐标，x0为终点d在二维栅格地图中的横坐标，y0为终点d在二维栅格地图中的纵坐标，本方法从所有附近节点中选出代价函数最小的附近节点记作n*并判断n*是否达到了终点d，若已经达到终点d，则退出路径规划程序，若没有达到终点d，就将n
*
节点作为下一轮循环规划的初始节点c移入到close_list集合内，同时清空open_list集合中的所有节点，循环上述寻点和计算代价函数的过程，找到最优全局路径；步骤三：利用基于拓扑结构的并行teb算法开展局部路径规划本方法将步骤二规划的全局路径进行离散，得到整条轨迹上若干关键点，在外界环境没有发生变化的状态下，agv一直沿这些关键点运行，若突然出现动态障碍物，则将agv当前位置p
s
作为局部路径起点，将局部子目标点p
d
作为局部路径终点，检测动态障碍物o
dynamic
的上边界o1和下边界o2，构建局部拓扑地图，得到若干条可行路径，本方法将路径是否跨越障碍物作为区分不同路径的方式，对于未跨越障碍物的路径，算法设定为同源路径，具有相同的路径函数h，计算方法如式3:h＝[ln(p
s-o
dynamic
)-ln(o
dynamic-p
d
)+imin(arg(p
s-ε)-arg(p
d-ε))] (3)式中(p
s-o
dynamic
)表示路径起点与障碍物边界间距离，(o
dynamic-p
d
)表示路径终点与障碍物边界间距离，arg(p
s-ε)-arg(p
d-ε)为由两条距离线段组成的邻边角度之差，利用不同h值，算法过滤掉同源路径，为保证agv行驶安全，本方法引入障碍物距离约束条件，如式4；d
obstacle
＜d
min (4)式中d
obstacle
表示agv与障碍物之间的距离，通过传感器测距获取，初始标定agv与障碍物的最小极限距离为d
min
，距离代价函数f
obstacle
如式5，当agv与障碍物距离大于极限距离时，距离约束权重ρ
obstacle
为0，不考虑距离约束条件，
在满足安全性的基础上，本方法引入时间约束，要求agv运行时间最短，利用agv各段局部路程时间间隔构建时间代价函数f
time
，如式6：为保证agv运行不超速，本方法引入速度约束，如式7，(x
i+1
,y
i+1
)表示下一时刻agv所在位置坐标，(x
i
,y
i
)为当前时刻agv所在位置坐标，v
ix
为沿x方向分速度，v
iy
为沿y方向分速度，初始标定x方向和y方向速度上限分别为v
x_max
和v
y_max
，速度代价函数f
v
如式8：为避免agv旋转角度过大，本方法引入角速度约束如式9，θ
i+1
和θ
i
分别为agv下一时刻角坐标和当前时刻角坐标，标定角速度上限为ω
max
，角速度代价函数f
ω
如式10，为保证agv行驶平顺，本方法引入加速度约束和角加速度约束，如式11，标定加速度上限为a
max
，角加速度上限为β
max
，加速度代价函数f
a
和角加速度代价函数f
β
如式12所示，本方法利用上述所有约束构成评价路径优劣的总代价函数f，计算过滤掉同源路径后，剩余路径的代价函数，如式13，f＝ρ
obstacle
f
obstacle
+ρ
time
f
time
+ρ
v
f
v
+ρ
ω
f
ω
+ρ
a
f
a
+ρ
β
f
β
ꢀꢀꢀꢀ
(13)式中ρ
obstacle
、ρ
time
、ρ
v
、ρ
ω
、ρ
a
、ρ
β
分别对应各自约束权重，可根据代价主次进行标定，选取代价函数f最小的路径作为局部最优路径；
步骤四：循环迭代局部目标点，生成路径轨迹判断步骤三中局部目标点p
d
是否为轨迹终点d，若是，则退出规划；若不是，则循环步骤三，直至局部目标点达到轨迹终点d，路径规划结束。

技术总结
本发明提出一种基于A*和并行TEB的AGV混合路径规划方法，旨在解决传统全局路径规划方法控制AGV行驶轨迹时，无法做到动态避障、计算节点数目过于庞大，耗时过长的问题，以及使用局部路径规划方法控制行驶轨迹易导致局部极值的问题。所述方法包括以下步骤：S1、设定轨迹起始点、终点和障碍物位置；S2、利用A*算法开展全局路径规划；S3、利用基于拓扑结构的并行TEB算法开展局部路径规划；S4、循环迭代局部目标点，生成路径轨迹。本发明可以提升AGV对动态环境的适应性，提高AGV通过临近障碍物所形成的狭小空间的能力，减少路径规划运行时间，提升路径规划效率，有助于充分发挥AGV灵活便捷的优势。优势。优势。


技术研发人员：曾小华 李凯旋 宋大凤 李敦迈 黄钰峰 王云叶 吴佳俊 高皓铭 程龙 周岚琦
受保护的技术使用者：吉林大学
技术研发日：2023.04.28
技术公布日：2023/8/1