You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
88 lines
2.9 KiB
88 lines
2.9 KiB
8 years ago
|
#!/usr/bin/env python
|
||
|
|
import numpy as np
|
||
|
|
from numpy.linalg import inv
|
||
|
|
from selfdrive.car.honda.interface import CarInterface
|
||
|
|
|
||
|
|
## dynamic bycicle model from "The Science of Vehicle Dynamics (2014), M. Guiggiani"##
|
||
|
|
|
||
|
|
# Xdot = A*X + B*U
|
||
|
|
# where X = [v, r], with v and r lateral speed and rotational speed, respectively
|
||
|
|
# and U is the steering angle (controller input)
|
||
|
|
#
|
||
|
|
# A depends on longitudinal speed, u, and vehicle parameters CP
|
||
|
|
|
||
|
|
def create_dyn_state_matrices(u, CP):
|
||
|
|
A = np.zeros((2,2))
|
||
|
|
B = np.zeros((2,1))
|
||
|
|
A[0,0] = - (CP.cF + CP.cR)/(CP.m*u)
|
||
|
|
A[0,1] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.m*u) - u
|
||
|
|
A[1,0] = - (CP.cF*CP.aF - CP.cR*CP.aR) / (CP.j*u)
|
||
|
|
A[1,1] = - (CP.cF*CP.aF**2 + CP.cR*CP.aR**2) / (CP.j*u)
|
||
|
|
B[0,0] = (CP.cF + CP.chi*CP.cR) / CP.m / CP.sR
|
||
|
|
B[1,0] = (CP.cF*CP.aF - CP.chi*CP.cR*CP.aR) / CP.j / CP.sR
|
||
|
|
return A, B
|
||
|
|
|
||
|
|
|
||
|
|
def kin_ss_sol(sa, u, CP):
|
||
|
|
# kinematic solution, useful when speed ~ 0
|
||
|
|
K = np.zeros((2,1))
|
||
|
|
K[0,0] = CP.aR / CP.sR / CP.l * u
|
||
|
|
K[1,0] = 1. / CP.sR / CP.l * u
|
||
|
|
return K * sa
|
||
|
|
|
||
|
|
|
||
|
|
def dyn_ss_sol(sa, u, CP):
|
||
|
|
# Dynamic solution, useful when speed > 0
|
||
|
|
A, B = create_dyn_state_matrices(u, CP)
|
||
|
|
return - np.matmul(inv(A), B) * sa
|
||
|
|
|
||
|
|
def calc_slip_factor(CP):
|
||
|
|
# the slip factor is a measure of how the curvature changes with speed
|
||
|
|
# it's positive for Oversteering vehicle, negative (usual case) otherwise
|
||
|
|
return CP.m * (CP.cF * CP.aF - CP.cR * CP.aR) / (CP.l**2 * CP.cF * CP.cR)
|
||
|
|
|
||
|
|
class VehicleModel(object):
|
||
|
|
def __init__(self, CP, init_state=np.asarray([[0.],[0.]])):
|
||
|
|
self.dt = 0.1
|
||
|
|
lookahead = 2. # s
|
||
|
|
self.steps = int(lookahead / self.dt)
|
||
|
|
self.update_state(init_state)
|
||
|
|
self.state_pred = np.zeros((self.steps, self.state.shape[0]))
|
||
|
|
self.CP = CP
|
||
|
|
|
||
|
|
def update_state(self, state):
|
||
|
|
self.state = state
|
||
|
|
|
||
|
|
def steady_state_sol(self, sa, u):
|
||
|
|
# if the speed is too small we can't use the dynamic model
|
||
|
|
# (tire slip is undefined), we then use the kinematic model
|
||
|
|
if u > 0.1:
|
||
|
|
return dyn_ss_sol(sa, u, self.CP)
|
||
|
|
else:
|
||
|
|
return kin_ss_sol(sa, u, self.CP)
|
||
|
|
|
||
|
|
def calc_curvature(self, sa, u):
|
||
|
|
# this formula can be derived from state equations in steady state conditions
|
||
|
|
sf = calc_slip_factor(self.CP)
|
||
|
|
return (1. - self.CP.chi)/(1. - sf * u**2) * sa / self.CP.sR / self.CP.l
|
||
|
|
|
||
|
|
def get_steer_from_curvature(self, curv, u):
|
||
|
|
# this function is the exact inverse of calc_curvature, returning steer angle given curvature
|
||
|
|
sf = calc_slip_factor(self.CP)
|
||
|
|
return self.CP.l * self.CP.sR * (1. - sf * u**2) / (1. - self.CP.chi) * curv
|
||
|
|
|
||
|
|
def state_prediction(self, sa, u):
|
||
|
|
# U is the matrix of the controls
|
||
|
|
# u is the long speed
|
||
|
|
A, B = create_dyn_state_matrices(u, self.CP)
|
||
|
|
return np.matmul((A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
|
||
|
|
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
# load car params
|
||
|
|
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
|
||
|
|
print CP
|
||
|
|
VM = VehicleModel(CP)
|
||
|
|
print VM.steady_state_sol(.1, 0.15)
|
||
|
|
|