Proportional-Integral-Derivative Controller

Hongtao Hao / 2023-02-28


The material below is based on Professor Xiaopeng Li ’s course of CIV ENGR 679 Connected and Automated Transport

This notebook runs in Julia and is rendered by Hupyter .

PID controller #

PID is short for “Proportional-Integral-Derivative”. What it does is that if you have a planned trajectory for a vehicle which is now at a specific location and have a specific initial speed, PID will allow this vehicle to get to the planned trajectory. You accomplish this through PID by tweaking the three parameters:

  • $k_P$
  • $k_I$
  • $k_D$

If you multiply $k_P$ by the distance between the planned location and the actual location at any timestamp, you get $a_P$. If you multiply $k_I$ by the difference between the integral of the planned location and that of the actual location at any timestamp, you get $a_I$. If you multiply $k_D$ by the speed difference between the planned trajectory and the actual vehicle at any timestamp, you get $a_D$.

If you add up $a_P$, $a_I$, and $a_D$, you get $a$, which is the acceleration for the actual vehicle.

PID for constant speed #

The planned trajectory starts from location 0 at time 0. It is cruising at 30m/s speed for 50 seconds.The actual location is 3 at time 0 and the actual speed is 28m/s at time 0.

using Plots, Distributions, Random
function get_data(t, k_p=2, k_i=0, k_d=1)
    # initial state
    # y for the planned vehicle, x for the actual vehicle
    
    y = 0 # position
    int_y = 0 # integral of position
    y_dot = 30 # speed
    x = 3
    int_x = 0
    x_dot = 28
    
    a_p = k_p * (y - x)
    a_i = k_i * (int_y - int_x)
    a_d = k_d * (y_dot - x_dot)
    
    a = a_p + a_i + a_d
    
    # if time is 0, return the initial state
    if t == 0
        return (y, x)
    else
        for i in time_delta:time_delta:t
            # 0.2, 0.4, 0.6, ...
            x_dot_prev = x_dot
            x_dot += a * time_delta
            # THIS IS USING APPROXIMATION!
            x += 0.5*(x_dot_prev + x_dot) * time_delta
            int_x += x * time_delta

            # update y
            y_dot = y_dot # constant speed at all time
            y += y_dot * time_delta
            int_y += y * time_delta

            # update a_p, a_i, a_d, and a for usage at next timestamp
            a_p = k_p * (y - x)
            a_i = k_i * (int_y - int_x)
            a_d = k_d * (y_dot - x_dot)
            a = a_p + a_i + a_d
        end
        return (y, x)
    end
end
get_data (generic function with 5 methods)
time_delta = 0.2 
time_span = 0.0:time_delta:50
0.0:0.2:50.0
function make_plot(data, title, label, xlabel, ylabel)
    Plots.plot(time_span, data,
    title=title,
    label=label,
    linewidth=0.5,
    markershape = :auto,
    markersize = 2,
    linestyle = :auto,
    mc= :auto,
    xlabel = xlabel,
    ylabel = ylabel,
    legend=:bottom
    )
end
make_plot (generic function with 1 method)
ys = [get_data(i, 2, 0, 1)[1] for i in time_span]
xs = [get_data(i, 2, 0, 1)[2] for i in time_span]
251-element Vector{Real}:
    3
    8.52
   13.915199999999999
   19.253951999999998
   24.59818752
   29.999490355200003
   35.496625508352004
   41.114488996331524
   46.8643352065278
   52.745059206570446
   58.74526263008063
   64.84581249542273
   71.02260938267628
    ⋮
 1434.0000000008072
 1440.0000000013742
 1446.0000000017405
 1452.000000001909
 1458.000000001898
 1464.0000000017367
 1470.0000000014622
 1476.0000000011148
 1482.0000000007337
 1488.000000000355
 1494.0000000000084
 1499.9999999997167
make_plot(
    [ys, xs],
    "planned and actual",
    ["planned" "actual"],
    "time",
    "planned and actual"
)

savefig("/en/blog/2023-02-28-pid_files/pid-01.png")

make_plot(
    ys-xs,
    "planned - actual",
    "planned - actual",
    "time",
    "planned - actual"
)

savefig("/en/blog/2023-02-28-pid_files/pid-02.png")

Dynamic speed (with random noise) #

The planned trajectory starts from location 0 at time 0. It is cruising at 30m/s speed for the first 10 seconds, then decreasing to 10 m/s with deceleration $-2𝑚/𝑠^2$ for 10 seconds and then accelerating to 30m/s with acceleration $2𝑚/𝑠^2$ for 10 seconds, and then cruising at 30 m/s for 20 seconds.

The actual location is 3 at time 0 and the actual speed is 28m/s at time 0.

Assuming the acceleration cannot be precisely controlled, and always has a random error uniformly distributed over $[-0.2, 0.2] m/s$

function get_y_dot(t)
    """This function gets the speed of the planned vehicle
    """
    if t <= 10
        y_dot = 30
    elseif t <= 20
        y_dot = 30
        for i in 10+time_delta:time_delta:t
            y_dot += time_delta*(-2)
        end
    elseif t <= 30
        y_dot = 10
        for i in 20+time_delta:time_delta:t
            y_dot += time_delta*2
        end
    else
        y_dot = 30
    end
    return y_dot
end
get_y_dot (generic function with 1 method)

Tesing the function of get_y_dot:

get_y_dot(15)
20.000000000000036
get_y_dot(22)
14.000000000000004
function get_data(t, k_p=2, k_i=0, k_d=1, noise = true)
    """This function returns the trajectory of the planned vehicle (y) 
    and the actual vehicle (x)
    
    The key is that the initial acceleration of the actual vehicle can be calculated 
    through the initial state and the set parameters. Using this acceleration at t=0,
    we can calculate the velocity of the actual vehicle at the next timestamp. Then, 
    using x_dot, we can calculate x and int_x.
    
    y_dot, y, and the int_y can be calculated easily. 
    """
    # initial state
    # y for the planned vehicle, x for the actual vehicle
    
    y = 0 # position
    int_y = 0 # integral of position
    y_dot = 30 # speed
    x = 3
    int_x = 0
    x_dot = 28
    
    a_p = k_p * (y - x)
    a_i = k_i * (int_y - int_x)
    a_d = k_d * (y_dot - x_dot)
    if noise
        a = a_p + a_i + a_d + rand(Uniform(-0.2, 0.2))
    else
        a = a_p + a_i + a_d
    end
    if t == 0
        return (y, x)
    else
        for i in time_delta:time_delta:t
            x_dot_prev = x_dot
            x_dot += a * time_delta
            # THIS IS USING APPROXIMATION!
            x += 0.5*(x_dot_prev + x_dot) * time_delta
            int_x += x * time_delta

            # update y
            y_dot = get_y_dot(i)
            y += y_dot * time_delta
            int_y += y * time_delta

            # update a_p, a_i, a_d, and a for usage at next timestamp
            a_p = k_p * (y - x)
            a_i = k_i * (int_y - int_x)
            a_d = k_d * (y_dot - x_dot)
            if noise
                a = a_p + a_i + a_d + rand(Uniform(-0.2, 0.2))
            else
                a = a_p + a_i + a_d
            end
        end
        return (y, x)
    end
end
get_data (generic function with 5 methods)
ys = [get_data(i, 2, 0, 1)[1] for i in time_span]
xs = [get_data(i, 2, 0, 1)[2] for i in time_span]
251-element Vector{Real}:
    3
    8.518715767262622
   13.906232802166503
   19.268572854391913
   24.6096425130715
   30.02599498962064
   35.45927678410515
   41.09226062787909
   46.86326171393311
   52.7531619121299
   58.74007729213587
   64.88326196481631
   71.03580647914927
    ⋮
 1233.9981479653238
 1240.010372398277
 1246.0066976688347
 1252.0239301638433
 1258.0021171428577
 1264.008764132957
 1270.021276479265
 1275.9923591224674
 1282.033955662245
 1288.014337231835
 1293.9907637632514
 1299.9826222077306
make_plot(
    [ys, xs],
    "planned and actual",
    ["planned" "actual"],
    "time",
    "planned and actual"
)

savefig("/en/blog/2023-02-28-pid_files/pid-03.png")

make_plot(
    ys-xs,
    "planned - actual",
    "planned - actual",
    "time",
    "planned - actual"
)

savefig("/en/blog/2023-02-28-pid_files/pid-04.png")

Questions #

  • In pid, the x/y_dot, and int_y/x is using approximation, can we get the exact form?

It’s very difficult to get the analytical form.

  • How to get the optimal value

It’s also very difficult. It involves non-linear programming.

#self-driving

Last modified on 2023-03-02