一种动态环境下的基于流场的轨迹规划方法与流程

本发明涉及一种无人车轨迹规划方法，尤其是涉及一种动态环境下的基于流场的轨迹规划方法。

背景技术：

无人驾驶汽车是一种智能汽车，可以实现在无人驾驶的条件下自动行驶。无人驾驶汽车一般是利用激光雷达、视觉传感器、毫米波雷达等车载传感器感知车辆周围环境，根据环境感知获得的环境信息，决策所执行的行为。依据行为生成所需的参考轨迹，并依此控制车辆的转向和速度，从而使车辆能够安全、可靠地在道路上行驶。无人驾驶汽车从根本上改变了传统的“人—车—路”闭环控制方式，将不可控的驾驶员从该闭环系统中请出去，从而大大提高了交通系统的效率和安全性。

无人驾驶技术的实现主要包括环境感知，驾驶行为决策，运动规划，车辆控制和车辆定位等五个部分。轨迹规划技术是无人驾驶技术中十分关键的组成部分，在环境感知、行为决策和运动控制之间起着承上启下的作用。基于环境感知数据和决策系统所提供的信息，无人驾驶系统需要在含有动态障碍物的道路环境中，规划出一条安全可靠的、车辆可行驶的轨迹。随着无人驾驶技术的迅速发展，无人驾驶汽车的轨迹规划问题再次成为国内外学界和工业界关注的问题之一。

如今已有许多研究轨迹规划方法的成果，也取得了一些成果，但是都不可避免的存在各自的局限性。

基于几何曲线的轨迹规划方法。传统的基于图搜索概念的算法主要包括nelson多项式法、螺旋线法、样条曲线法和bezier算法，这些算法的光滑性强，可以在特定情景下求解轨迹，但是由于其生成轨迹的规则较为简单，不适用于求解动态环境下的规划问题，具有完备性较差的缺点。

基于状态空间采样的轨迹规划方法。这种算法在现有无人车的轨迹规划内有广泛应用。但是这种方法的缺点在于处理一些动态障碍物时有不稳定的特点，而且生成轨迹不可保证可以被非完整系统执行。

基于控制空间的采样方法是目前研究较为集中的地方，然而控制空间采样的方法耗时较高，无法控制对应状态空间并且不收敛的特性称为这种方法投入广泛使用的阻碍。

最近一些研究主要在于将路径规划方法应用于三维配置空间中，从而实现轨迹规划的方法。主要有a*，快速随机树法(rrt)，这两种方法都是基于路径规划方法，可以发挥两者在路径规划中的固有优势。但是这种方法缺点在于无法应用于非完整性系统，轨迹不光滑。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种兼顾轨迹可执行性的动态环境下的基于流场的轨迹规划方法。

本发明的目的可以通过以下技术方案来实现：

一种动态环境下的基于流场的轨迹规划方法，包括以下步骤：

根据车辆的起点、终点和环境中的障碍物，建立三维流场计算模型；

以车身的前向速度为输入量，坐标和航向角为状态量，建立车辆运动学模型；

采用车辆运动学模型作为滚动方程，求解流场的滚动时域优化问题，将流场速度向量分布作为轨迹规划的引导信息，得到规划轨迹，其中，优化量为车身的前向速度和横摆角速度，优化目标包括车辆运动和流场运动达到一致，约束条件包括车辆的横摆角速度值约束和加速度值约束。

进一步的，根据车辆运动学模型和车身几何量，计算得到车身上任意点速度与车后轴中心点速度之间的关系，根据流场的速度分布和车身的速度分布之间的差异，描述车辆运动和流场运动的差异量，通过最小化差异量，使车辆运动和流场运动达到一致。

进一步的，采用加权最小二乘法求解优化目标，表征车辆运动与流场运动一致性的方程为：

其中，v为车身的前向速度(即车后轴中心点速度)，ω为横摆角速度，xi、yi为车身上的第i个点相对车后轴中心点的横、纵坐标，ui、vi为流场速度向量的横、纵分量。

进一步的，在求解优化问题中，越接近障碍物的流程速度向量的权重越大。

进一步的，权重计算式为：其中k为可调量，di为流体距离障碍物的距离。

进一步的，所述的流场计算模型中，流场的入口为车辆起点的正后方。

进一步的，所述的流场为分层流场。

进一步的，所述的流场计算模型中，流体重力为零。

进一步的，所述的车辆运动学模型满足以下运动学方程：

其中，x、y分别是以车后轴中心点为原点建立的坐标系下的横向和纵向的坐标，θ是车身相对于x轴的航向角，δ为前轮转角，l是前轴与后轴之间的距离，分别对应x、y、θ的一阶导数。

进一步的，所述的动时域优化问题中，流场速度向量分布采用fluent软件，依据n-s方程计算得到。

与现有技术相比，本发明具有以下优点：

(1)由于流场始终可以找到出口，动态条件下的求解能力非常强，完备性好。

(2)流场本身具有流线光滑的特点，因此由流场生成的轨迹的形状也较为光滑。

(3)基于车辆运动学模型和约束条件，可以保证生成的轨迹满足车辆运动学各项模型约束，能够在复杂地形环境下，稳定生成光滑、避免碰撞的轨迹，确保生成的轨迹的可执行性。

(4)可以解决多种任务，对情景、环境和任务不敏感，在多种情况下表现均较好。

(5)基于流场计算和车辆运动学模型，考虑车辆各项约束和运动障碍物的情况，应用三维配置空间的方法，将流场信息考虑轨迹规划方法内，在可行驶区域内生成可执行轨迹。流场具有流线光滑、在复杂情景下得到轨迹的能力强的优点，可以作为轨迹规划方法的引导信息。然而由于流场本身没有车辆模型的信息，不满足车辆运动学的非完整性约束和车辆横向纵向运动约束。所以单独由流场生成的轨迹不能保证车辆可执行。本专利通过设计滚动时域优化问题，将车辆模型和约束加入对流场的运用中，可以确保生成的轨迹满足车辆的各项约束指标。本专利对于任务不敏感，可以应用于多种任务的轨迹规划中，甚至可以应用于其他非完整系统中，具有较强的普适性。

(6)约束条件包含横摆角速度和径向加速度，可以规划速度，从而达成躲避动态障碍物的目的。

附图说明

图1为本实施例情景1流场计算结果；

图2为车辆运动学模型；

图3为车身速度推导示意图；

图4为权重函数；

图5为本实施例情景1轨迹规划结果；

图6为本实施例情景1轨迹规划性能指标-横摆角；

图7为本实施例情景1轨迹规划性能指标-速度；

图8为本实施例情景2轨迹规划结果；

图9为本实施例情景2性能指标-横摆角；

图10为本实施例情景2性能指标-速度；

图11为本发明方法流程图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例

如图11所示，一种动态环境下的基于流场的轨迹规划方法，包括以下步骤：

根据车辆的起点、终点和环境中的障碍物，建立三维流场计算模型；

以车身的前向速度为输入量，坐标和航向角为状态量，建立车辆运动学模型；

采用车辆运动学模型作为滚动方程，求解流场的滚动时域优化问题，将流场速度向量分布作为轨迹规划的引导信息，得到规划轨迹，其中，优化量为车身的前向速度和横摆角速度，优化目标包括车辆运动和流场运动达到一致，约束条件包括车辆的横摆角速度值约束和加速度值约束。

流场具有流线光滑、在复杂动态地图条件下得到轨迹的能力强的优点，可以作为轨迹规划方法的引导信息。然而由于流场本身没有车辆模型的特点，不满足车辆运动学的非完整性约束。所以单独由流场生成的轨迹不足以保证车辆可执行，因此，在方法中加入车辆模型约束，可以发挥流场的优点的基础上，兼顾轨迹的可执行性。

具体步骤如下：

1.针对任务建立流场

根据决策层所指定的任务，由起点和终点确定流场的入口和出口，检测的障碍物设置为墙，构建计算流场的基本元素。使用icemcfd软件，将可行驶区域划分成网格，利用fluent软件，依据n-s方程，计算出可行驶区域的流场速度向量分布。将流场速度向量分布作为轨迹规划的引导信息。

1)基本假设

由于流场并非用来生成一个真实的流动现象，而是为轨迹规划提供一个参考。所以为了简便计算，这里做了一些必要的假设：

由于轨迹光滑性的要求，流场的湍流特性无益于轨迹的生成，因此生成的流场是分层的并且其雷诺数应较低。

为了对于所有可能的情况都能得到一致的轨迹规划策略，边界条件的设定应当与情景无关，只需确定流场的起点、终点和障碍物。

出于引导车辆运动的考虑，离散化的网格的大小应当相对较小，一般而言，每一个网格占据0.5～1m²的面积，车身大小面积可以包括10～20个网格。

对于流场设置的简化有助于摒弃流场其他一些不利于规划的特性，由此可以为轨迹规划提供一个比较好的参考，而不必把关注点放在解决一个真实复杂的物理问题。简化了的设置包括：

流体的物性是常数；

流体不可压缩；

忽视重力作用。

2)控制方程

流场的控制方程是连续性方程和动量守恒方程：

其中，u是速度向量，ρ是流体密度，μ是分子粘度。根据在正常状况下的空气来选择这些值。

3)边界条件

在求解流场的过程，对于流场的入出口和壁面作了如下设置：

流场的入口被设置为车的后方，入口流速为10^-5m/s，空气粘性系数μ为17.9×10^-6pa·s，空气密度ρ为1.293g/l。流场速度方向不指向墙，一般平行于墙，指向求解区域。

目标点前设置壁面为完全出口；

所有墙壁被设置为光滑。

最终生成流场作为下一步搜索轨迹的参考信息，如图1所示。

2.建立车辆模型

车辆模型如图2所示。模型的状态为x＝[x,y,θ]^t，其中(x,y)是以车辆后轴中心点为原点建立的坐标系下的横纵坐标，θ是车体相对于x轴的航向角，v和δ分别为车后轴中点速和前轮转角，这里作为模型的输入，l是前轴与后轴之间的距离。车辆的运动学方程为：

3.分析车身速度分布

由于流场的表达形式是散点，所以需要分析车辆作为一个整体在运动过程中，车身各个点的运动情况，才可以将流场与车辆作对比，描述车辆与流场的运动状态的差异。

由车辆运动学模型可知，车辆在运动过程中满足一定的几何规律，而车身是刚体，车身上各点的相对位置保持不变。因此，可以由车身上一点的运动和其他点与此点的相对位置关系，推算出车身其他点的运动。

依据车辆运动学模型，可知车辆后轴中心点的运动状态可以被求得，所以需要推导出后轴中心点与车身上任意一点的速度的相对关系。

以车身左前方任意一点为例，如图3所示，车身左前方任意一点ni(xi,yi)的坐标转化为车身坐标(xi,yi)，车的前向速度为v，车的横摆角速度为ω，经过运动分解，则任意一点的速度为：[v-vxvy]＝[v-ω×yω×x]

其中，vx、vy、x、y分别为旋转运动的横向速度和纵向速度，车身坐标系下点的横坐标和纵坐标。

4.建立滚动时域优化问题

建立滚动时域优化问题，优化目标方程为与流场的一致性的指标项，约束包括车辆运动学模型的最大横摆角速度约束，加速减速约束等，根据流场的线性化表示，选用高效的加权最小二乘法求解，滚动方程采用车辆运动学模型。

1)表征流场的一致性

如图4所示，根据流场的速度分布和车辆车身的速度分布之间的差异，描述车辆运动和流场运动的差异，通过最小化差异量，确保车辆跟随流场。

求解对应的[v，ω]作为解，而对于所有的流场向量，均可得到此方程组，然而车辆作为刚体，不可能满足所有的方程组，所以，得到一个尽可能满足所有流场方程组的[v，ω]成为本优化问题的目标。

此问题的矩阵形式为：

2)权重函数

对于本问题，越接近障碍物的流场速度向量，其重要性越高，因此权重越大，本专利选择满足此趋势的权重函数：

3)求解优化问题

本优化问题的形式是线性方程组的最优解，符合加权最小二乘法的适用条件，由于其显式的表现形式和优异的实时性能，所以选用加权最小二乘法。

定义：

求解为：

4)更新车辆位置

依据车辆模型，输入方向盘转角和恒定速度，可以推算出在此输入下，下一时刻车辆的各个状态。

其中，d为一个时间单位内向前移动的距离。

由于状态不断滚动，最终会得到一条尽可能跟随流场、避免碰撞、满足车辆约束条件的轨迹。

本实施例设计了两个测试案例：

1.无人车前两辆汽车，两者速度均为5m/s，两者之间有10m的间隔，如图5～7。

2.无人车前两辆汽车，快车为5m/s，慢车为2.5m/s，初始状态时，两者之间没有间隔，如图7～9。