第三章：车辆控制与动力学模型
车辆控制，说白了就是让车听话。你规划了一条完美的路径，但车能不能沿着它走？这就是控制要解决的问题。我在做自动驾驶项目时，发现很多同学把精力都放在感知和规划上，结果一上路，车像喝醉了一样左右摇摆。嗯，控制这块，其实才是真正考验工程能力的地方。
3.1 车辆控制接口
CARLA 给我们提供了非常直接的控制接口。你不需要关心底层怎么和车辆 ECU 通信，只需要调用一个函数就行。
核心接口就一个：vehicle.apply_control()。它接收一个 VehicleControl 对象。这个对象里有什么？我列一下常用的：
- throttle：油门，范围 0.0 到 1.0。0 就是松油门，1 就是地板油。
- steer：转向，范围 -1.0 到 1.0。负值左转，正值右转。
- brake：刹车，范围 0.0 到 1.0。0 不刹，1 抱死。
- hand_brake：手刹，布尔值。一般别用，除非你想玩漂移。
- reverse：倒车，布尔值。
- gear：档位。自动挡模式下可以不管。
核心要点：油门和刹车不要同时给。我见过有人油门设 0.5，刹车也设 0.3，结果车纹丝不动。控制器的逻辑是刹车优先，但这样浪费能量。
来个最简单的例子，让车直着走：
import carla
client = carla.Client('localhost', 2000)
world = client.get_world()
vehicle = world.get_actors().filter('vehicle.*')[0]
control = carla.VehicleControl()
control.throttle = 0.5
control.steer = 0.0
control.brake = 0.0
vehicle.apply_control(control)
你看，就这么几行，车就能动了。但实际项目中，你不可能手动调这些值。你需要一个控制器，自动计算油门、刹车和转向。
3.2 PID 控制
PID 控制，全称比例-积分-微分控制。它是工业界最经典的控制算法。为什么经典？因为它简单、有效、不需要知道系统的精确模型。
我刚开始做控制时，觉得 PID 太简单了，想用更高级的模型预测控制（MPC）。结果调了两个月，效果还不如一个调好的 PID。后来我学乖了：能用 PID 解决的问题，别折腾。
3.2.1 PID 的原理
PID 的核心思想就一句话：根据误差，计算控制量。
误差是什么？比如你想让车速到 30 km/h，现在实际是 20 km/h，误差就是 10 km/h。PID 根据这个误差，算出油门该给多少。
三个分量各司其职：
- P（比例）：误差越大，控制量越大。但光有 P，系统会震荡，永远到不了目标值。
- I（积分）：把过去的误差累积起来。消除稳态误差。比如有上坡，P 给的不够，I 会慢慢加上去。
- D（微分）：预测误差的变化趋势。抑制震荡，让系统更稳定。
我的经验：调参时先调 P，让系统能响应。再加 D 抑制震荡。最后加 I 消除静差。顺序别搞反了。
3.2.2 纵向控制（速度控制）
纵向控制就是控制油门和刹车，让车达到目标速度。我习惯用一个 PID 控制器来处理：
class PIDController:
def __init__(self, Kp, Ki, Kd):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.integral = 0.0
self.prev_error = 0.0
def control(self, target, current, dt):
error = target - current
self.integral += error * dt
derivative = (error - self.prev_error) / dt
self.prev_error = error
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
return output
# 使用示例
pid_speed = PIDController(Kp=1.0, Ki=0.1, Kd=0.05)
target_speed = 30.0 # km/h
while True:
current_speed = vehicle.get_velocity().length() * 3.6 # 转成 km/h
throttle_brake = pid_speed.control(target_speed, current_speed, 0.05)
control = carla.VehicleControl()
if throttle_brake > 0:
control.throttle = min(throttle_brake, 1.0)
control.brake = 0.0
else:
control.throttle = 0.0
control.brake = min(-throttle_brake, 1.0)
vehicle.apply_control(control)
这里有个细节：PID 输出是正数就踩油门，负数就踩刹车。我见过有人把油门和刹车分开用两个 PID，其实没必要，一个就够了。
3.2.3 横向控制（转向控制）
横向控制就是让车沿着路径走。最常用的方法是纯追踪（Pure Pursuit），但这里我们先用 PID 实现一个简单的转向控制器。
思路是这样的：计算车辆当前位置与目标路径的横向偏差，然后用 PID 算出转向角。
def lateral_control(vehicle, waypoint, pid_steer):
# 获取车辆位置和朝向
vehicle_location = vehicle.get_location()
vehicle_transform = vehicle.get_transform()
forward_vector = vehicle_transform.get_forward_vector()
# 计算目标点相对于车辆的位置
target_vector = waypoint.transform.location - vehicle_location
# 计算横向偏差（叉积的 z 分量）
cross = forward_vector.x * target_vector.y - forward_vector.y * target_vector.x
steer = pid_steer.control(0.0, cross, 0.05)
return max(min(steer, 1.0), -1.0)
注意：横向控制的 PID 参数和纵向完全不同。我曾经把纵向的 PID 参数直接用到横向上，结果车在路上画龙。横向的 P 要小一些，D 要大一些。
3.3 车辆动力学参数调整
CARLA 的车辆模型基于物理引擎，有很多动力学参数可以调。这些参数直接影响车辆的操控特性。
3.3.1 关键参数
| 参数名 | 说明 | 默认值 | 调整建议 |
|---|---|---|---|
| mass | 车辆质量（kg） | 1500 | 越大越稳，但加速慢 |
| drag_coefficient | 空气阻力系数 | 0.3 | 越大高速越费油 |
| center_of_mass | 质心位置（x, y, z） | (0, 0, 0.3) | 调低 z 值更稳定 |
| max_steer_angle | 最大转向角（度） | 70 | 调小可减少甩尾 |
| tire_friction | 轮胎摩擦系数 | 3.5 | 调大抓地力更强 |
这些参数怎么调？我一般用 vehicle.get_physics_control() 获取当前参数，然后修改：
physics_control = vehicle.get_physics_control()
physics_control.mass = 1800 # 加重，更稳
physics_control.tire_friction = 4.0 # 增加抓地力
vehicle.apply_physics_control(physics_control)
3.3.2 实际调参经验
我在做高速场景测试时，发现车一到 80 km/h 以上就飘。查了半天，原来是质心太高。把 center_of_mass.z 从 0.3 降到 0.1，问题解决了。
还有一个坑：轮胎摩擦系数。CARLA 默认是 3.5，但如果你在雨天场景测试，这个值要降到 2.0 左右。我曾经没改这个参数，结果雨天测试时车完全刹不住，直接撞墙。
避坑指南：调参时一次只改一个参数。改完跑一遍测试，看效果。同时改多个参数，你根本不知道是哪个起了作用。
3.4 本章知识体系
下面这张图总结了车辆控制与动力学模型的核心逻辑：
这张图把本章内容串起来了。控制接口是基础，PID 是核心算法，动力学参数是调优手段。三者缺一不可。
我个人觉得，学控制最好的方法就是动手调。把 PID 参数改一改，看看车怎么反应。把质量改大，看看刹车距离怎么变。实践出真知，这句话在控制领域尤其适用。
小技巧：在 CARLA 里可以用 vehicle.get_velocity() 和 vehicle.get_location() 实时打印数据，配合 matplotlib 画个曲线，调参效率翻倍。
好了，这一章就到这里。记住，控制不是玄学，是数学和经验的结合。多调、多试、多总结，你也能调出一手好车。