CN111352416A

CN111352416A - 一种基于运动模型的动态窗口局部轨迹规划方法及***

Info

Publication number: CN111352416A
Application number: CN201911386808.7A
Authority: CN
Inventors: 吴月路
Original assignee: Dilu Technology Co Ltd
Current assignee: Dilu Technology Co Ltd
Priority date: 2019-12-29
Filing date: 2019-12-29
Publication date: 2020-06-30

Abstract

本发明公开了一种基于运动模型的动态窗口局部轨迹规划方法及***，包括构建运动模型；将速度控制在一定的范围，并采样速度推算出轨迹；采用评价函数评价每条所述轨迹，并选取出最优的轨迹，改进运动约束模型，替换单车模型，提高了预测轨迹的真实性及精确度；同时增加三层与逻辑停止判断，在到达位置、到达航向、速度满足等停止条件，解决实际规划停止状态车辆状态问题。

Description

一种基于运动模型的动态窗口局部轨迹规划方法及***

技术领域

本发明涉及自动驾驶运动规划的技术领域，尤其涉及一种基于运动模型的动态窗口局部轨迹规划方法及***。

背景技术

动态窗口方法是一种基于速度的局部规划器，可计算达到目标所需的机器人的最佳无碰撞(允许)速度，将笛卡尔目标(x，y)转换成移动机器人的速度(v，w)命令。该算法有两个主要的步骤：计算有效的速度搜索空间，并选择最佳速度。搜索空间由一组能够产生安全的运动轨迹(即允许机器人在碰撞前停止)的速度构成，这些速度是机器人在其动态(动态窗口)的下一个时间片中可以实现的速度集；选择最佳速度以最大化机器人与障碍物的间隙，最大化速度并获得最接近目标的航向。

现有的动态窗口规划运动约束模型基于单车模型，预测采样的轨迹精确度不高；且动态窗口规划在到达目标停止判断只考虑了车和终点距离，未考虑车的最终朝向状态以及速度停止条件，导致车辆最终停止不符合实际自动驾驶要求。

发明内容

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述现有动态窗口规划存在的问题，提出了本发明。

因此，本发明解决的技术问题是：解决现有动态窗口规划预测采样的轨迹精确度不高，以及未考虑车的最终朝向状态以及速度停止条件，导致车辆最终停止不符合实际自动驾驶要求的问题。

为解决上述技术问题，本发明提供如下技术方案：一种基于运动模型的动态窗口局部轨迹规划方法，包括：构建运动模型；将速度控制在一定的范围，并采样速度推算出轨迹；采用评价函数评价每条所述轨迹，并选取出最优的轨迹。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：作车辆圆弧轨迹运动的半径为R＝V/W，当W不为0时，构建的所述运动模型如下，

X＝x-R*sin(yaw)+R*sin(yaw+Wt*dt)

Y＝y-R*cos(yaw)+R*cos(yaw+Wt*dt)

Yaw＝yaw+Wt*dt

其中，在笛卡尔坐标系下X表示下一时刻的X轴位置，Y表示下一时刻的Y轴位置，Yaw表示下一时刻航向角，x表示上一时刻的X轴位置，y表示上一时刻的Y轴位置，yaw表示上一时刻航向角，R表示最小转弯半径，W表示旋转角速度，t表示时间。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：将速度控制在一定的范围，并采样速度推算出轨迹具体包括，限制车辆自身的最大速度和最小速度；

限制车辆最大加速度和最大减速度；

初步采样出速度；

依据所述初步采样出的速度初步模拟出机器人运动轨迹；

定义障碍物的位置后计算得到所述机器人到所述障碍物的距离；

根据所述最大减速度判断出所述初步采样的速度是否合格，并选择出合格的速度，进一步限制速度范围；

依据所述进一步限制的速度模拟出机器人的运动轨迹。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：限制车辆自身的所述最大速度和所述最小速度如下公式，

V_m＝{v∈[v_min，v_max]，w∈「w_min，W_max]}

其中，Vmin表示最小速度，Vmax表示最大速度，Wmin表示最小旋转角速度，Wmax表示最大旋转角速度。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：限制车辆所述最大加速度和所述最大减速度如下公式，

其中，Vc、Wc表示机器人当前的速度和旋转角速度，

表示瞬时减速度，

表示瞬时旋转减速度，

表示瞬时加速度，

表示瞬时旋转加速度。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：根据所述最大减速度判断出所述初步采样的速度是否合格具体为，依据所述障碍物的位置和所述机器人到所述障碍物的距离计算出在所述最大减速度的条件下，所述初步采样的速度能否在碰到所述障碍物之前降为0，若一组所述初步采样的速度能够在碰到所述障碍物之前降为0，则保留，否则抛去。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：判断所述初步采样的速度能否在碰到所述障碍物之前降为0的公式如下，

其中，dist(v,w)表示在这组采样的速度v以及旋转角速度w下能够行驶的距离。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：所述机器人在往前模拟轨迹的这段时间内速度不变，直至下一时刻采样的规定新的速度命令。

作为本发明所述的基于运动模型的动态窗口局部轨迹规划方法的一种优选方案，其中：所述评价函数如下，

G(v，w)＝σ(α·heading(v，w)+(β·dist(v，w)+γ·velocity(v，w))。

为解决上述技术问题，本发明还提供如下技术方案：一种基于运动模型的动态窗口局部轨迹规划***，包括：构建模块，用于构建运动模型；采样模块，用于进行速度的筛选采集；轨迹生成模块，用于根据所述采样模块采集的所述速度生成轨迹；评价模块，用于对所述轨迹生成模块生成的所述轨迹进行评价，并选取出最优的轨迹。

本发明的有益效果：本发明改进运动约束模型，替换单车模型，提高了预测轨迹的真实性及精确度；同时增加三层与逻辑停止判断，在到达位置、到达航向、速度满足等停止条件，解决实际规划停止状态车辆状态问题。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其它的附图。其中：

图1为本发明提供的基于运动模型的动态窗口局部轨迹规划方法的流程示意图；

图2为本发明提供的基于运动模型的动态窗口局部轨迹规划***的模块示意图；

图3为本发明提供的基于运动模型的动态窗口局部轨迹规划方法中在最大减速度条件下进行再次速度限制的弧线示意图；

图4为本发明提供的基于运动模型的动态窗口局部轨迹规划方法的采样轨迹示意图；

图5为本发明提供的基于运动模型的动态窗口局部轨迹规划方法的评价函数中的方位角评价函数示意图；

图6为本发明提供的基于运动模型的动态窗口局部轨迹规划方法的一运行效果图；

图7本发明提供的基于运动模型的动态窗口局部轨迹规划方法的又一运行效果图；

图8本发明提供的基于运动模型的动态窗口局部轨迹规划方法的再一运行效果图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。

同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

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

实施例1

请参阅图1和图3～8，为本发明提供的基于运动模型的动态窗口局部轨迹规划方法的第一个实施例：一种基于运动模型的动态窗口局部轨迹规划方法，包括：

构建运动模型；

将速度控制在一定的范围，并采样速度推算出轨迹；

采用评价函数评价每条轨迹，并选取出最优的轨迹。

在模拟轨迹之前，首先进行数据的接收和解析，具体为接收感知数据、定位数据、footprint数据，将接收的感知数据和定位数据进行同步融合，将三种数据进行解析后送至速度采样。

具体的，要模拟车辆的行驶轨迹，首先需要构建运动模型，假设移动轨迹是一段一段圆弧或者直线，一对(Vt，Wt)代表一个圆弧的轨迹。假设相邻时间段内的机器人轨迹是直线不够精确的，更精确的做法是圆弧替代，假设车辆不是全向移动的，作车辆圆弧轨迹运动的半径为R＝V/W，当W不为0时，构建的运动模型如下：

X＝x-R*sin(yaw)+R*sin(yaw+Wt*dt)

Y＝y-R*cos(yaw)+R*cos(yaw+Wt*dt)

Yaw＝yaw+Wt*dt

从在已知运动模型的前提下，根据速度可以推算出轨迹，采样速度推算轨迹。在速度(V,W)的二维空间中，存在无穷多组速度，根据车辆本身的限制和环境限制将速度控制在一定范围，并采样速度推算出轨迹具体为：

首先，限制车辆自身的最大速度和最小速度如下公式：

V_m＝(v∈[v_mmin，v_max]，w∈「w_min，W_max]}

齐次，由于车辆扭矩因素，存在最大的加减速度限制，移动轨迹前向模拟时间内，存在动态窗口，该窗口内速度是车辆能够到达的速度，通过以下公式限制车辆最大加速度和最大减速度：

其中，Vc、Wc表示机器人当前的速度和旋转角速度，

表示瞬时减速度，

表示瞬时旋转减速度，

表示瞬时加速度，

表示瞬时旋转加速度。

而后，基于车辆运动安全考虑，需使车辆能够在碰到障碍物之前停下来，因此在最大减速度条件下，将速度再次限定一个范围，具体过程如下：

①通过上述限制车辆自身的最大速度、最小速度、最大加速度和最大减速度后初步采样出速度；

②依据初步采样出的速度初步模拟出机器人运动轨迹；

③找到障碍物位置，计算出机器人到障碍物之间的距离；

④查看当前采样的这对速度能否在碰撞到障碍物之前停下来，如果能够停下来，那这对速度就是可以接受的，如果不能停下来，这堆速度就得抛弃掉，实现速度的再次限制采样，公式如下：

其中，dist(v,w)表示在这组采样的速度v以及旋转角速度w下能够行驶的距离，如图3所示。

最后通过再次采样的速度模拟出运动轨迹，如图4所示。

需要注意的是：为了简化速度对应轨迹的计算，算法假设机器人在往前模拟轨迹的这段时间内速度不变，直到下一时刻采样的规定新的速度命令，即根据以上综合的约束，选出的一组速度和角速度，在模拟时间t内不变。然后等下一个t，再选出下一个t的速度以及角速度，进行迭代循环。

综上所述，根据车辆本身的限制和环境限制将速度控制在一定范围，并采样速度推算出轨迹具体为：

限制车辆自身的最大速度和最小速度；

限制车辆最大加速度和最大减速度；

初步采样出速度；

依据初步采样出的速度初步模拟出机器人运动轨迹；

定义障碍物的位置后计算得到机器人到障碍物的距离；

根据最大减速度判断出初步采样的速度是否合格，并选择出合格的速度，进一步限制速度范围；

依据进一步限制的速度模拟出机器人的运动轨迹。

进一步的，在采样速度组中，有若干轨迹是可行的，因此采用评价函数为每条轨迹进行评价，选取最优的轨迹。如下公式所示：

G(v，w)＝σ(α·heading(v，w)+(β·dist(v，w)+γ·velocity(v，w))。

具体的，公式具体如以下三方面解释：

①方位角评价函数，heading(v,w)用来评估车辆当前设定的采样速度下，达到模拟轨迹末端时的朝向和目标之间的角度差。如图5所示，采用180-theta 的方式来评价，即theta越小，评价得分越高；

②空隙dist(v,w)，表示车辆在当前轨迹上与最近的障碍物之间的距离，如果这条轨迹没有障碍物，就将其设置为一个常数。障碍物距离评价限定一个最大值，如果不设定，一旦一条轨迹没有障碍物，将太占比重。导致其他比重占比太小，轨迹目标不可达等危险，一般设置成车辆2倍的宽度；

③速度velocity(v,w)，评价当前轨迹的速度大小；

④平滑处理，对于以上三个部分的值，不是直接相加，而是归一化以后，进行相加。归一化处理能够使得轨迹平滑。如何进行归一化，具体见如下公式：

其中，n为采样的所有轨迹，i为待评价的当前轨迹。

需要注意的是：归一化的目的是平滑轨迹。譬如障碍物距离，机器人传感器检测到的最小障碍物距离在二维空间中是不连续的，这条轨迹能够遇到障碍，旁边那边不一定能遇到，并且这条轨迹最小的障碍物距离是1米，旁边那条就是10米，则障碍物距离的这种评价标准导致评价函数不连续，也会导致某个项在评价函数中太占优势，如这里的离障碍物距离10米相对于1米就太占优势，所以将他们归一化，通过如下公式：

归一化后都变成统一的百分比了，每个障碍物最小距离都是这100份中的一份。

综上所述：三者构成评价函数对的物理意义是：在局部导航过程中，使得机器人避开障碍，朝着目标以较快的速度行驶，缺一不可。

如图6～8所示，验证改进算法的可行性，分别选用不同的场景，设置起点和终点，运行本发明方法。

如图1和图2所示，起点位于左下角，终点位于右上角，中间圆代表障碍物，从效果图中可以看出，改进后的规划算法能够平滑的生成一条轨迹，且有效的避开障碍物。

图3所示是较复杂的场景，起点是右下角，终点是左上角，如图线条轨迹，平滑且能较快的能到达目标终点。

实施例2

请参阅图2，为本发明提供的基于运动模型的动态窗口局部轨迹规划*** 的第一个实施例：一种基于运动模型的动态窗口局部轨迹规划***，包括：

构建模块100，用于构建运动模型；

采样模块200，用于进行速度的筛选采集；

轨迹生成模块300，用于根据所述采样模块200采集的所述速度生成轨迹；

评价模块400，用于对所述轨迹生成模块300生成的所述轨迹进行评价，并选取出最优的轨迹。

应当认识到，本发明的实施例可以由计算机硬件、硬件和软件的组合、或者通过存储在非暂时性计算机可读存储器中的计算机指令来实现或实施。所述方法可以使用标准编程技术-包括配置有计算机程序的非暂时性计算机可读存储介质在计算机程序中实现，其中如此配置的存储介质使得计算机以特定和预定义的方式操作——根据在具体实施例中描述的方法和附图。每个程序可以以高级过程或面向对象的编程语言来实现以与计算机***通信。然而，若需要，该程序可以以汇编或机器语言实现。在任何情况下，该语言可以是编译或解释的语言。此外，为此目的该程序能够在编程的专用集成电路上运行。

此外，可按任何合适的顺序来执行本文描述的过程的操作，除非本文另外指示或以其他方式明显地与上下文矛盾。本文描述的过程(或变型和/或其组合) 可在配置有可执行指令的一个或多个计算机***的控制下执行，并且可作为共同地在一个或多个处理器上执行的代码(例如，可执行指令、一个或多个计算机程序或一个或多个应用)、由硬件或其组合来实现。所述计算机程序包括可由一个或多个处理器执行的多个指令。

进一步，所述方法可以在可操作地连接至合适的任何类型的计算平台中实现，包括但不限于个人电脑、迷你计算机、主框架、工作站、网络或分布式计算环境、单独的或集成的计算机平台、或者与带电粒子工具或其它成像装置通信等等。本发明的各方面可以以存储在非暂时性存储介质或设备上的机器可读代码来实现，无论是可移动的还是集成至计算平台，如硬盘、光学读取和/或写入存储介质、RAM、ROM等，使得其可由可编程计算机读取，当存储介质或设备由计算机读取时可用于配置和操作计算机以执行在此所描述的过程。此外，机器可读代码，或其部分可以通过有线或无线网络传输。当此类媒体包括结合微处理器或其他数据处理器实现上文所述步骤的指令或程序时，本文所述的发明包括这些和其他不同类型的非暂时性计算机可读存储介质。当根据本发明所述的方法和技术编程时，本发明还包括计算机本身。计算机程序能够应用于输入数据以执行本文所述的功能，从而转换输入数据以生成存储至非易失性存储器的输出数据。输出信息还可以应用于一个或多个输出设备如显示器。在本发明优选的实施例中，转换的数据表示物理和有形的对象，包括显示器上产生的物理和有形对象的特定视觉描绘。

如在本申请所使用的，术语“组件”、“模块”、“***”等等旨在指代计算机相关实体，该计算机相关实体可以是硬件、固件、硬件和软件的结合、软件或者运行中的软件。例如，组件可以是，但不限于是：在处理器上运行的处理、处理器、对象、可执行文件、执行中的线程、程序和/或计算机。作为示例，在计算设备上运行的应用和该计算设备都可以是组件。一个或多个组件可以存在于执行中的过程和/或线程中，并且组件可以位于一个计算机中以及/或者分布在两个或更多个计算机之间。此外，这些组件能够从在其上具有各种数据结构的各种计算机可读介质中执行。这些组件可以通过诸如根据具有一个或多个数据分组(例如，来自一个组件的数据，该组件与本地***、分布式***中的另一个组件进行交互和/或以信号的方式通过诸如互联网之类的网络与其它系统进行交互)的信号，以本地和/或远程过程的方式进行通信。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims

1.一种基于运动模型的动态窗口局部轨迹规划方法，其特征在于：包括，

构建车辆运动模型；

将车辆速度控制在一定的范围，并采样速度推算出轨迹；

采用评价函数评价每条所述轨迹，并选取出最优的轨迹。

2.根据权利要求1所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：定义车辆圆弧轨迹运动的半径为R＝V/W，当W不为0时，

构建的所述运动模型如下，

X＝x-R*sin(yaw)+R*sin(yaw+Wt*dt)

Y＝y-R*cos(yaw)+R*cos(yaw+Wt*dt)

Yaw＝yaw+Wt*dt

3.根据权利要求1所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：将速度控制在一定的范围，并采样速度推算出轨迹包括，

限制车辆自身的最大速度和最小速度；

限制车辆最大加速度和最大减速度；

初步采样出速度；

依据所述初步采样出的速度初步模拟出机器人运动轨迹；

依据所述进一步限制的速度模拟出机器人的运动轨迹。

4.根据权利要求3所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：限制车辆自身的所述最大速度和所述最小速度如下公式，

Vm＝{v∈[Vmin，Vmax]，w∈[Wmin，Wmax]}

5.根据权利要求3所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：限制车辆所述最大加速度和所述最大减速度如下公式，

其中，Vc、Wc表示机器人当前的速度和旋转角速度，

表示瞬时减速度，

表示瞬时旋转减速度，

表示瞬时加速度，

表示瞬时旋转加速度。

6.根据权利要求3所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：根据所述最大减速度判断出所述初步采样的速度是否合格具体为，依据所述障碍物的位置和所述机器人到所述障碍物的距离计算出在所述最大减速度的条件下，所述初步采样的速度能否在碰到所述障碍物之前降为0，若一组所述初步采样的速度能够在碰到所述障碍物之前降为0，则保留，否则抛去。

7.根据权利要求6所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：判断所述初步采样的速度能否在碰到所述障碍物之前降为0的公式如下，

8.根据权利要求1～7任一项所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：所述机器人在往前模拟轨迹的这段时间内速度不变，直至下一时刻采样的规定新的速度命令。

9.根据权利要求1所述的基于运动模型的动态窗口局部轨迹规划方法，其特征在于：所述评价函数如下，

G(v，w)＝σ(α·heading(v，w)+β·dist(v，w)+γ·velocity(v，w))

其中，heading(v,w)为方位角评价函数，σ为其系数；dist(v,w)为空隙评价函数，β为其系数；velocity(v,w)为速度评价函数，γ为其系数。

10.一种基于运动模型的动态窗口局部轨迹规划***，其特征在于：包括，

构建模块(100)，用于构建运动模型；

采样模块(200)，用于进行速度的筛选采集；

轨迹生成模块(300)，用于根据所述采样模块(200)采集的所述速度生成轨迹；

评价模块(400)，用于对所述轨迹生成模块(300)生成的所述轨迹进行评价，并选取出最优的轨迹。