[自动驾驶]带传感器融合的自适应巡航控制

本文链接： https ://blog.csdn.net/KYJL888/article/details/102895481

带传感器融合的自适应巡航控制

本示例使用：

• 自动驾驶工具箱
• 嵌入式编码器
• 模型预测控制工具箱
• Simulink控制设计
• Simulink

开放式

此示例显示了如何使用传感器融合为在弯路上行驶的车辆实现基于传感器融合的汽车自适应巡航控制器。

在此示例中，您：

1. 回顾结合了传感器融合和自适应巡航控制器（ACC）的控制系统。 提供了两种ACC变体：模型预测控制工具箱中的经典控制器和自适应巡航控制系统模块。

2. 使用自动驾驶工具箱生成的综合数据，在闭环Simulink模型中测试控制系统。

3. 配置用于软件在环仿真的代码生成设置，并自动生成用于控制算法的代码。

介绍

自适应巡航控制系统是一种控制系统，其可以响应于道路状况来改变自我车辆的速度。 与常规巡航控制一样，驾驶员可以为汽车设置所需的速度。 此外，如果在前面的车道中还有另一辆行驶速度较慢的车辆，则自适应巡航控制系统可以降低自我车辆的速度。

为了使ACC正常工作，自我车辆必须确定其前方车道如何弯曲，以及哪辆车是“领头车”，即车道中的自我车辆前方。 下图显示了从自我车辆的角度来看的典型场景。 自我车辆（蓝色）沿着弯曲的道路行驶。 最初，领头车是粉红色车。 然后，紫色汽车切入自我车辆的车道，成为主导车辆。 过了一会儿，紫色的汽车改变到另一个车道，而粉色的汽车再次成为领头车。 之后，粉红色的汽车仍然是领先的汽车。 ACC设计必须对道路上有头车的变化做出反应。

 

 

当前的ACC设计主要依赖于从雷达获得的距离和测距率测量，并且设计为在直路上最佳工作。 在使用模型预测控制的自适应巡航控制系统 （模型预测控制工具箱）和使用FMCW技术的汽车自适应巡航控制系统（相控阵系统工具箱）中给出了这样一个系统的示例。 从先进的驾驶员辅助系统（ADAS）设计过渡到更多的自治系统，ACC必须应对以下挑战：

1. 估算靠近自我车辆且相对于自我车辆有明显横向运动的汽车的相对位置和速度。

2. 估算自我车辆前方的车道，以找出自我车辆前方的哪辆车是同一车道中最接近的车。

3. 对环境中其他车辆的侵略性动作做出反应，尤其是当另一辆车辆驶入自我车辆车道时。

本示例演示了现有ACC设计中两个主要的补充，它们可以应对这些挑战：添加传感器融合系统和基于模型预测控制（MPC）更新控制器设计。 同时使用视觉和雷达传感器的传感器融合和跟踪系统具有以下优点：

1. 它结合了从视觉传感器获得的更好的横向位置测量和速度测量结果，以及从雷达传感器获得的距离和测距率测量结果。

2. 视觉传感器可以检测车道，提供车道相对于自我车辆的横向位置的估计，以及相对于自我车道将其他车辆定位在场景中。 本示例假设理想的车道检测。

先进的MPC控制器增加了对环境中其他车辆做出的更猛烈反应做出反应的能力。 与使用具有恒定增益的PID设计的经典控制器相比，MPC控制器在保持严格的安全距离约束的同时调节自我车辆的速度。 因此，当环境以与人类驾驶员将执行的操作类似的方式快速变化时，控制器可以采取更具侵略性的操纵。

将示例的文件夹添加到MATLAB®路径。

addpath(fullfile(matlabroot,'examples','mpc','main')); 

试验台模型和仿真结果概述

要打开主要的Simulink模型，请使用以下命令：

open_system('ACCTestBenchExample') 

该模型包含两个主要子系统：

1. 具有传感器融合功能的ACC，可对传感器融合进行建模并控制车辆的纵向加速度。 该组件允许您选择设计的经典或模型预测控制版本。

2. 在车辆和环境子系统，自主车辆和模型环境的哪个模型运动。 雷达和视觉传感器的仿真将合成数据提供给控制子系统。

要在运行模型之前运行关联的初始化脚本，请在Simulink模型中，单击“运行安装程序脚本”，或者在命令提示符下键入以下内容：

helperACCSetUp 

该脚本加载Simulink模型所需的某些常数，例如车辆和ACC设计参数。 默认的ACC是经典控制器。 该脚本还创建定义控制系统参考模型的输入和输出所需的总线。 在编译模型之前，必须在工作空间中定义这些总线。 编译模型时，其他Simulink总线将由其各自的模块自动生成。

要绘制仿真结果并描绘自我车辆的周围环境（包括被跟踪的物体），请使用Bird-Eye Scope 。 Bird's-Eye Scope是一个模型级别的可视化工具，可以从Simulink工具栏中打开。 在“ 模拟”选项卡上的“ 查看结果”下 ，单击“ 鸟瞰范围” 。 打开示波器后，单击“ 查找信号”以设置信号。 以下命令将模拟运行15秒钟，以获取模拟中间的图片，然后再次运行直至模拟结束，以收集结果。

sim('ACCTestBenchExample','StopTime','15') %Simulate 15 seconds sim('ACCTestBenchExample') %Simulate to end of scenario 

ans = Simulink.SimulationOutput: logsout: [1x1 Simulink.SimulationData.Dataset] tout: [151x1 double] SimulationMetadata: [1x1 Simulink.SimulationMetadata] ErrorMessage: [0x0 char] 

 

 

 

鸟瞰范围显示了传感器融合的结果。 它显示了雷达和视觉传感器如何在其传感器覆盖区域内检测到车辆。 它还示出了由多目标跟踪块保持的曲目。 黄色的轨道显示了最重要的对象（MIO）：在其车道中，自我车辆前面最近的轨道。 我们看到，在场景开始时，最重要的对象是快速移动的汽车，其位置超过了自我车辆。 当经过的汽车靠近慢速行驶的汽车时，它会越过左车道，并且传感器融合系统将其识别为MIO。 这辆车离自我车近得多，但比它慢得多。 因此，ACC必须减慢自我车辆的速度。

在对经典的ACC系统下面的结果，在：

• 上图显示了自我车辆的速度。

• 中间图显示了自我车辆与领头车之间的相对距离。

• 下图显示了自我车辆的加速度。

在这个例子中，从跟踪和传感器融合系统的原始数据被用于ACC设计无后处理。 由于传感器模型的不确定性，您可能会期望看到一些“尖峰”（中间图），尤其是当另一辆汽车驶入或驶出自我车辆车道时。

要查看仿真结果，请使用以下命令。

helperPlotACCResults(logsout,default_spacing,time_gap) 

• 在第11秒时，领头的车是远远领先于自身车辆（中图）的。 自主车辆加速并达到驾驶员设定的速度V_set（上图）。

• 当另一辆车驶入自我车辆车道（中间地段）时，它将在11到20秒内成为领头车。 当领头车与自我车辆之间的距离较大时（11-15秒），自我车辆仍会以驾驶员设定的速度行驶。 当距离变短（15-20秒）时，自我车辆减速以保持与有头车的安全距离（顶部图）。

• 从20到34秒，前面的汽车移到另一个车道，并出现了一辆新的领先汽车（中间图）。 因为有头车与自我车辆之间的距离较大，所以自我车辆会加速直到达到驾驶员设定的速度（27秒）。 然后，自主车辆继续以驾驶员设定的速度行驶（上图）。

• 该底部曲线表明加速度距离范围[-3,2]米/秒^ 2。 在平滑的过渡行为表明驾驶舒适性是令人满意的。

在基于MPC的ACC设计中，潜在的优化问题是通过跟踪驾驶员设定的速度来制定的，该速度取决于与领先汽车的安全距离。 MPC控制器设计在“自适应巡航控制器”部分中进行了介绍。 要使用MPC设计运行模型，请首先激活MPC变体，然后运行以下命令。 此步骤需要Model Predictive Control Toolbox软件。 您可以使用下面的代码检查该许可证的存在。 如果不存在代码，将描述类似结果的样本。

hasMPCLicense = license('checkout','MPC_Toolbox'); if hasMPCLicense controller_type = 2; sim('ACCTestBenchExample','StopTime','15') %Simulate 15 seconds sim('ACCTestBenchExample') %Simulate to end of scenario else load data_mpc end 

-->Converting model to discrete time. -->Assuming output disturbance added to measured output channel #2 is integrated white noise. Assuming no disturbance added to measured output channel #1. -->The "Model.Noise" property of the "mpc" object is empty. Assuming white noise on each measured output channel. ans = Simulink.SimulationOutput: logsout: [1x1 Simulink.SimulationData.Dataset] tout: [151x1 double] SimulationMetadata: [1x1 Simulink.SimulationMetadata] ErrorMessage: [0x0 char] -->Converting model to discrete time. -->Assuming output disturbance added to measured output channel #2 is integrated white noise. Assuming no disturbance added to measured output channel #1. -->The "Model.Noise" property of the "mpc" object is empty. Assuming white noise on each measured output channel. 

 

 

在基于MPC的ACC的仿真结果中，类似于经典ACC设计，可以实现速度和间距控制的目标。 与传统的ACC设计相比，基于MPC的ACC更具侵略性，因为它使用全油门或制动来加速或减速。 这种现象是由于在相对距离的明确约束。 当道路上突然发生变化时，例如当领先的汽车变成慢速汽车时，可能会倾向于具有侵略性。 要降低控制器的攻击性，请打开“自适应巡航控制系统”块的掩码，并减小“ 控制器行为”参数的值。 如前所述，中间图中的峰值是由于传感器模型中的不确定性所致。

要查看模拟与基于MPC-ACC的结果，使用以下命令。

helperPlotACCResults(logsout,default_spacing,time_gap) 

在下文中，在试验台模型各子系统的功能更详细地描述。 具有传感器融合子系统的自适应巡航控制器包含两个主要组件：

1. 跟踪和传感器融合子系统

2. 自适应巡航控制器子系统

open_system('ACCTestBenchExample/ACC with Sensor Fusion') 

跟踪和传感器融合

跟踪和传感器融合子系统处理来自“车辆和环境”子系统的视觉和雷达检测，并生成有关自我车辆周围环境的全面情况图片。 另外，它提供了最接近的汽车在车道自身车辆前面的估计ACC。

open_system('ACCWithSensorFusionMdlRef/Tracking and Sensor Fusion') 

跟踪和传感器融合子系统的主要块是多对象跟踪器块，其输入是所有传感器检测和预测时间的组合列表。 多对象跟踪器块的输出是已确认轨道的列表。

“ 检测级联”模块将视觉和雷达检测级联。 预测时间由“车辆和环境”子系统中的时钟驱动。

“检测聚类”模块将多个雷达检测聚类，因为跟踪器希望每个传感器每个物体最多检测一次。

MATLAB的findLeadCar功能块使用已确认的轨道和道路的曲率列表，在同一车道中找到最接近自我车辆并在其前方的汽车。 这辆车称为领头车，当汽车驶入和驶出自我车辆前方的车道时可能会发生变化。 该功能提供了领头车相对于自我车辆的位置和速度，以及最重要物体（MIO）轨迹的索引。

自适应巡航控制器

自适应巡航控制器有两个变体：经典设计（默认）和基于MPC的设计。 对于这两种设计，都应用以下设计原理。 配备ACC的车辆（自我车辆）使用传感器融合来估计与领头车的相对距离和相对速度。 在ACC使得在驾驶员设定的速度自车辆行驶，同时保持从引导车的安全距离。 引导汽车和自我车辆之间的安全距离被定义为

默认间距 和时间间隔 是设计参数和 是自我车辆的纵向速度。 ACC根据以下输入生成自我车辆的纵向加速度：

• 自主车辆的纵向速度

• 铅车与自我车辆之间的相对距离（来自“跟踪和传感器融合”系统）

• 领先车与自我车之间的相对速度（来自跟踪和传感器融合系统）

考虑到自身车辆的物理限制，纵向加速度被限制在范围[-3,2] 。

在经典ACC设计中，如果相对距离小于安全距离，则主要目标是放慢速度并保持安全距离。 如果相对距离大于安全距离，则主要目标是在保持安全距离的同时达到驾驶员设定的速度。 这些设计原则通过Min和交换机模块来实现的。

open_system('ACCWithSensorFusionMdlRef/Adaptive Cruise Controller/ACC Classical') 

在基于MPC的ACC设计中，通过跟踪受约束的驾驶员设定速度来表达潜在的优化问题。 约束要求相对距离始终大于安全距离。

 

 

要配置自适应巡航控制系统模块，请使用helperACCSetUp文件中定义的参数。 例如，用于ACC设计的线性模型 和 从车辆动力学获得。 两个Switch块实现了简单的逻辑，以处理来自传感器的大量数字（例如，传感器在未检测到MIO时可能返回Inf ）。

open_system('ACCWithSensorFusionMdlRef/Adaptive Cruise Controller/ACC Model Predictive Control') 

有关ACC的MPC设计的更多信息，请参阅使用模型预测控制的自适应巡航控制系统 （模型预测控制工具箱）。

车辆与环境

车辆和环境子系统由两部分组成：

1. 车辆动力学和全局坐标

2. 演员和传感器仿真

open_system('ACCTestBenchExample/Vehicle and Environment') 

车辆动力学子系统使用“自动驾驶工具箱”中的“自行车模型-力输入”模块对车辆动力学进行建模。 车辆动态，具有输入 （纵向加速度）和前转向角 ，近似为：

 

 

在状态向量， 表示横向速度， 表示纵向速度， 表示偏航角。 车辆参数在helperACCSetUp文件中提供。

车辆动力学的输出（例如纵向速度） 和横向速度 ）基于身体固定的坐标。 为了获得车辆所经过的轨迹，通过以下关系将车身固定坐标转换为整体坐标：

偏航角 和偏航角率 也转换为度单位。

驾驶员转向模型的目标是通过控制前转向角使车辆保持在车道内并遵循弯道 。 通过驱动偏航角误差可以实现该目标 和横向位移误差 为零（请参见下图），其中

所需的偏航角速率由下式给出 （ 表示道路曲率的半径）。

Actor和Sensor Simulation子系统生成跟踪和传感器融合所需的合成传感器数据。 在运行此示例之前，使用了Driving Scenario Designer应用程序来创建具有弯曲道路和多个参与者在道路上移动的场景。 然后，此场景中的道路和参与者被保存到场景文件ACCTestBenchScenario.mat 。 若要查看如何定义方案，请参阅“方案创建”部分。

open_system('ACCTestBenchExample/Vehicle and Environment/Actors and Sensor Simulation') 

自主车辆的运动由控制系统控制，并且不从脚本文件中读取。 取而代之的是，自车辆位置，速度，偏航角和偏航率作为输入从“车辆动力学”模块接收，并使用packEgo MATLAB功能模块打包为单个演员姿势结构。

场景读取器块从场景文件ACCTestBenchScenario.mat读取角色姿势数据。 该块将演员的姿势从场景的世界坐标转换为自我车辆坐标。 演员姿势在由块生成的总线上流式传输。 在这个例子中，您使用的是视觉检测发生器模块和雷达探测发生器块。 两种传感器都具有远距离和前瞻性，并且可以根据ACC的需要很好地覆盖自我车辆的前部。 传感器使用自我车辆坐标中的角色姿势生成自我车辆前方的车辆检测列表。 最后，以时钟块为例，说明车辆如何具有集中的时间源。 该时间由“多对象跟踪器”块使用。

场景创建

Driving Scenario Designer应用程序可让您定义道路和在道路上行驶的车辆。 在这个例子中，可以定义常曲率的两条平行的道路。 要定义的路上，你定义的道路中心，道路宽度和倾斜角（如果需要）。 通过沿圆弧采样点（跨度为60度的恒定曲率半径）来选择道路中心。

您定义方案中的所有载具。 要定义车辆的运动，您可以通过一组的航点和速度的定义他们的轨迹。 定义路点的一种快速方法是选择前面定义的道路中心的一个子集，并在道路中心的左侧或右侧偏移以控制车辆行驶的车道。

此示例显示了四辆车：左车道上的快速行驶的汽车，右车道上的缓慢行驶的汽车，靠近道路另一侧的汽车以及从右车道开始但随后移动的汽车到左车道通过缓慢移动的车。

可以使用Driving Scenario Designer应用程序修改该方案，并将其重新保存到同一方案文件ACCTestBenchScenario.mat 。 重新运行模拟后，“场景读取器”块将自动获取更改。 要以编程方式构建方案，可以使用helperScenarioAuthoring函数。

plotACCScenario 

生成控制算法的代码

该ACCWithSensorFusionMdlRef模型被配置成使用嵌入式编码软件，以支持生成C代码。 要检查是否有机会获得嵌入式编码器，运行：

hasEmbeddedCoderLicense = license('checkout','RTW_Embedded_Coder') 

您可以为模型生成一个C函数，并通过运行以下代码来探索代码生成报告：

if hasEmbeddedCoderLicense rtwbuild('ACCWithSensorFusionMdlRef') end 

您可以使用软件在环（SIL）仿真来验证编译的C代码是否按预期运行。 要在SIL模式下模拟ACCWithSensorFusionMdlRef引用的模型，请使用：

if hasEmbeddedCoderLicense set_param('ACCTestBenchExample/ACC with Sensor Fusion',... 'SimulationMode','Software-in-the-loop (SIL)') end 

当您运行ACCTestBenchExample模型时， ACCTestBenchExample模型生成，编译和执行代码。 这使您可以通过仿真来测试已编译代码的行为。

结论

此示例显示了如何在带有传感器融合功能的弯道上实施集成自适应巡航控制器（ACC），如何使用自动驾驶工具箱生成的合成数据在Simulink中对其进行测试，对其进行组件化并为其自动生成代码。

从MATLAB路径中删除示例文件夹。

rmpath(fullfile(matlabroot,'examples','mpc','main')); bdclose all 

也可以看看

职能

• record | roadBoundaries

积木

• 自适应巡航控制系统 | 检测级联 | 多对象跟踪器 | 雷达探测发生器 | 视觉检测发生器

对象

• drivingScenario

相关话题

• 自适应巡航控制系统使用模型预测控制 （模型预测控制工具箱）
• 使用FMCW技术的汽车自适应巡航控制 （相控阵系统工具箱）