自动驾驶的模型预测控制

刘盼

发布于 2018-03-16 11:51:13

1.8K0

发布于 2018-03-16 11:51:13

文章被收录于专栏：人人都是极客人人都是极客

我们实施了一个模型预测控制来驱动赛道上的赛车。但是这一次我们没有交叉错误，我们必须自己计算。另外，在连接延迟之上的执行命令之间有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.# ------------
 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, 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
            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

本文参与腾讯云自媒体分享计划，分享自微信公众号。

原始发表：2017-12-06，如有侵权请联系 cloudcommunity@tencent.com 删除

自动驾驶

python

本文分享自人人都是极客微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体分享计划，欢迎热爱写作的你一起参与！

自动驾驶

python

登录后参与评论

0 条评论

热度