PID控制算法

PID控制算法是一个在工业控制应用中常见的反馈回路算法，它把收集到的数据和一个参考值进行比较，然后把这个差别用于计算新的输入值，从而使得整个系统更加准确而稳定。
PID控制算法由比例单元(Proportional)、积分单元(Integral)和微分单元(Derivative)三部分组成，通过这三个单元的增益， K_p，K_I和K_d来达到理想的控制效果。
PID主要适用于基本上线性，且动态特性不随时间变化的系统。

下面我们主要了解PID控制算法的细节及其在机器人/自动驾驶领域的应用。在机器人/自动驾驶领域，一个常见的任务就是使得机器人/自动驾驶车辆移动到目标轨迹上。如下图所示，车辆以速度v前进，我们的目标是让其沿着Reference Trajectory行驶。Crosstrack Error是目标偏差，PID的目标就是不断缩小该偏差，使其无限接近于0。

1.车辆模型
为了解决上述问题，需要先定义一个车辆模型，用以描述车辆的属性和运动特性。(代码来自Udacity的免费Artificial Intelligence for Robotics【2】)。

import random
import numpy as np
import matplotlib.pyplot as plt

class robot(object):
    def __init__(self, length=20.0):
        """
        Creates robot and initializes location/orientation to 0, 0, 0.
        """
        self.x = 0.0
        self.y = 0.0
        self.orientation = 0.0
        self.length = length
        self.steering_noise = 0.0
        self.distance_noise = 0.0
        self.steering_drift = 0.0

    def set(self, x, y, orientation):
        """
        Sets a robot coordinate.
        """
        self.x = x
        self.y = y
        self.orientation = orientation % (2.0 * np.pi)

    def set_noise(self, steering_noise, distance_noise):
        """
        Sets the noise parameters.
        """
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise = steering_noise
        self.distance_noise = distance_noise

    def set_steering_drift(self, drift):
        """
        Sets the systematical steering drift parameter
        """
        self.steering_drift = drift

    def move(self, steering, distance, tolerance=0.001, max_steering_angle=np.pi / 4.0):
        """
        steering = front wheel steering angle, limited by max_steering_angle
        distance = total distance driven, most be non-negative
        """
        if steering > max_steering_angle:
            steering = max_steering_angle
        if steering < -max_steering_angle:
            steering = -max_steering_angle
        if distance < 0.0:
            distance = 0.0

        # apply noise
        steering2 = random.gauss(steering, self.steering_noise)
        distance2 = random.gauss(distance, self.distance_noise)

        # apply steering drift
        steering2 += self.steering_drift

        # Execute motion
        turn = np.tan(steering2) * distance2 / self.length

        if abs(turn) < tolerance:
            # approximate by straight line motion
            self.x += distance2 * np.cos(self.orientation)
            self.y += distance2 * np.sin(self.orientation)
            self.orientation = (self.orientation + turn) % (2.0 * np.pi)
        else:
            # approximate bicycle model for motion
            radius = distance2 / turn
            cx = self.x - (np.sin(self.orientation) * radius)
            cy = self.y + (np.cos(self.orientation) * radius)
            self.orientation = (self.orientation + turn) % (2.0 * np.pi)
            self.x = cx + (np.sin(self.orientation) * radius)
            self.y = cy - (np.cos(self.orientation) * radius)

    def __repr__(self):
        return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)

2.Proportional Control
Proportional Control考虑当前偏差，偏差越大就让车辆越快的向中心线靠拢。
α=γ·CTE
上式中，α是车辆的Steering Angle，γ是增益系数，CTE是Cross Track Error。

def run(param):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equalt to speed (we assume time = 1)
    N = 100 

    for i in range(N):
        crosstrack_error = myrobot.y
        steer = -param * crosstrack_error
        myrobot.move(steer, speed)

run(0.1)

仅对车辆施加Proportional Control的车辆运动效果如下(绿色的是车辆运动轨迹，红色是目标轨迹):

做一个动图，看起来更加直观。可以看到，车辆发生了overshoot的问题，沿着目标轨迹上下震荡，始终不能做到稳定的沿着目标轨迹运动。

我们调大增益系数，从γ=0.1到γ=0.3，观察到车辆震荡的频率更高了。

3.P&D Control
为了解决OverShoot引起的震荡问题，引入Derivative Control。Derivative Control考虑CTE的变化，并根据变化反方向校正Steering Angle，使得车辆可以平滑的靠近目标轨迹。

代码实现:

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

def run(robot, tau_p, tau_d, n=150, speed=1.0):
    x_trajectory = []
    y_trajectory = []

    crosstrack_error = robot.y

    for i in range(n):
        diff_crosstrack_error = robot.y - crosstrack_error
        steer = -tau_p * crosstrack_error - tau_d * diff_crosstrack_error
        crosstrack_error = robot.y
        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.3, 3.0)
n = len(x_trajectory)

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

plt.show()

当τ_p-0.1,τ_d=0.3时的效果：

当τ_p-0.2,τ_d=0.3时的效果：

当τ_p-0.3,τ_d=0.3时的效果：

看起来已经很完美了，但是实际还存在一个系统偏差(Systematic Bias)的问题。如下图所示，控制指令要求车辆转向为0度，但实际上它转了0.5度，这种误差对于人类司机来讲，会自动校正；但是对于自动驾驶系统，需要消除这种误差。

给Robot增加一个drift：

robot.set_steering_drift(10.0 * math.pi / 180.0)

可以看到由于系统误差的存在，导致最终车辆稳定在一个非目标位置。

4.PID Control
如何解决系统偏差导致的目标偏差的问题？直观的感觉是，需要向右打方向盘，校正车辆的行驶方向，使得车辆不断靠近目标轨迹。这就是Integral Control的效果。

代码实现：

robot = Robot()
robot.set(0, 1, 0)
robot.set_steering_drift(10.0 * math.pi / 180.0)

def run(robot, tau_p, tau_d, tau_i, n=200, speed=1.0):
    x_trajectory = []
    y_trajectory = []

    int_crosstrack_error = 0

    crosstrack_error = robot.y

    for i in range(n):
        diff_crosstrack_error = robot.y - crosstrack_error

        crosstrack_error = robot.y
        int_crosstrack_error += crosstrack_error

        steer = -tau_p * crosstrack_error - tau_d * diff_crosstrack_error -tau_i * int_crosstrack_error

        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.0, 0.004)
n = len(x_trajectory)

plt.plot(x_trajectory, y_trajectory, 'g', label='PID controller')
plt.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.legend()
plt.show()

实际效果如下:

本文代码均来自：Udacity的Artificial Intelligence for Robotics。
参考材料
1.https://zh.wikipedia.org/wiki/PID%E6%8E%A7%E5%88%B6%E5%99%A8
2.Udacity的Artificial Intelligence for Robotics