retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

vehicle model types (#1631)

albatross
Willem Melching 2020-06-03 13:47:47 -07:00 committed by GitHub
parent ab83e48ec4
commit 2400417084
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 132 additions and 128 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.pre-commit-config.yaml
View File

@ -15,6 +15,7 @@ repos:
hooks:
- id: mypy
exclude: '^(pyextra)|(external)|(cereal)|(rednose)|(panda)|(laika)|(opendbc)|(laika_repo)|(rednose_repo)/'
additional_dependencies: ['git+https://github.com/numpy/numpy-stubs']
- repo: https://github.com/PyCQA/flake8
rev: master
hooks:

256
selfdrive/controls/lib/vehicle_model.py
View File

@ -14,9 +14,136 @@ A depends on longitudinal speed, u [m/s], and vehicle parameters CP
"""
import numpy as np
from numpy.linalg import solve
from typing import Tuple
from cereal import car
def create_dyn_state_matrices(u, VM):
class VehicleModel:
def __init__(self, CP: car.CarParams):
"""
Args:
CP: Car Parameters
"""
# for math readability, convert long names car params into short names
self.m = CP.mass
self.j = CP.rotationalInertia
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
self.chi = CP.steerRatioRear
self.cF_orig = CP.tireStiffnessFront
self.cR_orig = 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 = stiffness_factor * self.cF_orig
self.cR = stiffness_factor * self.cR_orig
self.sR = steer_ratio
def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
"""Returns the steady state solution.
If the speed is too small 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]
Returns:
2x1 matrix with steady state solution (lateral speed, rotational speed)
"""
if u > 0.1:
return dyn_ss_sol(sa, u, self)
else:
return kin_ss_sol(sa, u, self)
def calc_curvature(self, sa: float, u: 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]
Returns:
Curvature factor [1/m]
"""
return self.curvature_factor(u) * sa / self.sR
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) -> float:
"""Calculates the required steering wheel angle for a given curvature
Args:
curv: Desired curvature [1/m]
u: Speed [m/s]
Returns:
Steering wheel angle [rad]
"""
return curv * self.sR * 1.0 / self.curvature_factor(u)
def get_steer_from_yaw_rate(self, yaw_rate: float, u: 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]
Returns:
Steering wheel angle [rad]
"""
curv = yaw_rate / u
return self.get_steer_from_curvature(curv, u)
def yaw_rate(self, sa: float, u: float) -> float:
"""Calculate yaw rate
Args:
sa: Steering wheel angle [rad]
u: Speed [m/s]
Returns:
Yaw rate [rad/s]
"""
return self.calc_curvature(sa, u) * 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:
@ -47,26 +174,7 @@ def create_dyn_state_matrices(u, VM):
return A, B
def kin_ss_sol(sa, u, VM):
"""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 dyn_ss_sol(sa, u, VM):
def dyn_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
"""Calculate the steady state solution when x_dot = 0,
Ax + Bu = 0 => x = A^{-1} B u
@ -87,109 +195,3 @@ def calc_slip_factor(VM):
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)
class VehicleModel():
def __init__(self, CP):
"""
Args:
CP: Car Parameters
"""
# for math readability, convert long names car params into short names
self.m = CP.mass
self.j = CP.rotationalInertia
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
self.chi = CP.steerRatioRear
self.cF_orig = CP.tireStiffnessFront
self.cR_orig = CP.tireStiffnessRear
self.update_params(1.0, CP.steerRatio)
def update_params(self, stiffness_factor, steer_ratio):
"""Update the vehicle model with a new stiffness factor and steer ratio"""
self.cF = stiffness_factor * self.cF_orig
self.cR = stiffness_factor * self.cR_orig
self.sR = steer_ratio
def steady_state_sol(self, sa, u):
"""Returns the steady state solution.
If the speed is too small 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]
Returns:
2x1 matrix with steady state solution (lateral speed, rotational speed)
"""
if u > 0.1:
return dyn_ss_sol(sa, u, self)
else:
return kin_ss_sol(sa, u, self)
def calc_curvature(self, sa, u):
"""Returns the curvature. Multiplied by the speed this will give the yaw rate.
Args:
sa: Steering wheel angle [rad]
u: Speed [m/s]
Returns:
Curvature factor [1/m]
"""
return self.curvature_factor(u) * sa / self.sR
def curvature_factor(self, u):
"""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, u):
"""Calculates the required steering wheel angle for a given curvature
Args:
curv: Desired curvature [1/m]
u: Speed [m/s]
Returns:
Steering wheel angle [rad]
"""
return curv * self.sR * 1.0 / self.curvature_factor(u)
def get_steer_from_yaw_rate(self, yaw_rate, u):
"""Calculates the required steering wheel angle for a given yaw_rate
Args:
yaw_rate: Desired yaw rate [rad/s]
u: Speed [m/s]
Returns:
Steering wheel angle [rad]
"""
curv = yaw_rate / u
return self.get_steer_from_curvature(curv, u)
def yaw_rate(self, sa, u):
"""Calculate yaw rate
Args:
sa: Steering wheel angle [rad]
u: Speed [m/s]
Returns:
Yaw rate [rad/s]
"""
return self.calc_curvature(sa, u) * u

3
selfdrive/debug/internal/test_paramsd.py
View File

@ -4,6 +4,7 @@
import numpy as np
import math
from tqdm import tqdm
from typing import cast
import seaborn as sns
import matplotlib.pyplot as plt
@ -34,7 +35,7 @@ speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25
angle_offsets = math.radians(1.0) * np.ones_like(ts)
angle_offsets[ts > 60] = 0
steering_angles = np.radians(5 * np.cos(2 * np.pi * ts / 100.))
steering_angles = cast(np.ndarray, np.radians(5 * np.cos(2 * np.pi * ts / 100.)))
xs = []
ys = []