"""
Module implementing vehicle model to translate different values into others.
Translate steering wheel angle to ackermann angle and the other way around
as well as translating acceleration to necessary motor torque and service brake pressure.
"""
from typing import Union
import numpy as np
import numpy.typing as npt
from scipy.interpolate import interp1d
[docs]class VehicleModel:
"""
Vehicle Model class to implement functionalities to translate different values into others.
Translate steering wheel angle to ackermann angle and the other way around
as well as translating acceleration to necessary motor torque and service brake pressure.
Attributes
----------
m_ges : float
Vehicle mass [kg].
r : float
Wheel radius [m].
ue : float
Ratio of gear on rear axle [i].
normalization : float
Normalization of ackermann_angle.
track_width : float
Track width of vehicle [m].
deadband : float
tbd.
l_arm : float
Length of steering arm [m].
l_rack : float
Rack casing length [m].
l_tie : float
Tie rod length [m]
D : float
Distance between rack an axes [m].
r_pin : float
Pin radius [m].
steering_wheel_angle_range : float
Range of steering [degrees] from most left to most right steering wheel angle.
steering_wheel_angle_offset : float
Offset of steering wheel angle, where the vehicle really drives straight.
steering_wheel_to_ackermann_angle_factor : float
Factor to convert steering wheel angles [rad] to ackermann angle [rad].
A_HBZ_f : float
Kolbenflaeche Hauptbremszylinder vorne [m^2].
A_HBZ_r : float
Kolbenflaeche Hauptbremszylinder hinten [m^2].
A_RBZ : float
Zylinderflaeche Radbremse [m^2].
n_RBZ_f : float
Anzahl Radbremszylinder vorne.
n_RBZ_r : float
Anzahl Radbremszylinder hinten.
r_Bf : float
Effektivradius Bremssattel vorne [m].
r_Br : float
Effektivradius Bremssattel hinten [m].
C : float
Bremskoeffizient [1].
nu_Sattel : float
Wirkungsgrad Bremssattel [1].
C_BS : float
Temperaturkoeffizient Bremsscheibe [J/(kg*K)].
m_BS : float
Masse Bremsscheibe [kg].
A_PNEU_f : float
Kolbenflaeche Pneumatik vorne [m^2].
A_PNEU_r : float
Kolbenflaeche Pneumatik hinten [m^2].
A_HYD : float
Kolbenflaeche Hydraulik [m^2].
r_reib_f : float
wirksamer Reibradius vorne [m].
r_reib_r : float
wirksamer Reibradius hinten [m].
nu_mech : float
mechanischer Wirkungsrad.
ackermann_to_steering_wheel_angle_lut : interp1d
LookUpTable between ackermann angle [rad] and steering wheel angle [degrees].
steering_wheel_to_ackermann_angle_lut : interp1d
LookUpTable between steering wheel angle [degrees] and ackermann angle [rad].
"""
def __init__(self, m: float, radius: float, gear_ratio: float, steering_wheel_angle_range: float,
steering_wheel_angle_offset: float, steering_wheel_to_ackermann_angle_factor: float):
"""
Initialize Vehicle Model.
Static Parmaters derived by Benjamin Deml, Birk Blumhoff and Oliver Zbaranski.
Parameters
----------
m : float
Vehicle mass [kg].
radius : float
Wheel radius [m].
gear_ratio : float
Ratio of gear on rear axle.
steering_wheel_angle_range : float
Range of steering [degrees] from most left to most right steering wheel angle.
steering_wheel_angle_offset : float
Offset of steering wheel angle, where the vehicle really drives straight.
steering_wheel_to_ackermann_angle_factor : float
Factor to convert steering wheel angles [rad] to ackermann angle [rad].
"""
# Initialize parameters
# Generale Paramaters
self.m_ges = m # Vehicle mass [kg]
self.r = radius # Static tire radius [m]
self.ue = gear_ratio # Gear ratio [i]
# Steering parameters
self.normalization = 0.030084524000747237 # Normalization of ackermann_angle
self.track_width = 1.250 # Track width [m]
self.deadband = 0
self.l_arm = 0.06 # Length of steering arm [m]
self.l_rack = 0.36576 # Rack casing length [m]
self.l_tie = 0.392811 # Tie rod length [m]
self.D = 0.1460910 # Distance between rack an axes [m]
self.r_pin = 0.015987 # Pinn radius [m]
self.steering_wheel_angle_range = np.radians(steering_wheel_angle_range)
# Steering radius
self.steering_wheel_angle_offset = steering_wheel_angle_offset
# Offset between measured steering wheel angle and steering wheel angle to drive completly straight
self.steering_to_ackermann = steering_wheel_to_ackermann_angle_factor
# Approximate factor between steering wheel and ackermann angle
# Brake parameters
self.A_HBZ_f = 153.94e-06 # Kolbenflaeche Hauptbremszylinder vorne [m^2]
self.A_HBZ_r = 286e-06 # Kolbenflaeche Hauptbremszylinder hinten [m^2]
self.A_RBZ = 490e-06 # Zylinderflaeche Radbremse [m^2]
self.n_RBZ_f = 2 # Anzahl Radbremszylinder vorne
self.n_RBZ_r = 1 # Anzahl Radbremszylinder hinten
self.r_Bf = 107.25e-03 # Effektivradius Bremssattel vorne [m]
self.r_Br = 102.25e-03 # Effektivradius Bremssattel hinten [m]
self.C = 1 # Bremskoeffizient [1]
self.nu_Sattel = 0.75 # Wirkungsgrad Bremssattel [1]
self.C_BS = 460 # Temperaturkoeffizient Bremsscheibe [J/(kg*K)]
self.m_BS = 0.5 # Masse Bremsscheibe [kg]
# CM22x addition
self.A_PNEU_f = 2463e-06 # Kolbenflaeche Pneumatik vorne [m^2]
self.A_PNEU_r = 1520.53e-06 # Kolbenflaeche Pneumatik hinten [m^2]
self.A_HYD = 153.95e-06 # Kolbenflaeche Hydraulik [m^2]
self.r_reib_f = 0.1035 # wirksamer Reibradius vorne [m]
self.r_reib_r = 0.096 # wirksamer Reibradius hinten [m]
self.nu_mech = 0.8 # mechanischer Wirkungsrad
self.prepare_steering_look_up_tables()
self.prepare_acceleration_look_up_tables()
[docs] def steering_wheel_to_ackermann_angle_precise(self, steering_wheel_angle: Union[npt.NDArray, float]
) -> Union[npt.NDArray, float]:
"""
Precisely calculate ackermann angle(s) [rad] for given steering wheel angle(s) [rad] based on steering model.
Steering Model derived by Birk Blumhoff. Overwritten by linear relationship
defined by an steering wheel offset and steering wheel angle factor.
Parameters
----------
steering_wheel_angle : Union[npt.NDArray, float]
Single steering wheel angle or multiple steering wheel angles [rad].
Returns
-------
Union[npt.NDArray, float]
Converted ackermann angle or multiple steackermann angles [rad].
"""
"""
outdated calculation based on a steering model:
# check if the setvalue is valid (not outside the steering kinematcs range)
steering_wheel_angle = np.clip(steering_wheel_angle, -0.5 * self.steering_wheel_angle_range,
0.5 * self.steering_wheel_angle_range)
# calculate left side
delta_p = self.r_pin * steering_wheel_angle # ggf Normalisierung hinzufügen
l1_l = (self.track_width - self.l_rack) / 2 - delta_p
l2_l = np.sqrt(l1_l**2 + self.D**2)
beta_l = np.pi / 2 - np.arctan(self.D / l1_l) - np.arccos((self.l_arm**2 + l2_l**2 - self.l_tie**2)
/ (2 * self.l_arm * l2_l))
# calculate left tire Ackermann-angle
delta_l = -(beta_l + self.normalization)
# calculate right side
delta_p = -self.r_pin * steering_wheel_angle # ggf Normalisierung hinzufügen
l1_r = (self.track_width - self.l_rack) / 2 - delta_p
l2_r = np.sqrt(l1_r**2 + self.D**2)
beta_r = np.pi / 2 - np.arctan(self.D / l1_r) - np.arccos((self.l_arm**2 + l2_r**2 - self.l_tie**2)
/ (2 * self.l_arm * l2_r))
# calculate left tire Ackermann-angle
delta_r = (beta_r + self.normalization)
"""
# OVERRIDE STEERING MODEL WITH SIMPLE FACTOR
delta_l = steering_wheel_angle / self.steering_to_ackermann
delta_r = steering_wheel_angle / self.steering_to_ackermann
# return both tire-angles [rad]
return np.c_[delta_l, delta_r]
[docs] def prepare_steering_look_up_tables(self):
"""Prepare look up table between steering wheel angle and ackermann angle and vice versa."""
steering_wheel_angle = np.linspace(-0.51 * self.steering_wheel_angle_range,
0.51 * self.steering_wheel_angle_range, 1000, endpoint=True)
ackermann_angle = np.sum(self.steering_wheel_to_ackermann_angle_precise(steering_wheel_angle), axis=1) / 2
self.ackermann_to_steering_wheel_angle_lut = interp1d(
ackermann_angle, np.degrees(steering_wheel_angle) + self.steering_wheel_angle_offset)
self.steering_wheel_to_ackermann_angle_lut = interp1d(
np.degrees(steering_wheel_angle) + self.steering_wheel_angle_offset, ackermann_angle)
[docs] def acceleration_to_motor_torque(self, acceleration: Union[npt.NDArray, float]
) -> Union[npt.NDArray, float]:
"""
Translate reference acceleration [m/s^2] into necessary motor torque for total powertrain [Nm].
Calculations derived by Oliver Zbaranski.
Parameters
----------
acceleration : float
Reference vehicle acceleration in x direction [m/s^2].
Returns
-------
float
Necessary motor torque for total powertrain [Nm].
"""
# Desired force
F_x = acceleration * self.m_ges
# Desired tire torque
M_tire = F_x * self.r
# Desired motor torque
M_motor = M_tire / self.ue
return M_motor / 2.0
[docs] def acceleration_to_pneumatic_pressure(self, brake_acceleration: Union[npt.NDArray, float]
) -> Union[npt.NDArray, float]:
"""
Translate reference acceleration [m/s^2] into necessary pneumatic pressure for the service brake [bar].
Calculations based on Benjamin Deml.
Parameters
----------
acceleration : float
Reference vehicle acceleration in x direction [m/s^2].
Returns
-------
float
Necessary pneumatic pressure for the service brake [bar].
"""
# Ratio of front brake force and rear brake force
ratio = (self.r_reib_f * self.n_RBZ_f * self.A_PNEU_f) / (self.r_reib_r * self.n_RBZ_r * self.A_PNEU_r)
# Desired Force
F_x = brake_acceleration * self.m_ges
# Front brake torque
M_x_f = F_x * self.r * ratio / (ratio + 1.0)
# Rear brake torque
M_x_r = F_x * self.r * 1 / (ratio + 1.0)
# Hydraulic pressures per wheel
p_hyd_f = M_x_f / (self.A_RBZ * self.nu_Sattel * self.C * self.r_reib_f * self.n_RBZ_f * 2)
p_hyd_r = M_x_r / (self.A_RBZ * self.nu_Sattel * self.C * self.r_reib_r * self.n_RBZ_r * 2)
# Pneumatic pressures per wheel in bar (should be equal)
p_pneu_f = p_hyd_f * self.A_HYD / (self.A_PNEU_f * self.nu_mech * 100000.00)
p_pneu_r = p_hyd_r * self.A_HYD / (self.A_PNEU_r * self.nu_mech * 100000.00) # noqa: F841
return p_pneu_f
[docs] def prepare_acceleration_look_up_tables(self):
acceleration = np.linspace(0, 10, 2, endpoint=True)
motor_torque = self.acceleration_to_motor_torque(acceleration)
brake_acceleration = np.linspace(0, 23, 2, endpoint=True)
pneumatic_pressure = self.acceleration_to_pneumatic_pressure(brake_acceleration)
self.motor_torque_lut = interp1d(acceleration, motor_torque)
self.pneumatic_pressure_lut = interp1d(brake_acceleration, pneumatic_pressure)
[docs] def motor_torque(self, acceleration: float) -> float:
"""
Translate reference acceleration [m/s^2] into necessary motor torque for total powertrain [Nm].
Calculations derived by Oliver Zbaranski.
Parameters
----------
acceleration : float
Reference vehicle acceleration in x direction [m/s^2].
Returns
-------
float
Necessary motor torque for total powertrain [Nm].
"""
# Lower limit
acceleration = np.maximum(0, acceleration)
return self.motor_torque_lut(acceleration)
[docs] def pneumatic_pressure(self, acceleration: float) -> float:
"""
Translate reference acceleration [m/s^2] into necessary pneumatic pressure for the service brake [bar].
Calculations based on Benjamin Deml.
Parameters
----------
acceleration : float
Reference vehicle acceleration in x direction [m/s^2].
Returns
-------
float
Necessary pneumatic pressure for the service brake [bar].
"""
brake_acceleration = - np.minimum(0, acceleration)
return self.pneumatic_pressure_lut(brake_acceleration)
[docs] def ackermann_to_steering_wheel_angle(self, ackermann_angle: float) -> float:
"""
Translate ackermann angle [rad] to steering wheel angle [degrees].
Parameters
----------
ackermann_angle : float
Ackermann angle [rad].
Returns
-------
float
Steering wheel angle [degrees].
"""
return self.ackermann_to_steering_wheel_angle_lut(ackermann_angle)
[docs] def steering_wheel_to_ackermann_angle(self, steering_wheel_angle: float) -> float:
"""
Translate steering wheel angle [degrees] to ackermann angle [rad].
Uses the previously calculated look up table.
Parameters
----------
steering_wheel_angle : float
Steering wheel angle [degrees].
Returns
-------
float
Respective ackermann angle [rad.]
"""
return self.steering_wheel_to_ackermann_angle_lut(steering_wheel_angle)
if __name__ == '__main__':
print("initializing vehicle model")
v = VehicleModel(m=245, radius=0.225, gear_ratio=6, steering_wheel_angle_range=240, steering_wheel_angle_offset=-2.75,
steering_wheel_to_ackermann_angle_factor=3.96)
print("finished")
print(v.pneumatic_pressure(-2))
print(v.motor_torque(2))
print(v.pneumatic_pressure(0))
print(v.motor_torque(0))