Introduction to Self-Driving Cars Final Project-W7_PID实现车辆横向和纵向控制

1 项目介绍

该项目是Coursera中自动驾驶系列课程第一部分最后的综合项目。考察的是车辆横纵向控制器的实现。
官方项目使用的是CARLA 模拟器，总体需求是编写实现一个控制器，能够通过导航预设的航路点来控制车辆沿着路径行驶。车辆需要以特定的期望速度到达这些航路点，因此需要纵向和横向控制。
地址：https://www.coursera.org/learn/intro-self-driving-cars/programming/ac8R5/final-project-self-driving-vehicle-control/instructions

要想完整地实现这个流程，需要安装 CARLA 模拟器和分配代码。 当然可以只参考其中有价值的程序，那么可以直接跳到最后程序部分。

2 操作过程

如果选择安装Carla请遵循以下说明(步骤1-8)：

准备工作（1-2）

1. 按照CARLA 安装指南安装 CARLA 模拟器
2. 下载“Course1FinalProject.zip”文件并解压到“CarlaSimulator”（根）文件夹内的子文件夹“PythonClient”中。 这将在“PythonClient”下创建一个包含作业文件的子文件夹“Course1FinalProject”。这一步很重要，将其放到另一个目录可能会导致运行时出现问题。成功安装CARLA 和解压脚本后，可以开始编程作业。

实现并运行（3-5）

3. 编辑“controller2d.py”类文件（在“Course1FinalProject”文件夹中，“PythonClient”下)，这个文件主要是用来实现控制器。

“controller2d.py”文件包含一个控制器对象，需要在注释块标记的 update_controls 方法中实现控制器（可以查找包含“MODULE 7”的所有注释块，在它们下面实现代码）。
控制器提供以下实施所需的相关输入信息。所有单位都是SI（米、秒、弧度），CARLA工作在左手坐标系（虚幻引擎采用左手坐标系）。由于控制器在 x、y 和 yaw 空间中运行，因此这通常不应该成为此分配的问题。

描述	变量名称	单位
车辆位置	[x, y]	[m,m]
车辆方向	yaw	radians
车速	v	m/s
游戏时间	t	s
所需速度	v_desired	m/s
要跟踪的航点 (x, y, v)	waypoints	[m，m，m/s]

其中，waypoints 变量是要跟踪的航点的列表list，其中航点中每行表示格式为 [x, y, v]，分别是 x 和 y 位置以及该位置的所需速度。访问第三个航点的 y 位置的示例是：

waypoints[2][1]

在 controller2d.py 文件的注释中写明了航点变量结构的更多细节。
航点和其他变量一起将在每个模拟步骤中更新，所以请不要假设航点变量永远不会改变。 这里的航路点是整个航路点集（来自racetrack_waypoints.txt）的线性插值（用于位置和速度）子集。 换言之，航路点变量是车辆附近的整个航路点集的增强（更精细的分辨率）部分。 这样做是为了减少计算时间和控制器的性能。
同样，期望速度是距离车辆最近的航路点的航路点速度。

输出信息
将此信息用于控制器，您将输出车辆油门、转向和制动。 “controller2d.py”中这些输出的详细信息如下。

描述	变量名称	限制
节气门	throttle_output	0 到 1（百分比）
转向角度	steer_output	-1.22 到 1.22（以弧度为单位，从左到右）
制动	brake_output	0 到 1（百分比）

可以将来自 CARLA 的所有测量值都视为以车辆的中心位置为原点。中心位置到车辆前轴的距离为 1.5 米（这个项目里的值）。

4. 在一个终端中，以 30hz 的固定时间步长启动 CARLA 模拟器：

Ubuntu:

./CarlaUE4.sh /Game/Maps/RaceTrack -windowed -carla-server -benchmark -fps=30

Windows:

CarlaUE4.exe /Game/Maps/RaceTrack -windowed -carla-server -benchmark -fps=30

注：windows打开终端需要win+R，然后cmd打开终端，最后切换路径到CarlaUE4的路径。Ubutu同理。
5) 在另一个终端中，更改路径到“PythonClient”文件夹下的“Course1FinalProject”文件夹。运行控制器，在 CARLA 打开时执行以下命令：
Ubuntu（如果下面的命令不起作用，请使用替代的 python 命令，如 CARLA 安装指南中所述）：

python3 module_7.py

windows：

python module_7.py

注意，运行时可能需要一些其他的库，可以用pip install （库名）来安装一下。
如果 module_7 客户端正确连接到服务器，模拟器将开始运行。它将打开两个新的反馈窗口（除非 live_plotting 被禁用 - 有关更多详细信息，请参阅下面的 options.cfg介绍），其中一个运动轨迹，另一个显示控制输出。
轨迹包含汽车、起点和终点位置、整个路径以及一个小的阴影区域，该区域表示要发送到控制器进行控制更新的内插值点的子集。航路点之间使用线性插值来为控制器提供更精细的分辨率路径/速度期望。 X 轴和Y 轴以m为单位。
控制输出显示油门、转向和制动输出，以及模拟的速度响应（单图中的期望速度和当前速度），用于查看客户端在控制命令方面向 CARLA 服务器发送的内容。期望速度设置为最接近汽车当前位置的插值速度点。速度以m/s为单位，油门（0 到 1）、刹车（0 到 1）和转向（-1 到 1，或从左到右转）是没有单位的。
注意，在将命令发送到 CARLA 服务器之前，controller2d.py 中的转向命令输出会自动从弧度（-1.22 到 1.22 弧度）转换为百分比（-1 到 1）。控制输出中的四个图的 X轴表示时间，以s为单位。
注意，如果模拟运行缓慢，可以尝试增大实时绘图刷新时间（降低频率），或完全禁用实时绘图。禁用实时绘图不会影响模拟结束时的绘图输出。
如果想要禁用实时绘图，请编辑“Course1FinalProject”文件夹中的 options.cfg 文件。 下表解释了每个选项：

范围	描述	价值
live_plotting	启用或禁用实时绘图	true/false
live_plotting_period	实时绘图将在屏幕上刷新的周期（以秒为单位)，设置为“0”以更新每个模拟时间步。	[s]

结果处理与延伸

6.一旦到达最终航点，或者在游戏中经过大约 200 到 250 秒后，客户端将关闭。 模拟完成后，将保存一个包含控制器生成的轨迹的文本文件。 该文件名称是“trajectory.txt”，位于“Course1FinalProject”文件夹下的“controller_output”文件夹内。 速度、油门、制动、转向和执行的 2D 轨迹图也保存在此文件夹中。
评分脚本（网页有提供）将运行后轨迹与航点进行比较并对其性能进行评分。 将此评分器解压缩到“Course1FinalProject”文件夹中。
要运行评分脚本，请在新终端的“Course1FinalProject”目录中执行以下命令：
Ubuntu（如果下面的命令不起作用，请使用替代的 python 命令，如 CARLA 安装指南中所述）：

python3 grade_c1m7.py racetrack_waypoints.txt /controller_output/trajectory.txt

windows:

python grade_c1m7.py racetrack_waypoints.txt /controller_output/trajectory.txt

脚本绘制运行轨迹以及每个航路点的速度、距离和速度误差界限。 如果运行轨迹成功到达 50% 或更多的航点，则轨迹能够通过。

7. 一旦最终轨迹通过评分脚本，可以将其文件（trajectory.txt）上传到 Coursera 进行官方评估。
8. 最后，对于更大的挑战，尝试修改参考速度或路径，看看你能多快完成跟踪。
   要修改参考路径和速度，请编辑“racetrack_waypoints.txt”文件。 该文件将包含路径中每个航路点的路径和参考速度。
   txt文件的每一行依次为每个航点，列格式如下：“X位置（m）、Y位置（m）、参考速度（m/s）”，可以通过修改参考速度文件的第三列，以及通过更改第一列和第二列来修改路径的路径。

3 最终实现

本次采用PID的方式进行横纵向控制，将PID控制器单独成一个类实现，然后在controller.py中调用PID控制器。

module_7的代码没有变动。
其他两个文件如下，可以主要看修改的部分。

#controller2d.py
#!/usr/bin/env python3
#
"""
2D Controller Class to be used for the CARLA waypoint follower demo.
"""

import cutils
import numpy as np
from pid import PID


class Controller2D(object):
    def __init__(self, waypoints):
        self.vars = cutils.CUtils()
        self._current_x = 0
        self._current_y = 0
        self._current_yaw = 0
        self._current_speed = 0
        self._desired_speed = 0
        self._current_frame = 0
        self._current_timestamp = 0
        self._start_control_loop = False
        self._set_throttle = 0
        self._set_brake = 0
        self._set_steer = 0
        self._waypoints = waypoints
        self._conv_rad_to_steer = 180.0 / 70.0 / np.pi
        self._pi = np.pi
        self._2pi = 2.0 * np.pi

        ## PIDs
        self.steering_pid = PID(P=0.34611, I=0.0370736, D=3.5349)
        self.steering_pid.setSampleTime = 0.033  ##这个值是根据fps=30计算出的

        self.throttle_brake_pid = PID(P=7.0, I=1.0, D=1.026185)
        self.throttle_brake_pid.setSampleTime = 0.033

    def update_values(self, x, y, yaw, speed, timestamp, frame):
        self._current_x = x
        self._current_y = y
        self._current_yaw = yaw
        self._current_speed = speed
        self._current_timestamp = timestamp
        self._current_frame = frame
        if self._current_frame:
            self._start_control_loop = True

    def update_desired_speed(self):
        min_idx = 0
        min_dist = float("inf")
        desired_speed = 0
        for i in range(len(self._waypoints)):
            dist = np.linalg.norm(
                np.array([
                    self._waypoints[i][0] - self._current_x,
                    self._waypoints[i][1] - self._current_y
                ]))
            if dist < min_dist:
                min_dist = dist
                min_idx = i
        if min_idx < len(self._waypoints) - 1:
            desired_speed = self._waypoints[min_idx][2]
        else:
            desired_speed = self._waypoints[-1][2]
        self._desired_speed = desired_speed

    def update_waypoints(self, new_waypoints):
        self._waypoints = new_waypoints

    def get_commands(self):
        return self._set_throttle, self._set_steer, self._set_brake

    def set_throttle(self, input_throttle):
        # Clamp the throttle command to valid bounds
        throttle = np.fmax(np.fmin(input_throttle, 1.0), 0.0)
        self._set_throttle = throttle

    def set_steer(self, input_steer_in_rad):
        # Covnert radians to [-1, 1]
        input_steer = self._conv_rad_to_steer * input_steer_in_rad

        # Clamp the steering command to valid bounds
        steer = np.fmax(np.fmin(input_steer, 1.0), -1.0)
        self._set_steer = steer

    def set_brake(self, input_brake):
        # Clamp the steering command to valid bounds
        brake = np.fmax(np.fmin(input_brake, 1.0), 0.0)
        self._set_brake = brake

    def map_coord_2_Car_coord(self, x, y, yaw, waypoints):

        wps = np.squeeze(waypoints)
        wps_x = wps[:, 0]
        wps_y = wps[:, 1]

        num_wp = wps.shape[0]

        ## create the Matrix with 3 vectors for the waypoint x and y coordinates w.r.t. car
        wp_vehRef = np.zeros(shape=(3, num_wp))
        cos_yaw = np.cos(-yaw)
        sin_yaw = np.sin(-yaw)

        wp_vehRef[0, :] = cos_yaw * (wps_x - x) - sin_yaw * (wps_y - y)
        wp_vehRef[1, :] = sin_yaw * (wps_x - x) + cos_yaw * (wps_y - y)

        return wp_vehRef

    def update_controls(self):
        ######################################################
        # RETRIEVE SIMULATOR FEEDBACK
        ######################################################
        x = self._current_x
        y = self._current_y
        yaw = self._current_yaw
        v = self._current_speed
        self.update_desired_speed()
        v_desired = self._desired_speed
        t = self._current_timestamp
        waypoints = self._waypoints
        throttle_output = 0
        steer_output = 0
        brake_output = 0

        ######################################################
        ######################################################
        # MODULE 7: DECLARE USAGE VARIABLES HERE
        ######################################################
        ######################################################
        """
            Use 'self.vars.create_var(<variable name>, <default value>)'
            to create a persistent variable (not destroyed at each iteration).
            This means that the value can be stored for use in the next
            iteration of the control loop.

            Example: Creation of 'v_previous', default value to be 0
            self.vars.create_var('v_previous', 0.0)

            Example: Setting 'v_previous' to be 1.0
            self.vars.v_previous = 1.0

            Example: Accessing the value from 'v_previous' to be used
            throttle_output = 0.5 * self.vars.v_previous
        """
        self.vars.create_var('v_previous', 0.0)

        # Skip the first frame to store previous values properly
        if self._start_control_loop:
            """
                Controller iteration code block.

                Controller Feedback Variables:
                    x               : Current X position (meters)
                    y               : Current Y position (meters)
                    yaw             : Current yaw pose (radians)
                    v               : Current forward speed (meters per second)
                    t               : Current time (seconds)
                    v_desired       : Current desired speed (meters per second)
                                      (Computed as the speed to track at the
                                      closest waypoint to the vehicle.)
                    waypoints       : Current waypoints to track
                                      (Includes speed to track at each x,y
                                      location.)
                                      Format: [[x0, y0, v0],
                                               [x1, y1, v1],
                                               ...
                                               [xn, yn, vn]]
                                      Example:
                                          waypoints[2][1]: 
                                          Returns the 3rd waypoint's y position

                                          waypoints[5]:
                                          Returns [x5, y5, v5] (6th waypoint)
                
                Controller Output Variables:
                    throttle_output : Throttle output (0 to 1)
                    steer_output    : Steer output (-1.22 rad to 1.22 rad)
                    brake_output    : Brake output (0 to 1)
            """

            wps_vehRef = self.map_coord_2_Car_coord(x, y, yaw, waypoints)
            wps_vehRef_x = wps_vehRef[0, :]
            wps_vehRef_y = wps_vehRef[1, :]

            ## 拟合七次期望路径，得到曲线的拟合参数
            coeffs = np.polyfit(wps_vehRef_x, wps_vehRef_y, 7)
            #vel_poly = np.polyfit(wps_vehRef_x, wps_vehRef_y, 3)

            ## get cross-track error from fit
            # In vehicle coordinates the cross-track error is the intercept at x = 0, That means that since we have a fit of the form:
            # Y = C0 + C1*X + C2*X^2 + C3X^3 + ....
            # when we evaluate using x=0 we just get C0.
            # But to understand where this is coming from I like to keep the whole evaluation, even though this is exactly C0
            CarRef_x = CarRef_y = CarRef_yaw = 0.0

            # 计算跟踪距离误差
            cte = np.polyval(coeffs, CarRef_x) - CarRef_y

            # 计算横摆角误差
            yaw_err = CarRef_yaw - np.arctan(coeffs[1])

            speed_err = v_desired - v

            ######################################################
            ######################################################
            #                      MODULE 7
            #               LONGITUDINAL CONTROLLER
            #                        AND
            #               LATERAL CONTROLLER HERE
            ######################################################
            ######################################################
            """
                Implement a longitudinal controller here. Remember that you can
                access the persistent variables declared above here. For
                example, can treat self.vars.v_previous like a "global variable".
            """
            """
                Implement a lateral controller here. Remember that you can
                access the persistent variables declared above here. For
                example, can treat self.vars.v_previous like a "global variable".
            """

            #### PID ####
            self.steering_pid.update(cte, output_limits=[-1.22, 1.22])
            steer_output = self.steering_pid.output

            self.throttle_brake_pid.update(speed_err,
                                           output_limits=[-1.0, 1.00])
            if self.throttle_brake_pid.output < 0.0:
                throttle_output = 0
                brake_output = -self.throttle_brake_pid.output
            else:
                throttle_output = self.throttle_brake_pid.output
                brake_output = 0

            print("speed Err: ", speed_err)
            print("cte : ", cte)

            ######################################################
            # SET CONTROLS OUTPUT
            ######################################################
            self.set_throttle(throttle_output)  # in percent (0 to 1)
            self.set_steer(steer_output)  # in rad (-1.22 to 1.22)
            self.set_brake(brake_output)  # in percent (0 to 1)

        ######################################################
        ######################################################
        # MODULE 7: STORE OLD VALUES HERE (ADD MORE IF NECESSARY)
        ######################################################
        ######################################################
        """
            Use this block to store old values (for example, we can store the
            current x, y, and yaw values here using persistent variables for use
            in the next iteration)
        """
        self.vars.v_previous = v  # Store forward speed to be used in next step

注意：其实源程序中通过插值给出了离本车位置最近的航点集（可以理解成移动到当前位置之后未完成跟踪的点集），update_waypoints就是更新航点集的作用，这里并没有使用，有兴趣可以尝试改一改。这里采用的方式是整个航点集转化到本车坐标系中，具体见map_coord_2_Car_coord（）函数，然后将点集拟合成一个多项式曲线，在这条曲线令X=0即求出本车对应的横向跟踪误差和横摆角误差。

#pid.py

import numpy as np
import time

class PID(object):
    """PID Controller
    """

    def __init__(self, P=0.2, I=0.0, D=0.0):

        self.Kp = P
        self.Ki = I
        self.Kd = D

        self.sample_time = 0.00
        self.current_time = time.time()
        self.last_time = self.current_time

        self.clear()

    def clear(self):
        """Clears PID computations and coefficients"""        
        self.PTerm = 0.0
        self.ITerm = 0.0
        self.DTerm = 0.0
        self.last_error = 0.0

        # Windup Guard
        self.int_error = 0.0
        self.windup_guard = 20.0

        self.output = 0.0

    def update(self, error, output_limits=[-1.0, 1.0]):
        """ Calculates PID value for given reference feedback
        .. Equation
            u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}        
        """       
        
        self.current_time = time.time()
        delta_time = self.current_time - self.last_time
        delta_error = error - self.last_error

        if (delta_time >= self.sample_time):
            self.PTerm = self.Kp * error
            self.ITerm += error * delta_time

            if (self.ITerm < -self.windup_guard):
                self.ITerm = -self.windup_guard
            elif (self.ITerm > self.windup_guard):
                self.ITerm = self.windup_guard

            self.DTerm = 0.0
            if delta_time > 0:
                self.DTerm = delta_error / delta_time

            # Remember last time and last error for next calculation
            self.last_time = self.current_time
            self.last_error = error

            self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm)
            self.output = np.clip(self.output, output_limits[0], output_limits[1])


    def setKp(self, proportional_gain):
        """Determines how aggressively the PID reacts to the current error with setting Proportional Gain"""
        self.Kp = proportional_gain

    def setKi(self, integral_gain):
        """Determines how aggressively the PID reacts to the current error with setting Integral Gain"""
        self.Ki = integral_gain

    def setKd(self, derivative_gain):
        """Determines how aggressively the PID reacts to the current error with setting Derivative Gain"""
        self.Kd = derivative_gain

    def setSampleTime(self, sample_time):
        """PID that should be updated at a regular interval.
        Based on a pre-determined sampe time, the PID decides if it should compute or return immediately.
        """
        self.sample_time = sample_time

pid的主要公式

PID核心思想就是对误差进行放大、积分、微分操作，修正控制值。

pid各参数对控制效果的影响

结果：
只展示轨迹

横向控制的方法还包括PP(pure pursuit、Stanley、MPC、模糊控制)，
另起文章。
参考自Coursera。
以上。