[pilot智驾系统] 控制守护进程(controlsd) | 纵向横向 | 比例-积分-微分(PID)

第4章：控制守护进程(controlsd)

在前一章中，我们了解到自动驾驶守护进程(selfdrived)是sunnypilot的中央大脑，负责做出诸如是否激活、退出或显示警报等高层决策。selfdrived决定sunnypilot应该做什么。

但sunnypilot实际上是如何执行这些决策的？

如果selfdrived说"加速到30英里/小时并跟随这个平缓的弯道"，sunnypilot的哪一部分将这些想法转化为精确的转向调整、油门踏板踩下和刹车应用？

这就是**控制守护进程(controlsd)**发挥作用的地方

将controlsd视为sunnypilot的精确飞行员或熟练的手脚。它接收来自selfdrived（和其他规划组件）的高层命令，并将其转换为车辆能够理解的非常具体的实时信号。

它的任务是确保车辆以惊人的精度准确跟随期望的路径和速度，不断为现实条件（如风、道路拱度或车辆行为的微小变化）做出微小调整。

为什么需要控制守护进程？

没有controlsd，sunnypilot就像一个知道确切目的地的船长🚢，却无法告诉舵手和机舱船员如何到达那里。selfdrived的决策是抽象的（“保持这个速度”、“跟随这个曲率”）。

然而，车辆的硬件需要具体的指令：“施加X量的转向扭矩”、“踩下油门踏板Y百分比”、“施加Z量的刹车压力”。

controlsd填补了这一空白。它是理解驾驶物理和特定车辆特性的工程核心。它使用复杂的"控制循环"持续比较期望与实际发生的情况，然后调整车辆控制以最小化差异。这确保了平稳、准确和安全的驾驶体验。

用例：执行期望的路径和速度

假设sunnypilot已激活，selfdrived（与纵向规划器(LongitudinalPlanner)和模型守护进程(modeld)协作）已确定：

1. 期望曲率：车辆需要跟随具有特定转弯半径的路径（例如，一个平缓的右弯）。
2. 期望加速度：车辆需要以特定速率加速以达到目标速度或保持速度。

controlsd如何让车辆实际执行这些操作？

控制守护进程的工作原理

controlsd持续执行以下步骤，每秒多次（100次每秒）：

1. 监听命令：首先监听来自sunnypilot其他部分的消息，获取最新的期望动作：

   • 期望曲率：来自模型守护进程(modeld)。
   • 期望加速度：来自纵向规划器(LongitudinalPlanner)。
   • 车辆状态：来自车辆接口(CarInterface)，获取车辆的当前速度、转向角度、加速度等。
   • 自动驾驶状态：来自自动驾驶守护进程(selfdrived)，以确认是否允许控制车辆。

2. 计算纵向控制（油门/刹车）：确定踩下油门或刹车的力度以实现期望加速度，考虑车辆的当前速度和道路条件。它使用"PID控制器"来完成这一任务，这就像是车辆速度的智能恒温器。

3. 计算横向控制（转向）：确定转向的力度（例如，特定的转向角度或扭矩）以匹配期望曲率，同时考虑车速和其他因素。不同车辆可能使用不同类型的横向控制（如基于角度或基于扭矩的控制）。

4. 发送车辆控制命令：最后，controlsd将所有计算出的命令（如"转向这么多"、"加速这么多"）打包成一个特殊的carControl消息，并发送给车辆接口(CarInterface)，后者直接与车辆的硬件通信。

以下是这一流程的简化可视化：

如何与控制守护进程交互（其输出）

作为初学者，我们不会直接调用controlsd中的函数。相反，我们会观察它发布的关键carControl消息。此消息包含sunnypilot发送给车辆的实际命令。

以下是sunnypilot的另一部分如何查看controlsd正在发送的命令的简化示例：

（对象间的传参~）

import cereal.messaging as messaging
from cereal import car # 访问CarControl消息结构# 创建SubMaster以监听消息
sm = messaging.SubMaster(["carControl"])# 在运行非常频繁的循环中（如在监控工具中）
while True:sm.update(0) # 检查新消息if sm.updated["carControl"]:# 从controlsd获取最新的CarControl消息cc = sm["carControl"]# 访问执行器命令steer_angle_deg = cc.actuators.steeringAngleDeg # 期望转向角度accel = cc.actuators.accel                     # 期望加速度print(f"控制守护进程命令：转向角度 = {steer_angle_deg:.2f} 度，加速度 = {accel:.2f} m/s^2")# 检查sunnypilot当前是否在主动控制if cc.latActive and cc.longActive:print("  (sunnypilot正在同时控制转向和速度)")elif cc.latActive:print("  (sunnypilot仅控制转向)")elif cc.longActive:print("  (sunnypilot仅控制速度)")else:print("  (sunnypilot未主动控制)")

这段代码展示了任何sunnypilot组件读取carControl消息并理解发送给车辆执行器（转向、油门、刹车）的精确命令是多么简单。

底层原理：controlsd核心循环

controlsd主要在selfdrive/controls/controlsd.py中实现。它作为持续的后台进程运行，每秒执行其主逻辑100次。

其核心功能发生在state_control方法中，该方法被重复调用。让我们看看这个循环内部的简化版本：

# 摘自selfdrive/controls/controlsd.py（简化版）
from cereal import car
# ... (导入LatControl、LongControl等) ...class Controls:# ... (初始化方法) ...def state_control(self):# 从消息中获取当前车辆状态CS = self.sm['carState']# 从纵向规划器获取期望加速度long_plan = self.sm['longitudinalPlan']desired_accel = long_plan.aTarget# 从模型守护进程获取期望曲率model_v2 = self.sm['modelV2']desired_curvature_from_model = model_v2.action.desiredCurvature# 创建一个新的CarControl消息以填充我们的命令CC = car.CarControl.new_message()# 确定sunnypilot是否应纵向控制（油门/刹车）CC.longActive = self.sm['selfdriveState'].enabled # 简化示例# 确定sunnypilot是否应横向控制（转向）CC.latActive = self.sm['selfdriveState'].enabled # 简化示例# --- 1. 纵向控制（油门/刹车）---# LoC（LongitudinalControl）使用PID计算加速度命令accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)CC.actuators.accel = float(self.LoC.update(CC.longActive, CS, desired_accel, long_plan.shouldStop, accel_limits))# --- 2. 横向控制（转向）---# 更新我们内部跟踪的期望曲率，确保其平滑self.desired_curvature, _ = clip_curvature(CS.vEgo, self.desired_curvature, desired_curvature_from_model, self.sm['liveParameters'].roll)CC.actuators.curvature = self.desired_curvature# LaC（LateralControl）计算转向命令steer_cmd, steering_angle_deg_cmd, _ = self.LaC.update(CC.latActive, CS, self.VM, self.sm['liveParameters'],self.steer_limited_by_safety, self.desired_curvature,self.calibrated_pose, _ # 曲率限制)CC.actuators.torque = float(steer_cmd)       # 某些车辆的扭矩命令CC.actuators.steeringAngleDeg = float(steering_angle_deg_cmd) # 其他车辆的角度命令return CC, _ # 返回填充的CarControl消息

这个简化的state_control展示了controlsd如何收集所有必要的输入，使用其专用控制器（self.LoC用于纵向控制和self.LaC用于横向控制）计算加速度和转向命令，然后将它们打包到CC（CarControl）消息中。

让我们简要看看两个主要的控制组件：

A. 纵向控制（LongControl）

LongControl组件（位于selfdrive/controls/lib/longcontrol.py）负责通过油门和刹车踏板精确控制车辆速度。

它使用"比例-积分-微分"（PID）控制器，这是一种通过持续调整输出来维持期望值的常见方法。

• 比例（P）：对当前误差（期望与实际加速度之间的差异）做出反应。
• 积分（I）：考虑过去的误差，帮助消除稳态误差。
• 微分（D）：基于当前误差的变化率预测未来误差。

其update方法的简化视图：

# 摘自selfdrive/controls/lib/longcontrol.py（简化版）
from cereal import car
# ... (其他导入) ...LongCtrlState = car.CarControl.Actuators.LongControlStateclass LongControl:def __init__(self, CP):# ... (PID控制器设置) ...self.long_control_state = LongCtrlState.off # 当前状态（如停止、启动、PID）def update(self, active, CS, a_target, should_stop, accel_limits):"""更新纵向控制。参数：active: 长控制当前是否启用？CS: 当前车辆状态。a_target: 规划器提供的期望加速度。should_stop: 如果车辆需要完全停止则为True。accel_limits: 允许的最大/最小加速度。"""# 1. 更新状态机（关闭、停止、启动、PID）self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state,CS.vEgo, should_stop, CS.brakePressed,CS.cruiseState.standstill)# 2. 根据状态确定输出加速度output_accel = 0.0if self.long_control_state == LongCtrlState.stopping:# 施加受控减速度以停止output_accel = self.last_output_accel - self.CP.stoppingDecelRate * DT_CTRLelif self.long_control_state == LongCtrlState.starting:# 施加受控加速度以启动output_accel = self.CP.startAccelelif self.long_control_state == LongCtrlState.pid:# 使用PID控制器基于a_target计算加速度error = a_target - CS.aEgooutput_accel = self.pid.update(error, speed=CS.vEgo, feedforward=a_target)# 确保输出在安全限制内self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])return self.last_output_accel

LongControl的update方法首先使用状态机（long_control_state_trans）决定车辆是否应停止、启动或处于常规"PID"加速模式。

然后，基于该状态，它要么应用特定的启动/停止加速度，要么使用其PID控制器精确匹配纵向规划器(LongitudinalPlanner)提供的a_target（期望加速度）。

B. 横向控制（LatControl）

LatControl组件（在selfdrive/controls/lib/latcontrol.py中定义为抽象基类）负责精确控制车辆的转向以跟随期望路径（曲率）。

sunnypilot根据车辆的转向能力和调校有不同的LatControl实现：

横向控制类型	描述	使用场景
LatControlPID	使用PID控制器实现期望的转向扭矩。	通用，较旧的调校方法。
LatControlAngle	直接向车辆发送期望的转向角度命令。	适用于具有精确角度转向控制的车辆。
LatControlTorque	更高级的控制，建模车辆动力学以发送转向扭矩命令。	现代，性能更好的调校方法。

controlsd根据车辆的配置初始化其中一种特定的LatControl类型。

无论哪种类型，它们都提供一个update方法，该方法接收期望曲率并输出适当的转向命令。

简化的LatControl基础更新签名：

# 摘自selfdrive/controls/lib/latcontrol.py（简化版）
from abc import abstractmethod, ABCclass LatControl(ABC): # ABC表示抽象基类# ... (初始化方法) ...@abstractmethod # 这意味着子类必须实现此方法def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, calibrated_pose, curvature_limited):"""更新横向控制的抽象方法。参数：active: 横向控制当前是否启用？CS: 当前车辆状态。VM: 车辆模型。params: 实时车辆参数（如转向比）。desired_curvature: 模型提供的期望路径曲率。# ... 其他输入 ...返回：steer_cmd, steering_angle_deg_cmd, log_data"""pass # 此方法由LatControlPID、LatControlAngle等实现

这个抽象的update方法表明，任何LatControl实现都将接收当前车辆状态（CS）、车辆模型（VM）和关键的desired_curvature，以计算跟随该路径所需的精确steer_cmd（可能是扭矩或角度）。

总结

**控制守护进程(controlsd)**是sunnypilot的实际执行者。它接收来自自动驾驶守护进程(selfdrived)的高层决策以及来自纵向规划器(LongitudinalPlanner)和模型守护进程(modeld)的规划，并将其细致地转换为车辆转向、油门和刹车的精确实时命令。

通过使用复杂的控制循环，controlsd确保sunnypilot平稳、准确且安全地执行其驾驶计划，不断调整以保持车辆在需要的位置。

接下来，我们将探索**模型守护进程(modeld)**，它负责理解前方道路并预测车辆应行驶的路径

下一章：模型守护进程(modeld)