基于ASIL场景的自动驾驶控制算法开发(下):实施及评估

4.1客户实施

Client类旨在使用模型预测控制器（MPC）或比例积分微分（PID）控制器来管理模拟运行的生命周期。此类可确保正确初始化、执行和清理仿真，为在CARLA仿真环境中运行各种控制算法提供了一个健壮的框架。

Client类被实现为单例，以避免多次初始化，这可能会导致不一致的状态或资源冲突。班级结构如下：

• ___new___：确保只创建一个Client类实例。

• ___init___：初始化模拟设置，并根据输入参数选择适当的控制器。

• _get_controller：一个类方法，根据指定的类型返回控制器类（MPC或PID）。

• run_simulation：管理整个模拟过程，包括车辆和传感器设置、航路点导航和清理。

单例模式使用新方法实现，该方法确保只创建一个Client类实例：

在___init___方法中，实例化SetupLuxad类以处理环境设置，并设置控制器类型：

__ge__控制器方法根据输入参数选择适当的控制器类：

run_simulation方法管理整个模拟过程。最初，它会检查是否需要用于紧急制动目的的假车。如果场景包括虚拟车辆，则将生成虚拟车辆；否则，将跳过它，以避免不必要的资源分配和潜在的模拟再生问题。

然后，该方法使用所选控制器驱动车辆通过计划路线，处理车辆生成、传感器设置和航路点导航。它还包括错误处理，以管理中断并确保资源得到妥善清理：

4.2设置环境和对象

Setup类旨在配置模拟环境，包括创建世界、设置航路点以及生成车辆和传感器。

此类可确保环境正确初始化，并为运行模拟提供必要的基础设施。

4.2.1世界创造

create_world方法在客户端和服务器之间建立连接，确保通信设置正确。它将世界初始化为特定的地图“Town01”，并设置常量中定义的天气条件。此设置对于保持一致的仿真环境至关重要。

4.2.2同步设置

为了确保确定性行为并避免服务器和客户端之间的异步问题，create_synch_sets方法将模拟配置为在同步模式下运行。这种同步确保服务器在每次迭代中等待客户端完成其任务。增量秒是模拟的离散频率。

4.2.3航点可视化

draw_waypoints方法用于在仿真环境中直观地表示航路点。通过绘制航路点，我们可以直观地跟踪车辆对计划路线的遵守情况。路点用不同的标记绘制，以指示其位置，对于调试和确保正确的车辆导航至关重要。

图4.1：在路径上绘制的路线点

4.2.4车辆生成

在CARLA（汽车学习行动）的复杂框架内，车辆生成过程成为一个基础要素，促进了虚拟环境中车辆实体的创建和集成。此功能的核心是车辆生成功能，这是一段精心设计的代码，旨在协调车辆的无缝实例化。在本次讨论的背景下，我们的重点转向了以Microlino为蓝本的车辆的实例化，Microlino是一款为城市交通设计的紧凑型创新电动汽车。通过利用vehicle_spawn的功能，我们深入研究了在CARLA模拟环境中将Microlino变为现实的过程。

图4.2：将使用的Microlino汽车

4.2.5传感器生成

在自动驾驶汽车中，摄像头和激光雷达传感器的作用至关重要，因为它们构成了感知系统的支柱，使车辆能够感知和解释周围的环境。这些传感器收集有关车辆周围环境的关键数据，包括物体、障碍物、行人和道路状况。负责导航、避障和决策的算法在很大程度上依赖于这些传感器提供的信息。

值得注意的是，在我们的项目中，感知和传感器融合方面是使用Carla模拟器（通过obstacle_sensor）进行模拟的。虽然模拟器生成的数据可能无法完全复制现实世界的条件，但它为开发和微调我们的算法提供了一个有价值的测试环境。通过利用Carla模拟器提供的模拟功能，我们可以模拟各种场景，并评估我们的感知算法在不同条件下的性能。

图4.3：现实生活中的相机和激光雷达

camera_spawn方法将摄像头连接到车辆上，提供车辆周围环境的视觉反馈。这对于在模拟过程中监控车辆的行为和导航至关重要。

obs_sensor_swawn方法在车辆上生成障碍物传感器。该传感器检测指定半径内的障碍物，并触发障碍物检测事件的回调。其工作原理结合了激光雷达和相机传感器，通过分割进行图像分类。这使我们能够检测行人或汽车等物体。通过利用这些信息，我们可以确保在各种情况下为我们的车辆提供更安全的导航。传感器的检测距离是为最坏的情况选择的，即30米。最后，将传感器连接到车辆上。

检测到障碍物回调

在Carla UE4（服务器）中，传感器以指定的恒定频率运行。在每个离散步骤中，传感器都会产生结果。但是，如果每次调用回调函数，它可能会在循环迭代秒之间错过检测到的对象。随着循环间隔的增加，遗漏值的数量也会增加。为了避免这种情况，我们使用队列，一个同步线程库。实现的回调函数如下所示：

制动距离计算

braking_distance方法计算障碍物传感器的最小检测距离，确保车辆能够安全减速以避免碰撞。

4.2.6清理和资源管理

最后，delete_objects_and_settings方法可确保销毁所有创建的对象，并将模拟设置恢复为默认值。此清理步骤对于为后续运行维护稳定的模拟环境至关重要。

4.3控制器；开车穿过小路

如前所述，在这项研究中，自适应巡航控制器（ACC）是使用模型预测控制（MPC）和比例积分微分（PID）控制器实现的。ACC起着运动规划器的作用，使用这两个不同的控制器控制汽车通过其路径。在客户端文件中，调用函数如下：

如上所述，摄像头、观众（模拟器视图）、障碍物传感器和车辆作为阵列传递给drive_through_plan函数。这些传感器是通过调用其设置函数创建的，其中它们的属性如前所述进行设置。

drive_through_plan函数位于self-control侧，根据客户端文件中给出的命令确定使用哪个控制器。当使用bash命令“run_client<controller_name>”启动客户端文件时，指定的控制器名称决定了是使用PID还是MPC控制器。该机制允许系统根据提供的控制器名称动态选择和初始化适当的控制器。

4.3.1 PID控制器的实现

比例积分微分（PID）控制器是一种广泛应用于各种工程应用的基本控制机制，包括车辆控制系统。在本节中，我们将深入研究CARLA自适应巡航控制器（ACC）系统中PID控制器实现的具体细节。

如背景技术部分所述，PID控制器被设计为通过基于期望和实际系统状态之间的差异（误差）调整控制输入来维持期望的设定点。PID控制器结合了三种控制策略：

• 比例（P）控制：与电流误差成正比。

• 积分（I）控制：基于过去错误的累积。

• 导数（D）控制：基于对未来误差的预测，考虑误差的变化率。

我们系统中的PID控制器实现被封装在ControllerLuxad类中，特别是在create_PID方法中。PID控制器专为自我车辆量身定制，确保对其运动的精确控制。

在此函数中，定义了两组PID参数：

• 横向控制参数：调整这些参数（K_P、K_D、K_I、dt）以控制车辆的横向位置，确保其准确遵循计划路线。

• 纵向控制参数：这些参数被调整以控制车辆的速度，确保其保持所需的速度。

与控制回路集成

drive_through_plan方法将PID控制器集成到车辆的控制回路中。该方法通过一系列航路点协调车辆的运动，同时根据传感器输入不断调整其速度和方向。汽车的控制是由汽车控制对象完成的。

在这种方法中：

1.使用create_PID方法创建PID控制器。

2.持续监测车辆的位置、速度和方向。

3.check_vehicle_to_wartm_distance函数计算交叉航迹和航向误差。

4.如果在临界距离内检测到障碍物，则upper_level_control_acc方法会调整车辆速度以保持安全距离。否则，车辆的速度将根据PID控制器的输出进行控制，以保持恒定速度。

5.使用apply_control方法应用车辆的控制输入。

这是由汽车控制分配的，汽车控制是一个CARLA对象，具有转向、油门、刹车和变速箱信息。总之，自适应巡航控制器系统中PID控制器的实现提供了对自我车辆运动的精确控制。通过持续监测其位置、速度和方向，并根据传感器输入和控制器输出调整其速度和方向。该系统确保在规划路线中安全高效地导航，同时实时动态响应障碍物。

记录和文件

在整个控制回路中，记录时间戳、交叉跟踪误差、航向误差和速度误差等关键变量以供分析。这是使用write_to_document方法完成的，该方法将这些值写入CSV文件以供以后查看。

这种方法可确保记录重要的性能指标，以便进行分析和系统改进。

4.3.2 MPC控制器的实施

在本节中，我们将深入研究用于在Carla模拟器环境中控制自动驾驶汽车的模型预测控制（MPC）算法的实现。本小节将介绍ControllerMPCLuxad类的设置、功能和关键组件，该类负责基于MPC的路径跟踪和避障。

车辆参数的数据类

我们定义数据类来存储MPC算法日志记录的当前和历史车辆参数和航路点。

Listing 4.1：车辆参数的数据类

这些数据类封装了管理车辆状态及其必须遵循的航路点所需的属性。

MPCLuxad类控制器

模型预测控制（MPC）是一种先进的控制策略，它利用系统的模型来预测未来的行为，并在指定的预测范围内优化控制动作。在自动驾驶和自适应巡航控制（ACC）系统领域，MPC在操纵约束、优化性能和确保安全方面具有显著优势。

在本节中，我们将深入研究CARLA ACC系统中MPC实施的具体细节。MPC控制器旨在遵循预定义的路径，同时动态调整车辆的速度和转向，以保持最佳性能和安全性。与传统控制器不同，MPC考虑了未来的预测和约束，使其非常适合自动驾驶中的复杂实时应用。

如文章“上部分”的背景技术部分所述，MPC算法涉及在每个控制步骤中解决优化问题。该问题公式包括：

一个成本函数，用于捕捉与期望轨迹和控制努力的偏差。确保车辆在安全可行的范围内运行的约束，如最大速度、加速度和转向角。基于当前条件和控制输入模拟车辆未来状态的预测模型。我们系统中的MPC实现被封装在ControllerMPCLuxad类中，特别是在drive_through_plan方法中。

这种实现是为自我车辆量身定制的，确保在动态环境中对其运动、路径跟踪和避障进行精确控制。

初始化

构造器初始化各种参数，包括控制器速度、障碍物队列、历史车辆参数和航路点。

驾车计划

在PID实现中，drive_through_plan方法使用MPC算法动态更新控制命令，沿计划路线控制车辆。主要区别在于，在模型预测控制器中，我们试图预测未来的路径，迭代地纠正车辆的行为，并将其保持在计划的路径上。因此，在每次迭代中都需要收集车辆数据的x和y位置和航向。循环一直执行到路径的最后一点。

与之前实现的控制循环相同，总是从队列中检查障碍物列表。如果在安全距离内检测到障碍物，车辆将通过撤销“distance_breaking_speed”来采用前方车速。

航路点插值

MPC控制器中的航路点插值方法通过在给定的计划航路点之间生成插值航路点，在为车辆创建平滑的导航路径方面起着至关重要的作用。这种更精细的航路点分辨率有助于控制器更精确地跟踪路径，确保更平稳的过渡和更准确地遵循所需的轨迹。

该方法确保以更精细的分辨率插值航路点，以实现更平滑的路径跟踪。

航路点插值方法首先将一系列计划航路点作为输入。对这些计划的航路点进行处理，以提取其x和y坐标，并使用从km/h到m/s的恒定转换因子（constant.MS_to_KMH）对速度分量进行归一化。然后将这些值作为NumPy数组存储在mpc_wathpoints.mpc属性中，以进行高效计算。

接下来，该方法计算每个连续航路点之间的欧几里德距离。这是通过从第二个航路点开始迭代航路点并使用毕达哥拉斯定理计算距离来实现的。这些距离存储在mpc_watherts.distance列表中。距离列表中附加了一个额外的0，以说明从最后一个航路点到自身的距离，该距离本质上为零。

然后，该方法初始化插值计数器interp_counter，以跟踪插值航路点的索引。对于每对连续的航路点，该方法首先将原始航路点添加到插值列表中，并将该索引映射到hash_interpol列表中。

为了确定每对路点之间要插值的点数，该方法根据常数定义的所需分辨率计算所需的插值点数。interpret_crement_ITERATION。它计算当前和下一个航路点之间的向量，并对其进行归一化，以创建插值单位向量。使用这些单位向量，该方法生成中间航路点并将其附加到插值列表中，为每个新的插值点递增interp_counter。

最后，将最后一个航点附加到插值列表中，以确保包含路径的末尾，并将相应的索引记录在hash_interpol列表中。使用np.seterr（invalid=“ignore”）有助于抑制计算过程中的潜在警告，确保方法平稳运行而不中断。

总的来说，这种插值过程确保了路点的分辨率更高，从而实现了更平稳的路径跟踪和更精确的车辆控制。

控制器分配和更新

控制器分配和更新控制器方法是MPC控制器的重要组成部分，分别负责生成控制命令和用新的航路点和车辆参数更新控制器。

controller_assignment方法将控制器对象作为输入，并创建一个VehicleControl对象，其中包含使用get_commands方法从控制器获得的油门、转向和制动命令。其他属性，如手刹、手动换档和倒档，都设置为默认值。

最后，该方法返回控制对象。

update_controller方法使用新的路径点和车辆参数更新MPC控制器。它首先根据当前车辆位置使用new_propper_waypoint_method生成新的航路点。然后，它用这些航路点和当前车辆参数更新控制器，包括位置、偏航角、速度和时间。此外，它还会设置一个标志来指示车辆是否正在行驶。

最后，该方法用当前速度限制更新控制器。这些方法根据车辆的当前状态和更新的航路点动态调整控制命令，确保车辆的准确和响应性控制。

新航路点迭代

new_propper_waypoint_iteration方法负责根据当前车辆位置和前方距离生成车辆将遵循的航路点子集。该方法确保车辆从MPC控制器接收可管理数量的航路点，以实现更平稳的导航。

new_propper_waypoint_iteration方法对于确定车辆在导航过程中任何给定时刻将遵循的航路点子集至关重要。它通过根据车辆的当前位置和系统指定的前视距离动态选择航路点来运行。

首先，该方法计算距离车辆当前位置最近的航路点。这是通过计算车辆坐标与计划路线上每个航路点坐标之间的欧几里德距离来实现的。与车辆距离最小的航路点被识别为最近的航路点。

一旦确定了最近的航路点，该方法就会选择车辆要遵循的航路点子集。该子集包括车辆前方和后方的航路点，为平稳导航提供缓冲。子集中包含的航路点数量是根据指定的前方距离确定的，该前方距离表示控制器在规划其轨迹时应考虑的车辆前方距离。

该方法迭代计划路线，从最近的航路点开始递增和递减航路点索引，以识别航路点的子集。它确保子集至少包括车辆后方的一个路点，并充分延伸到车辆前方，以覆盖指定的前方距离。

一旦确定了航路点的子集，它就会返回给MPC控制器使用。这些航路点是轨迹规划和控制的关键参考点，使车辆能够沿着计划路线安全有效地导航。

总体而言，new_propper_waypoint_iteration方法在确保车辆接收到可管理数量的航路点进行导航方面发挥着至关重要的作用，从而促进了平稳高效的路径跟踪。

控制器定义

控制器定义方法负责创建用于轨迹规划和控制的模型预测控制（MPC）控制器的实例。该方法在用从计划路线中提取的适当航路点集初始化控制器方面起着至关重要的作用。该方法采用路线点（航路点）列表作为输入，通常表示为一个矩阵，其中包含沿计划路线的每个航路点的（x，y，z）坐标。这些航路点对于引导车辆的轨迹并确保其准确遵循所需路径至关重要。

为了创建MPC控制器，该方法迭代路线点列表，并将每个航路点转换为适合MPC轨迹规划的格式。这涉及提取每个航路点的x和y坐标，并根据MPC控制器的要求将速度限制从每秒米（m/s）转换为每小时公里（km/h）。

一旦准备好转换的航路点列表，它就会作为输入传递给MPC控制器类（MPC_control.Controller2D）的构造函数。这将使用轨迹规划和控制所需的必要航路点和其他参数初始化MPC控制器。

然后，该方法返回初始化的MPC控制器对象，准备用于引导车辆沿其计划路线行驶。通过将MPC控制器的创建封装在这种方法中，该系统保持了模块化和灵活性，允许轻松集成不同的控制器实现或修改轨迹规划算法。

总体而言，控制器定义方法是MPC控制器初始化的重要组成部分，确保其正确配置了相关航路点，以进行有效的轨迹规划和控制。

距离制动速度

距离制动速度法，通过根据与环境中障碍物的距离计算适当的速度来确保车辆的安全。

调用时，此方法有三个参数：obstacle_dist，表示到最近障碍物的距离；critical_dist，需要紧急制动的阈值距离；以及减速，车辆应减速的速率。

该方法通过首先确定障碍物距离（obstacle_dist）和临界距离（critical_dist）之间的差值来计算安全制动速度（speed_br）。然后将该差值乘以减速率的两倍（减速）。

随后，取所得值的平方根以获得最终的安全制动速度。最后，使用常数MS_to_KMH提供的转换系数将速度从米每秒（m/s）转换为公里每小时（km/h）。该方法返回计算出的安全制动速度，为车辆提供必要的信息，以便根据其路径上障碍物的接近程度动态调整其速度。

5.1控制操作和算法基准

如前所述，本研究中实现的自动驾驶控制算法是用于运动规划中的自适应巡航控制（ACC）的PID（比例积分微分）和MPC（模型预测控制）。这些算法对于在适应不断变化的路况和障碍物的同时保持车辆的理想速度和轨迹至关重要。

为了全面评估这些控制算法的性能，在农村和城市环境中创建并测试了场景。乡村场景通常具有更少的汽车和更简单的道路布局，这提供了一个受控的环境来观察控制算法的基本性能和调整。另一方面，城市场景包括更高密度的行人、自行车和复杂的交通模式，为测试算法的鲁棒性和适应性提供了更具挑战性的环境。

PID和MPC算法之间的比较是使用两个主要指标进行的：航向误差和交叉跟踪误差。航向误差测量车辆实际航向和期望航向之间的差异，反映车辆转向控制的准确性。另一方面，交叉轨迹误差测量车辆当前位置和所需轨迹之间的横向距离，表明车辆保持其路径的程度。

下图显示了所创建的乡村场景（汽车较少）和城市场景（行人和自行车较多）。这些场景经过精心设计，用于在各种条件和复杂性下测试控制算法：

图5.1：乡村场景下的运行视图

乡村场景中PID实施的分析如下：

图5.2：乡村场景中的PID控制器错误

同样，乡村场景下的MPC实施情况如下：

图5.3：乡村场景中的MPC控制器错误

如图所示，MPC的峰值交叉跟踪误差在5厘米以内，而PID控制器的峰值交叉轨道误差超过2米。另一个关键方面是航向误差，它衡量了车辆转向的正确性。对于PID控制器，航向误差约为25度，而对于MPC，航向误差仅为5度。

在乡村场景中，MPC明显优于PID控制器。接下来，我们将研究在交通和行人较多的城市环境中测试控制器的场景。

图5.4：复杂的城市情景

如图所示，城市场景更为复杂，前方有两名骑自行车的人和一辆汽车。在这种情况下，车辆必须检测前方障碍物，并根据自适应巡航控制保持前方车辆的速度。

两种不同的ACC控制算法在城市场景中的性能如下：

图5.5：城市场景中的MPC算法操作

城市场景中PID实施的分析如下：

图5.6：城市场景中的PID算法操作

从错误分析中可以清楚地看出，两种算法的响应方式相似，但纠正错误所需的时间各不相同。PID控制器在响应中显示出更多的峰值，特别是在施加向心力的曲线上，表明性能不太平稳。

总之，更复杂的控制算法方法，即模型预测控制，在评估危害场景并提供考虑自动驾驶模型中驾驶安全的响应时表现更好。因此，MPC更适合复杂环境中的自适应巡航控制，确保更平稳、更安全的驾驶体验。

5.2危害和风险评估：PID与MPC

作为本研究最重要的部分之一，ISO分析将用于软件算法应用分析。在前面的部分中，场景是在实施各种操作的不同环境中创建的。

这些场景包括最危害和最不危害的情况：城市复杂的交通和更舒适的农村地区。这些将根据紧急制动、自适应巡航控制驾驶和横向运动进行评估。

场景可分为以下几类：

1.场景1（S1）：在城市中使用自适应巡航控制。

2.场景2（S2）：在城市中紧急制动。

3.场景3（S3）：城市中的横向（曲线）运动。

4.场景4（S4）：在农村地区，自适应巡航控制运动。

5.场景5（S5）：在农村地区，紧急制动运动。

6.情景6（S6）：在农村地区，横向（曲线）运动。

考虑的两种算法是：

• 算法1（A1）：PID控制算法

• 算法2（A2）：MPC控制算法

危害分析的参数包括严重性（S）、暴露（E）和控制不稳定性（C）。

表5.1：关于PID算法（A1）和MPC算法（A2）的场景（1-6）的HARA分析

该分析基于交叉轨迹和航向误差（图5.2、5.3、5.6和5.5）。主要差异发生在横向移动过程中，在城市场景中变得更加严重。因此，场景3是两种算法中风险最大的场景。对于PID控制器，在横向移动的城市场景中，它具有25度航向误差和2米交叉轨迹误差，这导致ASIL D等级。相比之下，由于其预测行为，MPC控制器的航向误差为5度，峰值交叉跟踪误差为20cm，从而获得ASIL B评级。

5.3最终考虑因素

综上所述，PID和MPC控制器都是ACC系统的组成部分，但它们表现出不同的特点和起源。CARLA提供的PID控制器库提供了一种完善的控制机制，用于维持所需的设定点。它的实现相对简单，依靠比例、积分和微分控制策略根据误差信号调整车辆行为。虽然PID控制器在许多情况下都是有效的，并且易于实现，但它们可能会在具有非线性动力学或严格约束的复杂环境中挣扎。

相反，本项目中使用的MPC库来自多伦多大学的自动驾驶课程。MPC在处理约束、优化性能和确保安全方面比PID具有显著优势。其预测性使其能够预测未来状态并在指定范围内优化控制动作，使其非常适合自动驾驶中的复杂实时应用。然而，MPC控制器的实现往往更复杂，需要对系统动力学进行详细的建模。

我们的探索表明，与PID相比，MPC控制器表现出更大的灵活性和适应性，特别是在有动态障碍物、复杂道路几何形状或严格安全需求的情况下。通过测试各种场景，如高速公路合并、城市导航和紧急制动，在不同情况下评估了每个控制器的优缺点。

总体而言，MPC算法与PID算法相比表现出更优的性能，特别是在更复杂的城市场景中。MPC的预测性使其能够处理动态环境，并更好地控制车辆的轨迹和速度。这带来了更安全、更可靠的性能，使MPC成为自动驾驶应用中自适应巡航控制和其他先进驾驶员辅助系统的首选。

如表5.1中的初步结果所示，HARA分析得出MPC控制器的ASIL B和PID控制器的ASII D，表明MPC比PID提供了两个更高级别的安全防护。

在这项研究中，由于时间限制，我们没有时间实施高速公路方案。相反，我们专注于HARA分析中最危害和最不危害的场景。对于未来的工作，建议创建高速公路场景并实施基于人工智能的非线性MPC，以预测MPC的成本函数变量。强化学习算法可以与快速探索随机树（RRT）方法结合使用，以增强运动规划。这种方法将有可能提高控制系统在更复杂和动态环境中的适应性和性能，进一步提高自动驾驶技术的安全性和可靠性。

免责声明：文中观点仅供分享交流，文章版权及解释权归原作者及发布单位所有，如涉及版权等问题，请您联系alpha.yuan@houwa-tech.com告知，我们会在第一时间做出处理。