[翻译]Adaptive Cruise Control with Sensor Fusion

In this example, you:

Review a control system that combines sensor fusion and an adaptive cruise controller (ACC). Two variants of ACC are provided: a classical controller and an Adaptive Cruise Control System block from Model Predictive Control Toolbox.

Test the control system in a closed-loop Simulink model using synthetic data generated by the Automated Driving Toolbox.

Configure the code generation settings for software-in-the-loop simulation, and automatically generate code for the control algorithm.

在此示例中，您：

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

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

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

介绍

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

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

当前的ACC设计主要依赖于从雷达获得的距离和测距率测量结果，并且设计为在笔直的道路上效果最佳。

• 在使用模型预测控制的自适应巡航控制系统（模型预测控制工具箱），https://www.mathworks.com/help/mpc/ug/adaptive-cruise-control-using-model-predictive-controller.html
• 使用FMCW技术的汽车自适应巡航控制系统（相控阵系统工具箱）中给出了这样一个系统的示例。https://www.mathworks.com/help/phased/examples/automotive-adaptive-cruise-control-using-fmcw-technology.html

从先进的驾驶员辅助系统（ADAS）设计过渡到更多的系统，ACC必须应对以下挑战：

• Estimating the relative positions and velocities ，估计靠近自我车辆且相对于自我车辆具有明显横向运动的汽车的相对位置和速度。

• Estimating the lane ahead of the ego vehicle，估算自我车辆前方的车道，以找出自我车辆前方的哪辆车是同一车道中最接近的车。

• Reacting to aggressive maneuvers by other vehicles ，对环境中其他车辆的侵略性动作做出反应，尤其是当另一辆车辆驶入自我车辆车道时。

本示例说明了现有ACC设计中的两个主要补充，以应对这些挑战：

• 添加传感器融合系统
• 基于模型预测控制（MPC）更新控制器设计。

同时使用视觉和雷达传感器的传感器融合和跟踪系统具有以下优点：

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

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

先进的MPC控制器增加了对环境中其他车辆做出的侵略性的行为（例如强行插道）做出反应的能力。

与使用具有恒定增益的PID设计的经典控制器相比，MPC控制器在保持严格的安全距离约束的同时调节自我车辆的速度。

因此，当环境以与人类驾驶员将执行的操作类似的方式快速变化时，控制器可以采取更具积极的操纵。

Model

To open the main Simulink model, use the following command:

open_system('ACCTestBenchExample')

The model contains two main subsystems:

• ACC with Sensor Fusion, which models the sensor fusion and controls the longitudinal acceleration of the vehicle. This component allows you to select either a classical or model predictive control version of the design.

• A Vehicle and Environment subsystem, which models the motion of the ego vehicle and models the environment. A simulation of radar and vision sensors provides synthetic data to the control subsystem.
  该脚本加载Simulink模型所需的某些常数，例如车辆和ACC设计参数。
  默认的ACC是经典控制器。
  该脚本还创建定义控制系统参考模型的输入和输出所需的总线。
  在编译模型之前，必须在工作空间中定义这些总线。
  编译模型时，其他Simulink总线将由它们各自的模块自动生成。

To plot the results of the simulation and depict the surroundings of the ego vehicle, including the tracked objects, use the Bird’s-Eye Scope.
The Bird’s-Eye Scope is a model-level visualization tool that you can open from the Simulink toolstrip.
Bird’s-Eye Scope是一个模型级的可视化工具，您可以从Simulink工具栏中打开它。

On the Simulation tab, under Review Results, click Bird’s-Eye Scope. After opening the scope, click Find Signals to set up the signals.

The following commands run the simulation to 15 seconds to get a mid-simulation picture and run again all the way to end of the simulation to gather results.

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

The Bird’s-Eye Scope shows the results of the sensor fusion. It shows how the radar and vision sensors detect the vehicles within their sensors coverage areas. It also shows the tracks maintained by the Multi Object Tracker block.
黄色的轨道显示了最重要的对象（MIO）：在其车道中自我车辆前方最近的轨道。 我们看到，在方案开始时，最重要的对象是快速移动的汽车，其位置超过了自我车辆。 当经过的汽车靠近慢速行驶的汽车时，它会越过左车道，并且传感器融合系统将其识别为MIO。 这辆车离自我车近得多，但比它慢得多。 因此，ACC必须减慢自我车辆的速度。

在经典ACC系统的以下结果中：

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

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

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

在此示例中，来自跟踪和传感器融合系统的原始数据用于ACC设计，而无需进行后处理。

由于传感器模型的不确定性，您可能会期望看到一些“尖峰”（中间图），尤其是当另一辆汽车驶入或驶出自我车辆车道时。
To view the simulation results, use the following command.

helperPlotACCResults(logsout,default_spacing,time_gap)

在基于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

在基于MPC的ACC的仿真结果中，类似于经典ACC设计，可以实现速度和间距控制的目标。 与经典的ACC设计相比，基于MPC的ACC更具侵略性，因为它使用全油门或制动来加速或减速。 此行为是由于相对距离受到显式约束。 当道路上突然发生变化时，例如当领先的汽车变成慢速汽车时，可能会倾向于具有侵略性。

要降低控制器的攻击性，请打开“自适应巡航控制系统”块，并减小“控制器行为”参数的值。 如前所述，中间图中的峰值是由于传感器模型中的不确定性所致。

详细描述

下面，将更详细地描述测试平台模型中每个子系统的功能。 具有传感器融合子系统的自适应巡航控制器包含两个主要组件Sensor Fusion subsystem：

• Tracking and Sensor Fusion subsystem

• Adaptive Cruise Controller subsystem

open_system('ACCTestBenchExample/ACC with Sensor Fusion')

Tracking and Sensor Fusion

The Tracking and Sensor Fusion subsystem processes vision and radar detections coming from the Vehicle and Environment subsystem and generates a comprehensive situation picture of the environment around the ego vehicle. Also, it provides the ACC with an estimate of the closest car in the lane in front of the ego vehicle.
跟踪和传感器融合子系统处理来自“车辆和环境”子系统的视觉和雷达检测，并生成有关自我车辆周围环境的全面情况图片。 此外，它还向ACC提供了关于自我车辆前方车道中最近的汽车的估算值。

open_system('ACCWithSensorFusionMdlRef/Tracking and Sensor Fusion')

跟踪和传感器融合子系统的主要模块是

多目标跟踪器块Multi-Object Tracker block'，其输入是所有传感器检测和预测时间的组合列表。多对象跟踪器块的输出是已确认轨道的列表。

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

检测聚类模块The Detection Clustering block对多个雷达检测进行聚类，因为跟踪器希望每个传感器每个物体最多检测一次。

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

Adaptive Cruise Controller

自适应巡航控制器有两个变体：

• 经典设计（默认）
• • 基于MPC的设计。

对于这两种设计，都应用以下设计原理。 配备ACC的车辆（自我车辆）使用传感器融合来估计与领头车的相对距离和相对速度。 ACC使自我车辆以驾驶员设定的速度行驶，同时保持与领头车的安全距离。 引导车与自我车辆之间的安全距离定义为

其中默认间距 $D_{default}$和时间间隔$T_{gap}$是设计参数，而$V_x$是自我车辆的纵向速度。 ACC根据以下输入生成自我车辆的纵向加速度：

• 自主车辆的纵向速度
• 领头车’'和自我车辆之间的相对距离（来自跟踪和传感器融合系统）
• 领先车与自我车之间的相对速度（来自跟踪和传感器融合系统）
  考虑到自我车辆的物理限制，纵向加速度被限制在[-3,2] $m/s^2$范围内。

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

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

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

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

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

For more information on MPC design for ACC, see Adaptive Cruise Control System Using Model Predictive Control (Model Predictive Control Toolbox).
https://www.mathworks.com/help/mpc/ug/adaptive-cruise-control-using-model-predictive-controller.html

Vehicle and Environment

The Vehicle and Environment subsystem is composed of two parts:

• Vehicle Dynamics and Global Coordinates
• Actor and Sensor Simulation

open_system('ACCTestBenchExample/Vehicle and Environment')

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

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

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

偏航角$\psi$和偏航角速率$\dot{\psi }$也转换为度单位。

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

所需的偏航角速率由$Vx/R$给出（$R$表示道路曲率的半径）。

Actor和Sensor Simulation子系统生成跟踪和传感器融合所需的合成传感器数据。

在运行此示例之前，使用了“驾驶场景设计器”应用程序创建了道路上有“弯曲的道路和多个参与者移动”的场景。https://www.mathworks.com/help/driving/ref/drivingscenariodesigner-app.html

然后将该场景中的道路和参与者保存到场景文件ACCTestBenchScenario.mat中。

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

自我车辆的运动由控制系统控制，而不是从脚本文件 scenario file. 中读取的。

将接收到的自主车辆位置，速度，偏航角和偏航率作为输入,来自``the Vehicle Dynamics block’’

并使用``packEgo MATLAB function block’'打包为Action结构。

The Scenario Reader block场景读取器’'块从场景文件ACCTestBenchScenario.mat中读取action姿势数据。

该块将演员的姿势从场景的世界坐标转换为自我车辆坐标。

演员姿势在由块生成的总线上流式传输。

在此示例中，您使用了视觉检测生成器块''和雷达检测生成器块’’。
https://www.mathworks.com/help/driving/ref/scenarioreader.html
两种传感器很好地覆盖自我车辆的前部。
https://www.mathworks.com/help/driving/ref/visiondetectiongenerator.html
https://www.mathworks.com/help/driving/ref/radardetectiongenerator.html

传感器使用自我车辆坐标中的角色姿势来生成自我车辆前方的车辆检测列表。

最后，以时钟块为例，说明车辆如何具有集中式时间源。 该时间由``多对象跟踪程序块’'使用。

Scenario Creation

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

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

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

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

生成控制算法的代码

ACCWithSensorFusionMdlRef模型配置为支持使用嵌入式编码器软件生成C代码。要检查您是否有权访问Embedded Coder，请运行：

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模型时，将为ACCWithSensorFusionMdlRef模型生成，编译和执行代码。这使您可以通过仿真来测试已编译代码的行为。

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

---

参考：https://www.mathworks.com/help/driving/examples/adaptive-cruise-control-with-sensor-fusion.html