CN114442633A - 一种无人驾驶园区物流车局部路径规划方法 - Google Patents

Abstract

本发明提供了一种无人驾驶园区物流车局部路径规划方法，包括如下步骤：S1、获取障碍物、全局参考路径以及自车位姿信息；S2、通过动态滑动窗口算法对车辆在全局路径上进行定位，并获取车辆在全局路径上的定位点索引值，根据定位点索引值选择车辆前方若干个路径点作为局部路径；S3、根据道路宽度，将步骤S2中得到的局部路径以一定间隔左右平移出多条局部路径，作为障碍物路段的备选路径；S4、由步骤S3中得到的备选路径，通过障碍物碰撞检测算法筛选出车辆通行的所有局部路径；S5、通过最优局部路径选择算法，从所有的通行路径中筛选出最优的局部避障路径。本发明可使车辆能够安全、平稳、高效地进行避障，提高园区物流车辆自动驾驶性能。

Description

一种无人驾驶园区物流车局部路径规划方法

技术领域

本发明属于园区智能物流车辆自动驾驶技术领域，尤其是涉及一种无人驾驶园区物流车局部路径规划方法。

背景技术

园区智能物流车辆运输是当前自动驾驶理论研究和技术应用的热点场景之一，局部路径规划是自动驾驶的核心技术；针对园区物流运输场景，研究安全、平稳、高效的局部路径规划方法，对提高物流车辆自动驾驶性能和园区物流运输智能化水平具有重要意义。

一般来说，自动驾驶系统的结构主要分为以下几层：感知层、数据融合层、决策规划层和控制层；智能车辆系统利用数据融合技术对环境信息进行处理和评估，并根据评估结果进行决策和路径规划，进而实现车辆的自动驾驶。自动驾驶系统中的路径规划模块通常细分为全局规划和局部规划，局部路径规划是在全局路径的基础上，规划出连接车辆当前运动状态与局部目标状态的最优或次优局部安全路径，并提供给车辆控制层，使得车辆安全、平稳、高效地运行；因此，局部路径规划是连接环境感知与车辆控制的重要纽带，也是自动驾驶系统中的关键组成部分。

局部路径规划根据动态交通环境，并在全局参考路径的指引下，动态的规划出一段安全可行的行车轨迹，其中轨迹是考虑了时间维度的路径。而全局路径规划则是基于已知地图和任务指标，搜索出任务指标代价最小的全局路径，引导自动驾驶车辆从起始位置到用户定义的目的地；全局路径规划范围更大且相对静态，而局部路径规划用于解决局部动态环境下的规划问题，因而较多研究将两者结合实现优势互补；全局和局部路径规划在方法上并没有严格的界限；目前针对路径/轨迹规划及轨迹优化问题，常见的规划算法包括图搜索法、人工势场法、随机采样法、插值曲线法以及基于优化的方法等。

由于行车环境的复杂性和时变性，以及智能车辆需满足的各种约束条件和行车任务指标，使得规划模块生成的路径或轨迹难以满足实际需求，存在碰撞的危险性和实时性不高的问题，对此，本专利申请发明了适合园区智能物流车辆的局部路径规划方法，以规划出安全、平稳的路径为目标，提高园区物流车辆自动驾驶性能。

发明内容

有鉴于此，本发明旨在提出一种无人驾驶园区物流车局部路径规划方法，以解决现有的局部路径规划难以满足实际需求，存在碰撞的危险性和适用性较差的问题。

为达到上述目的，本发明的技术方案是这样实现的：

一种无人驾驶园区物流车局部路径规划方法，包括如下步骤：S1、获取障碍物、全局参考路径以及自车位姿信息；

S2、通过动态滑动窗口算法对车辆在全局路径上进行定位，并获取车辆在全局路径上的定位点索引值，根据定位点索引值选择车辆前方若干个路径点作为局部路径；

S3、根据道路宽度，将步骤S2中得到的局部路径以设定间隔左右平移出多条局部路径，作为障碍物路段的备选路径；

S4、由步骤S3中得到的备选路径，通过障碍物碰撞检测算法筛选出车辆通行的所有局部路径；

S5、通过最优局部路径选择算法，从所有的通行路径中筛选出最优的局部避障路径。

进一步的，步骤S1中，在位置坐标系下，

障碍物信息中包含的障碍物以长方体框的形式呈现，其中，长方体框的底边四个顶点坐标和长方体中心点坐标表示为(x_i,y_i)，i＝0,1,...,4；

全局参考路径信息包含的路径点坐标为(x_m,y_m,θ_m,gear_m)，其中，x_m为横坐标，y_m为纵坐标，θ_m为航向角，gear_m为档位；

自车位姿信息中包含的自车位姿坐标为(x_n,y_n,θ_n)，x_n为横坐标，y_n为纵坐标，θ_n为航向角。

进一步的，步骤S2具体方法为：

将全局路径起始位置的10个点初始化为第一个滑动窗口，第一个滑动窗口坐标为(x_p,y_p,θ_p,gear_p,id_p)，其中，x_p为横坐标,y_p为纵坐标,θ_p为航向角,gear_p为档位,id_p为点在全局路径上的索引值，p＝0,1,...,9；

计算当前车辆定位点与滑动窗口中每个点的距离，其中，当前车辆定位点坐标表示为(x_current,y_current,θ_current)，计算距离公式为：

d＝sqrt((x_current-x_p)^2+(y_current-y_p)^2),i＝0,1,2,...,9；

距离d中的最小值d_min对应的滑动窗口上的点坐标为

即为车辆在全局路径上的定位点；

其中，

坐标即为定位点的索引值；

取第一个滑动窗口后面的10个点作为局部路径，这10个点即为下一次车辆在全局路径上定位的滑动窗口。

进一步的，步骤S3根据路径平移算法左右平移得出多条局部路径，作为障碍物路段的备选路径；其中，利用路径平移算法左右平移得出多条局部路径的方法为：

向右平移公式如下：

向左平移公式如下：

其中，(x_p,y_p,θ_p)是步骤S2中得到的局部路径点的横、纵坐标及航向角，其中p＝0,1,...,9，将主局部路径上的每一个点进行左右平移，每边平移出q条路经，那么j＝1,2,...,q，l为相邻两条局部路径之间的间隔。

进一步的，步骤S4中，利用障碍物碰撞检测算法筛选出车辆通行的所有局部路径的具体方法为：

将步骤S1中的障碍物坐标(x_i,y_i)，其中i＝0,1,...,4；将长方体框底面的每条边等分成n份，底面每条边利用n个点表示；对步骤S3中平移出的每条局部路径与障碍物长方体框进行碰撞检测，即将每条局部路径上的点与障碍物底面4条边上的点计算距离d，如果其中某条局部路径上存在一点与障碍物底边上任意一边的点的距离小于测量半径r，则这条局部路径被认为将会与障碍物发生碰撞，即不能将其加入最终的备选路径。

进一步的，步骤S5中的最优局部路径选择算法具体包括：

获取当前时刻局部路径的编号，如果此编号在下一时刻对应的局部路径还能继续通行，则优先选择此编号对应的局部路径；如果此编号在下一时刻对应的局部路径不能通行，则将离主局部路径最近的通行路径作为最优选择。

相对于现有技术，本发明所述的一种无人驾驶园区物流车局部路径规划方法具有以下有益效果：

本发明所述的一种无人驾驶园区物流车局部路径规划方法相对于其他局部路径规划算法安全性和实效性更好，经实车实验，可实现车辆能够安全、平稳、高效的进行避障，提高园区物流车辆自动驾驶性能，满足日常行驶需求。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例所述的直线路段的滑动窗口定位示意图；

图2为本发明实施例所述的曲线路段的滑动窗口定位示意图；

图3为本发明实施例所述的车辆行驶定位点示意图；

图4为本发明实施例所述的平移路径示意图；

图5为本发明实施例所述的所有局部路径示意图；

图6为本发明实施例所述的下一时刻对应的可通行局部路径一示意图；

图7为本发明实施例所述的下一时刻对应的可通行局部路径二示意图；

图8为本发明实施例所述的实车可视化测试图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。

下面将参考附图并结合实施例来详细说明本发明。

一种无人驾驶园区物流车局部路径规划方法，包括如下步骤：S1、获取障碍物信息、全局参考路径以及自车位姿信息；

S2、通过动态滑动窗口算法对车辆在全局路径上进行定位，并获取车辆在全局路径上的定位点索引值，根据定位点索引值选择车辆前方若干个路径点作为局部路径；

S3、根据道路宽度，将步骤S2中得到的局部路径以设定间隔左右平移出多条局部路径，作为障碍物路段的备选路径；

S4、由步骤S3中得到的备选路径，通过障碍物碰撞检测算法筛选出车辆通行的所有局部路径；

S5、通过最优局部路径选择算法，从所有的通行路径中筛选出最优的局部避障路径。

步骤S1中，在位置坐标系下，

障碍物信息中包含的障碍物以长方体框的形式呈现，其中，长方体框的底边四个顶点坐标和长方体中心点坐标表示为(x_i,y_i)，i＝0,1,...,4；

全局参考路径信息包含的路径点坐标为(x_m,y_m,θ_m,gear_m)，其中，x_m为横坐标，y_m为纵坐标，θ_m为航向角，gear_m为档位；

自车位姿信息中包含的自车位姿坐标为(x_n,y_n,θ_n)，x_n为横坐标，y_n为纵坐标，θ_n为航向角。

采用动态滑动窗口方法对车辆在全局路径上进行定位，并选择车辆前方10个路径点作为局部路径，动态滑动窗口方法的优势是实时性好，定位精度高，可以避免车辆定位到相邻车道(例如全局路径上拐弯的地方)；

(1)、直线路段的滑动窗口定位，如图1所示；

(2)、曲线路段的滑动窗口定位，如图2所示；

步骤S2具体方法为：

将全局路径起始位置的10个点初始化为第一个滑动窗口，第一个滑动窗口坐标为(x_p,y_p,θ_p,gear_p,id_p)，其中，x_p为横坐标,y_p为纵坐标,θ_p为航向角,gear_p为档位,id_p为点在全局路径上的索引值，p＝0,1,...,9；

计算当前车辆定位点与滑动窗口中每个点的欧式距离，其中，当前车辆定位点坐标表示为(x_current,y_current,θ_current)，计算欧式距离公式为：

d＝sqrt((x_current-x_p)^2+(y_current-y_p)^2),i＝0,1,2,...,9；

距离d中的最小值d_min对应的滑动窗口上的点坐标为

即为车辆在全局路径上的定位点；

其中，

坐标即为定位点的索引值；

取第一个滑动窗口后面的10个点作为局部路径，这10个点即为下一次车辆在全局路径上定位的滑动窗口。

步骤S2中，车辆在全局路径上定位的过程中存在以下问题：

A、车辆挡位发生变化时，车辆可能会定位到相邻车道；

B、车辆挡位发生变化时，车辆可能出现定位到其后面的点；

针对A情况，判断滑动窗口中的10个点是否挡位均一致，若挡位一致，则保留全部点；否则仅保留前面挡位一致的点，删掉后面挡位不一致的点；

针对B情况，如图3所示，车辆的当前位置为T点，行驶方向为箭头的方向，显然，B点是距离T点最近的点，但是D点是所需要的定位点(即滑动窗口的第一个点)；为此可以考虑，在平面坐标中，若T点距离D点近，由于D点的下一个点是E点，以X轴为例，(X_E-X_T)*(X_D-X_T)一定为非负数；若T点距离B点近，由于B点的下一个点是D点，则(X_D-X_T)*(X_B-X_T)一定为负数；因此可以通过正负号来获得车辆在全局路径上正确的定位点。

步骤S3根据路径平移算法左右平移得出多条局部路径，作为障碍物路段的备选路径；其中，利用路径平移算法左右平移得出多条局部路径的方法为：

向右平移公式如下：

向左平移公式如下：

其中，(x_p,y_p,θ_p)是步骤S2中得到的局部路径点的横、纵坐标及航向角，其中p＝0,1,...,9，将主局部路径(全局路径上的局部路径)上的每一个点进行左右平移，每边平移出q条路经，那么j＝1,2,...,q，l为相邻两条局部路径之间的间隔，平移出的路径如图4所示。

步骤S4中，利用障碍物碰撞检测算法筛选出车辆通行的所有局部路径的具体方法为：

在位置坐标系下，障碍物呈现为长方体框，底边四个顶点和长方体中心点五个点坐标可表示为{(x_i,y_i),i＝0,1,...,4}；将长方体框底面的每条边等分成n份，底面每条边利用n个点表示；对步骤S3中平移出的每条局部路径与障碍物长方体框进行碰撞检测，即将每条局部路径上的点与障碍物底面4条边上的点计算欧式距离d，如果其中某条局部路径上存在一点与障碍物底边上任意一边的点的距离小于测量半径r，则这条局部路径被认为将会与障碍物发生碰撞，即不能将其加入最终的备选路径，如图5所示。

步骤S5中的最优局部路径选择算法具体包括：

获取当前时刻局部路径的编号，如果此编号在下一时刻对应的局部路径还能继续通行，则优先选择此编号对应的局部路径(编号中正负代表方向，数字代表顺序)，这样会使车辆行驶比较平稳，不会左右跳变，如图6所示；如果此编号在下一时刻对应的局部路径不能通行，则将离主局部路径最近的通行路径作为最优选择，如图7所示。

基于本专利发明的局部路径规划方法，经过实车测试，发现车辆能够安全、平稳、高效地进行避障，相对于其他局部路径规划算法在安全性和实效性更好，可以通过Rviz三维可视化软件对实车测试数据进行可视化，如图8所示。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种无人驾驶园区物流车局部路径规划方法，其特征在于，包括如下步骤：

S1、获取障碍物、全局参考路径以及自车位姿信息；

S2、通过动态滑动窗口算法对车辆在全局路径上进行定位，并获取车辆在全局路径上的定位点索引值，根据定位点索引值选择车辆前方若干个路径点作为局部路径；

S3、根据道路宽度，将步骤S2中得到的局部路径以设定间隔左右平移出多条局部路径，作为障碍物路段的备选路径；

S4、由步骤S3中得到的备选路径，通过障碍物碰撞检测算法筛选出车辆通行的所有局部路径；

S5、通过最优局部路径选择算法，从所有的通行路径中筛选出最优的局部避障路径。

2.根据权利要求1所述的一种无人驾驶园区物流车局部路径规划方法，其特征在于：步骤S1中，在位置坐标系下，

障碍物信息中包含的障碍物以长方体框的形式呈现，其中，长方体框的底边四个顶点坐标和长方体中心点坐标表示为(x_i,y_i)，i＝0,1,...,4；

全局参考路径信息包含的路径点坐标为(x_m,y_m,θ_m,gear_m)，其中，x_m为横坐标，y_m为纵坐标，θ_m为航向角，gear_m为档位；

自车位姿信息中包含的自车位姿坐标为(x_n,y_n,θ_n)，x_n为横坐标，y_n为纵坐标，θ_n为航向角。

3.根据权利要求2所述的一种无人驾驶园区物流车局部路径规划方法，其特征在于，步骤S2具体方法为：

将全局路径起始位置的10个点初始化为第一个滑动窗口，第一个滑动窗口坐标为(x_p,y_p,θ_p,gear_p,id_p)，其中，x_p为横坐标,y_p为纵坐标,θ_p为航向角,gear_p为档位,id_p为点在全局路径上的索引值，p＝0,1,...,9；

计算当前车辆定位点与滑动窗口中每个点的距离，其中，当前车辆定位点坐标表示为(x_current,y_current,θ_current)，计算距离公式为：

d＝sqrt((x_current-x_p)^2+(y_current-y_p)^2),i＝0,1,2,...,9；

距离d中的最小值d_min对应的滑动窗口上的点坐标为

即为车辆在全局路径上的定位点；

其中，

坐标即为定位点的索引值；

取第一个滑动窗口后面的10个点作为局部路径，这10个点即为下一次车辆在全局路径上定位的滑动窗口。

4.根据权利要求3所述的一种无人驾驶园区物流车局部路径规划方法，其特征在于：步骤S3根据路径平移算法左右平移得出多条局部路径，作为障碍物路段的备选路径；其中，利用路径平移算法左右平移得出多条局部路径的方法为：

向右平移公式如下：

向左平移公式如下：

其中，(x_p,y_p,θ_p)是步骤S2中得到的局部路径点的横、纵坐标及航向角，其中p＝0,1,...,9，将主局部路径上的每一个点进行左右平移，每边平移出q条路经，那么j＝1,2,...,q，l为相邻两条局部路径之间的间隔。

5.根据权利要求4所述的一种无人驾驶园区物流车局部路径规划方法，其特征在于，步骤S4中，利用障碍物碰撞检测算法筛选出车辆通行的所有局部路径的具体方法为：

将步骤S1中的障碍物坐标(x_i,y_i)，其中i＝0,1,...,4；将长方体框底面的每条边等分成n份，底面每条边利用n个点表示；对步骤S3中平移出的每条局部路径与障碍物长方体框进行碰撞检测，即将每条局部路径上的点与障碍物底面4条边上的点计算距离l，如果其中某条局部路径上存在一点与障碍物底边上任意一边的点的距离小于测量半径r，则这条局部路径被认为将会与障碍物发生碰撞，即不能将其加入最终的备选路径。

6.根据权利要求5所述的一种无人驾驶园区物流车局部路径规划方法，其特征在于：步骤S5中的最优局部路径选择算法具体包括：

获取当前时刻局部路径的编号，如果此编号在下一时刻对应的局部路径还能继续通行，则优先选择此编号对应的局部路径；如果此编号在下一时刻对应的局部路径不能通行，则将离主局部路径最近的通行路径作为最优选择。

Images