我们实施了一个模型预测控制来驱动赛道上的赛车。但是这一次我们没有交叉错误，我们必须自己计算。另外，在连接延迟之上的执行命令之间有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.# ------------importrandomimportnumpyasnpimportmatplotlib.pyplotasplt# ------------------------------------------------## this is the Robot class#classRobot(object):def__init__(self, length=20.0):"""
Creates robot and initializes location/orientation to 0, 0, 0.
"""self.x =0.0self.y =0.0self.orientation =0.0self.length = length self.steering_noise =0.0self.distance_noise =0.0self.steering_drift =0.0defset(self, x, y, orientation):"""
Sets a robot coordinate.
"""self.x = x self.y = y self.orientation = orientation % (2.0* np.pi)defset_noise(self, steering_noise, distance_noise):"""
Sets the noise parameters.
"""# makes it possible to change the noise parameters# this is often useful in particle filtersself.steering_noise = steering_noise self.distance_noise = distance_noisedefset_steering_drift(self, drift):"""
Sets the systematical steering drift parameter
"""self.steering_drift = driftdefmove(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
"""ifsteering > max_steering_angle: steering = max_steering_angleifsteering < -max_steering_angle: steering = -max_steering_angleifdistance <0.0: distance =0.0# apply noisesteering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise)# apply steering driftsteering2 += self.steering_drift# Execute motionturn = np.tan(steering2) * distance2 / self.lengthifabs(turn) < tolerance:# approximate by straight line motionself.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 motionradius = 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)defrun(robot, tau, n=100, speed=1.0):x_trajectory = [] y_trajectory = []#TODO:your code heresteering =0.0foriinrange(n): cte = robot.y steer = -tau * cte robot.move(steer, speed) x_trajectory.append(robot.x) y_trajectory.append(robot.y)returnx_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# ------------importrandomimportnumpyasnpimportmatplotlib.pyplotasplt# ------------------------------------------------## this is the Robot class#classRobot(object):def__init__(self, length=20.0):"""
Creates robot and initializes location/orientation to 0, 0, 0.
"""self.x =0.0self.y =0.0self.orientation =0.0self.length = length self.steering_noise =0.0self.distance_noise =0.0self.steering_drift =0.0defset(self, x, y, orientation):"""
Sets a robot coordinate.
"""self.x = x self.y = y self.orientation = orientation % (2.0* np.pi)defset_noise(self, steering_noise, distance_noise):"""
Sets the noise parameters.
"""# makes it possible to change the noise parameters# this is often useful in particle filtersself.steering_noise = steering_noise self.distance_noise = distance_noisedefset_steering_drift(self, drift):"""
Sets the systematical steering drift parameter
"""self.steering_drift = driftdefmove(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
"""ifsteering > max_steering_angle: steering = max_steering_angleifsteering < -max_steering_angle: steering = -max_steering_angleifdistance <0.0: distance =0.0# apply noisesteering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise)# apply steering driftsteering2 += self.steering_drift# Execute motionturn = np.tan(steering2) * distance2 / self.lengthifabs(turn) < tolerance:# approximate by straight line motionself.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 motionradius = 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 controllerdefrun_p(robot, tau, n=100, speed=1.0):x_trajectory = [] y_trajectory = []foriinrange(n): cte = robot.y steer = -tau * cte robot.move(steer, speed) x_trajectory.append(robot.x) y_trajectory.append(robot.y)returnx_trajectory, y_trajectory robot = Robot()robot.set(0,1,0)defrun(robot, tau_p, tau_d, n=100, speed=1.0):x_trajectory = [] y_trajectory = []#TODO:your code hereCTE_prev =0foriinrange(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)returnx_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.# ------------importrandomimportnumpyasnpimportmatplotlib.pyplotasplt# ------------------------------------------------## this is the Robot class#classRobot(object):def__init__(self, length=20.0):"""
Creates robot and initializes location/orientation to 0, 0, 0.
"""self.x =0.0self.y =0.0self.orientation =0.0self.length = length self.steering_noise =0.0self.distance_noise =0.0self.steering_drift =0.0defset(self, x, y, orientation):"""
Sets a robot coordinate.
"""self.x = x self.y = y self.orientation = orientation % (2.0* np.pi)defset_noise(self, steering_noise, distance_noise):"""
Sets the noise parameters.
"""# makes it possible to change the noise parameters# this is often useful in particle filtersself.steering_noise = steering_noise self.distance_noise = distance_noisedefset_steering_drift(self, drift):"""
Sets the systematical steering drift parameter
"""self.steering_drift = driftdefmove(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
"""ifsteering > max_steering_angle: steering = max_steering_angleifsteering < -max_steering_angle: steering = -max_steering_angleifdistance <0.0: distance =0.0# apply noisesteering2 = random.gauss(steering, self.steering_noise) distance2 = random.gauss(distance, self.distance_noise)# apply steering driftsteering2 += self.steering_drift# Execute motionturn = np.tan(steering2) * distance2 / self.lengthifabs(turn) < tolerance:# approximate by straight line motionself.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 motionradius = 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)defrun(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):x_trajectory = [] y_trajectory = []#TODO:your code hereCTE_prev =0int_CTE =0foriinrange(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)returnx_trajectory, y_trajectoryx_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期的目的是弥补偏差，而目前的机器人没有偏见。
模型预测控制重构了追踪轨迹作为优化问题的任务。优化问题的解决方案是最佳的轨迹。模型预测控制涉及模拟不同的执行器输入，预测最终的轨迹并以最小的成本选择该轨迹。当前状态和参考轨迹是已知的。在每个时间步骤中，致动器输入被优化以便最小化预测轨迹的成本。一旦找到最低成本的轨迹，执行第一组致动器命令，其余部分被丢弃，并且在计算新的最佳轨迹时在每个时间步重复。
成本函数的一个好的开始是想想我们想要最小化的错误。例如，测量从车道中心的偏移量，其中车道中心可以称为参考或期望的状态。同样，如果目标是在两个地点之间移动，我们想惩罚缓慢或停止的汽车。另外，我们希望汽车尽可能平稳地改变车道。
doublecost =0;for(intt =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可以更有效地处理延迟，通过明确考虑延迟。
完整的源代码可以在公众号里留言找到。
