These codes were written a long time ago，which is simple for car following. The car in front can be in IDM mode or constant speed mode. It uses the car following dynamic model and MPC-DCBF controller（NLP problem solved by ipopt） to control the follower.
#单辆lear car IDM
class IDM:
    def __init__(self,Leader_V,Leader_X,Leader_Y,Leader_a,A_max,dt,V0):
        
        self.Leader_V = Leader_V
        self.Leader_X = Leader_X
        self.Leader_Y = Leader_Y
        self.Leader_a = Leader_a
        self.dt = dt
        self.V0 = V0
        self.A_max = A_max
    
    def update_state(self):
        self.Leader_X = self.Leader_X + self.Leader_V*self.dt
        self.Leader_V = self.Leader_V + self.Leader_a*self.dt   
        self.Leader_a = self.A_max*(1-round((self.Leader_V/self.V0),1)**4)

    def get_state(self):
        return self.Leader_X, self.Leader_V, self.Leader_a
    
    #定义lead car预测序列
    def get_x_lead_list(self,H):
        x_lead_list = [self.Leader_X]
        for item in range(H):
            x_mid = self.Leader_X + self.Leader_V*self.dt
            x_lead_list.append(x_mid)
        return x_lead_list
code/IDM.py
YimingShu-teay/Car-Following
#动力学模型
class KinematicModel:
    def __init__(self,x,v,dt,f0,f1,f2,m):
        self.x = x
        self.v = v
        self.dt = dt
        self.f0 = f0
        self.f1 = f1
        self.f2 = f2
        self.m = m

    def update_state(self,a_x):
        self.x = self.x + self.v*self.dt
        self.v = self.v + (a_x/self.m - self.v*(self.f1+2*self.f2*self.v)/self.m)*self.dt

    def get_state(self):
        return self.x, self.v
    
    

code/dynamical.py
YimingShu-teay/Car-Following
import numpy as np
from pyomo.environ import *
from pyomo.dae import *
import numpy as np
import warnings
warnings.filterwarnings("ignore")
import pandas as pd
import math
from math import factorial
from pyomo.util.infeasible import log_infeasible_constraints
from decimal import Decimal
import matplotlib.pyplot as plt
from dynamical import *
from solver import Nonlinear_MPC
from IDM import IDM
from utils import *

#参数常量
Params = {}
Params['Nx'] = 2
Params['Nu'] = 1
Params['H'] = 5
Params['dt'] = 0.1
Params['m'] = 1650
Params['g'] = 9.81
Params['ca'] = 0.2
Params['cd'] = 0.2
Params['f0'] = 0.1
Params['f1'] = 1.8
Params['f2'] = 0.15
Params['Line_Y'] = 10.0
Params['gamma_clf'] = 0.5
Params['gamma_cbf'] = 0.2
Params['slack_clf'] = 2e-2
Params['slack_cbf'] = 2e-2
Params['a_max'] = Params['ca']*Params['m']*Params['g']
Params['a_min'] = -Params['cd']*Params['m']*Params['g']
Params['v_d'] = 25.0
Params['x_lead0'] = 30.0
Params['y_lead0'] = 10.0
Params['v_lead0'] = 15.0
Params['a_lead0'] = 0.0
Params['A_max'] = 1.0
Params['V0'] = 15.0
Params['X_start'] = [5.0,10.0]
Params['MAX_TIME'] = 10.0
Params['r'] = 10.0
Params['Line_Y'] = 10.0
Params['Th'] = 1.8
Params['v0'] = 14.0
Params['show_animation'] = True


def run_simulation(x_lead0,y_lead0,v_lead0,a_lead0,X_start,H,dt,MAX_TIME,show_animation,Nx,r,m,a_min,a_max,f0,f1,f2,
                   gamma_clf,gamma_cbf,slack_clf,slack_cbf,v_d,A_max,V0,Th,cd,g,v0):
    #list用来记录，之后用来画图
    time = 0.0
    t = [0.0]
    
    ugv = KinematicModel(X_start[0],X_start[1],dt,f0,f1,f2,m)
    ugv_lead = IDM(v_lead0,x_lead0,y_lead0,a_lead0,A_max,dt,V0)
    x_lead_list = ugv_lead.get_x_lead_list(H)
    
    x = [ugv.x]
    v = [ugv.v]
    ax = [0.0]
    x_lead = [ugv_lead.Leader_X]
    v_lead = [ugv_lead.Leader_V]
    a_lead = [ugv_lead.Leader_a]
    slack = [0.0]
    
    while MAX_TIME >= time:
        X0 = [ugv.x, ugv.v]
        x_lead_list = ugv_lead.get_x_lead_list(H)
        MPC_Model = Nonlinear_MPC(x_lead_list,X0,Nx,m,r,dt,H,a_min,a_max,f0,f1,f2,gamma_clf,gamma_cbf,slack_clf,slack_cbf,v_d,Th,cd,g,v0)
        MPC_Model.update_lead(x_lead_list)
        x_opt, v_opt, ax_opt, slack_clf_opt = MPC_Model.Solve(X0)
        slack.append(slack_clf_opt)
        if ax_opt is not None:
            ai = ax_opt[0]
 
        ugv.update_state(ai)
        ugv_lead.update_state()
        print(ugv_lead.Leader_X,ugv.x,Th,ugv.v)
        print(ugv_lead.Leader_X - ugv.x-10.0)
        time = time + dt
        t.append(time)
        x.append(ugv.x)
        v.append(ugv.v)
        ax.append(ai)
        
        x_lead.append(ugv_lead.Leader_X)
        v_lead.append(ugv_lead.Leader_V)
        a_lead.append(ugv_lead.Leader_a)
        
        if show_animation:
            
            plt.cla()
            plot_car(ugv.x, 10.0, 0, 0)
            plot_car(ugv_lead.Leader_X, ugv_lead.Leader_Y, 0, 0)
         
            plt.axis("equal")
            plt.grid(True)
            
            plt.pause(0.0001)

    return x,v,ax,x_lead,v_lead,a_lead,t

if __name__ == '__main__':
    x,v,ax,x_lead,v_lead,a_lead,t = run_simulation(Params['x_lead0'],Params['y_lead0'],Params['v_lead0'],Params['a_lead0'],
                   Params['X_start'],Params['H'],Params['dt'],Params['MAX_TIME'],Params['show_animation'],
                   Params['Nx'],Params['r'],Params['m'],Params['a_min'],Params['a_max'],
                   Params['f0'],Params['f1'],Params['f2'],Params['gamma_clf'],Params['gamma_cbf'],
                   Params['slack_clf'],Params['slack_cbf'],Params['v_d'],
                   Params['A_max'],Params['V0'],Params['Th'],Params['cd'],Params['g'],Params['v0'])
    plt.subplots()
    plt.plot(t, v, "-r", label="speed")
    plt.grid(True)
    plt.xlabel("Time [s]")
    plt.ylabel("Speed [m/s]")

    plt.subplots()
    plt.plot(t, ax, "-r", label="speed")
    plt.grid(True)
    plt.xlabel("Time [s]")
    plt.ylabel("Acc [m/s]")
    
    plt.show()
code/run.py
YimingShu-teay/Car-Following
#MPC Model
import numpy as np
from pyomo.environ import *
from pyomo.dae import *
import numpy as np
import warnings
warnings.filterwarnings("ignore")
import pandas as pd
import math
from math import factorial
from celluloid import Camera
from pyomo.util.infeasible import log_infeasible_constraints
from decimal import Decimal
import matplotlib.pyplot as plt
pyomo.__version__
class Nonlinear_MPC:
    def __init__(self,x_lead_list,X_start,state_number,m,r,dt,H,a_min,a_max,f0,f1,f2,gamma_clf,gamma_cbf,slack_clf,slack_cbf,v_d,Th,cd,g,v0):
        
        # Model
        model = ConcreteModel()
        model.xk_num = RangeSet(1,H)
        model.uk_num = RangeSet(0,H)
        model.duk_num = RangeSet(0,H-1)
        
        # Parameters
        model.x_obj = x_lead_list
        model.dt = dt
        model.m = m
        model.state_number = state_number
        model.X0 = Param(RangeSet(0,state_number-1),initialize={0:X_start[0],1:X_start[1]},mutable = True)
        model.R = Param(RangeSet(1,5),initialize={1:10,2:10,3:1000,4:30},mutable=True)

        # Variables
        model.X = Var(RangeSet(0,state_number-1),model.uk_num)
        model.a = Var(model.uk_num,bounds=(a_min,a_max))
        model.slack_cbf = Var(model.uk_num,bounds=(0,1))
        
        # Constraint
        model.X0_update = Constraint(RangeSet(0,state_number-1),rule=lambda model,i:model.X[i,0]==model.X0[i])
        model.x_update = Constraint(model.uk_num,rule=lambda model,k:model.X[0,k+1] == model.X[0,k]+model.X[1,k]*model.dt
                                if k<=H-1 else Constraint.Skip)
        model.v_update = Constraint(model.uk_num,rule=lambda model,k:model.X[1,k+1] == model.X[1,k]+model.a[k]*model.dt/model.m-model.X[1,k]*(f1+2*f2*model.X[1,k])*model.dt/model.m
                                if k<=H-1 else Constraint.Skip)
#         model.clf_update = Constraint(model.uk_num,rule=lambda model,k:(v_d-model.X[1,k+1])-(v_d-model.X[1,k])+gamma_clf*(model.X[1,k]-v_d) <= model.slack_clf[k]
#                                 if k<=H-1 else Constraint.Skip)
        model.cbf_update = Constraint(model.uk_num,rule=lambda model,k:((model.x_obj[k+1]-model.X[0,k+1])**2-r**2)-((model.x_obj[k] - model.X[0,k])**2-r**2)+gamma_cbf*((model.x_obj[k]-model.X[0,k])**2-r**2)>= -model.slack_cbf[k]
                                if k<=H-1 else Constraint.Skip)
        
        #Objectives
        model.sbRsb = model.R[1]*sum(model.slack_cbf[i]**2 for i in model.uk_num)
        model.aRa = model.R[2]*sum(((model.a[i]-(f0+f1*model.X[1,i]+f2*model.X[1,i]**2))/model.m)**2 for i in model.uk_num)
        model.daRda = model.R[3]*sum((((model.a[i+1]-(f0+f1*model.X[1,i+1]+f2*model.X[1,i+1]**2))/model.m)-((model.a[i]-(f0+f1*model.X[1,i]+f2*model.X[1,i]**2))/model.m))**2 for i in model.duk_num)
        model.vRv = model.R[4]*sum(((model.X[1,i+1]-v_d))**2 for i in model.duk_num)
        model.obj = Objective(expr=model.sbRsb+model.vRv+model.aRa+model.daRda,sense=minimize)

        self.iN = model
    
    def update_lead(self,x_lead_list):
        self.iN.x_obj = x_lead_list
        
    def Solve(self,X_state):
        self.iN.X0.reconstruct({0:X_state[0], 1: X_state[1]})
        self.iN.x_update.reconstruct()
        self.iN.v_update.reconstruct()
#         self.iN.clf_update.reconstruct()
        self.iN.cbf_update.reconstruct()
        
        SolverFactory("ipopt").solve(self.iN)
        log_infeasible_constraints(self.iN)
        x_opt = [self.iN.X[0,k]() for k in self.iN.xk_num]
        v_opt = [self.iN.X[1,k]() for k in self.iN.xk_num]
        ax_opt = [self.iN.a[k]() for k in self.iN.uk_num]
        slack_cbf_opt = [self.iN.slack_cbf[k]() for k in self.iN.uk_num]
        return x_opt, v_opt, ax_opt, slack_cbf_opt
    
    

code/solver.py
YimingShu-teay/Car-Following
import math
import numpy as np
import matplotlib.pyplot as plt
LENGTH = 4.5  # [m]
WIDTH = 2.0  # [m]
BACKTOWHEEL = 1.0  # [m]
WHEEL_LEN = 0.3  # [m]
WHEEL_WIDTH = 0.2  # [m]
TREAD = 0.7  # [m]
WB = 2.5  # [m]

def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
    
    outline = np.matrix([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
                         [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])

    fr_wheel = np.matrix([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
                          [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])

    rr_wheel = np.copy(fr_wheel)

    fl_wheel = np.copy(fr_wheel)
    fl_wheel[1, :] *= -1
    rl_wheel = np.copy(rr_wheel)
    rl_wheel[1, :] *= -1

    Rot1 = np.matrix([[math.cos(yaw), math.sin(yaw)],
                      [-math.sin(yaw), math.cos(yaw)]])
    Rot2 = np.matrix([[math.cos(steer), math.sin(steer)],
                      [-math.sin(steer), math.cos(steer)]])

    fr_wheel = (fr_wheel.T * Rot2).T
    fl_wheel = (fl_wheel.T * Rot2).T
    fr_wheel[0, :] += WB
    fl_wheel[0, :] += WB

    fr_wheel = (fr_wheel.T * Rot1).T
    fl_wheel = (fl_wheel.T * Rot1).T

    outline = (outline.T * Rot1).T
    rr_wheel = (rr_wheel.T * Rot1).T
    rl_wheel = (rl_wheel.T * Rot1).T

    outline[0, :] += x
    outline[1, :] += y
    fr_wheel[0, :] += x
    fr_wheel[1, :] += y
    rr_wheel[0, :] += x
    rr_wheel[1, :] += y
    fl_wheel[0, :] += x
    fl_wheel[1, :] += y
    rl_wheel[0, :] += x
    rl_wheel[1, :] += y

    plt.plot(np.array(outline[0, :]).flatten(),
             np.array(outline[1, :]).flatten(), truckcolor)
    plt.plot(np.array(fr_wheel[0, :]).flatten(),
             np.array(fr_wheel[1, :]).flatten(), truckcolor)
    plt.plot(np.array(rr_wheel[0, :]).flatten(),
             np.array(rr_wheel[1, :]).flatten(), truckcolor)
    plt.plot(np.array(fl_wheel[0, :]).flatten(),
             np.array(fl_wheel[1, :]).flatten(), truckcolor)
    plt.plot(np.array(rl_wheel[0, :]).flatten(),
             np.array(rl_wheel[1, :]).flatten(), truckcolor)
    plt.plot(x, y, "*")
code/utils.py
YimingShu-teay/Car-Following