CN212828326U

CN212828326U - 一种基于纵横向协调的轨迹跟踪控制系统

Info

Publication number: CN212828326U
Application number: CN202020289368.5U
Authority: CN
Inventors: 高秀晶; 陶林君; 陈波波; 赵可道
Original assignee: Xiamen University of Technology
Current assignee: Xiamen University of Technology
Priority date: 2020-03-10
Filing date: 2020-03-10
Publication date: 2021-03-30
Anticipated expiration: 2030-03-10

Abstract

本实用新型公开了一种基于纵横向协调的轨迹跟踪控制系统，包括感知层、轨迹规划层、控制层和驱动执行层；所述感知层实时采集智能汽车的交通环境信息和车辆状态信息，将相关事件进行实时收集并传递至控制层供控制层预先判断调用；所述路径规划根据信息感知的数据规划出一条期望路径，此规划路径为从出发点到目的地点的全局轨迹；所述控制层建立纵横向协调策略，对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值；所述驱动执行层根据控制层建模输出的控制指令物理值驱动操纵车辆制动执行机构，实现车辆的整体控制。本实用新型采用简单的两轮相比于传统的PID控制和非线性模型预测控制增加了精确度和实时性。

Description

一种基于纵横向协调的轨迹跟踪控制系统

技术领域

本实用新型涉及智能汽车控制领域，尤其是涉及的是一种基于纵横向协调的轨迹跟踪控制系统。

背景技术

无人驾驶车辆的控制技术分为横向控制和纵向控制，因此自动轨迹跟踪技术需要同时实现二者的融合控制。当前，轨迹跟踪控制只单独考虑在恒速或者一定速度下车辆横向控制，以及基于目标速度的纵向控制，该类技术只能保证在简单的道路工况例如高速公路及小曲率道路等的轨迹跟踪。在一般道路工况下，驾驶员会根据道路曲率不同适当调节车速以确保车辆稳定行驶，此时车辆转向的控制也需根据车速变化进行调节。因此，无人驾驶车辆需要根据道路的实际状况，实时地自适应控制纵向的车速；当车速发生变化时，横向控制也需根据车速变化自适应地调整，以达到平稳、舒适、精准的控制效果。

当前现有技术中，无人驾驶车辆轨迹跟踪纵横向控制中主要有两大类技术方案：基于车辆运动几何学的方法设计控制算法和基于车辆动力学的方法设计控制算法。基于运动几何学设计的车辆轨迹跟踪纵横向控制算法在低速低曲率道路下能保证一定的控制性能，且具有控制器设计比较简单、控制参数易于调节等优点，但是在高速或大曲率的道路上，由于车辆自身动力学特性的改变会减低轨迹跟踪控制的性能，无法满足高速和大曲率道路的自动驾驶控制。采用基于复杂车辆动力学模型来耦合纵横向控制，考虑车辆的动力学要素较为丰富，因此模型较为精确，但其模型复杂，不利于控制算法设计，同时基于复杂动力学设计的控制算法往往计算耗时大，实时性差；此外以上两种算法在动力学模型和控制模型上均未全面地考虑车辆以外的因素(如道路因素、环境因素等) 对驾驶性能的影响，无法平稳且精确地满足无人驾驶轨迹跟踪的纵横向控制的要求。

实用新型内容

本实用新型的目的在于克服上述不足，提供一种基于纵横向协调的轨迹跟踪控制系统。

为实现上述目的，本实用新型的技术解决方案是：

一种基于纵横向协调的轨迹跟踪控制系统，包括感知层、轨迹规划层、控制层和驱动执行层；所述感知层实时采集智能汽车的交通环境信息和车辆状态信息，将相关事件进行实时收集并传递至控制层供控制层预先判断调用；所述轨迹规划层根据信息感知的数据规划出一条期望路径，此规划路径为从出发点到目的地点的路程最短、耗时最小的全局轨迹；所述控制层根据预瞄原理，建立纵横向协调策略，对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值；所述驱动执行层根据控制层建模输出的控制指令物理值驱动操纵车辆制动执行机构，实现车辆的整体控制。

优选的，所述感知层包括定位模块、障碍物感知模块和车辆状态感知模块，其中定位模块获取当前车辆所在的位置、航向信息，障碍物感知模块获取周边车辆、行人、驾驶条件，车辆状态感知模块获取本车当前运行的横摆角速度、纵横向加速度、纵横向速度、车辆质心侧偏角。

优选的，所述轨迹规划层包括滤波模块、高精度地图模块、轨迹规划模块，其中滤波模块对感知层采集到的感知信息进行有效的滤波处理，结合高精度地图模块，在轨迹规划模块中实时进行轨迹规划。

优选的，所述控制层包括纵横向协调策略模块、纵横向控制模块和控制信号处理模块，所述纵横向协调策略模块根据预瞄原理，建立纵横向策略，所述纵横向控制模块结合二轮车辆模型和反步法原理，形成前馈+反馈相结合的纵横向控制方法，所述控制信号处理模块对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值。

优选的，所述驱动执行层包括转向执行机构、油门执行机构、制动执行机构和中控执行机构，所述转向执行机构实现车辆转向控制，所述油门执行机构实现车辆加速控制，所述制动执行机构实现车辆刹车控制，所述中控执行机构实现中控控制。

优选的，所述轨迹规划模块包括全局规划模块和局部规划模块，所述全局规划模块根据高精度地图模块上设定目的地点，通过轨迹规划算法规划出一条从出发点到目的地点的路程最短、耗时最小的全局轨迹；所述局部规划模块根据实时交通环境来对目标轨迹实施局部规划。

优选的，还包括监测层，所述监测层包括监视模块和扩展，所述监视模块用于实时监测各个层级的运行状况，所述扩展模块为人机交互界面。

通过采用上述的技术方案，本实用新型的有益效果是：本申请提出了一种新型的基于纵横向协调轨迹跟踪控制方法及其系统，根据驾驶员驾驶的习惯，结合预瞄控制理论，提出基于纵横向协调的无人驾驶车辆的路径跟踪控制方法，并设计简单、实用、参数易于调节的横纵向控制系统。预瞄控制策略根据预瞄前方一定距离内的道路曲率通过模糊控制方法自适应选取轨迹跟踪预瞄点，结合二轮车辆模型和反步法设计前馈+反馈的横向控制算法；同时根据前方道路曲率，通过车辆侧向加速度约束自适应调节能保证安全行驶和舒适性的预瞄速度，并通过PID控制方法控制车辆速度，从而协调车辆进行纵横向协调控制。

附图说明

图1为本实用新型整体结构示意图；

图2为本实用新型侧面结构示意图；

图3为本实用新型轨迹跟踪示意图。

主要附图标记：S10、感知层；S110、定位模块；S120、障碍物感知模块；S130车辆状态感知模块；S20、轨迹规划层；S210、滤波模块；S220、高精度地图模块；S230、轨迹规划模块；S231、全局规划模块；S232、局部规划模块； S30、控制层；S310、纵横向协调策略模块；S320、纵横向控制模块；S330、控制信号处理模块；S40、驱动执行层；S410、转向执行机构；S420、油门执行机构；S430、制动执行机构；S440、中控执行机构；S50、监测层；S510、监视模块；S520、扩展；F10、纵横向协调预瞄算法；F110、前方道路信息获取模型； F120、模糊算法；F130、预瞄点选取；F140、横向控制偏差计算模型；F150、预瞄速度计算；F160、速度决策；F170、纵向控制偏差计算模型；F20、纵横向控制算法模块；F210、横向控制算法；F220、纵向控制算法；F30、控制信号处理模块；F310、转向增量限制器；F320、转向饱和限制器；F330、信息转化模块；F340、加/减速度增量限制器；F350、加/减速度饱和限制器；F360、信息转化模块。

具体实施方式

为了使本实用新型的目的、技术方案及优点更加清楚明白，以下结合具体实施方式对本实用新型进一步详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本实用新型，但并不用于限定本实用新型。

另外，在本实用新型的描述中，需要理解的是，术语“中心”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本实用新型和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本实用新型的限制。

如图1所示，一种基于纵横向协调的轨迹跟踪控制系统，包括感知层S10、轨迹规划层S20、控制层S30、车辆执行层S40和监测层S50。

感知层S10主要利用车载传感器构成，感知层S10主要包括定位模块S110、障碍物感知模块S120和车辆状态感知模块S130三大模块。定位模块S110是获取当前车辆所在的位置、航向信息；障碍物感知模块S120是获取周边车辆、行人、驾驶条件等交通环境信息；车辆状态感知模块S130是获取本车当前运行的横摆角速度、纵横向加速度、纵横向速度、车辆质心侧偏角等车辆状态信息。

轨迹规划层S20包括：滤波模块S210、高精度地图模块S220、轨迹规划模块 S230。轨迹规划层S20获取来自感知层S10的感知信息，经过滤波模块S210进行有效的滤波处理以消除传感器噪声干扰的影响，再结合高精度地图模块S220，在轨迹规划模块S230中实时进行轨迹规划。轨迹信息包括位置、航向、道路曲率、道路坡度、速度限制、关键特征路点标识等信息的路点组成。轨迹规划模块S230分为全局规划模块S231和局部规划模块S232。全局规划模块S231是根据高精度地图模块S220上设定目的地点，通过轨迹规划算法规划出一条从出发点到目的地点的路程最短、耗时最小的全局轨迹。而车辆在全局轨迹下行驶时，还会受到交通环境的影响，因此，还需要局部规划模块S232根据实时交通环境来对目标轨迹实施局部规划，如自动换道超车轨迹、紧急避让轨迹、中转站靠站轨迹等。

控制层S30是根据感知层S10和轨迹规划层S20提供的感知信息和轨迹信息，建模根据预瞄原理，建立纵横向协调策略，对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值。控制层S30 包括纵横向协调策略模块S310、纵横向控制模块S320和控制信号处理模块S330。纵横向协调策略模块S310根据预瞄原理，为满足乘坐舒适度和行驶安全，建立纵横向协调策略；纵横向控制模块S320结合二轮车辆模型和反步法原理，设计一种新型的前馈+反馈相结合的纵横向控制方法。通过控制方法计算期望纵横向控制指令，包含转向信号、加/减速度信号、中控信号(如：转向灯、刹车灯等)；为保证车辆稳定控制，通过设计的控制信号处理模块S330对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值。

驱动执行层S40包含转向执行机构S410、油门执行机构S420、制动执行机构 S430和中控执行机构S440。用于执行控制层S30的控制指令物理值，S410实现车辆转向控制，S420实现车辆加速控制，S430实现车辆刹车控制，S440包括转向灯、刹车灯、双闪等中控控制，以实现车辆的整体控制。

监测层S50包括监视模块S510和扩展S520。监视模块S510用于实时监测各个层级的运行状况，自动检测各层的运行安全和系统错误。当自检出错误时，使车辆采取相应措施以避免危险发生。如断开发生错误的层级的信息输出、控制层的控制指令输出，并紧急停车、自动亮起应急灯提醒周围车辆注意、靠边停车等，提高系统的自检能力和功能安全。扩展S520为人机交互界面(HMI)，当车辆为SAE-L3(含)以下的人机共存的车辆时，可为驾驶员提供系统控制的状态信息，当车辆为SAE-L4(含)以上的无人驾驶车辆时，监视层的信息也可通过无线通讯(V2X)的方式传输至管理中心，可远程监视车辆状态。

一种基于纵横向协调的轨迹跟踪控制方法，包括信息感知、轨迹规划、控制层建模、以及驱动执行；

所述信息感知实时采集智能汽车的交通环境信息和车辆状态信息，将相关事件进行实时收集并传递至控制层供控制层预先判断调用；

所述路径规划根据信息感知的数据规划出一条期望路径，此规划路径为从出发点到目的地点的路程最短、耗时最小的全局轨迹；

所述控制层建模根据预瞄原理，建立纵横向协调策略，对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值；所述控制层建模包括预瞄距离建模、横向控制偏差建模和纵向控制偏差建模；

如图2所示，纵横向协调预瞄算法F10包括前方道路信息获取模型F110、模糊算法F120、预瞄点选取F130、横向控制偏差计算模型F140、预瞄速度计算F150、速度决策F160、纵向控制偏差计算模型F170；纵横向控制算法模块F20包括互相协调的横向控制算法F210和纵向控制算法F220；控制信号处理模块F30包括转向增量限制器F310、转向饱和限制器F320、信息转化模块F330、加/减速度增量限制器F340、加/减速度饱和限制器F350、信息转化模块F360。

首先，纵横向协调预瞄算法F10接收轨迹与感知信息，前方道路信息获取模型F110计算出当前车辆质心到目标轨迹上的最近路点P_n，并获取由该路点沿着目标轨迹向车辆行驶方向一定距离(s₁,s₂)内的道路平均曲率

s₁、s₂为目标轨迹上的2个不同路点。

然后，根据道路平均曲率

分别建立横向控制偏差计算模型F140和纵向控制偏差计算模型F170：

如图3所示，将道路平均曲率

和当前车速V作为F120的输入，根据模糊算法输出为预瞄距离L_p，L_p为P_n沿着目标轨迹向车辆行驶方向的距离。F130根据以P_n为原点，L_p为半径作圆与目标轨迹的交点的方法计算得到预瞄点P_p，此处所述 F130中的交点可能有多个，规定以车辆在目标轨迹上的行驶方向的交点为P_p。 F140用于计算横向偏差e_y、预瞄偏差e_yL和航向偏差

1)预瞄距离建模

在实际工程预瞄应用中，预瞄距离长，车辆行驶稳定性好；预瞄距离短，则轨迹跟踪误差小。而预瞄距离跟道路曲率和车辆速度有关，因此本实用新型采用模糊算法，综合考虑道路曲率和车辆速度，提出了一种新型的基于模糊算法的预瞄距离自适应选取方法。所述模糊算法求取预瞄距离L_p的具体步骤如下：

①变量模糊化。在实际应用中认为，道路曲率越大，速度越小，则预瞄距离越小。此处将前方道路曲率和当前车辆速度作为输入变量，其论域分别为[a， b]m^-1和[c，d]km/h。输出变量为预瞄距离L_p，其论域为[e，f]m。根据难度和控制效果，将输入输出模糊集论域分为n档，道路曲率为E_i(i表示道路曲率论域的分级，i＝1…n),车辆速度为F_j(j表示车辆速度论域的分级，j＝1…n)，预瞄距离为D_q(q表示预瞄距离论域的分级，q＝1…n)。

②隶属度函数选取。为保证输出平缓并兼顾预瞄距离调节的灵敏度，本实用新型采用梯形隶属度函数和三角形隶属度函数相结合的方法。

③生成模糊规则。通过模糊条件语句根据①可知有n²条规则组成的模糊规则库。模糊推理则采用Mamdani法，则规则可以表示如下：

IF：

THEN：L_p＝D_q式中，E_i、F_j、D_q分别为输入、输出变量的语言变量。

④解模糊化。本实施例选取重心法作为解模糊化的方法。

2)纵向控制偏差建模横向控制偏差计算模型F140根据预瞄点信息，计算出横向偏差、预瞄偏差和航向偏差计算式如下：

二自由度车辆横向动力学方程：

其中，β表示质心侧偏角，γ表示横摆角速度，a₁₁、a₁₂、a₂₁、a₂₂、b₁、b₂分别表示车辆参数，δ_f表示车辆前轮转角。

车辆质心到目标轨迹最近点的横向偏差e_y和航向偏差

为：

车辆质心到预瞄点的预瞄偏差为：

若L_p＝0，则e_yL＝e_y。

其中，

表示车辆质心处的X轴坐标值、Y轴坐标值和横摆角；

表示最近点P_n的X轴坐标值、Y轴坐标值和横摆角，ρ为P_n处的道路曲率，e_y、e_yL、

Figure DEST_PATH_GDA00029397411800000910

根据位置反馈和车辆状态反馈实时更新。

3)纵向控制偏差建模

将

Figure DEST_PATH_GDA00029397411800000911

和车辆最优侧向加速度约束a_yd作为预瞄速度计算F150输入，输出为满足安全性和舒适性的预瞄速度V_p。在速度决策F160中根据预瞄速度V_p、当前路段限速V_rmax关系决策出目标速度。将目标速度和当前车速进行差值计算作为纵向控制偏差计算模型F170的输出。预瞄速度计算F150如下

Figure DEST_PATH_GDA00029397411800000912

ε为一个很小的正数，以防止

Figure DEST_PATH_GDA00029397411800000913

时，出现计算错误。

速度决策F160决策的最优目标速度

V_d＝MIN(V_p，V_rmax) (9)

从式(8)(9)可知，V_d是求预瞄速度和当前路段限速V_rmax二者间的最小值，当道路曲率绝对值为0(很小)时，表明前方路径为直线道路(低曲率)，车辆按照路段限速行驶即可，则此时V_d＝V_rmax；当前方路径曲率很大时，若无人驾驶车辆仍然按照路段限速行驶可能会产生很大的侧向加速度，甚至会造成横向控制误差变大而偏离本车道的危险，此时V_d＝V_p，从而保证车辆行驶的速度在最优侧向加速度约束和道路曲率约束下的。

道路曲率和当前速度变化自适应选取合适值，而当前速度又与道路曲率有关，因此，通过预瞄道路前方一定距离内的道路曲率来协调车辆纵横向控制。

纵向控制偏差计算模型F170速度误差e_v＝V_d-V(10)

其次，F20是将F10输出的横向控制偏差和纵向控制偏差作为输入，并结合位置反馈和车辆状态反馈，通过纵横向控制算法来修正、消除偏差。所述横向控制算法F210提出了基于反步法(back stepping method)的反馈和前馈相结合的控制策略；纵向速度控制算法F220则提出PID反馈的控制方法实现对目标速度的控制，所述控制方法算式如下：

其中，f(β)是自变量为车辆质心侧偏角β的函数，f(V,γ)是自变量为车速V和横摆角速度γ的函数，b是包含车辆参数的常数，K₁、K₂为反馈控制增益，K_ρ为前馈控制增益，前轮转角控制反馈项

前轮转角控制前馈项

K_p1、K_I、K_D是纵向控制增益。从式(11)和式(8)(9)(10)(12)可以看出，横向控制算法包含重要的速度信息，而当前车速通过纵向控制算法跟踪最优目标速度V_d求得，V_d则是根据道路前方曲率和车辆最优侧向加速度约束求得，从而实现纵横向协调控制。

所述驱动执行根据控制层建模输出的控制指令物理值操纵车辆的执行机构，实现车辆的整体控制。

最后，为使期望纵横向控制指令满足一定约束，如控制指令增量约束和饱和值约束以获得平滑且安全的车辆转向行为或加减速行为，本实用新型提出一种控制信号增量限制器和饱和限制器的组合设计，通过控制信号处理模块F30对纵横向控制算法模块F20输出的期望前轮转角值和期望加/减速度值进行限制处理。针对前轮转角指令，转向增量限制器F310和转向饱和限制器F320能防止快速转向造成乘坐不适和车辆失控；针对加/减速度指令，加/减速度增量限制器 F340和F350防止快速加/减速度造成乘坐舒适度下降和增加执行器的压力，保障行驶安全和系统的功能安全。此外，根据车辆执行层的执行结构的要求，信息转化模块F330和信息转化模块F360将控制层输出的控制指令转换成控制指令物理值，如将方向盘转角信号、加/减速度信号、以及中控信号等转换成执行器能够执行的电压或电流指令信号。

整体来说，本实用新型提供种基于纵横向协调的轨迹跟踪控制方法及系统，相较于现有基于车辆运动几何学的方法设计控制算法和基于车辆动力学的方法设计控制算法来说，本实用新型具有如下优点①模型简单：本实用新型采用预瞄控制原理，设计出根据预瞄的车辆前方道路曲率采用模糊控制和根据车辆侧向加速度约束求解最优预瞄点和预瞄速度，从而协调纵横向控制，避免采用复杂的纵横向动力学模型耦合纵横向控制造成的控制难度大问题。②兼顾控制精度和控制实时性：本实用新型采用简单的两轮动力学模型和运动学对无人驾驶车辆进行建模，根据道路信息和车辆运动特性通过反步法原理设计反馈+前馈的转向控制，其结构简单，参数调节方便，相比于传统的PID控制增加了控制系统的精确度，而相对于非线性模型预测控制增加了系统运行的实时性。③安全舒适：本实用新型考虑驾驶安全和乘坐舒适性，采用增量限制器和饱和限制器相结合的设计防止车辆过度控制造成的失稳或乘坐舒适性差。④自检：本实用新型设计监测层S50，实时监测各层级运行状况进行自检，防止系统错误造成的经济损失和人员生命财产损失。

⑤状态提示：本实用新型设计监测层S50的扩展S520，为无人驾驶或辅助驾驶提供一种信息交互界面，便于人员监测和查看车辆的运行状况，当出现系统或车辆运行出现意外提供一种即时的信息提示。⑥应用范围广：根据轨迹规划层S20 规划的换道轨迹、超车轨迹等和控制层的速度决策，适用于加速超车控制、换道控制、车道保持控制、速度巡航控制等。

以上所述的，仅为本实用新型的较佳实施例而已，不能限定本实用新型实施的范围，凡是依本实用新型申请专利范围所作的均等变化与装饰，皆应仍属于本实用新型涵盖的范围内。

Claims

1.一种基于纵横向协调的轨迹跟踪控制系统，其特征在于：包括感知层、轨迹规划层、控制层和驱动执行层；所述感知层实时采集智能汽车的交通环境信息和车辆状态信息，将相关事件进行实时收集并传递至控制层供控制层预先判断调用；所述轨迹规划层根据信息感知的数据规划出一条期望路径，此规划路径为从出发点到目的地点的全局轨迹；所述控制层建立纵横向协调策略，对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值；所述驱动执行层根据控制层建模输出的控制指令物理值驱动操纵车辆制动执行机构，实现车辆的整体控制；所述感知层包括定位模块、障碍物感知模块和车辆状态感知模块，其中定位模块获取当前车辆所在的位置、航向信息，障碍物感知模块获取周边车辆、行人、驾驶条件，车辆状态感知模块获取本车当前运行的横摆角速度、纵横向加速度、纵横向速度、车辆质心侧偏角；所述轨迹规划层包括滤波模块、高精度地图模块、轨迹规划模块，其中滤波模块对感知层采集到的感知信息进行有效的滤波处理，结合高精度地图模块，在轨迹规划模块中实时进行轨迹规划；所述控制层包括纵横向协调策略模块、纵横向控制模块和控制信号处理模块，所述纵横向协调策略模块根据预瞄原理，建立纵横向策略，所述纵横向控制模块结合二轮车辆模型和反步法原理，形成前馈+反馈相结合的纵横向控制方法，所述控制信号处理模块对期望纵横向控制指令加以处理，并将期望纵横向控制指令转换成硬件机构可执行的控制指令物理值；所述驱动执行层包括转向执行机构、油门执行机构、制动执行机构和中控执行机构，所述转向执行机构实现车辆转向控制，所述油门执行机构实现车辆加速控制，所述制动执行机构实现车辆刹车控制，所述中控执行机构实现中控控制。

2.根据权利要求1所述的一种基于纵横向协调的轨迹跟踪控制系统，其特征在于：所述轨迹规划模块包括全局规划模块和局部规划模块，所述全局规划模块根据高精度地图模块上设定目的地点，通过轨迹规划算法规划出一条从出发点到目的地点的路程最短、耗时最小的全局轨迹；所述局部规划模块根据实时交通环境来对目标轨迹实施局部规划。

3.根据权利要求1所述的一种基于纵横向协调的轨迹跟踪控制系统，其特征在于：还包括监测层，所述监测层包括监视模块和扩展模块，所述监视模块用于实时监测各个层级的运行状况，所述扩展模块为人机交互界面。