CN111176272B - 一种基于运动约束的人工势场轨迹规划方法及系统 - Google Patents

Abstract

本发明公开了一种基于运动约束的人工势场轨迹规划方法及系统，包括以下步骤，搭建模块建立车辆运动模型；利用预设模块设定所述车辆运动模型的当前运动信息；设置引力场、斥力场尺度系数信息构建人工势场模块；根据所述人工势场模块获取当前势场下的引力、斥力和合力；轨迹生成模块根据车辆的所述当前运动信息，结合所述人工势场模块的势场力，预测出车辆下一时刻的运动信息并生成轨迹。本发明的有益效果：一是增加运动约束，车的行驶航向角波动范围，以解决车的轨迹不顺和轨迹跳变问题；二是设置最小引力场，以解决物体难以到达目标的问题；三是记录当前时刻的速度以及航向角，能够实时规划下一时刻轨迹，有益于底层实时控制。

Description

一种基于运动约束的人工势场轨迹规划方法及系统

技术领域

本发明涉及车辆轨迹规划的技术领域，尤其涉及一种基于运动约束的人工势场轨迹规划方法及基于运动约束的人工势场轨迹规划系统。

背景技术

人工势场法路径规划是由Khatib提出的一种虚拟力法。它的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。应用势场法规划出来的路径一般是比较平滑并且安全，但是这种方法存在局部最优点问题。

现有的人工势场法路径规划中，当物体离目标点较远时，引力将变得特别大，相对较小的斥力在甚至可以忽略的情况下，物体路径上可能会碰到障碍物。同时当目标点附近有障碍物时，斥力将非常的大，引力相对较小，物体很难到达目标。并且在某个点，引力和斥力刚好大小相等，方向相反，则物体容易陷入局部最优解或者震荡。

发明内容

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

鉴于上述现有存在的问题，提出了本发明。

因此，本发明解决的一个技术问题是：提供一种基于运动约束的人工势场轨迹规划方法，解决未考虑航向运动约束，轨迹点不够顺滑的问题。

为解决上述技术问题，本发明提供如下技术方案：一种基于运动约束的人工势场轨迹规划方法，包括以下步骤，搭建模块建立车辆运动模型；利用预设模块设定所述车辆运动模型的当前运动信息；设置引力场、斥力场尺度系数信息构建人工势场模块；根据所述人工势场模块获取当前势场下的引力、斥力和合力；轨迹生成模块根据车辆的所述当前运动信息，结合所述人工势场模块的势场力，预测出车辆下一时刻的运动信息并生成轨迹，所述轨迹生成模块包括输出车辆的航向、速度和位置来预测车辆下一时刻的轨迹，其中车辆下一时刻的航向定义为：

其中V表示当前车辆的速度、θ表示当前车辆的航向、F_合力表示当前车辆所受引力和斥力的合力；

所述轨迹生成模块输出的所述速度定义为：

其中V表示当前车辆的速度、Angle表示下一时刻车辆的航向、F_合力x和F_合力y分别表示当前车辆所受x轴和y轴方向的合力；

所述轨迹生成模块输出的所述位置定义为：

其中p₀x和p₀y表示车辆当前时刻的位置、p₁x和p₁y表示车辆下一时刻的位置、V表示当前车辆的速度；

所述轨迹生成模块包括以下轨迹生成步骤，

根据合力场、速度和车辆航向信息计算预测航向偏差；

计算下一时刻车辆的航向角；

根据最大速度限制、最大加速度和车辆的当前速度计算下一时刻的速度；

利用当前位置信息、航向角和速度计算下一时刻的轨迹点位置信息。

作为本发明所述的基于运动约束的人工势场轨迹规划方法的一种优选方案，其中：所述当前运动信息包括车辆的启止信息、朝向信息、速度信息、最大加速度信息和最大偏航角信息。

作为本发明所述的基于运动约束的人工势场轨迹规划方法的一种优选方案，其中：所述搭建模块基于车辆建模内容，将车辆建模成长方形，且所述长方形设置的长和宽分别是5米和3米。

作为本发明所述的基于运动约束的人工势场轨迹规划方法的一种优选方案，其中：设定所述车辆运动模的当前运动信息包括以下步骤，设定地图上的起点和终点信息；设定初始状态的朝向信息；设定车的速度信息；设定最大加速度信息。

作为本发明所述的基于运动约束的人工势场轨迹规划方法的一种优选方案，其中：所述人工势场模块的势场力包括如下获取步骤，获取车前的障碍物到车的距离、车左方的障碍物到车的距离、车右方的障碍物到车的距离、左前45度的障碍物到车的距离、右前45度的障碍物到车的距离、目标点和车中心之间的角度信息以及目标点和车中心之间的距离信息；根据获取的距离、车的航向、目标点和车之间的角度信息计算斥力场、引力场；计算所述引力场和所述斥力场的差值作为合力场。

本发明解决的另一个技术问题是：提供一种基于运动约束的人工势场轨迹规划系统，解决未考虑航向运动约束，轨迹点不够顺滑的问题。

为解决上述技术问题，本发明提供如下技术方案：一种基于运动约束的人工势场轨迹规划系统，包括搭建模块、预设模块、人工势场模块和轨迹生成模块；所述搭建模块用于建立车辆运动模型；所述预设模块用于设定所述车辆运动模型的当前运动信息；所述人工势场模块用于设置引力场、斥力场尺度系数信息并能够获取当前势场下的引力、斥力和合力；所述轨迹生成模块用于根据车辆的所述当前运动信息，结合所述人工势场模块的势场力，预测出车辆下一时刻的运动信息并生成轨迹。

本发明的有益效果：一是增加运动约束，车的行驶航向角波动范围，以解决车的轨迹不顺和轨迹跳变问题；二是设置最小引力场，以解决物体难以到达目标的问题；三是记录当前时刻的速度以及航向角，能够实时规划下一时刻轨迹，有益于底层实时控制。

附图说明

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

图1为本发明第一种实施例所述基于运动约束的人工势场轨迹规划方法的整体流程示意图；

图2为本发明第一种实施例所述建立车辆运动模型的示意图；

图3为本发明第一种实施例所述采用传统算法的轨迹生成示意图；

图4为本发明第一种实施例所述采用基于运动约束的人工势场轨迹规划方法的轨迹生成效果示意图；

图5为本发明第二种实施例所述采用基于运动约束的人工势场轨迹规划系统的整体原理结构示意图。

具体实施方式

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

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

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

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

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

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

实施例1

参照图1～2的示意，本实施例中基于现有轨迹点质点模型未考虑航向运动约束，轨迹点不够顺的缺陷，通过增加运动约束，车的行驶航向角波动范围，可解决车的轨迹不顺，轨迹跳变问题。同时现有技术中当目标点附近有障碍物时，斥力将非常的大，引力相对较小，因此物体很难到达目标，本实施例中设定最小引力，当计算出的引力小于最小引力时，使用设定的最小引力，以解决目标不可达问题。引入的最小引力，也是体现在合力的计算上，传统算法会存在最小极值问题，导致车辆在一个地方打转，引入最小引力解决了陷入局部最优的问题。

最小引力传统算法是没有的，本实施例引入最小引力也是得益于实际算法落地存在的问题引入的改进。合力＝斥力+引力，通过合力来“牵引”物体朝着目标点行进，当车离障碍物越近的时候，斥力就会越大，这样排斥车不要靠近障碍物，实际项目上，当目标点附近有障碍物时候也即引力＝斥力的时候，那么合力＝0，就会出现车原地打转现象，引入最小引力则可以消除这种锁死打转现象。

增加约束可以理解为：首先建模，建立单车模型如图2所示，其次下一时刻的航向角计算。试想下一时刻和前一时刻航向角差的太大，车打方向盘就会过猛这是实际开车所不允许的(除非紧急状态)，对应于轨迹那就是轨迹不连续跳变。所以这里添加约束条件，最大的航向角弧度制是3*PI/180，当下一时刻航向角小于这个最大航向角时候，使用计算出的值，如果大于这个3*PI/180的话，则使用3*PI/180，此处PI为数学的圆周率PI＝3.1415926。

具体的，本实施例提出一种基于运动约束的人工势场轨迹规划方法，包括以下步骤，

S1：搭建模块100建立车辆运动模型；搭建模块100基于车辆建模内容，将车辆建模成长方形，且长方形设置的长和宽分别是5米和3米，本步骤运动模型搭建可参照图2的示意。

S2：利用预设模块200设定车辆运动模型的当前运动信息；当前运动信息包括车辆的启止信息、朝向信息、速度信息、最大加速度信息和最大偏航角信息。设定车辆运动模型的当前运动信息包括以下步骤，

设定地图上的起点和终点信息；

设定初始状态的朝向信息；

设定车的速度信息；

设定最大加速度信息。

S3：设置引力场、斥力场尺度系数信息构建人工势场模块300；

S4：根据人工势场模块300获取当前势场下的引力、斥力和合力；本步骤中的人工势场模块300的势场力包括如下获取步骤，

获取车前的障碍物到车的距离、车左方的障碍物到车的距离、车右方的障碍物到车的距离、左前45度的障碍物到车的距离、右前45度的障碍物到车的距离、目标点和车中心之间的角度信息以及目标点和车中心之间的距离信息；

根据获取的距离、车的航向、目标点和车之间的角度信息计算斥力场、引力场；

计算引力场和斥力场的差值作为合力场。

S5：轨迹生成模块400根据车辆的当前运动信息，结合人工势场模块300的势场力，预测出车辆下一时刻的运动信息并生成轨迹。

其中轨迹生成模块400包括输出车辆的航向、速度和位置来预测车辆下一时刻的轨迹，其中下一时刻的航向定义为：

其中V表示当前车辆的速度、θ表示当前车辆的航向、F_合力表示当前车辆所受引力和斥力的合力。

轨迹生成模块400输出的速度定义为：

其中V表示当前车辆的速度、Angle表示下一时刻车辆的航向、F_合力x和F_合力y分别表示当前车辆所受x轴和y轴方向的合力。

轨迹生成模块400输出的位置定义为：

其中p₀x和p₀y表示车辆当前时刻的位置、p₁x和p₁y表示车辆下一时刻的位置、V表示当前车辆的速度。

根据上述计算公式，轨迹生成模块400包括以下轨迹生成步骤，

根据合力场、速度和车辆航向信息计算预测航向偏差，下一时刻的航向角＝当前时刻航向+预测航向信息。预测的航向偏差如上述的第一个公式，包含了合力场、速度、以及上一时刻的航向信息；

计算下一时刻车辆的航向角；

根据最大速度限制、最大加速度和车辆的当前速度计算下一时刻的速度；

利用当前位置信息、航向角和速度计算下一时刻的轨迹点位置信息。

本实施例还需要说明的是，对于人工势场为人工建立势场，将障碍物设置成斥力，目标设置成吸引力，进行力的矢量相加，最后算出合力的方向。包括利用常用的引力函数、斥力函数建立的引力场合斥力场，引力就是引力场对距离的导数，斥力就是斥力场的梯度，总的场就是斥力场合引力场的叠加。本实施例的基本思想是将机器人在周围环境中的运动，设计成一种抽象的人造引力场中的运动，目标点对移动机器人产生“引力”，障碍物对移动机器人产生“斥力”，最后通过求合力来控制移动机器人的运动。

参照图为验证本实施例提出改进算法的可行性，选用室外停车场场景，设置起点和终点，分别利用传统算法效果和改进后的算法效果进行生成轨迹，最终的算法运行效果对比如下两图3和图4的示意，其中黑色线条代表环境地图的边界，黑色方块代表障碍物信息。图3是传统算法效果，图4是改进后的算法效果，由图中可以明显看到轨迹更加平滑且无跳变现象。本实施例是通过MATLAB仿真软件真实跑出的效果。

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

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

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

实施例2

参照图5的示意，示意出本实施例提出一种基于运动约束的人工势场轨迹规划系统的整体原理结构示意图，上述基于运动约束的人工势场轨迹规划方法能够依托于本实施例实现。具体的，该系统包括搭建模块100、预设模块200、人工势场模块300和轨迹生成模块400。

其中搭建模块100用于建立车辆运动模型；预设模块200用于设定车辆运动模型的当前运动信息；人工势场模块300用于设置引力场、斥力场尺度系数信息并能够获取当前势场下的引力、斥力和合力；轨迹生成模块400用于根据车辆的当前运动信息，结合人工势场模块300的势场力，预测出车辆下一时刻的运动信息并生成轨迹。

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

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

Claims (6)

1.一种基于运动约束的人工势场轨迹规划方法，其特征在于：包括以下步骤，

搭建模块(100)建立车辆运动模型；

利用预设模块(200)设定所述车辆运动模型的当前运动信息；

设置引力场、斥力场尺度系数信息构建人工势场模块(300)；

根据所述人工势场模块(300)获取当前势场下的引力、斥力和合力；

轨迹生成模块(400)根据车辆的所述当前运动信息，结合所述人工势场模块(300)的势场力，预测出车辆下一时刻的运动信息并生成轨迹，所述轨迹生成模块(400)通过输出车辆的航向、速度和位置来预测车辆下一时刻的轨迹，其中车辆下一时刻的航向定义为：

其中V表示当前车辆的速度、θ表示当前车辆的航向、F_合力表示当前车辆所受引力和斥力的合力；

所述轨迹生成模块(400)输出的所述速度定义为：

其中V表示当前车辆的速度、Angle表示下一时刻车辆的航向、F_合力x和F_合力y分别表示当前车辆所受x轴和y轴方向的合力；

所述轨迹生成模块(400)输出的所述位置定义为：

其中p₀x和p₀y表示车辆当前时刻的位置、p₁x和p₁y表示车辆下一时刻的位置、V表示当前车辆的速度；

所述轨迹生成模块(400)包括以下轨迹生成步骤，

根据合力场、速度和车辆航向信息计算预测航向偏差；

计算下一时刻车辆的航向角；

根据最大速度限制、最大加速度和车辆的当前速度计算下一时刻的速度；

利用当前位置信息、航向角和速度计算下一时刻的轨迹点位置信息。

2.如权利要求1所述的基于运动约束的人工势场轨迹规划方法，其特征在于：所述当前运动信息包括车辆的启止信息、朝向信息、速度信息、最大加速度信息和最大偏航角信息。

3.如权利要求1或2所述的基于运动约束的人工势场轨迹规划方法，其特征在于：所述搭建模块(100)基于车辆建模内容，将车辆建模成长方形，且所述长方形设置的长和宽分别是5米和3米。

4.如权利要求3所述的基于运动约束的人工势场轨迹规划方法，其特征在于：设定所述车辆运动模型的当前运动信息包括以下步骤，

设定地图上的起点和终点信息；

设定初始状态的朝向信息；

设定车的速度信息；

设定最大加速度信息。

5.如权利要求4所述的基于运动约束的人工势场轨迹规划方法，其特征在于：所述人工势场模块(300)的势场力包括如下获取步骤，

获取车前的障碍物到车的距离、车左方的障碍物到车的距离、车右方的障碍物到车的距离、左前45度的障碍物到车的距离、右前45度的障碍物到车的距离、目标点和车中心之间的角度信息以及目标点和车中心之间的距离信息；

根据获取的距离、车的航向、目标点和车之间的角度信息计算斥力场、引力场；

计算所述引力场和所述斥力场的差值作为合力场。

6.一种实现如权利要求1～5任一所述的基于运动约束的人工势场轨迹规划方法的系统，其特征在于：包括搭建模块(100)、预设模块(200)、人工势场模块(300)和轨迹生成模块(400)；

所述搭建模块(100)用于建立车辆运动模型；

所述预设模块(200)用于设定所述车辆运动模型的当前运动信息；

所述人工势场模块(300)用于设置引力场、斥力场尺度系数信息并能够获取当前势场下的引力、斥力和合力；

所述轨迹生成模块(400)用于根据车辆的所述当前运动信息，结合所述人工势场模块(300)的势场力，预测出车辆下一时刻的运动信息并生成轨迹。

Images