無人駕駛13：PID控制器

基於環境反饋的控制方法，叫做反饋控制。

控制模塊的目的是讓無人車能夠按照規劃好的路徑行駛，需要將環境當前的反饋和規劃的參考量進行比較，得到當前偏離參考量的誤差，並基於這個誤差設計一定的算法來產生輸出信號，使得誤差不斷變小，直到爲0.

PID控制器是目前應用最廣泛的控制理論。

PID控制器是指P(Proportion 比例)、I（Integral 積分），D(Derivative 微分) ，這三項如何使用誤差來產生控制指令。

1. P控制

假設控制輸出的是車輛的轉角；

偏離航跡誤差(Cross Track Error, CTE):就是車輛到參考線的距離，用CTE作爲誤差度量,即轉向角與CTE成比例；

$sterring angle = K_p *e(t)$

e(t)是車輛在t時刻的CTE，CTE偏大時，轉向角也大，反之偏轉小一點；

在P控制器中，係數$K_p$會直接影響實際控制效果，在合理範圍內，$K_p$越大，控制效果越好。

僞代碼實現：

def run(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

但是P控制器容易受零值影響，實際中表現爲來回多次穿過參考線

爲了矯正這個問題，我們引入一個新的參數，CTE變化率

2. PD控制

CTE的變化率，描述無人車向着參考線移動的多塊，如果無人車一直在參考線上運動，CTE變化率就是0；

$steering angle = K_p*e(t) + K_d\frac{de(t)}{dt}$

增大P係數會增大無人車向參考線運動的傾向，增大D係數則會增大無人車轉向角快速變化的“阻尼”；

對於P偏大，D偏小的系統，稱爲“欠阻尼系統(underdamped)”, 這種情況無人車將沿參考線震盪前行；

而P偏小，D偏大的系統，稱爲“過阻尼(overdamped)”，這使得無人車需較長時間才能糾正誤差；

合適的P，D係數，可以使無人車快速回到參考線，同時很好維持在參考線運動；

僞代碼實現：

def run(robot, tau_p, tau_d, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    prev_cte = robot.y
    for i in range(n):
        cte = robot.y
        diff_cte = cte - prev_cte
        prev_cte = cte
        steer = -tau_p * cte - tau_d * diff_cte
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory

系統偏差

PD控制器可以保證正常的控制需求，但是容易受系統偏差影響；

比如當車輪存在偏移角(非理想化)，對人類駕駛來說，這問題不大，但是對於無人駕駛，會產生巨大的CTE。

爲解決熊偏差問題，引入一個控制量來消除這種誤差。

3 PID控制器

引入一個積分項，加入到控制輸出函數中

$steering angle = K_p*e(t) + K_d\frac{de(t)}{dt} + k_i\int_0^te(t)dt$

$K_i$積分項，本質上是車的實際路線和參考線的圖形面積，加入積分項後，控制器會使車輛路線的積分儘可能小，從而避免穩態誤差情況。

僞代碼實現:

def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    prev_cte = robot.y
    int_cte = 0
    for i in range(n):
        cte = robot.y
        diff_cte = cte - prev_cte
        prev_cte = cte
        int_cte += cte
        steer = -tau_p * cte - tau_d * diff_cte - tau_i * int_cte
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory

積分向係數的大小也會影響整個控制系統的穩定性，$K_i$過大，會使系統“震盪”運行，$K_i$過小又會需要較長時間才能修正車輛穩態誤差。

PID控制器，就是由$K_p, K_i, K_d$三項共同決定的，其實現非常簡單，但是這三個係數的選擇缺很難。

PID算法Python實現：

# -----------
# 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 random
import numpy as np
import 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

robot = Robot()
robot.set(0, 1, 0)


def run(robot, tau_p, tau_d, tau_i, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    prev_cte = robot.y
    int_cte = 0
    for i in range(n):
        cte = robot.y
        diff_cte = cte - prev_cte
        prev_cte = cte
        int_cte += cte
        steer = -tau_p * cte - tau_d * diff_cte - tau_i * int_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.2, 3.0, 0.001)
n = len(x_trajectory)

plt.plot(x_trajectory, y_trajectory, 'b', label='PID controller')
plt.plot(x_trajectory, np.zeros(n), 'r', label='reference')
plt.savefig("PID_Contrl.png")

PID係數調整方法：

1.twiddle算法：

如上例PID算法實現，返回一個CTE的平均值來度量系統的誤差程度，問題轉化爲，尋找使CTE均值最小化的PID係數。

設立一組初始值, 比如係數初值p=[0, 0, 0]; 係數調整範圍初值dp=[1,1,1]

循環遍歷迭代係數p，

首先調大p[i]值，p[i] += dp[i]

若效果改善，保留該值，將對應係數範圍加大dp[i]*=1.1

若無改善，調小p[i], p[i] -= 2*dp[i] ;

若有效，保留該值，將對應係數範圍加大dp[i]*=1.1

若無效，p[i]保留原值，縮小dp[i]範圍 dp[i]*=1.1；

循環迭代，通過縮放得到的參數越來越精確，直到收斂(係數範圍和sum(dp[i])足夠小).

這是一種局部爬山算法，但是它非常高效.

僞代碼實現：

def twiddle(tol=0.2): 
    p = [0, 0, 0]
    dp = [1, 1, 1]
    robot = make_robot()
    x_trajectory, y_trajectory, best_err = run(robot, p)

    it = 0
    while sum(dp) > tol:
        print("Iteration {}, best error = {}".format(it, best_err))
        for i in range(len(p)):
            p[i] += dp[i]
            robot = make_robot()
            x_trajectory, y_trajectory, err = run(robot, p)

            if err < best_err:
                best_err = err
                dp[i] *= 1.1
            else:
                p[i] -= 2 * dp[i]
                robot = make_robot()
                x_trajectory, y_trajectory, err = run(robot, p)

                if err < best_err:
                    best_err = err
                    dp[i] *= 1.1
                else:
                    p[i] += dp[i]
                    dp[i] *= 0.9
        it += 1
    return p

2. 實踐調試方法

該方法是做優達學城項目過程中，mentor給的建議，實踐中效果挺不錯，哈哈

具體參考：

Additional Resources on Control

Model Predictive Control (MPC)

Vision-Based High Speed Driving with a Deep Dynamic Observer by P. Drews, et. al.

Abstract: In this paper we present a framework for combining deep learning-based road detection, particle filters, and Model Predictive Control (MPC) to drive aggressively using only a monocular camera, IMU, and wheel speed sensors. This framework uses deep convolutional neural networks combined with LSTMs to learn a local cost map representation of the track in front of the vehicle. A particle filter uses this dynamic observation model to localize in a schematic map, and MPC is used to drive aggressively using this particle filter based state estimate. We show extensive real world testing results, and demonstrate reliable operation of the vehicle at the friction limits on a complex dirt track. We reach speeds above 27 mph (12 m/s) on a dirt track with a 105 foot (32m) long straight using our 1:5 scale test vehicle. […]

Reinforcement Learning-based

Reinforcement Learning and Deep Learning based Lateral Control for Autonomous Driving by D. Li, et. al.

Abstract: This paper investigates the vision-based autonomous driving with deep learning and reinforcement learning methods. Different from the end-to-end learning method, our method breaks the vision-based lateral control system down into a perception module and a control module. The perception module which is based on a multi-task learning neural network first takes a driver-view image as its input and predicts the track features. The control module which is based on reinforcement learning then makes a control decision based on these features. In order to improve the data efficiency, we propose visual TORCS (VTORCS), a deep reinforcement learning environment which is based on the open racing car simulator (TORCS). By means of the provided functions, one can train an agent with the input of an image or various physical sensor measurement, or evaluate the perception algorithm on this simulator. The trained reinforcement learning controller outperforms the linear quadratic regulator (LQR) controller and model predictive control (MPC) controller on different tracks. The experiments demonstrate that the perception module shows promising performance and the controller is capable of controlling the vehicle drive well along the track center with visual input.

Behavioral Cloning

The below paper shows one of the techniques Waymo has researched using imitation learning (aka behavioral cloning) to drive a car.

ChauffeurNet: Learning to Drive by Imitating the Best and Synthesizing the Worst by M. Bansal, A. Krizhevsky and A. Ogale

Abstract: Our goal is to train a policy for autonomous driving via imitation learning that is robust enough to drive a real vehicle. We find that standard behavior cloning is insufficient for handling complex driving scenarios, even when we leverage a perception system for preprocessing the input and a controller for executing the output on the car: 30 million examples are still not enough. We propose exposing the learner to synthesized data in the form of perturbations to the expert’s driving, which creates interesting situations such as collisions and/or going off the road. Rather than purely imitating all data, we augment the imitation loss with additional losses that penalize undesirable events and encourage progress – the perturbations then provide an important signal for these losses and lead to robustness of the learned model. We show that the ChauffeurNet model can handle complex situations in simulation, and present ablation experiments that emphasize the importance of each of our proposed changes and show that the model is responding to the appropriate causal factors. Finally, we demonstrate the model driving a car in the real world.