物流配送车自适应运动规划方法、设备及存储介质

技术领域

本发明涉及物流配送车路径规划技术优化领域，特别是一种物流配送车自适应运动规划方法、设备及存储介质。

背景技术

随着自动驾驶技术的快速发展，无人驾驶车辆在生活中已经不再是空中楼阁，而路径规划技术作为物流配送车的关键技术，一直是自动驾驶领域研究的热点和难点。路径规划旨在考虑时效性的前提下规划出一条从起点到目标点的安全无碰撞路径，根据算法实现的特点，可分为全局路径规划和局部路径规划两类。

目前全局路径规划常用算法有跳点搜索算法，该算法避免了大量冗余节点的计算，有效的提升了计算速度，但由于该算法使用传统的欧氏距离作为启发式函数，计算得出的预估代价会低于实际的路径成本，进而增加了全局路径规划用时，故传统JPS存在启发式函数效率较低、规划时间长等问题，且将马氏距离处理低地形信息应用于RRT*的路径规划方法在处理复杂环境或窄缝的情况时，由于需要选择重连，导致计算量较大，生成路径的效率较低，而由于RRT*算法本身对于起始点和目标点的要求较高的特性，如果这两点位置选择不当，可能会导致无法找到有效的路径；局部路径规划常用算法有动态窗口算法，该算法因其考虑了机器人的运动学模型以及具有生成的路径平滑等特点，被广泛用于复杂环境的局部路径规划中，但在面对复杂情况多变的环境时，由于轨迹评价系数固定不变，该算法难以规划出较优的局部路径，甚至出现无法规划的情况，具有参数难以调整、适应性差等缺点。此外，发明人在实车实验中发现，当该算法的局部规划路径最终输出至物流配送车控制模块执行时，在场景单一、障碍物稀少的环境下存在仍然保持较为保守的速度行驶，缺乏效率的问题。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种物流配送车自适应运动规划方法、设备及存储介质，解决物流配送车路径规划中效率低、适应性差、速度保守的问题。

为解决上述技术问题，本发明所采用的技术方案是：一种物流配送车自适应运动规划方法，包括以下步骤：

S1、物流配送车获取配送终点后，基于马哈拉诺比斯距离和夹角信息，使用马氏距离计算函数作为启发式函数，得到优化启发式函数的跳点搜索全局路径；

S2、实时获取采集到的激光雷达环境信息，在优化启发式函数的跳点搜索全局路径基础上，建立当前环境下基于避免末端位姿碰撞原则的优化速度采样空间，根据采样速度分别进行轨迹推算，得到最终可行轨迹集合；

S3、在所述最终可行轨迹集合中选择适合物流配送车在当前环境下行驶的速度和角速度；

S4、对速度的激进程度进行选择，确定下一配送地点。

本发明首先在规划物流配送车全局路径时使用马哈拉诺比斯距离替换传统的欧式距离作为启发式函数，能够更准确地估计预估路径的距离长度，保证了快件配送分发的高效性；在局部路径规划时优化了速度采样空间，减少了物流配送车与障碍物发生碰撞的可能性；然后对当前路况的各方面复杂性进行量化分析，计算得到各项环境复杂系数，并以此对轨迹评价系数进行相应修改，保证了不同路况下速度选择的适应性与最优性；最后在输出速度指令前，进行简单路况直线行驶判定，如果是则适当提升速度指令大小，否则维持原速，提升幅度与环境复杂系数负相关，保证了简单路况下物流配送任务的时效性。

步骤S1的具体实现过程包括：

S1.1、获取物流配送车在带障碍物的全局栅格地图中的当前坐标，即得到路径规划起点S，确定路径规划终点D；

S1.2、初始化开放列表，并将所述路径规划起点S加入所述开放列表中；所述开放列表存储有路径规划的中间点坐标；

S1.3、遍历开放列表中现存所有点的坐标，分别对路径规划起点S到遍历对象点、遍历对象点到路径规划终点D计算实际代价值和预估代价值；

S1.4、查找所述开放列表中评价函数值最低的节点A，判断所述节点A是否为路径规划终点，若是，则结束；否则，基于节点A的父节点方向，按照上、下、左、右四个方向的先后次序进行直线搜索跳点，再按照左上、右上、右下、左下四个方向的先后次序进行斜向搜索跳点，若在某一方向上搜索到跳点，则将该跳点的坐标加入开放列表中，并在该已搜索到跳点的方向上停止搜索；

S1.5、重复步骤S1.3～S1.4，直至搜索到路径规划终点D，追溯路径规划终点D的父节点至路径规划起点S，并连接追溯过程中的所有中间节点，得到优化启发式函数的跳点搜索全局路径。

本发明通过改进启发式函数的优化JPS算法进行全局路径规划，克服了传统JPS算法启发式函数效率较低的问题，减少了规划时间，提高了物流配送车全局路径的搜索效率。

步骤S1.4中，评价函数值的计算公式为：

F(n)＝G(n)+H(n)；

其中，F(n)表示当前节点n的评价函数值，G(n)表示路径规划起点S到当前节点n的实际代价值，即实际最短距离，H(n)表示当前节点n到路径规划终点D的预估代价值，H(n)＝α·L(x

本发明使用马哈拉诺比斯距离函数替代传统的欧式距离函数，考虑到各变量之间的联系，并独立于测量尺度，当协方差矩阵为单位矩阵时，马氏距离退化为欧式距离，使用马氏距离作为跳点搜索时的启发式函数，能够更准确地预估中间点到终点的路径距离，提高搜索效率。

步骤S2的具体实现过程包括：

S2.1、确定物流配送车未来可行的速度采样空间V

S2.2、在速度采样空间V

其中v为初始速度，w为初始角速度，a

S2.3、将采样结果代入物流配送车运动模型进行轨迹推算，得到预测轨迹集合T

本发明基于轨迹末端时刻的速度与方向信息对速度采样空间进行了优化，去除了其中在紧急制动情况下会与障碍物发生碰撞的轨迹，减小了物流配送车碰撞风险；同时使用不同于传统等距采样的基于障碍物密度判断的不等间距采样规则，在障碍物密集区域能够产生更多可行轨迹，以保证最终结果的最优性。

步骤S2.3中，移除原预测轨迹集合T

1)通过速度采样空间获得物流配送车预测轨迹的末速度v

2)物流配送车制动过程中在全局地图上划过面积的点集Z：

其中

3)将点集Z转换到全局栅格地图中，若点集Z与全局栅格地图上的障碍物重复，则将点集Z对应的预测轨迹从预测轨迹集合T

本发明考虑物流配送车车身宽度，在末端时刻的速度与方向上模拟紧急制动，将制动过程中的划过的面积，即会发生碰撞的区域，与全局地图中的障碍物区域对比，如果发现两者重叠，则说明有发生碰撞的风险，需要将该轨迹从可行轨迹集合中去除，以确保物流配送车轨迹的安全性。

步骤S3中，在所述最终可行轨迹集合中选择适合物流配送车在当前环境下行驶的速度和角速度的具体实现过程包括：

S3.1、物流配送车搭载的激光雷达设备向周围环境发射激光束，并通过接收反射回的激光信号，测量出物流配送车与周围物体之间的距离和角度信息，以及周围环境的详细三维信息，对获取的数据结果进行格式转换、噪声滤除；

S3.2、对经过格式转换和噪声滤除后的数据进行分析，得到障碍物数量及分布情况、障碍物种类、障碍物密度、天气与道路条件指标，由此综合计算出三类环境复杂系数：道路结构复杂度C

本发明通过激光雷达、温湿度等传感器获取障碍物数量及分布情况、障碍物种类、道路结构复杂度、障碍物密度、天气与道路条件等可以表示路况复杂程度的信息，并将其量化后综合计算得到当前路况的各项环境复杂系数，然后以此作为依据对轨迹评价函数中的轨迹评价系数因地制宜地进行修正，得到最适应当下路况的轨迹，实现了不同环境下的自适应性。

计算三类环境复杂系数并优化轨迹评价系数的过程包括：

道路结构复杂度C

其中ε

天气条件系数C

其中T

障碍物复杂度计算公式如下：

C

其中，N

其中λ

本发明基于上述求得的基于路况的环境复杂系数对轨迹评价函数中的参数进行一定程度范围内的调整，使用双曲正切函数将环境复杂系数映射到[0,1]范围，再在设定评价参数最大最小值的基础上调整，保证系数在合理范围内的同时，又能对不同的路况环境进行自适应地调整，解决传统动态窗口算法适应性差的问题。

步骤S4中，对速度的激进程度进行选择，确定下一配送地点的具体实现过程包括：

S4.1、采集当前物流配送车实际的前向x方向线速度v

S4.2、使用调和平均数计算当前时刻与下一时刻的平均速度，结合障碍物复杂度C

其中v

S4.3、按照上述速度选择策略行驶至目标终点后，自动选择下一配送地点，重复步骤S1～S4进行全局、局部路径规划。

本发明在最优轨迹相应的速度指令输出给物流配送车控制模块前，对当前的路况、速度大小及转弯情况进行判断，当前述的环境复杂系数小于一定阈值、侧方向当前时刻与后一时刻的速度以及角速度的调和平均数小于一定阈值且物流配送车前向当前时刻与后一时刻的调和平均数大于一定阈值时，认为物流配送车处于简单路况下较高速直线行驶过程，基于路况的复杂性适当提高物流配送车各项速度，以保证物流配送车行驶过程的时效性。

作为一个发明构思，本发明还提供了一种计算机装置，包括存储器、处理器及存储在存储器上的计算机程序；所述处理器执行所述计算机程序，以实现本发明上述方法的步骤。

作为一个发明构思，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序/指令，所述计算机程序/指令被处理器执行时，实现本发明上述方法的步骤。

与现有技术相比，本发明所具有的有益效果为：

1)本发明通过改进启发式函数的优化JPS算法进行全局路径规划。使用马哈拉诺比斯距离函数替代传统的欧式距离函数，考虑到各变量之间的联系，并独立于测量尺度，当协方差矩阵为单位矩阵时，马氏距离退化为欧式距离，使用马氏距离作为JPS搜索时的启发式函数，能够更准确地预估中间点到终点的路径距离，提高了搜索效率，克服了传统JPS算法启发式函数效率较低的问题，减少了规划时间，提高了物流配送车全局路径的搜索效率。

2)本发明基于轨迹末端时刻速度与方向信息对DWA局部路径规划时建立的速度采样空间进行了优化，去除了其中在紧急制动情况下会与障碍物发生碰撞的轨迹，减小了物流配送车碰撞风险；同时使用不同于传统等距采样的基于障碍物密度判断的不等间距采样规则，在障碍物密集区域能够产生更多可行轨迹，以保证最终结果的速度最优性。

3)本发明通过激光雷达、温湿度等传感器获取障碍物数量及分布情况、障碍物种类、道路结构复杂度、障碍物密度、天气与道路条件等可以表示路况复杂程度的信息，并将其量化后综合计算得到当前路况的环境复杂系数，然后以此为依据对DWA轨迹评价函数中的轨迹评价系数因地制宜地进行修正，得到最适应当下路况的轨迹，克服了传统动态窗口法适应性差的特点，实现了不同路况下的自适应性。

4)本发明在最优轨迹相应的速度指令输出给物流配送车控制模块前，对当前的路况、速度大小及转弯情况进行判断，当前述的环境复杂系数小于一定阈值、侧方向当前时刻与后一时刻的速度以及角速度的调和平均数小于一定阈值且物流配送车前向当前时刻与后一时刻的调和平均数大于一定阈值时，认为物流配送车处于简单路况下较高速直线行驶过程，基于路况的复杂性适当提高物流配送车的各项速度，解决了实车实验时物流配送车在保守路径规划的控制作用下即使身处场景单一的环境，依旧保持极低的速度行驶的问题，极大提高了物流配送车行驶过程的时效性。

附图说明

图1为本发明实施例面向室外复杂环境的物流配送车自适应运动规划方法流程图；

图2为本发明实施例改进启发式函数的跳点搜索全局路径规划流程图；

图3为本发明实施例地下停车场场景改进规划的物流配送车全局路径；

图4为本发明实施例改进前后全局路径规划用时对比折线图；

图5为本发明实施例不等距速度采样规则示意图；

图6为本发明实施例物流配送车碰撞点集计算示意图；

图7为本发明实施例优化速度空间和考虑路况的局部路径规划流程图；

图8为本发明实施例某次规划过程环境系数和评价系数折线图；

图9为本发明实施例自适应动态窗口法规划的局部路径示意图；

图10为本发明实施例某次规划过程各项速度量折线图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

本实施例提供了一种面向室外复杂环境的物流配送车自适应运动规划方法，本发明实施例首先在规划物流配送车全局路径时使用马哈拉诺比斯距离替换传统的欧式距离作为启发式函数，能够更准确地估计预估路径的距离长度，保证了快件配送分发的高效性；在局部路径规划时优化了速度采样空间，减少了物流配送车与障碍物发生碰撞的可能性；然后对当前路况的各方面复杂性进行量化分析，计算得到各项环境复杂系数，并以此对轨迹评价系数进行相应修改，保证了不同路况下速度选择的适应性与最优性；最后在输出速度指令前，进行简单路况直线行驶判定，如果是则适当提升速度指令大小，否则维持原速，提升幅度与环境复杂系数负相关，保证了简单路况下物流配送任务的时效性。如图1所示，包括以下步骤：

步骤一：物流配送车获取配送终点后，需要使用跳点搜索算法进行全局路径规划时，基于马哈拉诺比斯距离和夹角信息，使用马氏距离计算函数作为启发式函数，更准确地计算估计路径长度，以此进行改进全局路径规划，有效提高了跳点搜索全局路径规划算法是搜索效率。流程如图2所示：

步骤1，物流配送车经过感知、建图、定位等过程，建立了带障碍物的全局栅格地图，并获得物流配送车在该图下的当前坐标(x，y)，取路径规划起点S＝(10，10)，然后通过物流配送车上位机给出的2D目标点，即路径规划终点取为D＝(20,20)；

步骤2，在物流配送车工控机中初始化列表OpenList和CloseList用于存储路径规划的中间点坐标，同时将起始点S加入开放列表OpenList中；

步骤3，判断OpenList是否为空，如果为空说明地图上已经没有需要搜索的点了，但此时仍没有搜索到终点，即搜索失败，否则遍历OpenList中现存所有点的坐标，分别对起始点S到遍历对象点、遍历对象点到终点D计算实际代价G(n)和估计代价H(n)以及总代价F(n)，具体过程如下：

当前遍历对象点节点n的评价函数值F(n)的计算公式为：

F(n)＝G(n)+H(n)

其中，G(n)表示起点S到当前节点n的实际代价值，也即实际最短距离，H(n)表示当前节点n到终点D的预估代价值，也称为启发式函数。

G(n)通过节点n追溯父节点并计算直线距离求和即可得到。

使用马哈拉诺比斯距离函数作为启发式函数，并融合当前节点n、起点S、终点D的重合度信息，H(n)计算方法为：

H(n)＝α·L(x

其中，α为距离信息权重值，β为方向信息权重值，取值根据实际车辆情况确定，本实例取α＝0.9、β＝0.5，S(n，S，D)表示节点n、起点S、终点D围成的面积，反映了已知路径加上预计直线路径与起终点直线路径的重合度，约束规划路径不偏离直线路径太多，L(x

计算当前节点n与终点D之间马氏距离L(x

其中，x

步骤4，查找出OpenList中评价函数计算值F(n)最低的节点N

步骤5，判断N

步骤6，判断搜索到的第一个跳点是否已经存在于OpenList中，如果是则说明此前已经搜索过该跳点，因此选择其中G(n)值较小的路径，即起点S到当前跳点距离更近路径保存，更新该跳点的更优实际代价G(n)和父节点，否则将新跳点添加至OpenList中并计算记录其G(n)值和父节点；

步骤7，重复步骤3-6，直至搜索成功或搜索失败，搜索成功的全局路径作为后续局部路径规划的基础，图3为实际场景的四组不同始终点的全局路径规划结果，在实车实验时采集了相同起终点的改进前后的全局路径规划时间，制成曲线如图4所示。

本发明实施例通过改进启发式函数的优化JPS算法进行全局路径规划，克服了传统JPS算法启发式函数效率较低的问题，减少了规划时间，提高了物流配送车全局路径的搜索效率。

步骤二：实时获取采集到的激光雷达环境信息，在生成的全局路径基础上，建立当前环境下避免末端位姿碰撞的速度采样空间，然后根据采样速度分别进行轨迹推算：

步骤1，依据物流配送车当前的位置和速度信息，以速度和角速度一一对应为一组元素(v，ω)，按照传统DWA算法初步确定物流配送车未来可行的速度采样空间Vs；

步骤2，在速度采样空间Vs中，区别于一般的等间距均匀采样，使用优化修改后的不等采样间距规则进行速度采样，其规则为在原有传统速度采样规则基础上，在速度角速度元素(v±a

其中v为初始速度，w为初始角速度，a

步骤3，将采样结果代入物流配送车运动模型进行未来轨迹推算，预测时间Δt＝3s，得到预测轨迹集合T

本发明实施例基于轨迹末端时刻的速度与方向信息对速度采样空间进行了优化，去除了其中在紧急制动情况下会与障碍物发生碰撞的轨迹，减小了物流配送车碰撞风险；同时使用不同于传统等距采样的基于障碍物密度判断的不等间距采样规则，在障碍物密集区域能够产生更多可行轨迹，以保证最终结果的最优性。

步骤3.1，依据上述数据代入物流配送车运动学模型可得到物流配送车制动过程中在全局地图上划过面积的点集Z，如图6所示，数学表达如下：

其中

步骤3.2，将点集Z转换到全局栅格地图中，如果与占据栅格地图上的障碍物存在重复，则说明在最大紧急制动加速度作用下极有可能与障碍物发生碰撞，需要将该轨迹从可行轨迹集合T

本发明实施例考虑物流配送车车身宽度，在末端时刻的速度与方向上模拟紧急制动，将制动过程中的划过的面积，即会发生碰撞的区域，与全局地图中的障碍物区域对比，如果发现两者重叠，则说明有发生碰撞的风险，需要将该轨迹从可行轨迹集合中去除，以确保物流配送车轨迹的安全性。

步骤三：依据室外环境非结构化的特点，使用激光雷达和各传感器信息对当前环境的各种复杂情况进行量化计算，以此对不同复杂环境下局部路径规划算法的轨迹评价系数进行调整，在推算轨迹的集合中自适应选择更适合物流配送车在当前环境下行驶的速度和角速度。整体流程如图7所示：

步骤1，物流配送车搭载的激光雷达设备向周围环境发射激光束，并通过接收反射回来的激光信号，测量出车辆与周围物体之间的距离和角度信息以及周围环境的详细三维信息，将获取的数据结果进行格式转换、噪声滤除后传输给物流配送车工控机；

步骤2，物流配送车工控机对数据进行处理分析可以得到障碍物数量及分布情况、障碍物种类、障碍物密度、天气与道路条件指标，由此综合计算出三类环境复杂系数：道路结构复杂度V

步骤2.1，计算道路结构复杂度V

其中ε

步骤2.2，计算天气条件系数C

其中T

步骤2.3，障碍物复杂度C

C

步骤3，依据计算得出的环境复杂系数对可行轨迹集合T

步骤3.1，完成采样轨迹推算后，DWA算法通过评价函数对每一条轨迹进行评价，DWA评价函数G(v，w)定义如下：

G(v，w)＝λ（G

其中Head(v，w)表示物流配送车航向角与目标方向的差值，Dist(v，w)表示与物流配送车最近障碍物的距离，Velo(v，ω)表示物流配送车的速度大小，而λ(C

步骤3.2，计算λ(M)、μ(M)、σ(M)方法如下：

其中λ

步骤4，对可行轨迹集合T

步骤四：通过车辆搭载的IMU采集当前物流配送车的速度、角速度以及相应的控制指令等信息，并以此进行简单路况直线行驶判定，对速度选择的激进程度进行自适应决策：

步骤4.1，通过车辆搭载的IMU采集当前物流配送车的前向和侧向线速度和角速度v

步骤4.2，使用调和平均数计算当前时刻与下一时刻的平均速度，并结合障碍物复杂度C

其中v

本发明实施例在最优轨迹相应的速度指令输出给物流配送车控制模块前，对当前的路况、速度大小及转弯情况进行判断，当前述的环境复杂系数小于一定阈值、侧方向当前时刻与后一时刻的速度以及角速度的调和平均数小于一定阈值且物流配送车前向当前时刻与后一时刻的调和平均数大于一定阈值时，认为物流配送车处于简单路况下较高速直线行驶过程，基于路况的复杂性适当提高物流配送车各项速度，以保证物流配送车行驶过程的时效性。

实施例2

本发明实施例2提供一种对应上述实施例1的计算机设备，计算机设备可以是用于客户端的处理设备，例如手机、笔记本电脑、平板电脑、台式机电脑等，以执行上述实施例的方法。

本实施例的计算机设备包括存储器、处理器及存储在存储器上的计算机程序；处理器执行存储器上的计算机程序，以实现上述实施例1方法的步骤。

在一些实现中，存储器可以是高速随机存取存储器(RAM：Random AccessMemory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。

在另一些实现中，处理器可以为中央处理器(CPU)、数字信号处理器(DSP)等各种类型通用处理器，在此不做限定。

实施例3

本发明实施例3提供了一种对应上述实施例1的计算机可读存储介质，其上存储有计算机程序/指令。计算机程序/指令被处理器执行时，实现上述实施例1方法的步骤。

计算机可读存储介质可以是保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。本申请实施例中的方案可以采用各种计算机语言实现，例如，面向对象的程序设计语言Java和直译式脚本语言JavaScript等。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本申请的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本申请范围的所有变更和修改。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。