PID控制器在无人驾驶控制器中的应用

PID控制器

• 在无人驾驶车辆中的控制是指通过使用油门、刹车、方向盘将车辆驾驶到期望的位置，控制看起来很简单但是实际上很复杂。人们在驾驶汽车通过十字路口的时候通过经验和直觉来确定什么时候应该转动方向盘，什么时候应该加减速，但是让计算机理解这些并不是一件那么容易的事情。控制算法在无人车项目中被称为控制器，PID控制器就是最为基础和常见的控制器之一。本片文章旨在讲述PID控制的基础部分来切入到无人驾驶中的控制器部分。

PID控制

P控制

• 如图所示，假使有一辆车以恒定的速度行驶，我们只可以控制车辆的方向盘角度来改变转向轮的角度，我们希望车辆可以按照图片中的车道线来行驶。那么我们现在可以提出问题：为了到达我们的目标我们需要以什么方式来控制方向盘的转角，是下发固定的转向角度还是下发随机的转向角亦或是按照横切误差(CTE)的某个比例来设置转向角度？
  实际上以横切误差和方向转角的比例关系来进行的控制即为PID控制中的P控制（比例控制）。
  $$\alpha =-K_p\ast CTE$$
  python实现代码：博客末尾有完整代码下载地址

robot = Robot()
robot.set(0, 1, 0) #设置车辆起始位置
def run(robot, tau, n=100, speed=1.0): #n表示循环100次 tau为比例增益项
    x_trajectory = []
    y_trajectory = []
    for i in range(n):
        cte = robot.y		#获取车辆的横切误差值
        steer = -tau * cte   #P控制器
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory
    
x_trajectory, y_trajectory = run(robot, 0.2)

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='P controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

return：

根据图片我们可以得知，单纯的P控制会产生震荡，当比例增益项的取值越大靠近参考轨迹的速度越快而且震荡越快。那么有没有什么方法能根本性的避免越界么，看起来单纯的P控制并不是一个好的选择，下面就让我们丰富我们的控制器，使用PD控制器。

PD控制

• 在PD控制中我们的方向盘角度alpha不仅由CTE与tua来决定，也会与CTE关于时间的导数相关。
  $$\alpha =-K_p\ast CTE-K_d\ast \frac{d}{dt}CTE$$
  这意味着当CTE在逐步减少的时候，车辆不会继续闷头冲过x轴，相反它会逐渐注意到减少了的CTE部分，会反方向转动前轮使车辆朝上方运动，这会让它平缓的接近航线。
  假设我们已经恰当的设定了比例增益项和微分增益项，在时刻t的导数等于时刻t的CTE与时刻t-1的CTE的差除以时刻t到时刻t-1之间经过的时间delat_t,在我们的代码里delat_t等于1。
  使用python来完成PD控制：

robot = Robot()
robot.set(0, 1, 0) #设置车辆起始位置
def run(robot, tau, param_d, n=100, speed=1.0): #n表示循环100次 tau为比例增益项 param_d为微分增益项
    x_trajectory = []
    y_trajectory = []
    temp_cte = 0.0 
    for i in range(n):
        cte = robot.y		#获取车辆的横切误差值
        steer = -tau * cte  - param_d * (cte - temp_cte)  #PD控制器
        temp_cte = cte
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory
    
x_trajectory, y_trajectory = run(robot, 0.2, 3.2)

fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='P controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

return:
至此，我们可以发现PD控制器在仿真模拟中表现出了很好的性能，在采用了恰当的了比例增益项和微分增益项后轨迹平缓的靠近并准确行驶在期望路径上。但是在实际使用中，无人车辆或者其他的被控本体的机械结构和传感器并不是那么完美，总是会产生一些误差量，那么在一个存在误差的被控系统应该怎么继续完善我们的控制器呢？

PID控制

• “系统偏差”是一个常见的机器人学问题，现在我们的被控车辆有这样的问题，在正常情况下我们希望车辆的转向前轮是完美的中位平行对齐的，但是因为机械的结构限制两个前轮出现了一些不正常的角度偏差。在这样的情况下车辆在方向盘归中的情况下车辆的行驶也不会呈直线，这对于我们人类驾驶员来说不是什么问题，我们只需要根据情况自行调整角度就好了，但是这种情况我们的PD控制器会有怎样的表现呢？我们可以在代码中模拟误差的产生再看一看它的表现情况如何：
  在代码中添加了10°的机械偏差后：
  很明显系统出现了巨大的CTE，而且没有办法收敛到合适的轨道上。
  通常在我们手动处理这类情况的时候，注意到过去一段时间也没有接近目标我们会采取相应的行动，随着的时间的过去会把方向盘打的越来越靠右，以抵消系统误差。如下图绿色粗线。为了达到这样的目标，我们需要巨大的偏差量持续的存在，这可以用CTE基于时间的积分来衡量。
  据此我们获得新的控制器表示：
  $$\alpha =-K_p\ast CTE-K_d\ast \frac{d}{dt}CTE-K_i\ast \Sigma CTE$$
  python：

robot = Robot()
robot.set(0, 1, 0)

def run(robot, tau, param, param1, n=100, speed=1.0):
   x_trajectory = []
   y_trajectory = []
   temp_cte = 0.0 
   temp_I = 0.0
   robot.set_steering_drift(10.0/180.0 * np.pi) #设置10°的系统误差
   for i in range(n):
       cte = robot.y
       temp_I += cte
       steer = -tau * cte - param * (cte - temp_cte) - param1 * temp_I
       temp_cte = cte
       robot.move(steer, speed)
       x_trajectory.append(robot.x)
       y_trajectory.append(robot.y)
   return x_trajectory, y_trajectory
   
x_trajectory, y_trajectory = run(robot, 0.2, 3.2, 0.006)
fig, (ax1, ax2) = plt.subplots(2, 1, figsize=(8, 8))
ax1.plot(x_trajectory, y_trajectory, 'g', label='P controller')
ax1.plot(x_trajectory, np.zeros(n), 'r', label='reference')

return:
下面关于PID控制器我们还有一个重要的问题，那就是如何去确定一组恰当的K_p、K_i、K_d的数值呢？

TWIDDLE参数整定算法

• 在twiddle里我们试图去优化一个参数集合，我们需要我们的run返回goodness。goodness可能是全部的CTE的均值。在这里我们假设twiddle的目标是最小化平均CTE。
  首先我们要为三个目标参数声明一个三维0向量P和另一个三维向量来初始化想要搜索的范围：

 P = [0,0,0]
 dP = [1,1,1]

我们希望function输入参数向量并输出goodness。

goodness = run(p)

我们的目标是修改P向量使得goodness最小。SO twiddle来了：

def twiddle(tol=0.2): 
    p = [0, 0, 0]
    dp = [1, 1, 1]
    robot = make_robot()
    x_trajectory, y_trajectory, best_err = run(robot, p)

    it = 0
    while sum(dp) > tol:
        print("Iteration {}, best error = {}".format(it, best_err))
        for i in range(len(p)):
            p[i] += dp[i]
            robot = make_robot()
            x_trajectory, y_trajectory, err = run(robot, p)

            if err < best_err:
                best_err = err
                dp[i] *= 1.1
            else:
                p[i] -= 2 * dp[i]
                robot = make_robot()
                x_trajectory, y_trajectory, err = run(robot, p)

                if err < best_err:
                    best_err = err
                    dp[i] *= 1.1
                else:
                    p[i] += dp[i]
                    dp[i] *= 0.9
        it += 1
    return p

整定后PID控制器效果

全博客终

PID控制器完整代码实现: python实现.