Self-driving Car ND B4, PID control

PID: proportional, integral, derivative.

It is a combination of 3 feedback mechanisms. Each is at different time scale: immediate, one interval, accumulative.

def run(robot, p,i,d, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    pre = robot.y
    integral = 0
    for j in range(n):
        cte = robot.y
        delta = cte -pre
        pre = cte
        integral += cte
        steer = -p*cte - i*integral - d*delta 
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
    return x_trajectory, y_trajectory

A method to find the best hyperparameter is called “twiddle”. Each time apply a change to a single parameter if the error is not reduced, reduce the change amount.

def run(robot, params, n=100, speed=1.0):
    x_trajectory = []
    y_trajectory = []
    err = 0
    prev_cte = robot.y
    int_cte = 0
    p , d , i = params
    for j in range(2 * n):
        cte = robot.y
        diff_cte = cte - prev_cte
        int_cte += cte
        prev_cte = cte
        steer = -p * cte - d * diff_cte - i * int_cte
        robot.move(steer, speed)
        x_trajectory.append(robot.x)
        y_trajectory.append(robot.y)
        if j >= n:  # discard the first n points to get more stable value
            err += cte ** 2
    return x_trajectory, y_trajectory, err / n

def twiddle(tol=0.2): 
    p = [0, 0, 0]
    dp = [1, 1, 1]
    robot = make_robot()
    x_track, y_track, best_err = run(robot, p)
    it = 0
    while sum(dp) > tol:
        it += 1
        print("Iteration {}, best error = {}".format(it, best_err))
        for i in range(len(p)):
            for j in range(2):
                if j==0:
                    p[i] += dp[i]  # positive attempt
                if j==1:
                    p[i] -= 2*dp[i]  # negative attempt
                robot = make_robot()
                x_track, y_track, err = run(robot, p)
                 # works, record result,increase delta
                if err < best_err: 
                    best_err = err
                    dp[i] *= 1.1
                    print p, dp
                    break
            # neither works, roll back, decrease delta    
            p[i] += dp[i] 
            dp[i] *= 0.9
    print p, dp        
    return p, best_err

project

In the actual project, there is no fixed line for the car to approach. So we have to manually implement the twiddle method.

​