[无人驾驶]Planning&Control

本章讨论下无人驾驶领域的路线规划和控制相关的技术。
1、路线规划
1.1 路线规划采用A*算法
(具体过程和流程后续补上)
1.2 路线规划转化为实际行驶路线

如上图所示，s表示路线规划起点，e表示路线规划终点，斜线填充的网格表示障碍物位置。
蓝色的线为路线规划算法规划出的路线，但在实际行驶中，车辆不可能直接原地90度的转弯，不仅可操作性不强，对于乘客也是非常危险的行为。在实际的行驶中，我们更希望车辆的路线如红色线路所示，非常平缓的从起点驶向终点（绿色路线虽然距离更近，但起点45度的转角也会让乘客不适）。
那么我们的问题来了，如何由路线规划算法生成的路线转换为实际行驶的路线，即如何由蓝色路线生成红色路线呢,如何由蓝色路线生成平滑的红色线路呢？
假设路线规划的结果为点序列:[x1,x2,…xn]
平滑后路线规划的点序列:[y1,y2,…yn]
路线平滑的过程本质上是求解 yi 使得： ||xi−yi||2 （平滑后的点与原始点的偏离程度）与 ||yi−yi+1|| (平滑后点之间的距离)取值最小，即使得

c1||xi−yi||2+c2||yi−yi+1||

最小化，其中 c1 与 c2 是绝对目标路线平滑程度的参数， c1 相对于 c2 越大，平滑后的点就越接近于原始点，反之，路线就越平滑。

求解最优解的方法采用梯度下降法(gradient descent)，即通过多次迭代，调整 yi 使得目标函数取得最小值。
起始值:
yi=xi i=[1,…n]
迭代：
遍历除起点和终点外的所有点，更新 yi
yi=yi+alpha∗(xi−yi)+beta∗(yi−1−2yi+yi+1)
循环执行迭代过程直到达到迭代次数上限或者目标函数值下降至指定阈值。
1.3 算法实现

上图代码一个5x5的网格地图，红色圆圈代表一条从(0,0)到(4,4)的规划路线，下面代码演示了如何由这条路线生成一条平滑线路。

# ­­­­­­­­­­­
# User Instructions
#
# Define a function smooth that takes a path as its input
# (with optional parameters for weight_data, weight_smooth)
# and returns a smooth path.
#
# Smoothing should be implemented by iteratively updating
# each entry in newpath until some desired level of accuracy
# is reached. The update should be done according to the
# gradient descent equations given in the previous video:
#
# If your function isn't submitting it is possible that the
# runtime is too long. Try sacrificing accuracy for speed.
# ­­­­­­­­­­­
from math import *
# Don't modify path inside your function.
path = [[0, 0],
        [0, 1],
        [0, 2],
        [1, 2],
        [2, 2],
        [3, 2],
        [4, 2],
        [4, 3],
        [4, 4]]

# ­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­­
# smooth coordinates
#
def smooth(path, weight_data = 0.5, weight_smooth = 0.1,
tolerance = 0.000001):
    # Make a deep copy of path into newpath
    newpath = [[0 for col in range(len(path[0]))]
        for row in range(len(path))]
    for i in range(len(path)):
        for j in range(len(path[0])):
            newpath[i][j] = path[i][j]
    #### ENTER CODE BELOW THIS LINE ###
    change = tolerance
    while (change >= tolerance):
        change = 0
        for i in range(1,len(path)­1):
            for j in range(len(path[0])):
                d1 = weight_data * (path[i][j]-­newpath[i][j])
                d2 = weight_smooth * (newpath[i­1][j] +
newpath[i+1][j] ­- 2 * newpath[i][j])
                change += abs(d1 + d2)
                newpath[i][j] += d1 + d2
    return newpath # Leave this line for the grader!
# feel free to leave this and the following lines if you want to print.
newpath = smooth(path)
# thank you ­ EnTerr ­ for posting this on our discussion forum
for i in range(len(path)):
    print '['+ ', '.join('%.3f'%x for x in path[i]) +'] ­> ['+', '.join('%.3f'%x for x in newpath[i]) +']'

2、车辆控制
平滑处理的路线还需要转换为实际的车辆行驶操作，汽车的控制算法是一个复杂的课题，本文只讨论PID算法简单的实现，不讨论背后复杂的理论体系，Google无人驾驶采用的控制算法的原理与此类似。
2.1 P Controller

如图所示、车辆A以恒定的速度向前运动，车辆的两个前轮可以随意控制其方向，后轮不可控制其方向；黑色带箭头的实线表示路线规划输出车辆目标运行轨迹，如何使得车辆按照目标轨迹运行呢？
当然，首先要控制车辆靠近目标轨迹并且沿着目标轨迹运行，实际操作中我们运用横向轨迹误差法(Cross-track Error,目标轨迹与车辆当前位置的距离)不断的计算调整车辆的状态，使其不断的靠近目标轨迹。Cross-track Error的值越大，车头偏离原行驶方向的角度就越大，随着Cross-track Erro的减小，车辆偏离原行驶方向的角度要不断减小，直至轨迹重合。这种类型的车辆控制称为P controller，P为proportional的简写。之所以称为proportional controller，This is because you steer in proportion to the system error: in this case, the cross-track error.
假设alpha表示车辆驾驶的角度，则有：

alpha=πp∗CTEt

CTEt 表示Cross-track Error
2.2 车辆抖动

单纯的Pcontroller会导致上图所示的问题，车辆在目标轨迹附近波动前行，这种波动可能非常小，以至于在某些场景下我们可以忽略。
2.3 P Controller算法实现

# -----------
# 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
#
# Note that tau is called "param" in the function
# below.
#
# Your code should print output that looks like
# the output shown in the video. That is, at each step:
# print myrobot, steering
#
# Only modify code at the bottom!
# ------------

from math import *
import random


# ------------------------------------------------
# 
# this is the robot class
#

class robot:

    # --------
    # init: 
    #    creates robot and initializes location/orientation to 0, 0, 0
    #

    def __init__(self, length = 20.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

    # --------
    # set: 
    #   sets a robot coordinate
    #

    def set(self, new_x, new_y, new_orientation):

        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation) % (2.0 * pi)


    # --------
    # set_noise: 
    #   sets the noise parameters
    #

    def set_noise(self, new_s_noise, new_d_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise = float(new_s_noise)
        self.distance_noise = float(new_d_noise)

    # --------
    # set_steering_drift: 
    #   sets the systematical steering drift parameter
    #

    def set_steering_drift(self, drift):
        self.steering_drift = drift

    # --------
    # move: 
    #    steering = front wheel steering angle, limited by max_steering_angle
    #    distance = total distance driven, most be non-negative

    def move(self, steering, distance, 
             tolerance = 0.001, max_steering_angle = pi / 4.0):

        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


        # make a new copy
        res = robot()
        res.length         = self.length
        res.steering_noise = self.steering_noise
        res.distance_noise = self.distance_noise
        res.steering_drift = self.steering_drift

        # 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 = tan(steering2) * distance2 / res.length

        if abs(turn) < tolerance:

            # approximate by straight line motion

            res.x = self.x + (distance2 * cos(self.orientation))
            res.y = self.y + (distance2 * sin(self.orientation))
            res.orientation = (self.orientation + turn) % (2.0 * pi)

        else:

            # approximate bicycle model for motion

            radius = distance2 / turn
            cx = self.x - (sin(self.orientation) * radius)
            cy = self.y + (cos(self.orientation) * radius)
            res.orientation = (self.orientation + turn) % (2.0 * pi)
            res.x = cx + (sin(res.orientation) * radius)
            res.y = cy - (cos(res.orientation) * radius)

        return res

    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


def run(param):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equal to speed (we assume time = 1)
    N = 100
    for i in range (N):
        crosstrack_error = myrobot.y
        steer = ­param * crosstrack_error
        myrobot = myrobot.move(steer, speed)
        print myrobot, steer

run(0.1) # call function with parameter tau of 0.1 and print results

2.3 PD Control
如何消除车辆在目标路线附近来回波动？PD Control可用来解决这一问题。PD Control不仅考虑Cross-Track Error还考虑到Cross-Track Error对时间的微分。

alpha=−πp∗CTEt−πdddtCTEt

其中:

ddtCTEt=CTEt−CTEt−▽t▽t

为简单期间，取 ▽t=1

ddtCTEt=CTEt−CTEt−1

这样一来，车辆的运动就不仅仅由Cross-Track Error相关，还与相邻Step之间的Cross-Track Error差值相关。
2.4 PD Control实现

# -----------
# User Instructions
#
# Implement a PD 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
# where differential crosstrack error (diff_CTE)
# is given by CTE(t) - CTE(t-1)
#
# Your code should print output that looks like
# the output shown in the video.
#
# Only modify code at the bottom!
# ------------

from math import *
import random


# ------------------------------------------------
# 
# this is the robot class
#

class robot:

    # --------
    # init: 
    #    creates robot and initializes location/orientation to 0, 0, 0
    #

    def __init__(self, length = 20.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

    # --------
    # set: 
    #   sets a robot coordinate
    #

    def set(self, new_x, new_y, new_orientation):

        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation) % (2.0 * pi)


    # --------
    # set_noise: 
    #   sets the noise parameters
    #

    def set_noise(self, new_s_noise, new_d_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise = float(new_s_noise)
        self.distance_noise = float(new_d_noise)

    # --------
    # set_steering_drift: 
    #   sets the systematical steering drift parameter
    #

    def set_steering_drift(self, drift):
        self.steering_drift = drift

    # --------
    # move: 
    #    steering = front wheel steering angle, limited by max_steering_angle
    #    distance = total distance driven, most be non-negative

    def move(self, steering, distance, 
             tolerance = 0.001, max_steering_angle = pi / 4.0):

        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


        # make a new copy
        res = robot()
        res.length         = self.length
        res.steering_noise = self.steering_noise
        res.distance_noise = self.distance_noise
        res.steering_drift = self.steering_drift

        # 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 = tan(steering2) * distance2 / res.length

        if abs(turn) < tolerance:

            # approximate by straight line motion

            res.x = self.x + (distance2 * cos(self.orientation))
            res.y = self.y + (distance2 * sin(self.orientation))
            res.orientation = (self.orientation + turn) % (2.0 * pi)

        else:

            # approximate bicycle model for motion

            radius = distance2 / turn
            cx = self.x - (sin(self.orientation) * radius)
            cy = self.y + (cos(self.orientation) * radius)
            res.orientation = (self.orientation + turn) % (2.0 * pi)
            res.x = cx + (sin(res.orientation) * radius)
            res.y = cy - (cos(res.orientation) * radius)

        return res

    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.


def run(param1, param2):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equal to speed (we assume time = 1)
    N = 100
    crosstrack_error = myrobot.y
    for i in range(N):
    diff_crosstrack_error = myrobot.y-crosstrack_error
    crosstrack_error = myrobot.y
    steer = ­ -param1 * crosstrack_error - param2 * diff_crosstrack_error
    myrobot = myrobot.move(steer, speed)
    print myrobot, steer

# Call your function with parameters of 0.2 and 3.0 and print results
run(0.2, 3.0)

通过结果可以看到，车辆更加平滑的移动到目标路线，随着时间推移，车辆停留在目标路线上。
还要注意一个问题是，车辆的系统偏差(systematic bias)问题。比如汽车的初始状态，比如车辆启动的时候，如果车辆的车轮方向存在倾斜，这对于人类司机不会造成困扰，但是对于自动驾驶的车辆，会导致行驶位置的偏离。
2.4 解决系统偏差(systematic bias)问题
假设你开着一辆车，按照经验习惯，调整到目标线路时，方向盘应该打到某个位置，但是有一天你突然发现相同的调整角度并未使你靠近目标位置，这时候你会将方向盘调整到更大角度，从而达到靠近目标位置的目的。
我们通过加权随时间不断累积的偏差(bias)值来消除系统偏差问题，公式如下:

alpha=−πp∗CTEt−πi∑CTEt−πdddtCTEt

这就是PID Controller,它包含了proportional、differential、integral三个部分。

def run(param1, param2, param3):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0 # motion distance is equal to speed (we assume time = 1)
    N = 100
    myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree bias
    int_crosstrack_error = 0
    for i in range(N):
        int_crosstrack_error += crosstrack_error
        diff_crosstrack_error = myrobot.y-crosstrack_error
        crosstrack_error = myrobot.y
        steer = ­- param1 * crosstrack_error
­               - param2 * diff_crosstrack_error
­               - param3 * int_crosstrack_error
    myrobot = myrobot.move(steer, speed)
    print myrobot, steer

2.5 参数优化问题
PID控制算法中的权重参数如何选择？我们运用了叫做”Twiddle”的算法，一次调整一个参数，最终生成最优参数集。
流程如下：
1) 初始化参数集合p=[0,0,0],表示我们目前为止对参数集最好的估计；初始化增量数组dp=[1,1,1],表示我们将要用来调整p的数值。
2) 选择参数集中的一个参数(假设为p[0]),更新p[0]的值p[0]=p[0]+dp[0],然后将参数集p代入PID算法，计算error，如果计算出的Error优于之前发现的所有Error，则保留p[0]的值，并更新dp[0]=dp[0]*1.1;反之，还原p[0]的值至更新前的值，然后更新p[0]=p[0]-dp[0]，并将参数集p代入PID算法计算Error，如果计算出的Error优于之前发现的所有Error，则保留p[0]的值，并更新dp[0]=dp[0]*1.1，否则还原p[0]至原始值，并且更新dp[0]=dp[0]*0.9,继续重复上述过程，直至找到合适的值或则dp数组元素的和超出设定阈值。

# ----------------
# User Instructions
#
# Implement twiddle as shown in the previous two videos.
# Your accumulated error should be very small!
#
# Your twiddle function should RETURN the accumulated
# error. Try adjusting the parameters p and dp to make
# this error as small as possible.
#
# Try to get your error below 1.0e-10 with as few iterations
# as possible (too many iterations will cause a timeout).
# No cheating!
# ------------

from math import *
import random


# ------------------------------------------------
# 
# this is the robot class
#

class robot:

    # --------
    # init: 
    #    creates robot and initializes location/orientation to 0, 0, 0
    #

    def __init__(self, length = 20.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

    # --------
    # set: 
    #   sets a robot coordinate
    #

    def set(self, new_x, new_y, new_orientation):

        self.x = float(new_x)
        self.y = float(new_y)
        self.orientation = float(new_orientation) % (2.0 * pi)


    # --------
    # set_noise: 
    #   sets the noise parameters
    #

    def set_noise(self, new_s_noise, new_d_noise):
        # makes it possible to change the noise parameters
        # this is often useful in particle filters
        self.steering_noise = float(new_s_noise)
        self.distance_noise = float(new_d_noise)

    # --------
    # set_steering_drift: 
    #   sets the systematical steering drift parameter
    #

    def set_steering_drift(self, drift):
        self.steering_drift = drift

    # --------
    # move: 
    #    steering = front wheel steering angle, limited by max_steering_angle
    #    distance = total distance driven, most be non-negative

    def move(self, steering, distance, 
             tolerance = 0.001, max_steering_angle = pi / 4.0):

        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


        # make a new copy
        res = robot()
        res.length         = self.length
        res.steering_noise = self.steering_noise
        res.distance_noise = self.distance_noise
        res.steering_drift = self.steering_drift

        # 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 = tan(steering2) * distance2 / res.length

        if abs(turn) < tolerance:

            # approximate by straight line motion

            res.x = self.x + (distance2 * cos(self.orientation))
            res.y = self.y + (distance2 * sin(self.orientation))
            res.orientation = (self.orientation + turn) % (2.0 * pi)

        else:

            # approximate bicycle model for motion

            radius = distance2 / turn
            cx = self.x - (sin(self.orientation) * radius)
            cy = self.y + (cos(self.orientation) * radius)
            res.orientation = (self.orientation + turn) % (2.0 * pi)
            res.x = cx + (sin(res.orientation) * radius)
            res.y = cy - (cos(res.orientation) * radius)

        return res




    def __repr__(self):
        return '[x=%.5f y=%.5f orient=%.5f]'  % (self.x, self.y, self.orientation)


# ------------------------------------------------------------------------
#
# run - does a single control run.


def run(params, printflag = False):
    myrobot = robot()
    myrobot.set(0.0, 1.0, 0.0)
    speed = 1.0
    err = 0.0
    int_crosstrack_error = 0.0
    N = 100
    # myrobot.set_noise(0.1, 0.0)
    myrobot.set_steering_drift(10.0 / 180.0 * pi) # 10 degree steering error


    crosstrack_error = myrobot.y


    for i in range(N * 2):

        diff_crosstrack_error = myrobot.y - crosstrack_error
        crosstrack_error = myrobot.y
        int_crosstrack_error += crosstrack_error

        steer = - params[0] * crosstrack_error  \
            - params[1] * diff_crosstrack_error \
            - int_crosstrack_error * params[2]
        myrobot = myrobot.move(steer, speed)
        if i >= N:
            err += (crosstrack_error ** 2)
        if printflag:
            print myrobot, steer
    return err / float(N)


def twiddle(tol = 0.2): #Make this tolerance bigger if you are timing out!
############## ADD CODE BELOW ####################
    n_params = 3
    dparams = [1.0 for row in range(n_params)]
    params = [0.0 for row in range(n_params)]
    best_error = run(params)
    n = 0
    while sum(dparams) > tol:
        for i in range(len(params)):
            params[i] += dparams[i]
            err = run(params)
            if err < best_error:
                best_error = err
                dparams[i] *= 1.1
            else:
                params[i] ­= 2.0 * dparams[i]
                err = run(params)
                if err < best_error:
                    best_error = err
                    dparams[i] *= 1.1
                else:
                    params[i] += dparams[i]
                    dparams[i] *= 0.9

    return run(params)