diff --git a/opendbc/car/tests/test_vehicle_model.py b/opendbc/car/tests/test_vehicle_model.py new file mode 100644 index 00000000000..b88a8dab4e0 --- /dev/null +++ b/opendbc/car/tests/test_vehicle_model.py @@ -0,0 +1,67 @@ +import pytest +import math + +import numpy as np + +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR +from opendbc.car.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices + + +class TestVehicleModel: + def setup_method(self): + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + self.VM = VehicleModel(CP) + + def test_round_trip_yaw_rate(self): + # TODO: fix VM to work at zero speed + for u in np.linspace(1, 30, num=10): + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + yr = self.VM.yaw_rate(sa, u, roll) + new_sa = self.VM.get_steer_from_yaw_rate(yr, u, roll) + + assert sa == pytest.approx(new_sa) + + def test_dyn_ss_sol_against_yaw_rate(self): + """Verify that the yaw_rate helper function matches the results + from the state space model.""" + + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for u in np.linspace(1, 30, num=10): + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + + # Compute yaw rate based on state space model + _, yr1 = dyn_ss_sol(sa, u, roll, self.VM) + + # Compute yaw rate using direct computations + yr2 = self.VM.yaw_rate(sa, u, roll) + assert float(yr1[0]) == pytest.approx(yr2) + + def test_syn_ss_sol_simulate(self): + """Verifies that dyn_ss_sol matches a simulation""" + + for roll in np.linspace(math.radians(-20), math.radians(20), num=11): + for u in np.linspace(1, 30, num=10): + A, B = create_dyn_state_matrices(u, self.VM) + + # Convert to discrete time system + dt = 0.01 + top = np.hstack((A, B)) + full = np.vstack((top, np.zeros_like(top))) * dt + Md = sum([np.linalg.matrix_power(full, k) / math.factorial(k) for k in range(25)]) + Ad = Md[:A.shape[0], :A.shape[1]] + Bd = Md[:A.shape[0], A.shape[1]:] + + for sa in np.linspace(math.radians(-20), math.radians(20), num=11): + inp = np.array([[sa], [roll]]) + + # Simulate for 1 second + x1 = np.zeros((2, 1)) + for _ in range(100): + x1 = Ad @ x1 + Bd @ inp + + # Compute steady state solution directly + x2 = dyn_ss_sol(sa, u, roll, self.VM) + + np.testing.assert_almost_equal(x1, x2, decimal=3) diff --git a/opendbc/car/vehicle_model.py b/opendbc/car/vehicle_model.py new file mode 100755 index 00000000000..6aa1ba38bb9 --- /dev/null +++ b/opendbc/car/vehicle_model.py @@ -0,0 +1,230 @@ +#!/usr/bin/env python3 +""" +Dynamic bicycle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani" + +The state is x = [v, r]^T +with v lateral speed [m/s], and r rotational speed [rad/s] + +The input u is the steering angle [rad], and roll [rad] + +The system is defined by +x_dot = A*x + B*u + +A depends on longitudinal speed, u [m/s], and vehicle parameters CP +""" + +import numpy as np +from numpy.linalg import solve + +from opendbc.car.structs import CarParams + +ACCELERATION_DUE_TO_GRAVITY = 9.8 + + +class VehicleModel: + def __init__(self, CP: CarParams): + """ + Args: + CP: Car Parameters + """ + # for math readability, convert long names car params into short names + self.m: float = CP.mass + self.j: float = CP.rotationalInertia + self.l: float = CP.wheelbase + self.aF: float = CP.centerToFront + self.aR: float = CP.wheelbase - CP.centerToFront + self.chi: float = CP.steerRatioRear + + self.cF_orig: float = CP.tireStiffnessFront + self.cR_orig: float = CP.tireStiffnessRear + self.update_params(1.0, CP.steerRatio) + + def update_params(self, stiffness_factor: float, steer_ratio: float) -> None: + """Update the vehicle model with a new stiffness factor and steer ratio""" + self.cF: float = stiffness_factor * self.cF_orig + self.cR: float = stiffness_factor * self.cR_orig + self.sR: float = steer_ratio + + def steady_state_sol(self, sa: float, u: float, roll: float) -> np.ndarray: + """Returns the steady state solution. + + If the speed is too low we can't use the dynamic model (tire slip is undefined), + we then have to use the kinematic model + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + 2x1 matrix with steady state solution (lateral speed, rotational speed) + """ + if u > 0.1: + return dyn_ss_sol(sa, u, roll, self) + else: + return kin_ss_sol(sa, u, self) + + def calc_curvature(self, sa: float, u: float, roll: float) -> float: + """Returns the curvature. Multiplied by the speed this will give the yaw rate. + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Curvature factor [1/m] + """ + return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u) + + def curvature_factor(self, u: float) -> float: + """Returns the curvature factor. + Multiplied by wheel angle (not steering wheel angle) this will give the curvature. + + Args: + u: Speed [m/s] + + Returns: + Curvature factor [1/m] + """ + sf = calc_slip_factor(self) + return (1. - self.chi) / (1. - sf * u**2) / self.l + + def get_steer_from_curvature(self, curv: float, u: float, roll: float) -> float: + """Calculates the required steering wheel angle for a given curvature + + Args: + curv: Desired curvature [1/m] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Steering wheel angle [rad] + """ + + return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u) + + def roll_compensation(self, roll: float, u: float) -> float: + """Calculates the roll-compensation to curvature + + Args: + roll: Road Roll [rad] + u: Speed [m/s] + + Returns: + Roll compensation curvature [rad] + """ + sf = calc_slip_factor(self) + + if abs(sf) < 1e-6: + return 0 + else: + return (ACCELERATION_DUE_TO_GRAVITY * roll) / ((1 / sf) - u**2) + + def get_steer_from_yaw_rate(self, yaw_rate: float, u: float, roll: float) -> float: + """Calculates the required steering wheel angle for a given yaw_rate + + Args: + yaw_rate: Desired yaw rate [rad/s] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Steering wheel angle [rad] + """ + curv = yaw_rate / u + return self.get_steer_from_curvature(curv, u, roll) + + def yaw_rate(self, sa: float, u: float, roll: float) -> float: + """Calculate yaw rate + + Args: + sa: Steering wheel angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + + Returns: + Yaw rate [rad/s] + """ + return self.calc_curvature(sa, u, roll) * u + + +def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray: + """Calculate the steady state solution at low speeds + At low speeds the tire slip is undefined, so a kinematic + model is used. + + Args: + sa: Steering angle [rad] + u: Speed [m/s] + VM: Vehicle model + + Returns: + 2x1 matrix with steady state solution + """ + K = np.zeros((2, 1)) + K[0, 0] = VM.aR / VM.sR / VM.l * u + K[1, 0] = 1. / VM.sR / VM.l * u + return K * sa + + +def create_dyn_state_matrices(u: float, VM: VehicleModel) -> tuple[np.ndarray, np.ndarray]: + """Returns the A and B matrix for the dynamics system + + Args: + u: Vehicle speed [m/s] + VM: Vehicle model + + Returns: + A tuple with the 2x2 A matrix, and 2x2 B matrix + + Parameters in the vehicle model: + cF: Tire stiffness Front [N/rad] + cR: Tire stiffness Front [N/rad] + aF: Distance from CG to front wheels [m] + aR: Distance from CG to rear wheels [m] + m: Mass [kg] + j: Rotational inertia [kg m^2] + sR: Steering ratio [-] + chi: Steer ratio rear [-] + """ + A = np.zeros((2, 2)) + B = np.zeros((2, 2)) + A[0, 0] = - (VM.cF + VM.cR) / (VM.m * u) + A[0, 1] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.m * u) - u + A[1, 0] = - (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.j * u) + A[1, 1] = - (VM.cF * VM.aF**2 + VM.cR * VM.aR**2) / (VM.j * u) + + # Steering input + B[0, 0] = (VM.cF + VM.chi * VM.cR) / VM.m / VM.sR + B[1, 0] = (VM.cF * VM.aF - VM.chi * VM.cR * VM.aR) / VM.j / VM.sR + + # Roll input + B[0, 1] = -ACCELERATION_DUE_TO_GRAVITY + + return A, B + + +def dyn_ss_sol(sa: float, u: float, roll: float, VM: VehicleModel) -> np.ndarray: + """Calculate the steady state solution when x_dot = 0, + Ax + Bu = 0 => x = -A^{-1} B u + + Args: + sa: Steering angle [rad] + u: Speed [m/s] + roll: Road Roll [rad] + VM: Vehicle model + + Returns: + 2x1 matrix with steady state solution + """ + A, B = create_dyn_state_matrices(u, VM) + inp = np.array([[sa], [roll]]) + return -solve(A, B) @ inp # type: ignore + + +def calc_slip_factor(VM: VehicleModel) -> float: + """The slip factor is a measure of how the curvature changes with speed + it's positive for Oversteering vehicle, negative (usual case) otherwise. + """ + return VM.m * (VM.cF * VM.aF - VM.cR * VM.aR) / (VM.l**2 * VM.cF * VM.cR)