CN117232511A - 一种局部路径规划方法、存储介质及设备 - Google Patents

Abstract

本发明公开了一种局部路径规划方法、存储介质及设备，步骤S1、对每帧图像进行检测，判断是否检测到动态物体；步骤S2、若检测到则建立对应的物体运动模型，根据物体运动模型建立对应的卡尔曼滤波模型，如果未检测到则转步骤S5；步骤S3、对每个动态物体的状态进行预测和更新，并以时间为变量计算一段时间内运动物体的预测路径；步骤S4、对动态窗口法采样的线速度‑角速度组进行路径推算；步骤S5、使用改进的路径评价函数对路径进行评价，选取评价最高的路径的线速度‑角速度组作为运动指令发送给机器人执行。优点：使动态窗口法可以在动态物体较多且运动速度较快的环境中能够稳定安全运行；可以准确地评估出一条最优的局部路径。

Description

一种局部路径规划方法、存储介质及设备

技术领域

本发明涉及一种局部路径规划方法、存储介质及设备，属于计算机视觉和机器人技术领域。

背景技术

局部路径规划是指在已知机器人当前位置和目标位置的情况下，规划机器人在局部范围内的运动轨迹，使其能够避开障碍物、保持安全距离，同时满足其他运动约束条件，最终到达目标位置。动态窗口法是一种典型的局部路径规划算法，其主要思想是在机器人运动过程中动态调整机器人的速度和转向角度，以保持机器人与障碍物之间的安全距离，从而避免碰撞。动态窗口法的局限性在于机器人的速度和转向角度是基于当前的障碍物位置和速度等信息计算出来的。但是，这种计算方法并没有考虑到障碍物可能会发生运动，从而导致计算出来的机器人运动轨迹不准确。当障碍物运动速度较快、运动方向复杂时，机器人的规划路径可能会发生偏差，导致机器人与障碍物发生碰撞。

为了解决这种局限性，通常在动态窗口法中加入对动态障碍物的未来状态的预测，以有效处理动态障碍物对机器人路径规划的负面影响。例如，现有技术的方法首先将最小转弯半径约束条件融合至DWA算法的速度和角速度约束中，然后采用速度障碍法，计算到当机器人和动态障碍物的距离小于设定的危险距离时，进行危险状态的评定。如果处于危险状态，则进行轨迹模拟和机器人与障碍物的速度信息的未来碰撞预测评估，该方法可以剔除碰撞风险较大的线速度和角速度。该方法基于对动态障碍物的瞬时速度的轨迹预测和机器人推算轨迹进行碰撞评估，没有考虑到障碍物的角速度，对于具有复杂运动的障碍物环境下，将导致障碍物轨迹的预测并不准确或者说最后评估出来的当前最优机器人线速度和角速度可能并不理想，最后可能导致机器人与动态障碍物发生碰撞。

现有技术中将动态障碍物与当前线速度和角速度组的模拟轨迹的垂直距离加入到算法评价函数计算当中，将其与静态障碍物的距离考量分开，可以设置不同的安全距离，在一定程度上可以提高通过动态障碍物的成功率，但是存在局限，局限在于模拟轨迹与动态障碍物的设定安全距离无法自适应动态障碍物的速度，而动态障碍物速度的不确定性太大，可能导致无法合理评估采样的线速度-角速度组，从而生成不合理的速度组导致与动态障碍物发生碰撞。

发明内容

本发明所要解决的技术问题是克服现有技术的缺陷，提供一种局部路径规划方法、存储介质及设备。

为解决上述技术问题，本发明提供一种局部路径规划方法，包括：

步骤S1、使用训练好的YOLO目标检测器对相机采集到的每帧图像进行检测，判断是否检测到可能的动态物体；

步骤S2、若步骤S1检测到可能的动态物体，对可能的动态物体建立对应的物体运动模型，根据物体运动模型建立对应的卡尔曼滤波模型，如果没有检测到可能的动态物体，则转步骤S5；

步骤S3、根据步骤S2建立的卡尔曼滤波模型对每个动态物体的状态进行预测和更新，并以时间为变量计算一段时间内运动物体的预测路径；

步骤S4、得到步骤S3运动物体的预测路径后，对动态窗口法采样的线速度-角速度组进行路径推算；

步骤S5、使用改进的路径评价函数对动态窗口法采样的线速度-角速度组推算出来的路径进行评价，选取评价最高的路径的线速度-角速度组作为运动指令发送给机器人执行。

进一步的，所述步骤S1，具体包括：

使用训练好的YOLO目标检测器对相机采集到的每帧图像进行检测，判断是否检测到可能的动态物体。

进一步的，所述步骤S3，具体包括：

步骤S3-1、对检测到的动态物体，依次编号为1-n；

步骤S3-2、以设定时间dt为间隔计算动态物体一段时间的预测路径，并统计此次计算所花费的时间errt1，对于预测的路径保存为P_i(t)＝(x,y)，其中i表示为第i个动态物体，x,y为t个时间dt时刻的二维坐标点。

进一步的，所述步骤S4具体包括：

步骤S4-1、动态窗口法采样的线速度-角速度组的同样以时间dt为间隔推算路径保存为p(v,w,t)＝(x,y)，并统计此次计算所花费的时间为errt2，其中v是动态窗口法采样得到的线速度，w是动态窗口法采样得到的角速度，x,y是t个时间dt时刻的二维坐标点；

步骤S4-2、计算步骤S4-1推算的路径(t_2start+errt2)到t_2end时间段的位置与步骤S3-2推算出来运动物体的路径(t_1start+errt1+errt2)到t_2end时间段的位置的对应同时刻最小的距离保存为ddist(v,w)，ddist(v,w)作为改进的路径评价函数中的动态物体距离平均函数，定义为：

其中，mdist(v,w)定义为：

mdist(v,w)＝min{d(i,t₁,v,w,t₂)|1≤i≤n，t_1start+err1+err2≤t₁≤t_2end，t_2start+err2≤t₂≤t_2end} (2)

其中，d(i,t₁,v,w,t₂)定义为：

其中，t_1start和t_1end、t_2start和t_2end分别是运动物体路径P_i(t)和动态窗口法采样路径P(v,w,t)的开始时间和结束时间，errt1,errt2是两阶段的花费时长r是设置的与动态障碍物的安全距离，r设置为机器人的制动距离。

进一步的，所述步骤S4具体包括：

所述步骤S5具体包括：

步骤S5-1、构建改进的路径评价函数G(v,w)，表示为：

G(v,w)＝σ(α*h(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*ddist(v,w)) (4)

其中，h(v,w)是机器人方向评价函数，用于预测路径末端位置机器人方向角与终点之间的角度差值的绝对值；dist(v,w)是与静态物体距离评价函数，用于预测路径末端位置与静态物体的距离；vel(v,w)是速度评价函数，用于表示当前采样的速度大小，α，β，γ，δ分别为机器人方向评价函数、静态物体距离评价函数、速度评价函数、动态物体距离平均函数的权重系数，δ是平滑系数；

步骤S5-2、使用步骤S5-1构建的改进的路径评价函数对窗口采样的速度推算的路径进行评价，选择评价最优路径对应的速度，将选出的速度发送给机器人执行。

一种存储一个或多个程序的计算机可读存储介质，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行任一所述方法。

一种计算机设备，包括，

一个或多个处理器、存储器以及一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行任一所述方法的指令。

本发明所达到的有益效果：

1)本发明利用YOLO检测器快速检测到潜在动态物体，然后使用卡尔曼滤波计算出动态物体的运动路径，将动态窗口法采样的路径与计算得到的动态物体路径进行同时刻距离计算得到最小距离，将其加入路径评价指标中，使动态窗口法可以在动态物体较多且运动速度较快的环境中能够稳定安全运行。

2)本发明通过连续多帧图像信息相对准确地得到动态物体的运动路径，利用得到的动态物体模拟路径作为算法推算的局部路径评价标准，可以准确地评估出一条最优的局部路径。

3)本发明采用机器人制动距离作为安全距离，可以适应多种场景。

附图说明

图1为本发明改进的动态窗口法流程图；

图2为本发明通过卡尔曼滤波法从连续多帧图像预测运动路径流程图；

图3为本发明所阐述的局部路径与动态物体距离评价方法示意图。

具体实施方式

下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。

实施例1

本发明公开了一种局部路径规划方法，整体流程如图1所示，将目标检测器YOLO、卡尔曼滤波、改进动态窗口法相结合，用YOLO快速检测出到相机采集到的图像中的可能动态物体，利用卡尔曼滤波对动态物体的运动路径进行预测，计算动态窗口法采样的速度的局部路径和动态物体的路径在同时刻的最短距离，将其作为局部路径删选标准之一。

为了达到上述目的，本发明采取如下技术方案，方案包括以下步骤：

步骤S1、使用现有的训练好的YOLO目标检测器对相机采集到的每帧图像进行检测，判断是否检测到可能的动态物体，例如人、车辆等；

步骤S2、若步骤S1检测到可能的动态物体，使用光流法建立对应的运动模型，将物体的运动模型输入到卡尔曼滤波算法得到对应卡尔曼滤波模型，如果没有检测到可能的动态物体，则转步骤S5；

步骤S3、根据步骤S2建立的卡尔曼滤波模型对每个动态物体的状态进行预测和更新，并以时间为变量计算一段时间内运动物体的预测路径；

步骤S4、得到步骤S3运动物体的预测路径后，对动态窗口法采样的线速度-角速度组进行路径推算，计算同时刻运动物体和路径的最小距离作为评价指标加入评价函数中；

步骤S5、使用改进的评价函数对动态窗口法采样的线速度-角速度组推算出来的路径进行评价，选取评价最高的路径的线速度-角速度组作为运动指令发送给机器人执行；

所述步骤S3如图2所示，具体包括：

步骤S3-1、对检测到的动态物体，则依次编号为1-n；

步骤S3-2、以设定时间dt为间隔计算动态物体一段时间的预测路径，并统计此次计算所花费的时间errt1，对于预测的路径保存为P_i(t)＝(x,y)，其中i表示为第i个动态物体，x,y为t个时间dt时刻的二维坐标点；

所述步骤S4中与动态物体距离评价方法如图3所示，具体包括：

步骤S4-1、动态窗口法采样的线速度-角速度组的同样以时间dt为间隔推算路径保存为p(v,w,t)＝(x,y)，并统计此次计算所花费的时间为errt2，其中v是线速度，w是角速度，x,y时t个时间dt时刻的二维坐标点；

步骤S4-2、计算步骤S4-1推算的路径(t_2start+errt2)到t_2end时间段的位置与步骤S3-2推算出来运动物体的路径(t_1start+errt1+errt2)到t_2end时间段的位置的对应同时刻最小的距离保存为ddist(v,w)，ddist(v,w)定义为：

其中，mdist(v,w)定义为：

mdist(v,w)＝min{d(i,t₁,v,w,t₂)|1≤i≤n，t_1start+err1+err2≤t₁≤t_2end，t_2start+err2≤t₂≤t_2end} (2)

其中，d(i,t₁,v,w,t₂)定义为：

其中，t_1start和t_1end、t_2start和t_2end分别是运动物体路径P_i(t)和动态窗口法采样路径P(v,w,t)的开始时间和结束时间，errt1,errt2是两阶段的花费时长，目的是为了准确计算同时刻两路径坐标位置的距离，v,w是动态窗口法采样得到的速度，r是设置的与动态障碍物的安全距离，r设置为机器人的制动距离，将ddist(v,w)加入到评价函数中；

所述步骤S5具体包括：

步骤S5-1、改进的路径评价函数加入如步骤S4-2的采样速度路径和运动物体路径一段时间最短距离，改进评价函数定义为：

G(v,w)＝σ(α*h(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*ddist(v,w)) (4)

其中，h(v,w)是机器人方向评价函数，即预测路径末端位置机器人方向角与终点之间的角度差值的绝对值，dist(v,w)是与静态物体距离评价函数，即预测路径末端位置与静态物体的距离，vel(v,w)是速度评价函数，即当前采样的速度大小，α，β，γ，δ分别为方向评价函数、静态物体距离评价函数、速度评价函数、动态物体距离平均函数的权重系数，δ是平滑系数。

步骤S5-2、使用步骤S5-1所定义的评价函数对窗口采样的速度推算的路径进行评价，选择评价最优路径对应的速度，将选出的速度发送给机器人执行。

实施例2

将本发明用于某动态场景的局部路径规划：

本实施例一共分为个部分：搭建软硬件环境、部署训练好的YOLO检测模型和卡尔曼滤波模型、设计动态物体避障算法并加入动态窗口法。

硬件环境需要一辆配有深度相机、激光雷达和Jetson Nano开发板的机器小车，在开发板上安装机器人操作系统(ROS)、ORB-SLAM3和其它需要用到的库。

部署YOLO检测模型之前需要先训练，YOLO可选用轻量级模型YOLOv4-Tiny，训练数据选用COCO数据集，将其按8:1:1的比例随机划分为训练集、验证集和测试集，对YOLOv4-Tiny进行训练，输出可以实时准确检测到动态物体的检测模型，然后将该模型和卡尔曼滤波模型部署到开发板上。

设计动态物体避障算法并加入动态窗口法主要包括动态物体检测器选择、物体运动模型与卡尔曼滤波模型建立、采样路径与动态物体路径同时刻距离最小值计算、将新评价指标加入评价函数四个部分。获取到搭载相机采集到的图像后，调用YOLO检测器接口，先利用YOLO检测器检测图像中是否包含动态物体，若未检测到动态物体，则直接跳至将新评价指标加入评价函数部分；若检测到动态物体，将检测到的动态物体编号为1-n，后对检测到的动态物体建立对应的运动模型和卡尔曼滤波器，对每一个动态物体建立运动模型和卡尔曼滤波器具体步骤如下：

1)将YOLO检测器检测到的动态物体编号为1-n，获取对应的位置信息和状态变量并保存；

2)利用保存的物体位置信息和状态信息建立运动模型，建立并初始化卡尔曼滤波器；

采样路径与动态物体路径同时刻距离最小值计算具体步骤如下：

1)利用上述建好的卡尔曼滤波器进行卡尔曼滤波，保存好卡尔曼滤波估计的物体的位置、速度和时间等状态信息；

2)根据步骤1)卡尔曼滤波估计的物体位置、速度和时间等信息以时间0.1s为变量计算物体的运动路径，将第i个物体的运动路径保存为P_i(t)＝(x,y)，其中x,y为t个0.1s时刻的二维坐标，并计算此次计算所花费时间并保存为errt1；

3)以时间0.1s为间隔计算动态窗口法采样生成的局部路径并保存为p(v,w,t)＝(x,y)，并获取此次计算所花费的时间为errt2，其中v,w是采样的线速度-角速度组，t表示第t个0.1s时刻，x,y是第t个0.1s时刻的二维坐标；

4)计算同时刻的动态窗口法采样的局部路径与运动物体1-n的最小距离，计算公式为ddist(v,w)，ddist(v,w)定义为：

其中，mdist(v,w)定义为：

mdist(v,w)＝min{d(i,t₁,v,w,t₂)|1≤i≤n，t_1start+err1+err2≤t₁≤t_2end，t_2start+err2≤t₂≤t_2end} (2)

其中，d(i,t₁,v,w,t₂)定义为：

其中，t_1start和t_1end、t_2start和t_2end分别是运动物体路径P_i(t)和动态窗口法采样路径P(v,w,t)的开始时间和结束时间，运动物体计算路径时长可设为5s，动态窗口法采样路径时长可设为3s，errt1,errt2是两阶段的花费时长，目的是为了准确计算同时刻两路径坐标位置的距离，v,w是动态窗口法采样得到的速度，r是设置的与动态物体的安全距离，具体实施时可设置为机器人制动距离。

将新评价指标加入评价函数具体步骤如下：

1)如果未检测到潜在动态物体，则将与动态物体距离指标ddist(v,w)置为0，如果检测有潜在动态物体，则按照上述方法计算ddist(v,w)；

2)将与动态物体距离评价指标ddist(v,w)加入到动态窗口法评价函数，新评价函数为：

G(v,w)＝σ(α*h(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*ddist(v,w)) (4)

其中，h(v,w)是机器人方向评价函数，即预测路径末端位置机器人方向角与终点之间的角度差值的绝对值，dist(v,w)是与静态物体距离评价函数，即预测路径末端位置与静态物体的距离，vel(v,w)是速度评价函数，即当前采样的速度大小，α,β,γ,δ分别为方向评价函数、静态物体距离评价函数、速度评价函数、动态物体距离平均函数的权重系数，σ是平滑系数。

选出评价最高的局部路径对应的线速度-角速度组保存并发送给机器人执行。

实施例3

与其它实施例基于相同的发明构思，本实施例介绍一种存储一个或多个程序的计算机可读存储介质，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行任一所述方法。

实施例4

与其它实施例基于相同的发明构思，本实施例介绍一种计算机设备，包括，

一个或多个处理器、存储器以及一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行任一所述方法的指令。

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

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

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

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

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (7)

1.一种局部路径规划方法，其特征在于，包括：

步骤S1、对相机采集到的每帧图像进行检测，判断是否检测到可能的动态物体；

步骤S2、若步骤S1检测到可能的动态物体，对可能的动态物体建立对应的物体运动模型，根据物体运动模型建立对应的卡尔曼滤波模型，如果没有检测到可能的动态物体，则转步骤S5；

步骤S3、根据步骤S2建立的卡尔曼滤波模型对每个动态物体的状态进行预测和更新，并以时间为变量计算一段时间内运动物体的预测路径；

步骤S4、得到步骤S3运动物体的预测路径后，对动态窗口法采样的线速度-角速度组进行路径推算；

步骤S5、使用改进的路径评价函数对动态窗口法采样的线速度-角速度组推算出来的路径进行评价，选取评价最高的路径的线速度-角速度组作为运动指令发送给机器人执行。

2.根据权利要求1所述的局部路径规划方法，其特征在于，所述步骤S1具体包括：

使用训练好的YOLO目标检测器对相机采集到的每帧图像进行检测，判断是否检测到可能的动态物体。

3.根据权利要求1所述的局部路径规划方法，其特征在于，所述步骤S3，具体包括：

步骤S3-1、对检测到的动态物体，依次编号为1-n；

步骤S3-2、以设定时间dt为间隔计算动态物体一段时间的预测路径，并统计此次计算所花费的时间errt1，对于预测的路径保存为P_i(t)＝(x,y)，其中i表示为第i个动态物体，x,y为t个时间dt时刻的二维坐标点。

4.根据权利要求2所述的局部路径规划方法，其特征在于，所述步骤S4具体包括：

步骤S4-1、动态窗口法采样的线速度-角速度组的同样以时间dt为间隔推算路径保存为p(v,w,t)＝(x,y)，并统计此次计算所花费的时间为errt2，其中v是动态窗口法采样得到的线速度，w是动态窗口法采样得到的角速度，x,y是t个时间dt时刻的二维坐标点；

步骤S4-2、计算步骤S4-1推算的路径(t_2start+errt2)到t_2end时间段的位置与步骤S3-2推算出来运动物体的路径(t_1start+errt1+errt2)到t_2end时间段的位置的对应同时刻最小的距离保存为ddist(v,w)，ddist(v,w)作为改进的路径评价函数中的动态物体距离平均函数，定义为：

其中，mdist(v,w)定义为：

mdist(v,w)＝min{d(i,t₁,v,w,t₂)|1≤i≤n，t_1start+err1+err2≤t₁≤t_2end，t_2start+err2≤t₂≤t_2end} (2)

其中，d(i,t₁,v,w,t₂)定义为：

其中，t_1start和t_1end、t_2start和t_2end分别是运动物体路径P_i(t)和动态窗口法采样路径P(v,w,t)的开始时间和结束时间，errt1,errt2是两阶段的花费时长r是设置的与动态障碍物的安全距离，r设置为机器人的制动距离。

5.根据权利要求4所述的局部路径规划方法，其特征在于，所述步骤S4具体包括：

所述步骤S5具体包括：

步骤S5-1、构建改进的路径评价函数G(v,w)，表示为：

G(v,w)＝σ(α*h(v,w)+β*dist(v,w)+γ*vel(v,w)+δ*ddist(v,w)) (4)

其中，h(v,w)是机器人方向评价函数，用于预测路径末端位置机器人方向角与终点之间的角度差值的绝对值；dist(v,w)是与静态物体距离评价函数，用于预测路径末端位置与静态物体的距离；vel(v,w)是速度评价函数，用于表示当前采样的速度大小，α，β，γ，δ分别为机器人方向评价函数、静态物体距离评价函数、速度评价函数、动态物体距离平均函数的权重系数，δ是平滑系数；

步骤S5-2、使用步骤S5-1构建的改进的路径评价函数对窗口采样的速度推算的路径进行评价，选择评价最优路径对应的速度，将选出的速度发送给机器人执行。

6.一种存储一个或多个程序的计算机可读存储介质，其特征在于，所述一个或多个程序包括指令，所述指令当由计算设备执行时，使得所述计算设备执行权利要求1至5中的任一所述方法。

7.一种计算机设备，其特征在于，包括，

一个或多个处理器、存储器以及一个或多个程序，其中一个或多个程序存储在所述存储器中并被配置为由所述一个或多个处理器执行，所述一个或多个程序包括用于执行权利要求1至5中的任一所述方法的指令。