# 自动驾驶的模型预测控制

• 用Python实现的P，PD和PID控制器
• 模型预测控制在C ++中实现

# P-控制器的实现

```# -----------# User Instructions## Implement a P controller by running 100 iterations# of robot motion. The desired trajectory for the # robot is the x-axis. The steering angle should be set# by the parameter tau so that:## steering = -tau * crosstrack_error## You'll only need to modify the `run` function at the bottom.# ------------
import randomimport numpy as npimport matplotlib.pyplot as plt# ------------------------------------------------# # this is the Robot class#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
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)############## ADD / MODIFY CODE BELOW ##################### ------------------------------------------------------------------------## run - does a single control runrobot = Robot()
robot.set(0, 1, 0)def run(robot, tau, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []    # TODO: your code here
steering = 0.0
for i in range(n):
cte = robot.y
steer = -tau * 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.1)
n = len(x_trajectory)

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')```

# PD控制器的实现

PD控制器与P控制器非常相似。增加了prev_cte分配给前一个CTE 的变量，以及diff_cte当前CTE和前一个CTE之间的差值。然后我们把它们和新的tau_d参数放在一起来计算新的转向值-tau_p * cte - tau_d * diff_cte。

```# -----------# User Instructions## Implement a PD controller by running 100 iterations# of robot motion. The steering angle should be set# by the parameter tau_p and tau_d so that:## steering = -tau_p * CTE - tau_d * diff_CTE# where differential crosstrack error (diff_CTE)# is given by CTE(t) - CTE(t-1)### Only modify code at the bottom! Look for the TODO# ------------
import randomimport numpy as npimport matplotlib.pyplot as plt# ------------------------------------------------# # this is the Robot class#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
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)############## ADD / MODIFY CODE BELOW ##################### ------------------------------------------------------------------------## run - does a single control run# previous P controllerdef run_p(robot, tau, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []    for i in range(n):
cte = robot.y
steer = -tau * cte
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)    return x_trajectory, y_trajectory

robot = Robot()
robot.set(0, 1, 0)def run(robot, tau_p, tau_d, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []    # TODO: your code here
CTE_prev = 0
for i in range(n):
CTE = robot.y
diff_CTE = CTE - CTE_prev
CTE_prev = CTE
steering = -tau_p * CTE - tau_d * diff_CTE
robot.move(steering, 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)
n = len(x_trajectory)

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

# PID控制器的实现

```# -----------# User Instructions## Implement a P controller by running 100 iterations# of robot motion. The steering angle should be set# by the parameter tau so that:## steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE## where the integrated crosstrack error (int_CTE) is# the sum of all the previous crosstrack errors.# This term works to cancel out steering drift.## Only modify code at the bottom! Look for the TODO.# ------------import randomimport numpy as npimport matplotlib.pyplot as plt# ------------------------------------------------# # this is the Robot class#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
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)############## ADD / MODIFY CODE BELOW ##################### ------------------------------------------------------------------------## run - does a single control runrobot = Robot()
robot.set(0, 1, 0)def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
x_trajectory = []
y_trajectory = []    # TODO: your code here
CTE_prev = 0
int_CTE = 0
for i in range(n):
CTE = robot.y
diff_CTE = CTE - CTE_prev
CTE_prev = CTE
int_CTE += CTE
steering = -tau_p * CTE - tau_d * diff_CTE - tau_i * int_CTE
robot.move(steering, 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)

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

```double cost = 0;for (int t = 0; t < N; t++) {
cost += pow(cte[t], 2);
cost += pow(epsi[t], 2);
}```

PID控制器将计算相对于当前状态的误差，但是当车辆处于未来（并且可能不同）状态时将执行启动。这有时会导致不稳定。

PID控制器可以尝试基于未来的误差来计算控制输入，但是如果没有车辆模型，则这不太可能是准确的。

# 资源

https://en.wikipedia.org/wiki/Model_predictive_control

https://en.wikipedia.org/wiki/PID_controller

https://projects.coin-or.org/Ipopt

https://github.com/ksakmann/CarND-MPC-Project

https://github.com/LevinJ/CarND-MPC-Project

220 篇文章79 人订阅

0 条评论

## 相关文章

4234

2077

4378

4166

4623

4019

### 软件分享 | SPSS 22 32位/64位 安装教程

SPSS for Windows是一个组合式软件包，它集数据录入、整理、分析功能于一身。用户可以根据实际需要和计算机的功能选择模块，以降低对系统硬盘容量的要求，...

1523

3676

90716

2908