Применение триангуляции к модели движения ТС по маршруту

using LinearAlgebra

# Параметры машины


# Геометрические [метры]
# Кузов
Vehicle_Front_Length = 4.0
Vehicle_Rear_Length = 4.0
Vehicle_Width = 4.0
Vehicle_Height = 2.0
Vehicle_Length = Vehicle_Front_Length + Vehicle_Rear_Length

# Колеса
Wheel_Radius = 0.85
Wheel_Depth = 0.5

# Массовые [кг]
Vehicle_Mass = 17000.0
Wheel_Mass = 1000.0

# Параметры груза
Cargo_Length = Vehicle_Front_Length + Vehicle_Rear_Length
Cargo_Width = Vehicle_Width
Cargo_Height = 1.0
Cargo_Mass = 8000.0

# Параметры подвески
Tire_Stiffness_Coefficient = 300000.0
Spring_Stiffness_Coefficient = 200000.0
Spring_Damping_Coefficient = 100000.0
Spring_Min_Length = 0.5
Spring_Undeformed_Length = 1.0
Spring_Max_Deformation = 2.0

# Расчёт положения центра масс
CM_Body = [0.0; Wheel_Radius + Spring_Undeformed_Length + Vehicle_Height/2; 0.0] # ЦМ кузова
CM_Cargo = [0.0; Wheel_Radius + Spring_Undeformed_Length + Vehicle_Height + Cargo_Height/2; 0.0] # ЦМ груза - настраивается
CM_Wheel_FL = [Vehicle_Front_Length; Wheel_Radius; -Vehicle_Width/2]
CM_Wheel_FR = [Vehicle_Front_Length; Wheel_Radius;  Vehicle_Width/2]
CM_Wheel_RL = [-Vehicle_Rear_Length; Wheel_Radius; -Vehicle_Width/2]
CM_Wheel_RR = [-Vehicle_Rear_Length; Wheel_Radius;  Vehicle_Width/2]

M_Total = Vehicle_Mass + Cargo_Mass + 4 * Wheel_Mass

CM = (Vehicle_Mass * CM_Body + Cargo_Mass * CM_Cargo + Wheel_Mass*(CM_Wheel_FL + CM_Wheel_FR + CM_Wheel_RL + CM_Wheel_RR))/M_Total

Mass_Lf = Vehicle_Front_Length - CM[1]
Mass_Lr = (Vehicle_Front_Length + Vehicle_Rear_Length) - Mass_Lf

Mass_Wr = Vehicle_Width/2 - CM[3]
Mass_Wl = Vehicle_Width - Mass_Wr

# Расчет пропорции нагрузки на колеса
# Площади сегментов платформы
S1 = Mass_Lf * Mass_Wl # FL
S2 = Mass_Lf * Mass_Wr # FR
S3 = Mass_Lr * Mass_Wl # RL
S4 = Mass_Lr * Mass_Wr # RR

m = Cargo_Mass + Vehicle_Mass

K = 1/S1 + 1/S2 + 1/S3 + 1/S4

m_fl = m * (1/S1)/K
m_fr = m * (1/S2)/K
m_rl = m * (1/S3)/K
m_rr = m * (1/S4)/K

# Тензор инерции
# Кузов
I_body = [Vehicle_Mass/12 * (Vehicle_Width^2 + Vehicle_Height^2) 0.0 0.0;
         0.0 Vehicle_Mass/12 * (Vehicle_Length^2 + Vehicle_Width^2) 0.0;
         0.0 0.0 Vehicle_Mass/12 * (Vehicle_Length^2 + Vehicle_Height^2)]
# Груз
I_cargo = [Cargo_Mass/12 * (Cargo_Width^2 + Cargo_Height^2) 0.0 0.0;
         0.0 Cargo_Mass/12 * (Cargo_Length^2 + Cargo_Width^2) 0.0;
         0.0 0.0 Cargo_Mass/12 * (Cargo_Length^2 + Cargo_Height^2)]
# Колеса
I_wheel = [Wheel_Mass/12 * (3 * Wheel_Radius^2 + Wheel_Depth^2) 0.0 0.0;
           0.0 Wheel_Mass/12 * (3 * Wheel_Radius^2 + Wheel_Depth^2) 0.0;
           0.0 0.0 Wheel_Mass/2 * Wheel_Radius^2]

# Тензор инерции в системе центра масс
d_v_cm = CM_Body - CM
I_body_CM = I_body + Vehicle_Mass*(dot(d_v_cm, d_v_cm) * I(3) - d_v_cm*d_v_cm')
d_c_cm = CM_Cargo - CM
I_cargo_CM = I_cargo + Cargo_Mass * (dot(d_c_cm, d_c_cm) * I(3) - d_c_cm*d_c_cm')
dv_fl_cm = CM_Wheel_FL - CM
dv_fr_cm = CM_Wheel_FR - CM
dv_rl_cm = CM_Wheel_RL - CM
dv_rr_cm = CM_Wheel_RR - CM
I_wheel_fl_CM = I_wheel + Wheel_Mass * (dot(dv_fl_cm, dv_fl_cm) * I(3) - dv_fl_cm*dv_fl_cm')
I_wheel_fr_CM = I_wheel + Wheel_Mass * (dot(dv_fr_cm, dv_fr_cm) * I(3) - dv_fr_cm*dv_fr_cm')
I_wheel_rl_CM = I_wheel + Wheel_Mass * (dot(dv_rl_cm, dv_rl_cm) * I(3) - dv_rl_cm*dv_rl_cm')
I_wheel_rr_CM = I_wheel + Wheel_Mass * (dot(dv_rr_cm, dv_rr_cm) * I(3) - dv_rr_cm*dv_rr_cm')

I_sys = I_body_CM + I_cargo_CM + I_wheel_fl_CM + I_wheel_fr_CM + I_wheel_rl_CM + I_wheel_rr_CM

# Гравитация
g_acceleration = 9.815

# Сцепление с дорогой
mu_global = 0.8

# Максимальный момент силовой установки
Max_Torque = 12000.0

# Максимальный угол поворота колес (осей)
Max_Steering_Angle = deg2rad(60)

# Постоянная времени рулевого механизма
Steering_Time_Period = 3.0

# Постоянная времени силовой установки
Powertrain_Time_Period = 1.0

# Максимальная скорость ТС
Vehicle_Max_Speed = 45/3.6 # [км/ч], деленные на 3.6 для приведения к м/с

# Коэффициент сопротивления
Dissipative_Coeff = Max_Torque/(4 * Wheel_Radius * Vehicle_Max_Speed^4)

# Начальные условия
Initial_Conditions = [
    route[1,1],           # X
    9.5,                  # Y
    -route[1,2],          # Z
    0.000,                # dX
    0.000,                # dY
    0.000,                # dZ
    0.000,                # Vg
    atan(-(route[2,2]-route[1,2])/(route[2,1]-route[1,1])), # Psi
    0.000,                # Theta
    0.000,                # Gamma
    0.000,                # dPsi
    0.000,                # dTheta
    0.000,                # dGamma
    Wheel_Radius + Spring_Undeformed_Length + 0.01, # Y_body_FL
    Wheel_Radius + Spring_Undeformed_Length + 0.01, # Y_body_FR
    Wheel_Radius + Spring_Undeformed_Length + 0.01, # Y_body_RL
    Wheel_Radius + Spring_Undeformed_Length + 0.01, # Y_body_RR
    0.000,                # d_Y_body_FL
    0.000,                # d_Y_body_FR
    0.000,                # d_Y_body_RL
    0.000,                # d_Y_body_RR
    9.5 - Spring_Undeformed_Length + 0.01, # Y_wheel_FL
    9.5 - Spring_Undeformed_Length + 0.01, # Y_wheel_FR
    9.5 - Spring_Undeformed_Length + 0.01, # Y_wheel_RL
    9.5 - Spring_Undeformed_Length + 0.01, # Y_wheel_RR
    0.0,                  # dY_wheel_FL
    0.0,                  # dY_wheel_FR
    0.0,                  # dY_wheel_RL
    0.0                      # dY_wheel_RR
]

# Структура с параметрами машины
In_Params = (
    # Геометрические параметры автомобиля и колёс
    Wheel_L_Front = Mass_Lf,
    Wheel_L_Rear = Mass_Lr,
    L_body = Vehicle_Front_Length + Vehicle_Rear_Length,
    W_body = Vehicle_Width,
    Wheel_W_Right = Mass_Wr,
    Wheel_W_Left = Mass_Wl,
    FL_MProp = m_fl/m,
    FR_MProp = m_fr/m,
    RL_MProp = m_rl/m,
    RR_MProp = m_rr/m,
    Inertia_X = I_sys[1,1],
    Inertia_Z = I_sys[3,3],
    H_body = Vehicle_Height,
    R_wheel = Wheel_Radius,
    D_wheel = Wheel_Depth,
    Body_CM_Pos = [0.0; Vehicle_Height/2; 0.0],
    Wheel_FL_CM_X0Z = [Vehicle_Front_Length; 0.0; -Vehicle_Width/2],
    Wheel_FR_CM_X0Z = [Vehicle_Front_Length; 0.0; Vehicle_Width/2],
    Wheel_RL_CM_X0Z = [-Vehicle_Rear_Length; 0.0; -Vehicle_Width/2],
    Wheel_RR_CM_X0Z = [-Vehicle_Rear_Length; 0.0; Vehicle_Width/2],
    # Массовые параметры автомобиля и колес
    m_wheel = Wheel_Mass,
    M_body = Vehicle_Mass,
    # Параметры груза
    m_cargo = Cargo_Mass,
    L_cargo = Cargo_Length,
    W_cargo = Cargo_Width,
    H_cargo = Cargo_Height,
    # Параметры подвески
    K_Tire = Tire_Stiffness_Coefficient,
    K_Suspension = Spring_Stiffness_Coefficient,
    B_Suspension = Spring_Damping_Coefficient,
    L0_suspension = Spring_Undeformed_Length,
    LMax_suspension = Spring_Max_Deformation,
    LMin_suspension = Spring_Min_Length,
    # Окружающая среда
    gravity_acc = g_acceleration,
    mu_road = mu_global,
    # Параметры силовой установки
    PT_MaxTorque = Max_Torque,
    PT_Tau = Powertrain_Time_Period,
    PT_MaxSpeed = Vehicle_Max_Speed,
    PT_Damping = Dissipative_Coeff,
    # Параметры рулевого управления
    STR_MaxAngle = Max_Steering_Angle,
    STR_Tau = Steering_Time_Period,
    # Начальные условия моделирования
    InitialConditions = Initial_Conditions,
    # Параметры карты
    X_grid = gridX,
    Y_grid = gridY,
    Z_grid = gridZ,
    route = route
)

# Структура с параметрами системы управления
Ctrl_Params = (
    V_cruiser = 30/3.6,
    V_maneuver = 4/3.6,
    Brk_distance = 40.,
    Waypoint_radius = 2.5,
    Ang_Err_Spd_Thres = deg2rad(60.0),
    dist2spd_P = 1.2,
    dist2spd_I = 0.1,
    accCtrl_P = 1.2,
    accCtrl_I = 0.7,
    brkCtrl_P = 5.,
    brkCtrl_I = 1.2,
    K_Delta_Angle = 0.0025,
    ang2angSpd_P = 4.4,
    ang2angSpd_I = 0.,
    angSpdCtrl_P = 4.8,
    angSpdCtrl_I = 0.25,
    dPsi_max = 0.75
)