PID控制算法是一个在工业控制应用中常见的反馈回路算法,它把收集到的数据和一个参考值进行比较,然后把这个差别用于计算新的输入值,从而使得整个系统更加准确而稳定。   PID控制算法由比例单元(Proportional)、积分单元(Integral)和微分单元(Derivative)三部分组成,通过这三个单元的增益, Kp,KI和Kd来达到理想的控制效果。   PID主要适用于基本上线性,且动态特性不随时间变化的系统。   微信图片_20201231115313   下面我们主要了解PID控制算法的细节及其在机器人/自动驾驶领域的应用。在机器人/自动驾驶领域,一个常见的任务就是使得机器人/自动驾驶车辆移动到目标轨迹上。   如下图所示,车辆以速度v前进,我们的目标是让其沿着Reference Trajectory行驶。Crosstrack Error是目标偏差,PID的目标就是不断缩小该偏差,使其无限接近于0。   微信图片_20201231115336   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的车辆运动效果如下(绿色的是车辆运动轨迹,红色是目标轨迹):   微信图片_20201231115413   做一个动图,看起来更加直观。可以看到,车辆发生了overshoot的问题,沿着目标轨迹上下震荡,始终不能做到稳定的沿着目标轨迹运动。   微信图片_20201231115430   我们调大增益系数,从γ=0.1到γ=0.3,观察到车辆震荡的频率更高了。   微信图片_20201231115448   3.P&D Control   为了解决OverShoot引起的震荡问题,引入Derivative Control。Derivative Control考虑CTE的变化,并根据变化反方向校正Steering Angle,使得车辆可以平滑的靠近目标轨迹。   微信图片_20201231115504   代码实现:  
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()
  1609386976(1)   微信图片_20201231115631   1609387010(1)   微信图片_20201231115753   1609387097(1)   微信图片_20201231115827   看起来已经很完美了,但是实际还存在一个系统偏差(Systematic Bias)的问题。如下图所示,控制指令要求车辆转向为0度,但实际上它转了0.5度,这种误差对于人类司机来讲,会自动校正;但是对于自动驾驶系统,需要消除这种误差。   微信图片_20201231115908   给Robot增加一个drift:  
robot.set_steering_drift(10.0 * math.pi / 180.0)
  可以看到由于系统误差的存在,导致最终车辆稳定在一个非目标位置。   微信图片_20201231120039   4.PID Control   如何解决系统偏差导致的目标偏差的问题?直观的感觉是,需要向右打方向盘,校正车辆的行驶方向,使得车辆不断靠近目标轨迹。这就是Integral Control的效果。   1609387289(1)   代码实现:  
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()
  实际效果如下:   微信图片_20201231120205   本文代码均来自: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