一种基于动态窗口的多虚拟目标点全局动态路径规划算法的制作方法

本发明涉及无人驾驶导航技术领域，特别是一种基于动态窗口的多虚拟目标点全局动态路径规划算法。

背景技术：

无人车路径规划就是在综合考虑行驶时间、速度、油耗和安全等因素的前提下，为无人车规划出最优的行驶轨迹。无人车为了完成在复杂道路上的安全行驶，其自主移动能力是其基础，其中路径规划和导航是保证无人车自主移动能力的核心算法，是体现车辆智慧水平的关键所在。

从路径规划的目标范围看，通常分为全局路径规划和局部路径规划；从规划环境是否随时间变化方面看，还可以分为静态路径规划和动态路径规划。全局路径规划通常假设环境信息完全已知，在无人车运动之前，离线计算出一条从起点到终点的路径；但是环境并不是一成不变的，当环境信息改变时全局路径规划方法并不能快速规划出一条安全可行的路径。由于局部路径规划方法有计算量小，实时性好，规划轨迹圆滑等优点，目前通常采用把全局与局部路径规划结合的二层避障结构，在第一层，通过已知的环境信息(一般为地图)，离线规划出一条从起点到目标点代价值(油耗或者时间)最小的路径，该路径不需平滑但应能避开地图上已知的静态障碍物；在第二层，无人车沿着第一层规划的路径行驶，当在行驶过程中通过传感器检测到新的障碍物时，再通过获得的障碍物详细的位置信息进行局部在线避障。

技术实现要素：

有鉴于此，本发明的目的是提出一种基于动态窗口的多虚拟目标点全局动态路径规划算法，能够有效地避开随机出现的障碍物，并安全稳定地完成全局路径的规划与追踪。

本发明采用以下方案实现：一种基于动态窗口的多虚拟目标点全局动态路径规划算法，具体包括以下步骤：

步骤s1：获取障碍物坐标信息、无人车运动模型和初始状态信息；

步骤s2：设置多个虚拟目标点goal；

步骤s3：判断虚拟目标点与无人车当前时刻的空间位置，选择合适的航向角；

步骤s4：随机产生障碍物；

步骤s5：根据采样的线速度和角速度，计算动态速度窗口；

步骤s6：判断是否需要开始避障；

步骤s7：在动态窗口中计算可达的速度矢量；

步骤s8：在可达的速度矢量窗口中，根据评价函数选择最佳避障速度矢量；

步骤s9：输出最佳避障速度的大小和方向；

步骤s10：重复步骤s3至步骤s9，直至到达终点完成全局的路径追踪，并描点连线对无人车运动轨迹可视化。

进一步地，步骤s2具体为：在原来一个目标点的基础上增加多个目标点，即最后一个目标点为实际的终点，而在终点之前的所有目标点为虚拟目标点，小车在到达虚拟目标点时不会停止运动，而是接着向下一个虚拟目标点运动，并且当前虚拟目标点消失不再对无人车起导向作用，直到下一个目标点为终点，至终点时将小车的速度更新为0，从而完成全局路径的追踪行驶。

进一步地，步骤s3具体包括以下步骤：

步骤s31：无人车当前时刻的航向角是由初始航向角加上无人车角速度wt对时间t的积分求出的，即：

yaw(t)∈[0，+∞)；

式中，yaw(t)表示当前时刻航向角，yaw(0)表示初始时刻航向角；wt＞0表示逆时针行驶，wt＜0表示顺时针行驶；

步骤s32：令评价函数为：

heading＝180°-κ；

式中，κ表示当前时刻无人车速度方向和与目标点方向之间的夹角；

当时：

当时：

式中，ygi表示第i个目标点的纵坐标，yt表示当前时刻无人车的纵坐标，xgi表示第i个目标点的横坐标，xt表示当前时刻无人车的横坐标。

进一步地，在无人车行驶过程中，判断虚拟目标点纵坐标ygi与当前时刻无人车的纵坐标yt的大小，当ygi>yt时，更新yaw(t)∈[0,π]。

进一步地，当ygi>yt时，取当ygi<yt时，取

进一步地，步骤s5具体包括以下步骤：

步骤s51：明确无人车运动学模型的线速度和角速度的最大值vmax、ωmax，通过设置一定的时间间隔，计算在当前时刻速度(v,ω)和无人车加减速度的限制下，所能达到的最大和最小线速度vtmax、vtmin以及最大和最小角速度ωtmax、ωtmin，通过将其与vmax、ωmax比较得出在当前时刻速度下(v,ω)的动态速度窗口；

步骤s52：计算速度空间中每组线速度和角速度在前向模拟时间δ内的运动轨迹。

进一步地，步骤s52中，所述前向模拟时间δ的取值为4s。

进一步地，步骤s6具体为：求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离stopdist；若dist>stopdist，则此时刻速度为安全速度，需刹车减速防止碰撞障碍物；若dist<stopdist，则需要舍弃动态窗口中的这些轨迹，选取可行的行驶轨迹，即需要开始避障。

进一步地，步骤s7中，所述可达的速度矢量为：无人车与最近障碍物之间的距离大于无人车到最近障碍物的刹车距离时的速度矢量。

进一步地，步骤s8具体为：经过步骤s7得到的可达速度窗口，其中包含有多组可行的运动轨迹，这些轨迹中只能选择其中的一条作为最优轨迹用来追踪，选择哪一条取决于评价函数：

g＝δ(α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω))；

取评价函数的最大值对应的速度(v,ω)作为最佳的避障速度矢量；评价函数中包含三个决定轨迹最优的因素，分别为heading(v,ω)、dist(v,ω)和velocity(v,ω)，其中velocity(v,ω)为在当前(v,ω)下无人车运动速度的大小，heading(v,ω)为无人车速度方向和与目标点方向之间夹角的补角，dist(v,ω)为无人车与最近障碍物之间的距离，β为安全权重，γ为速度权重，δ为前向模拟时间。

与现有技术相比，本发明有以下有益效果：本发明将动态窗口法应用在全局路径规划方向，改进传统全局规划路径不连续不平滑的缺陷。matlab仿真结果表明无人车能够有效地避开随机出现的障碍物，并安全稳定地完成全局路径的规划与追踪。

附图说明

图1为本发明实施例的绕“8”赛道示意图。

图2为本发明实施例的绕“8”赛道轨迹示意图。

图3为本发明实施例的无人车按比赛规则避障示意图。

图4为本发明实施例的无人车绕行赛道避障示意图。

图5为本发明实施例的方法流程示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

本实施例提供了一种基于动态窗口的多虚拟目标点全局动态路径规划算法，本实施例以“中国大学生无人驾驶方程式大赛”中的绕“8”赛道等复杂环境下，赛道如图1所示，规则如图2所示。中国大学生无人驾驶方程式大赛中的绕“8”赛道需要无人车按照规则在右圆转两圈，然后进入左圆行驶两圈，最后行驶出赛道。因为行驶第二圈其实是对第一圈的重复，仿真得到的轨迹会重合，从而为了仿真结果显示的可观性，本实施例在两圆各行驶一圈。

本实施所进行的工作具体为：第一，本实施例依据比赛行驶规则设计了多组虚拟目标点，虚拟目标点根据无人车和锥桶的尺寸，设置在靠近内圆的位置保证行驶路径最短，每个虚拟目标点依次显示并作用于无人车，最终实现了全局路径的行驶(绕“8”运动)。过程中：通过判定虚拟目标点和小车当前位置在全局坐标系下y值的大小，更新此时小车运动状态的航向角，避免了航向角在运动过程中的不断增大(逆时针运动)或减小(顺时针运动)，保证了动态窗口算法中函数始终为正，其中，heading＝180°-κ，κ为当前时刻无人车运动方向和与目标点方向之间的夹角。第二，本实施例在“8”型复杂赛道中加入了随机障碍物(默认为和赛道同型号的锥桶)，用于模拟无人车在实际行驶中遇到的行人或者车辆等其它障碍物，并在matlab环境下进行仿真，结果表明：当随机障碍物没有堵塞赛道时，无人车绕过障碍物安全实现了绕“8”行走；当随机出现的障碍物堵塞了赛道1时，无人车进入赛道2到达了虚拟目标点，此过程中没有触及赛道1的障碍物，从而更加验证了本发明的局部路径规划和跟踪的稳定性和安全性。

如图5所示，本实施例具体包括以下步骤：

步骤s1：获取障碍物坐标信息、无人车运动模型和初始状态信息；

步骤s2：设置多个虚拟目标点goal；

步骤s3：判断虚拟目标点与无人车当前时刻的空间位置，选择合适的航向角；

步骤s4：随机产生障碍物；

步骤s5：根据采样的线速度和角速度，计算动态速度窗口；

步骤s6：判断是否需要开始避障；

步骤s7：在动态窗口中计算可达的速度矢量；

步骤s8：在可达的速度矢量窗口中，根据评价函数选择最佳避障速度矢量；

步骤s9：输出最佳避障速度的大小和方向；

步骤s10：重复步骤s3至步骤s9，直至到达终点完成全局的路径追踪，并描点连线对无人车运动轨迹可视化。

在本实施例中，步骤s1具体为：获取障碍物坐标信息、无人车运动模型和初始状态信息通过无人车自身携带的摄像头、激光雷达、gps、惯导、里程计以及线加速度和角加速度等传感器的数据融合进行建模得到。其中，建模时考虑了无人车自身的尺寸和赛道所使用的锥桶尺寸，即将圆形锥桶的半径加上无人车的长半轴作为安全距离dist，当无人车质心至锥桶圆心的距离小于安全距离时就代表无人车碰撞了障碍物，反之则为安全通过。因此无人车和实际障碍物(锥桶)可以作为一个质点来进行环境建模，具体表现为坐标系中的坐标形式。

在本实施例中，步骤s2具体为：在原来一个目标点的基础上增加多个目标点，即最后一个目标点为实际的终点，而在终点之前的所有目标点为虚拟目标点，小车在到达虚拟目标点时不会停止运动，而是接着向下一个虚拟目标点运动，并且当前虚拟目标点消失不再对无人车起导向作用，直到下一个目标点为终点，至终点时将小车的速度更新为0，从而完成全局路径的追踪行驶。

在本实施例中，步骤s3具体包括以下步骤：

步骤s31：无人车当前时刻的航向角是由初始航向角加上无人车角速度wt对时间t的积分求出的，即：

yaw(t)∈[0，+∞)；

式中，yaw(t)表示当前时刻航向角，yaw(0)表示初始时刻航向角；wt＞0表示逆时针行驶，wt＜0表示顺时针行驶；

步骤s32：令评价函数为：

heading＝180°-κ；

式中，κ表示当前时刻无人车速度方向和与目标点方向之间的夹角；

当时：

当时：

式中，ygi表示第i个目标点的纵坐标，yt表示当前时刻无人车的纵坐标，xgi表示第i个目标点的横坐标，xt表示当前时刻无人车的横坐标。

根据以上推导，会出现heading＝180°-κ＜0，比如当yaw(t)＞π，时。评价函数影响因素heading＜0的发生，会造成无人车背离目标点的现象。heading＜0情况的出现，根本原因在于无人车在绕“8”赛道中需要持续逆时针或者顺时针行驶，使得|yaw(t)|不断地增大，导致κ也在增大。为保证heading＞0，本发明改进使其值域为(0,π)。

具体的，在本实施例中，在无人车行驶过程中，判断虚拟目标点纵坐标ygi与当前时刻无人车的纵坐标yt的大小，当ygi>yt时，更新yaw(t)∈[0,π]。

在本实施例中，根据赛道形状和虚拟目标点选取位置，当ygi>yt时，取当ygi<yt时，取经过仿真分析，验证了此方法的灵活性和可行性。

较佳的，在本实施例中，步骤s4具体为：在经过步骤s2和步骤s3的改进之后，无人车初步具有了全局路径规划的能力，但在实际行驶中意外障碍物可能随时出现，这就要求无人车在满足全局路径规划的要求下，实现实时的避障行驶。本实施例在“8”型复杂赛道中加入了随机障碍物(默认为和赛道同型号的锥桶)，当随机障碍物没有堵塞赛道时，无人车绕过障碍物按照规则安全实现了绕“8”行走，如附图3，其中圆圈标记的随机障碍物为无人车通过之后在运行轨迹上产生的，不是无人车碰撞了障碍物；当随机出现的障碍物堵塞了赛道1时，无人车进入赛道2到达了虚拟目标点，此过程中没有触及赛道1的障碍物，从而更加验证了本算法的稳定性和安全性，仿真结果如附图4。

在本实施例中，步骤s5具体包括以下步骤：

步骤s51：明确无人车运动学模型的线速度和角速度的最大值vmax、ωmax，通过设置一定的时间间隔(本实施例取δt＝0.1)，计算在当前时刻速度(v,ω)和无人车加减速度的限制下，所能达到的最大和最小线速度vtmax、vtmin以及最大和最小角速度ωtmax、ωtmin，通过将其与vmax、ωmax比较得出在当前时刻速度下(v,ω)的动态速度窗口；之所以称为动态窗口是因为无人车的加减速性能限定速度采样空间在一个可行的动态范围内；

步骤s52：计算速度空间中每组线速度和角速度在前向模拟时间δ内的运动轨迹。

在本实施例中，，前向模拟时间取得过大和过小会影响无人车的实时避障能力，经过调试参数，步骤s52中，所述前向模拟时间δ的取值为4s，很好的满足了本文所设计的复杂避障环境。

在本实施例中，通过判断动态窗口中每条轨迹上是否存在障碍物以及与最近障碍物之间的距离大小，进行无人车的刹车避障功能。具体说来，步骤s6具体为：求出无人车与最近障碍物之间的距离dist和无人车到最近障碍物的刹车距离stopdist；若dist>stopdist，则此时刻速度为安全速度，需刹车减速防止碰撞障碍物；若dist<stopdist，则需要舍弃动态窗口中的这些轨迹，选取可行的行驶轨迹，即需要开始避障。

在本实施例中，步骤s7中，所述可达的速度矢量为：无人车与最近障碍物之间的距离大于无人车到最近障碍物的刹车距离时的速度矢量。即：除去步骤s6中提到的使得dist＜stopdist的速度矢量，剩余的速度矢量即为可达的速度矢量。进入步骤s8选取最佳速度矢量。

在本实施例中，步骤s8具体为：经过步骤s7得到的可达速度窗口，其中包含有多组可行的运动轨迹，这些轨迹中只能选择其中的一条作为最优轨迹用来追踪，选择哪一条取决于评价函数：

g＝δ(α*heading(v,ω)+β*dist(v,ω)+γ*velocity(v,ω))；

取评价函数的最大值对应的速度(v,ω)作为最佳的避障速度矢量；评价函数中包含三个决定轨迹最优的因素，分别为heading(v,ω)、dist(v,ω)和velocity(v,ω)，其中velocity(v,ω)为在当前(v,ω)下无人车运动速度的大小，heading(v,ω)为无人车速度方向和与目标点方向之间夹角的补角，dist(v,ω)为无人车与最近障碍物之间的距离，β为安全权重，γ为速度权重，δ为前向模拟时间。总的来说，由三者构建的评价函数保证了，在局部导航过程中，使得无人车朝着目标以较快速度行驶并且避开障碍物。

较佳的，在本实施例中，步骤s9具体为：步骤s8中求出评价函数最大时对应轨迹的(v,ω)，并将其发送至无人车的执行机构，通过电信号控制电机的转速和方向盘转角，控制无人车沿着最优避障路径行驶。

步骤s10具体为：重复以上循环，直至到达终点完成全局的路径追踪，并描点连线对无人车运动轨迹可视化。

特别的，本实施例属于无人驾驶路径规划和导航技术领域，具体为一种基于动态窗口(dynamicwindowapproach，dwa)的多虚拟目标点全局动态路径规划方法。动态窗口法的典型应用是在局部路径规划方面，因动态窗口法具有路径平滑、计算效率高等优点，因此本实施例旨在将动态窗口法应用在全局路径规划方向，改进传统全局规划路径不连续不平滑的缺陷。具体操作是通过增加多个虚拟目标点、在特殊位置更新无人车的航向角并加入了随机障碍物，实现了无人车的全局动态路径规划，并将其应用在“中国大学生无人驾驶方程式大赛”中的绕“8”赛道等复杂环境下。matlab仿真结果表明无人车能够有效的避开随机出现的障碍物，并安全稳定地完成全局路径的规划与追踪。

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

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

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

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

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例。但是凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。