自動駕駛的模型預測控制
阿新 • • 發佈：2019-02-18
我們實施了一個模型預測控制來驅動賽道上的賽車。但是這一次我們沒有交叉錯誤，我們必須自己計算。另外，在連線延遲之上的執行命令之間有100毫秒的延遲。
這篇文章從非常簡單的P，PD和PID實現開始，到一個複雜的模型預測控制：
-
用Python實現的P，PD和PID控制器
-
模型預測控制在C ++中實現
用Python實現的P，PD和PID控制器 比例積分微分控制器（PID控制器或三項控制器）是廣泛用於工業控制系統和各種其他需要連續調節控制的應用的控制迴路反饋機構。一個PID控制器連續計算一個誤差值e（t），作為所需設定值（SP）和測量的過程變數（PV）之間的差值，並根據比例，積分和微分項（表示為P，I和D）分別給他們的名字控制器。實際上，它會自動對控制功能應用準確和響應的校正。
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. 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
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)############## 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控制器的實現
隨著積分項我們跟蹤所有以前的CTE，最初我們設定int_cte為0，然後將當前cte項新增到計數int_cte += cte。最後，我們-tau_p * cte - tau_d * diff_cte - tau_i * int_cte用新tau_i引數更新轉向值。
# -----------# 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
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)############## 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')這似乎並不令人印象深刻。PID似乎比PD控制器差。I期的目的是彌補偏差，而目前的機器人沒有偏見。
模型預測控制重構了追蹤軌跡作為優化問題的任務。優化問題的解決方案是最佳的軌跡。模型預測控制涉及模擬不同的執行器輸入，預測最終的軌跡並以最小的成本選擇該軌跡。當前狀態和參考軌跡是已知的。在每個時間步驟中，致動器輸入被優化以便最小化預測軌跡的成本。一旦找到最低成本的軌跡，執行第一組致動器命令，其餘部分被丟棄，並且在計算新的最佳軌跡時在每個時間步重複。
成本函式的一個好的開始是想想我們想要最小化的錯誤。例如，測量從車道中心的偏移量，其中車道中心可以稱為參考或期望的狀態。同樣，如果目標是在兩個地點之間移動，我們想懲罰緩慢或停止的汽車。另外，我們希望汽車儘可能平穩地改變車道。
double cost = 0;for (int t = 0; t < N; t++) {
cost += pow(cte[t], 2);
cost += pow(epsi[t], 2);
}模型預測控制使用優化器來查詢控制輸入並最小化成本函式。我們實際上執行第一組控制輸入，使車輛進入新的狀態，然後重複這個過程。
在下圖中，通過執行第一步（第一個控制輸入）將汽車移動到下一個黃點。之後，包括新點在內產生一個全新的軌跡。
首先，我們設定模型預測控制迴路所需的一切。這包括通過選擇N和dt來定義軌跡的持續時間T。接下來，我們定義車輛模型和限制，如實際限制。最後，我們定義成本函式。
隨著模型設定完成，我們開始狀態反饋迴路。首先，我們將當前狀態傳遞給模型預測控制器。
接下來，呼叫優化求解器。求解器使用初始狀態，模型約束和成本函式來返回使成本函式最小化的控制輸入向量。
使用的求解器是IPOPT。
我們首先將第一個控制輸入應用於車輛並重復迴圈。
在真實的汽車中，執行命令不會立即執行 - 命令在系統中傳播時會有延遲。實際的延遲可能在100毫秒左右。
這是一個叫做“等待時間”的問題，對於某些控制器（比如PID控制器）來說，這是一個很難克服的挑戰。但是模型預測控制器可以很好地適應，因為我們可以在系統中對這個延遲進行建模。
PID控制器將計算相對於當前狀態的誤差，但是當車輛處於未來（並且可能不同）狀態時將執行啟動。這有時會導致不穩定。
PID控制器可以嘗試基於未來的誤差來計算控制輸入，但是如果沒有車輛模型，則這不太可能是準確的。
致動器動力學是導致等待時間的一個因素 例如，從命令轉向角度到實際達到角度的時間間隔。這可以很容易地通過一個簡單的動態系統建模，並納入車輛模型。一種方法是使用從當前狀態開始的車輛模型在等待時間期間進行模擬。從模擬得到的狀態是MPC的新的初始狀態。
因此，與PID控制器相比，MPC可以更有效地處理延遲，通過明確考慮延遲。
完整的原始碼可以在公眾號裡留言找到。
資源
https://en.wikipedia.org/wiki/Model_predictive_control
https://en.wikipedia.org/wiki/PID_controller
https://projects.coin-or.org/Ipopt
https://www.coin-or.org/CppAD/
https://github.com/ksakmann/CarND-MPC-Project
https://github.com/LevinJ/CarND-MPC-Project