二维跟驰模型——TRC论文代码复现

二维交通流问题的二维车辆跟驰模型

Transportation Research Part C一篇关于交通流车辆跟驰理论的论文
以此作为个人学习参考
仅仅浅尝未曾识得顶刊精妙，期望能有更多进步！

二维跟车模型可解决以下传统的跟车模型难以简单模拟的交通流问题：
（i）在HOV车道和分流瓶颈中经常观察到的横向摩擦效应
（ii）合流瓶颈处的松弛现象
（iii）因换道而发生的事故
（iv）自动驾驶车辆（AV）的交通模型
基于社会力范式提出了一个简单的二维微观跟车模型，并建立了再现这些现象的模拟实验。更好地理解人类驾驶员在横向维度的行为可以转化为改进自主驾驶算法，使其人性化。

# coding=utf-8

import numpy as np
import math
import matplotlib.pyplot as plt
import matplotlib

matplotlib.rc('font', family='MicroSoft YaHei', weight='bold')


class Car:
    time_now = 10
    car_list = []

    def __init__(self, lx, ly, vx, vy):
        self.loc_x = lx
        self.loc_y = ly
        self.v_x = vx
        self.v_y = vy

    def update_loc(self, locx_det, locy_det):
        self.loc_x = round(self.loc_x + locx_det, 3)
        self.loc_y = round(self.loc_y + locy_det, 3)

    def update_v(self, vx_det, vy_det):
        self.v_x = round(self.v_x + vx_det, 3)
        self.v_y = round(self.v_y + vy_det, 3)




k2 = 1 / 4
k1 = 2 * math.sqrt(k2)
x_xing = 2
c1 = 1 / 10
c2 = 33 / 64
c3 = 9 / 64
tao = 8 / 10
s_r = 196 / 9
V_max = 26.5

line_cen_HOV = -2


def cal_frik(car_i, car_k):
    # 车辆间排斥力
    v_i = np.array([[car_i.v_x], [car_i.v_y]])
    v_k = np.array([[car_k.v_x], [car_k.v_y]])
    # print(v_k-v_i)

    v_i_t = np.linalg.norm(v_i)

    q_v = (v_i_t * tao + s_r) / x_xing

    Q = np.array([[1, 0], [0, q_v]])
    # print('Q为：',Q)
    Q_ni = np.linalg.inv(Q)

    x_i = np.array([[car_i.loc_x], [car_i.loc_y]])
    x_k = np.array([[car_k.loc_x], [car_k.loc_y]])

    r_ik = np.dot(Q_ni, x_k - x_i)
    # print(r_ik)

    r_ik_dw = r_ik / np.linalg.norm(r_ik)
    # print(r_ik_dw)
    # print(np.dot(Q_ni,v_k-v_i))

    v_i_det = np.dot(np.transpose(np.dot(Q_ni, v_k - v_i)), r_ik_dw)
    # print(v_i_det)

    # print(np.dot(Q, r_ik_dw))
    # print(c2 * v_i_det + (np.linalg.norm(r_ik) - x_xing) * c3)

    f_r_ik = np.dot(Q, r_ik_dw) * min(0, c2 * v_i_det + (np.linalg.norm(r_ik) - x_xing) * c3)
    # print('i车的排斥力：', f_r_ik)
    return f_r_ik


# cal_frik(car_1,car_2)


def cal_faik(car_i):
    # 车辆希望加速的力
    vmax_det = V_max - car_i.v_y
    f_a_i = np.array([[0], [c1 * vmax_det]])
    print('i车的加速力：', f_a_i)
    return f_a_i


# cal_faik(car_1)


def cal_flik(car_i):
    # 车辆希望行驶在车道中央的力
    v_i_x = car_i.v_x
    x_l_det = car_i.loc_x - line_cen_HOV
    f_l_i = np.array([[-v_i_x * k1 - x_l_det * k2], [0]])
    # print('i车的道中心力：', f_l_i)
    return f_l_i


# cal_flik(car_1)

def cal_fsumik(car_i, car_k):
    f_sum_ik = cal_faik(car_i) + cal_flik(car_i) + cal_frik(car_i, car_k)
    # print('%s车受力总和:' % car_i, f_sum_ik)
    print('i车受力总和:', f_sum_ik)
    return f_sum_ik

def cal_v_ave(list):
    vel_num = 0
    v_sum = 0
    for i in list:
        vel_num += 1
        v_sum += i
    v_ave = v_sum / vel_num
    return v_ave

v_hov_list = []
v_gp_list =[]


for vv in np.arange(0,30,1):

    car_1 = Car(-2.5, 20, 0.1, 25)
    car_2 = Car(1, 200, -1, vv)
    # print(car_2.v_y)

    x_list = []
    y_list = []
    v_list = []
    t_list = []

    for i in range(20):
        # print(car_1.__dict__)
        c1_vx_det = cal_fsumik(car_1, car_2)[0][0]
        c1_vy_det = cal_fsumik(car_1, car_2)[1][0]
        # print(c1_vx_det)

        car_1.update_loc(car_1.v_x, car_1.v_y)
        car_1.update_v(c1_vx_det, c1_vy_det)

        # print(car_1.__dict__)

        x_list.append(car_1.loc_x)
        y_list.append(car_1.loc_y)

        v_1mo = math.sqrt(math.pow(car_1.v_x,2)+math.pow(car_1.v_y,2))  #速度标量
        # print(v_1mo)

        v_list.append(v_1mo)
        t_list.append(i)

    v_avg = cal_v_ave(v_list)
    print(v_list)
    print(v_avg)

    v_hov_list.append(v_avg)
    v_gp_list.append(vv)






def draw_sk():
    x_dash_1 = [-4, -4]
    y_dash_1 = [0, 600]
    x_dash_2 = [0, 0]
    y_dash_2 = [0, 600]
    x_dash_3 = [4, 4]
    y_dash_3 = [0, 600]

    plt.title('空间轨迹图')
    plt.xlabel('道路空间x')
    plt.ylabel('道路空间y')
    plt.scatter(x_list, y_list, label='car_1=初始(-2.5, 20, 0.1, 24)')
    plt.scatter(car_2.loc_x, car_2.loc_y, label='car_2=初始(%.0f,%.0f,%.0f,%.0f)' %(car_2.loc_x,car_2.loc_y,car_2.v_x,car_2.v_y))
    # car_1 = Car(-2.5, 20, 0.1, 24)
    # car_2 = Car(1, 200, -1, 17)
    plt.scatter(car_2.loc_x, car_2.loc_y)
    plt.plot(x_dash_1, y_dash_1, linestyle='dashed', color='black')
    plt.plot(x_dash_2, y_dash_2, linestyle='dashed', color='black')
    plt.plot(x_dash_3, y_dash_3, linestyle='dashed', color='black')
    plt.legend()
    plt.show()

# draw_sk()

def draw_vt():
    plt.title('速度受横向摩擦影响变化图')
    plt.xlabel('HOV车道车速')
    plt.ylabel('时间步')
    plt.scatter(t_list,v_list)
    plt.show()

# draw_vt()


def draw_HG_v():
    plt.title('HOV车道和GP车道速度关系图')
    plt.xlabel('GP车道车速')
    plt.ylabel('HOV车道车速')
    plt.scatter(v_gp_list, v_hov_list)
    plt.show()

draw_HG_v()