PID控制算法是一个在工业控制应用中常见的反馈回路算法，它把收集到的数据和一个参考值进行比较，然后把这个差别用于计算新的输入值，从而使得整个系统更加准确而稳定。 PID控制算法由比例单元(Proportional)、积分单元(Integral)和微分单元(Derivative)三部分组成，通过这三个单元的增益， Kp，KI和Kd来达到理想的控制效果。 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()
看起来已经很完美了，但是实际还存在一个系统偏差(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
