自动驾驶算法———车辆控制（二）

“在本模块中，我们将在 Carla 模拟器中控制车辆。我们的算法的输入将是当前车辆速度以及期望速度和期望轨迹。算法的输出将是执行器信号：油门踏板和方向盘。我们的方法是使用 PID 控制器来控制油门踏板（纵向控制），并使用一种称为纯追踪的转向方法（横向控制）。我们将从学习PID 控制开始。随后，我们将介绍一个数学模型，该模型描述了车辆如何根据方向盘角度移动，即所谓的运动自行车模型。使用该模型，我们将介绍用于横向控制的纯追踪方法。在最后的练习中，您将在 Carla 中实现所学的车辆控制知识。如果您的计算机无法运行 Carla，请不要担心：您仍然可以使用我为本课程创建的简单模拟器。”

—

PID控制

想象一下，你准备了一些牛奶来喂宝宝，但牛奶的温度只有 25°C，而它应该是 37°C。你房间的环境温度是 20°C，这会降低你的牛奶温度。幸运的是，你有一个可以加热牛奶的设备。该设备使用电力产生热量，你可以控制功率𝑢。你还可以监控牛奶的温度𝑇. 你面临的是一个控制问题。

在探索解决方案之前，让我们先写下一些代码来模拟该场景。我们将通过牛顿冷却定律模拟温度动态

import numpy as npimport matplotlib.pyplot as plt
#define constantsalpha, beta = 1, 40T_ambient, T_desired, T_start = 20, 37, 25
# system update by discretization of the differential eq.def next_temp(u, T, dt):    return T+alpha*(T_ambient-T)*dt + beta * u *dt
def simulate_temp(controller, num_steps=20):    dt = 0.1 # Every time interval dt we set a new control value    T = T_start    T_list = [T]    for k in range(num_steps):        # ask controller for u value        u = controller.get_control(T,dt)        # device only allows to set u between 0 and 1:        u = np.clip(u, 0, 1)        # simulate what the temperature will be after time interval dt        T = next_temp(u, T, dt)        T_list.append(T)    time = dt*np.arange(num_steps+1)    plt.plot(time, np.repeat(T_desired, num_steps+1), ls="--")    plt.plot(time, T_list)    plt.xlabel("time"); plt.ylabel("Temperature");

该simulate_temp函数需要一个controller对象作为输入。它controller有一个函数get_control()，它查看当前温度以及自上次发出控制命令以来经过的T时间。它告诉我们要将哪个电源dt𝑢我们应该设置我们的加热装置。

首先让我们创建一个非常愚蠢的控制器：它将始终设置u𝑢至零。因此，我们不会加热牛奶，它会冷却至环境温度：

class SillyController:    def get_control(self,T,dt):        return 0silly_controller = SillyController()simulate_temp(silly_controller, num_steps=30)

确实，如果我们什么都不做，牛奶就会冷却下来。但现在让我们尝试创建一个合适的控制器。

—

P控制器

P 控制器或比例控制器的原理很简单：想象一下，你正在转动加热设备上的旋钮，该旋钮可以设置𝑢。您查看所需温度与当前温度之间的差异，即所谓的误差。如果误差很大且为正（所需温度 > 当前温度），则选择𝑢为大且为正。这将加热牛奶，误差也会减小。误差越小，您越需要将u𝑢-将旋钮调至零。如果误差为负，即当前牛奶温度过高（对婴儿有危险！），您需要通过设置来冷却牛奶𝑢变为负值。遗憾的是，您无法使用电加热器1来实现这一点。

综上所述，您选择𝑢u与误差成正比。我们可以按如下方式实现 P 控制器

class PController:    def __init__(self, Kp, set_point):        self.Kp = Kp        self.set_point = set_point        def get_control(self, measurement, dt):        error = self.set_point - measurement        return self.Kp * error

计算(所需温度) 和(当前温度)PController之间的差值。该差值称为。控制值只是一个正常数乘以。该常数称为比例增益，找到一个合适的值称为调整控制器。请注意，不了解热量或牛顿冷却定律。它不依赖于控制值如何变化的模型set_pointmeasurementerrorKperrorKpKpPController𝑢影响世界。

P 控制器的数学公式为

这里，𝑒e表示错误=误差。现在，让我们应用PController并看看牛奶会发生什么：‘

p_controller = PController(Kp=0.2, set_point=T_desired)simulate_temp(p_controller)

如您所见，牛奶的温度上升了，但还没有达到我们想要的温度。实际温度和所需温度之间仍然存在差距。这被称为稳态误差，是的典型问题PController。

在实际系统中，很难理解为什么会出现稳态误差，因为我们可能甚至没有系统模型。但是，对于我们的模拟牛奶系统，我们可以理解它：假设实际温度等于所需温度。那么误差T_desired-T为零，因此为零。这意味着没有添加热量，牛奶冷却到低于。当误差产生的热量等于散失到房间的热量时，就达到了稳定状态。u=K_d * (T_desired-T)T==T_desired

现在我们可以将温度提高set_point到“所需温度再多一点”。这将提高稳定状态温度，使其更接近我们所需的温度。但有一个更好的解决方案！比例积分或 PI 控制器！

—

PI控制器

PI 控制器的公式为：

class PIController:    def __init__(self, Kp, Ki, set_point):        self.Kp = Kp        self.Ki = Ki        self.set_point = set_point        self.int_term = 0        def get_control(self, measurement, dt):        error = self.set_point - measurement        self.int_term += error*self.Ki*dt        return self.Kp * error + self.int_term

pi_controller = PIController(Kp=0.2, Ki = 0.15, set_point=T_desired)simulate_temp(pi_controller)

现在我们对牛奶非常满意。对于这个问题，PI 控制器是一个很好的解决方案。但是，还有其他问题，仅使用积分项来回顾过去可能还不够。我们还需要展望未来。这就是 PID 控制器中的微分项的作用。

—

PID控制器

PID 控制器的公式为

实现如下

class PIDController:    def __init__(self, Kp, Ki, Kd, set_point):        self.Kp = Kp        self.Ki = Ki        self.Kd = Kd        self.set_point = set_point        self.int_term = 0        self.derivative_term = 0        self.last_error = None        def get_control(self, measurement, dt):        error = self.set_point - measurement        self.int_term += error*self.Ki*dt        if self.last_error is not None:            self.derivative_term = (error-self.last_error)/dt*self.Kd        self.last_error = error        return self.Kp * error + self.int_term + self.derivative_term

对于我们的牛奶问题，使用 PID 控制器并没有获得太多好处。

然而，当 PID 控制器的微分项非常重要时，就会出现问题。想象一下一架无人机在高空飞行𝑝离地面较远。我们希望它保持在所需的高度p=pd = 50𝑝=𝑝𝑑=50米。我们可以控制无人机的向上加速度𝑎a（因此u=a𝑢=𝑎）并且必须考虑到存在恒定的向下加速度𝑔g由于重力。让我们看一下无人机的玩具模拟。模拟的状态由无人机的位置和速度组成。控制器仅测量位置并发出加速命令。请注意，您不需要了解模拟代码的细节。

g = -9.81p_desired = 50
# system updatedef next_state(a, state, dt):    aeff = a + g    position, velocity = state     # integration by velocity verlet algorithm    position += velocity*dt + 0.5*aeff*dt**2    velocity += aeff*dt    return (position,velocity)
def simulate_drone(controller, num_steps = 1000):    dt = 0.02    state = (0,0)    state_list=[state]    for i in range(num_steps):        a = controller.get_control(state[0], dt)        # upwards acceleration is positive (>0)        # and limited by power supply (<100)        a = np.clip(a, 0, 100)        state = next_state(a, state, dt)        state_list.append(state)    s = np.array(state_list)    time = dt*np.arange(num_steps+1)    plt.plot(time, [p_desired]*(num_steps+1), ls="--")    plt.plot(time, s[:,0])    plt.gca().set_ylim(-100,100)    plt.xlabel("time"); plt.ylabel("drone height");

当您不使用微分项时，您会发现无人机在目标位置附近振荡。但为什么呢？如果无人机太低，控制器将使其增加向上位置，以便能够到达目标位置。但一旦到达该位置，它仍然具有一些向上的动量，因此将继续向上移动。由于惯性导致的这种超调可以通过微分项来解决，即

在介绍完 PID 之前，我们先简单提一下它的局限性。它的一个优点也是一个缺点：它不依赖于系统模型。PID 仅对错误做出反应。如果您没有模型，这很好，因为您可能能够控制一些您不了解细节的东西。但是，如果您有模型，它不会提高 PID 控制器的功能。如果您对 PID 控制器的性能不满意，那么基于模型的控制器在这种情况下可能是更好的选择。PID 还有一个问题，称为积分饱和。如果设定点远离当前值，则在系统向设定点移动时，积分项会累积并变得非常大。这将导致过冲。幸运的是，可以通过稍微调整的实现来解决此问题PIDController。例如，您可以限制积分项，这意味着不允许它超出某个用户指定的阈值。

关于婴儿奶粉和无人机就说这么多。你想在 Carla 模拟器中控制一辆车！我们可以使用 PID 来控制车辆的纵向，即正确设置油门踏板。PID 不太适合横向控制，即控制方向盘。为此，你将实现一种称为纯追踪的方法。一旦你理解了纯追踪是什么，你就会在 Carla 中应用 PID 和纯追踪。

—

自行车模型

横向车辆控制的纯追踪方法基于一种称为自行车模型的车辆数学模型。理解自行车模型的先决条件是瞬时旋转中心的概念。

瞬时旋转中心 (ICR)

阿克曼转向

请注意，方向盘角度与车轮转向角 不同。车轮转向角是车轮的角度，而方向盘角度是方向盘（驾驶员手中握着的物体）的角度。通常

wheel_steer_angle = a*(steering_wheel_angle - b)

在Carla模拟器中，您可以直接控制车轮转向角度，而不必担心方向盘角度。

运动自行车模型

对于自行车模型，两个前轮以及两个后轮分别合并为一个车轮。

运动学自行车模型是自行车模型加上所有滑移角为零的假设。利用这一假设以及我们对 ICR 的了解，我们可以使用图 25推导出运动学自行车模型的实用公式。

—

PurePursuit纯跟踪

Reference

https://thomasfermi.github.io/Algorithms-for-Automated-Driving/Control/PurePursuit.html
https://github.com/thomasfermi/Algorithms-for-Automated-Driving