CN108827331B - 一种基于邻域系统的智能车辆轨迹规划方法 - Google Patents

Abstract

本发明公开一种基于邻域系统的智能车辆轨迹规划方法，在基于被控系统的动态特性下，选定合适的邻域系统，进而实施周期性的测控过程：首先根据当前状态，在其邻域系统中找出满意的可行邻域，并将该可行邻域看作是静止不变的；再在找出的可行邻域基础上，进行轨迹规划，采用Hermite插值方法进行轨迹规划，选取了首末位置和中间位置三个节点，求出插值曲线。给出控制量并实施控制动作。本发明将邻域系统这一抽象的数学概念的具体应用到智能车控制上的所有相关理论，邻域系统理论将复杂的世界简化为局部的世界的叠加，将复杂动态控制转化为简单静态控制，大大简化了轨迹规划问题的复杂度。

Description

一种基于邻域系统的智能车辆轨迹规划方法

技术领域

本发明涉及智能汽车技术领域，具体为一种基于邻域系统的智能车辆轨迹规划方法。

背景技术

智能车辆的研究内容十分多样，例如它的结构设计，控制理论，路径规划。对于智能车的路径规划，设计最优的曲线是此问题的核心。智能车的路径规划按照范围可分为宏观与微观两类，按照状态可分为动态规划和静态规划两类。对于智能车的宏观路径规划，Dijkstra算法、Floyd-Warshall算法，支持向量机算法，A*算法等均有文献进行应用和改进。而对于智能车的动态路径规划，人工势场算法、神经网络算法、遗传算法等智能算法也被广泛的应用。上述算法大多是对宏观的道路网络节点进行路径规划，然而对于微观的道路上的轨迹规划问题，目前国内外文献较少从最优性这一角度进行考量，还需要进行进一步的探索与研究。对于轨迹规划问题，基于Dubins路径的轨迹生成方法是一种可行的方法。此外还有三角函数曲线，圆弧曲线等轨迹生成方法。考虑到曲率连续问题，基于五次多项式的轨迹规划方法是一种合适的方法。此外还有文献采用模糊逻辑控制，粒子群算法，蚁群算法等智能算法进行优化与控制。

上述文献给出了十分有价值的理论成果，但大多是对首末位置以及障碍物信息已知情况下的轨迹规划，适用性弱。不少文献采用GPS定位方式确定信息，鉴于目前GPS装置的精度和性能，可能会和实际路况存在较大误差，精度不够高。此外还需要进行大量的计算。

发明内容

针对上述问题，本发明的目的在于提供一种能够将复杂动态控制转化为简单静态控制，大大简化了轨迹规划问题的复杂度的邻域系统的智能车辆轨迹规划方法。技术方案如下：

一种基于邻域系统的智能车辆轨迹规划方法，包括以下步骤：

步骤a：确定标准可行领域，且标准可行领域为矩形领域，其底边与车体的尾部重合；

步骤b：建立智能车的邻域相对坐标系，车体的尾部中点记作o(0，0)，邻域的长为H，车体长为l，车体尾部中点与标准可行邻域的底部中点间的距离为s；车辆轨迹曲线方程记作y＝y(x)；

步骤c：选取初始数据：

设在节点0≤x₀＜x_m＜x₁≤H-l上，对于初始点满足：

导数值满足：

其中O₁(x₀,y₀)，O₃(x₁,y₁)为已知节点，需要确定节点O₂(x_m,y_m)的坐标值和导数值，选取初值为x_m＝a，y_m＝b，y_m'＝c；

步骤d：采用Hermite插值法计算轨迹曲线：

将上述初值带入，函数f(x)在节点处的插值多项式为：

其中，n为迭代次数；

步骤e：计算每一次曲线的长度指数和弯阻指数，选择合适的权重，计算综合评判指标，保留其中的指标满意的曲线；

步骤f：确定搜索区间：保持初值c不变，对初值a，b采用进退法确定搜索区间，重新执行步骤d，直到满足轨迹曲线对称的要求；

步骤g：确定步长因子：对导数值c确定步长因子t，重新执行步骤d,使得对于每次迭代c_i＝c_i-1+t，设满意度指数为α，若迭代的结果C_i*的值达到α，则迭代终止。

进一步的，所述步骤e中：

曲线的长度指数为：

其中，区间[x₀,x₁]代表智能车轨迹曲线首末位置所在的区间；

曲线的弯阻指数为：

则曲线的距离优度J*为：

简化得：

结合式(3)，有：

曲线的顺畅度K*为：

结合式(4)和式(8)进行简化得：

曲线L的综合评判指标为：

C*＝w₁·J*+w₂·K* (10)

其中，w₁，w₂为不同驾驶风格的驾驶员对于J*、K*指标的权重。

更进一步的，取所述满意度指数α＝0.8，权重w₁＝0.4，w_2＝0.6，则算出的节点O₂的坐标值和导数如下：

节点坐标为

节点一阶导数为

本发明的有益效果是：

1)本发明中基于邻域系统的动态决策模型的思想是将宏观复杂环境下的动态决策过程，分解为一系列邻域内的简单的静态决策过程。这种方法可以将整个决策过程置于一个有限的、局部的简单环境，而不是无限的、复杂的整个世界。该方法要点如下：在基于被控系统的动态特性下，选定合适的邻域系统，进而实施周期性的测控过程。该过程分为两步进行；第一步是根据当前状态，按照某种方法在其邻域系统中找出满意的可行邻域，并将该可行邻域看作是静止不变的；第二步是在找出的可行邻域基础上，依据某种满意优化控制算法给出控制量并实施控制动作。整个控制过程由各个邻域控制过程的顺序叠加而成。基于邻域系统的动态决策模型可以更好的实现仿人仿生物智能化。且由邻域系统相关的模型确定车辆可行邻域的选择与控制，相关的理论研究得到一定的发展。

2)本发明结合邻域系统的动态决策与控制模型是处理和实现智能驾驶技术的一种有效途径，其主要思想源于随时随地改变行为方式并实施动态决策的智能生物。它可以根据周围环境在当前时刻下的特点，以及自身已有的知识和经验，圈定一定的范围作为自己的安全活动范围，并设定出安全距离、警戒距离、逃逸距离和攻击距离。在这个过程中，智能生物只考虑安全范围以内的事物，而安全范围之外的一切事物都视为与之无关、静止的。一旦有外界的事物侵犯了自己的圈定范围，智能生物就要做出相应的决策和行为来保证自身的安全和正常的生活。借助于邻域系统的思想，将实际生活中智能车驾驶与之结合，即将智能车视为智能生物，根据当前时刻周围环境的特点规划其周围的某个安全范围作为安全邻域，与道路和车辆之间保持适当的距离，确认自身活动能够安全进行。

附图说明

图1为智能车标准可行邻域分析图。

图2为标准矩形邻域。

图3为智能车的动态轨迹规划。

图4为曲线构造图。

图5为智能车的标准可行邻域内轨迹规划图。

图6为智能车的邻域相对坐标系。

图7为道路仿真图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步详细说明。要实现智能车等智能轮式机器人的自主移动，首先需要模仿智能生物的行为决策过程。智能生物在面对复杂的动态环境时，通常先寻找一个短时间不变或保持相对稳定的环境作为其下一步活动的决策范围，然后再进行行为决策。这一决策范围即可行邻域，一系列的可行邻域构成邻域系统。基于邻域系统的动态决策模型就是描述这样一个基于邻域系统的控制决策过程。其控制过程由以下几个步骤组成：

1)根据控制过程中观测变量的特点选择合适的邻域系统；

2)通过一些优化方法，在当前状态下确定一个满意的可行邻域；

3)在满意的邻域中，根据一些规划方法给予满意的决策，实施新的决策行为；

4)在可行邻域的持续时间内保持相同的行为，直到产生下一个决策；

5)上述过程是一个完整的决策循环。当一个周期完成后，一个新的循环开始依次执行步骤(2)到(4)。

可以看到，各个邻域的控制过程不断重复叠加，从而构成了整个控制过程。模型将复杂的宏观环境下的动态决策过程分解为一系列简单的邻域内的静态决策过程的组合，简化了整个决策过程，使得面对的是一个有限的、局部的简单环境，而不是无限的、复杂的整个世界。由此可见，车辆在道路上的运动轨迹问题被转化为在每一个可行邻域里的运动规划问题的叠加。

如图1所示，为智能车某一时刻在道路上行驶的示意图。其中上下两侧的平行线为仿真道路的边界，道路内部的虚线所围成的不规则多边形是传感器能够确立的车辆行驶的一个可行邻域。其中图1内虚线围成的矩形框就是一个标准可行邻域。鉴于标准可行邻域没有一个统一的定义，本发明的标准可行邻域含义如下：

标准可行邻域：可行邻域中数量有限的特定邻域。

标准可行邻域的提出是为了在计算机仿真的过程易于进行模型的建立和数学描述，根据目的的不同可以建立不同形状，不同大小的标准可行邻域。如图2所示，为智能车的标准矩形邻域。要求标准矩形邻域的底边与车体的底边是重合的。智能车的邻域系统控制思想可以将复杂的动态控制转化为简单的静态控制。主要包括两个步骤，第一步是将控制范围缩小至一个局部的可行邻域，第二步是将局部可行邻域内的控制简化至有限个标准的可行邻域内的控制。

对于智能车在整条道路上的规划，本文采用基于智能车的邻域系统的动态决策模型。基于邻域系统的动态决策模型的主要思想是将复杂的宏观环境下的动态决策过程，分解为一系列简单的邻域内的静态决策过程。

如图3所示，给出了智能车在三个不同时刻的行驶状态，其中每一个矩形框为智能车此时确立的标准可行邻域。智能车在道路上的轨迹规划就是在一个个局部的标准可行邻域内的轨迹规划的累积过程。

邻域系统理论是一种可行的解决方式，邻域系统的基本思想是将车辆运动规划中近乎无限多的信息缩小至车辆的邻域系统内，继而缩小至标准可行邻域内。从而将复杂道路中的运动简化为车辆在标准可行邻域中的运动。实现了由复杂到简单，无限到有限的简化，简化了计算量。

此外，此方法具有天然避障性，在建立可行邻域时就已经避开了障碍物。模型简单，易于计算机处理并实现。

基本原理：首先仅研究某一时刻的单一邻域。直接从轨迹曲线的构造出发，选取坐标系来研究最优轨迹曲线的一些性质。

对于轨迹曲线L的轨迹方程：

y＝y(x) (1)

构造规则如下：

(1)轨迹曲线的首尾位置是固定的，起始位置为当前邻域相对坐标系的原点，末位置为当前标准可行邻域的尾部中点向左平移一个车体长度。

(2)轨迹曲线的方程为单值函数，即对于自变量x，其对应的函数值y(x)是唯一的。

(3)轨迹曲线是连续的。

如图4所示，曲线A不满足构造规则(2)，不是本文的轨迹曲线。这也是符合实际驾驶经验的，车辆在单行道路行驶时，不会出现反向行驶的情况。曲线B，C为车辆的轨迹曲线。对于B，C两条轨迹曲线，如何选取B，C曲线中的更好的轨迹曲线是急需解决的问题。进一步推广，如何在轨迹曲线族中选取最优的轨迹曲线是整个轨迹规划理论的关键。因此定义下述两个指标：

定义曲线L的长度指数为：

定义曲线L的弯阻指数为：

长度指数是衡量首末位置等同的不同轨迹的驾驶长度。一般来说，对于一固定首末点的曲线，轨迹长度越短，可以认为沿此曲线行驶的驾驶效率越好。

车辆行驶的弯阻指数主要取决于行驶轨迹的顺畅和车辆速度的稳定两个指标，不同于轨迹曲率这一局部概念，是一个整体性的概念。弯阻指数的定义，对于车辆行走是否顺畅，目前没有一个明确的定义。本发明采用曲线曲率的积分定义了轨迹曲线的弯阻指数，从而将行驶轨迹的顺畅程度转化为数学表达，便于定量研究和比较。

首先分析行驶轨迹的平稳，易知曲率越小，轨迹越顺畅。因此弯阻指数可以衡量不同轨迹的曲线驾驶舒适程度。由定义式可知，弯阻指数只取决于曲线的固有特征，与其他因素无关，是曲线性质的常数。因此，在首末位置相同的行驶轨迹中，如果如果轨迹曲线的K值越大，则会令人感到越难受；轨迹曲线的K值越小，则会令人感到越顺畅，与驾驶习惯无关。

其次由定义式可以看出车辆速度大小同该指数无关。

由上述定义，可给出如下推论：对于A、B两条首末位置相同的曲线，若A曲线每一点的曲率的绝对值都小于B曲线，则A曲线的弯阻指数小于B曲线。

由上述推论显然可以得出，对于两条定长曲线，曲率越小的曲线越令人感到顺畅。

为了更加合理的比较指标的优劣，需要使各指标处于同一数量级，这样便于进行综合对比评价和后续数据的处理，因此先对上述指标进行标准化处理。

定义曲线L的距离优度为：

对(4)进行化简，令

结合式(2)，有

曲线L的距离优度就是对长度指数的标准化。其中区间[x₀,x₁]代表智能车轨迹曲线首末位置所在的区间。因此最短距离为直线距离d＝x₁-x₀，J*_min为最短距离d的标准化形式。考虑到无限长的连续曲线可以围住一块有限的面积这一事实，标准化过程就是将长度指数的范围从[d,+∞]映射到[0,1]区间上。当长度指数J为最短距离d时，带入(6)式J*取1；当长度指数为+∞时，J*取0。因此J*的取值越接近1越令人满意。

定义曲线L的顺畅度为：

结合式(3)对(7)式进行化简，则有

曲线L的顺畅度是弯阻指数的标准化过程，考虑到直线的曲率为0，因此标准化的过程就是将弯阻指数的范围从[0,+∞]映射到[0,1]区间上。当弯阻指数K为0时，带入(8)式K*取1；当弯阻指数为+∞时，K*取0。因此K*的取值越接近1越令人感到顺畅。

在实际应用时，可根据不同需求选用指数型指标或标准化指标。如果需要精确的数值，可以选用指数型指标；如果需要指标的比较与评判，可选用标准化形式的指标。

对于不同驾驶风格的驾驶员，可初步划分为驾驶平稳型和追求速度型，他们对于上述两个指标是有着不同偏好的。为了综合评判不同驾驶员对于最优轨迹曲线的要求，现引入对于最优轨迹曲线的综合评判指标。

定义曲线L的综合评判指标为：

C*＝w₁·J*+w₂·K* (9)

其中w₁，w₂为不同驾驶风格的驾驶员对于J*、K*指标的权重，可由专家评判给出。

综上可知，在众多轨迹曲线中选取一个最优曲线，只需比较不同轨迹曲线的综合评判指标的值，数值最大的曲线即为所求。

如图5右图所示，为车辆在t时刻的标准可行邻域，是左图的一个局部放大，坐标轴取车体底部,P_t、Q_t分别为车体与标准可行邻域的底部中点，记其距离为s，邻域长为L，高为H，车体长为l，宽为h，纵轴速度为v_t，转角为α_t.，

本专利在于给出一种满意的轨迹曲线的构造方法。寻求满意曲线的过程是一个筛选与尝试的过程，需要建立合理的限定条件，提高筛选与尝试的效率。规则如下：

(1)控制规划的轨迹应较为光滑。

(2)车体的末位置应尽可能满足底边中点到邻域两边是等距的要求，即在邻域中部运动。

(3)车体的横向运动方向应和车体底部中点到邻域底部中点的方向一致。

基于上述规则，下面将给出满意路径曲线的构造方法。

当P_t与Q_t重合时，车辆始终做直线运动，满足规则(1)(2)(3)，则满意路径曲线为一条垂直与X轴并且过P_t的直线。

当P_t与Q_t分离时，假定最后一步车辆运动到邻域顶部且基本满足规则(2)。现将规则一细化为曲线二阶光滑。如图6所示，为图5的坐标系的翻转，其中车体与可行邻域底部的中点距离为s，邻域长为H，车体长为l。则满意轨迹曲线的算法如下：

1)确定已知信息：轨迹曲线二阶光滑，其中初始位置坐标为o(0，0)，末位置坐标为(H-l，s)，且两者一阶导数皆为0；

2)确定构造轨迹曲线的方法：Hermite插值法可以将函数值和导数值完全利用，本文采用此方法。

3)确定插值节点：根据插值节点的个数不同，可求出不同的轨迹曲线，兼顾满意度和复杂性，本文将插值节点的个数定为三个。

具体步骤如下：

a)选取初始数据：

设设在节点0≤x₀＜x_m＜x₁≤H-l上，对于初始点满足：

导数值满足：

其中O₁(x₀,y₀)，O₃(x₁,y₁)为已知节点，需要确定节点O₂(x_m,y_m)的坐标值和导数值，选取初值为x_m＝a，y_m＝b，y_m'＝c；

b)计算轨迹曲线：将上述数据带入，函数f(x)在节点处的插值多项式为：

之后执行步骤c)。

c)计算长度指数和弯阻指数：计算每一次曲线的长度指数和弯阻指数，选择合适的权重，计算式(9)的综合评判指标C_i*，保留其中的指标较好的曲线，执行步骤d。

d)确定搜索区间：为提高筛选效率，增加满意轨迹曲线对称的限定，可保持初值c不变，对初值a，b采用进退法确定搜索区间，重新执行步骤b，直到满足轨迹曲线对称的要求。再执行步骤e。

e)确定步长因子：对导数值c确定步长因子t，重新执行步骤b,使得对于每次迭代c_i＝c_i-1+t，设满意度指数为α，若迭代的结果C_i*的值达到α，则迭代终止。

按照上述算法，因为只需求得满意曲线，所以结果不唯一。本文以满意度指数α＝0.8，权重w₁＝0.4，w₂＝0.6，所计算出的节点O₂的坐标值和导数如下：

a)节点坐标为

恰好为曲线的中点。

b)节点一阶导数为

具体做法

1)记开始时刻t＝0，根据智能车辆所携带的各类传感器确定当前合适的邻域系统(根据现有技术，是很容易做到的，例如超声波传感器可以确定不同障碍物的距离，图像传感器可以确定道路边缘等等)；

2)通过一些优化方法，在当前状态下确定一个满意的可行邻域(第一步智能车辆是通过传感器建立的诸多可行邻域，这些可行邻域大小形状不一，需要选取一个满意的邻域作为具体规划对象)；

3)在已经选好的满意的这个邻域中，进行轨迹规划，按照上文基本原理分析的结果，采用Hermite插值方法进行轨迹规划，选取了首末位置和中间位置三个节点，中间位置节点坐标为

恰好为曲线的中点。节点一阶导数为

首末位置一阶导数均为0，可以求出插值曲线。

4)在可行邻域的持续时间内保持相同的行为，直到产生下一个决策(即在当前邻域让智能车延此曲线进行轨迹跟踪)。

5)上述过程是一个完整的决策循环。当一个周期完成后，一个新的循环开始依次执行步骤(2)到(4)。这里每个周期的保持时间都是比较短的，因此可以认为外界条件基本不发生大的变动，这是应用了数学中将动态问题转化为静态问题的思想，因此智能车辆在整体道路上的轨迹规划成为了一系列局部的邻域内规划的叠加。

结果展示：按照上述方此，逐步进行迭代，可得道路上的仿真结果图如图7所示轨迹曲线在道路上的仿真图。可以看出轨迹的平稳光滑性上有一定的优越性。

Claims (2)

1.一种基于邻域系统的智能车辆轨迹规划方法，其特征在于，包括以下步骤：

步骤a：确定标准可行邻域，且标准可行邻域为矩形邻域，其底边与车体的尾部重合；

步骤b：建立智能车的邻域相对坐标系，车体的尾部中点记作o(0，0)，邻域的长为H，车体长为l，车体尾部中点与标准可行邻域的底部中点间的距离为s；车辆轨迹曲线方程记作y＝y(x)；

步骤c：选取初始数据：

设在节点0≤x₀＜x_m＜x₁≤H-l上，对于初始点满足：

导数值满足：

其中O₁(x₀,y₀)，O₃(x₁,y₁)为已知节点，需要确定节点O₂(x_m,y_m)的坐标值和导数值，选取初值为x_m＝a，y_m＝b，y_m'＝c；

步骤d：采用Hermite插值法计算轨迹曲线：

将上述初值带入，函数y(x)在节点处的插值多项式为：

其中，n为迭代次数；

步骤e：计算每一次曲线的长度指数和弯阻指数，选择合适的权重，计算综合评判指标，保留其中的指标满意的曲线；

步骤f：确定搜索区间：保持初值c不变，对初值a，b采用进退法确定搜索区间，重新执行步骤d，直到满足轨迹曲线对称的要求；

步骤g：确定步长因子：对导数值c确定步长因子t，重新执行步骤d,使得对于每次迭代c_i＝c_i-1+t，设满意度指数为α，若迭代的结果C_i*的值达到α，则迭代终止；

所述步骤e中：

曲线的长度指数为：

其中，区间[x₀,x₁]代表智能车轨迹曲线首末位置所在的区间；

曲线的弯阻指数为：

则曲线的距离优度J*为：

简化得：

结合式(3)，有：

曲线的顺畅度K*为：

结合式(4)和式(8)进行简化得：

曲线L的综合评判指标为：

C*＝w₁·J*+w₂·K* (10)

其中，w₁，w₂为不同驾驶风格的驾驶员对于J*、K*指标的权重。

2.根据权利要求1所述的基于邻域系统的智能车辆轨迹规划方法，其特征在于，取所述满意度指数α＝0.8，权重w₁＝0.4，w₂＝0.6，则算出的节点O₂的坐标值和导数如下：节点坐标为

节点一阶导数为

Images