mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 20:03:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Road Roll Compensation Rebased (#23251)

Browse Source 
* first commit

* update refs
This commit is contained in:
HaraldSchafer
2021-12-16 17:34:12 -08:00
committed by GitHub
parent 285addeef2
commit cf466222f6
13 changed files with 177 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
selfdrive/car/ford/carcontroller.py
View File

@@ -32,7 +32,7 @@ class CarController():
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*math.pi/180., CS.out.vEgo)
curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:

1
selfdrive/car/honda/interface.py
View File

@@ -350,7 +350,6 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo)
buttonEvents = []

5
selfdrive/controls/controlsd.py
View File

@@ -631,8 +631,9 @@ class Controls:
# Curvature & Steering angle
params = self.sm['liveParameters']
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetAverageDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo)
steer_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg)
curvature = -self.VM.calc_curvature(steer_angle_without_offset, CS.vEgo, params.roll)
# controlsState
dat = messaging.new_message('controlsState')

2
selfdrive/controls/lib/latcontrol_angle.py
View File

@@ -18,7 +18,7 @@ class LatControlAngle():
angle_steers_des = float(CS.steeringAngleDeg)
else:
angle_log.active = True
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des += params.angleOffsetDeg
angle_log.saturated = False

4
selfdrive/controls/lib/latcontrol_indi.py
View File

@@ -93,11 +93,11 @@ class LatControlINDI():
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo)
steers_des = VM.get_steer_from_curvature(-curvature, CS.vEgo, params.roll)
steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)
if CS.vEgo < 0.3 or not active:

2
selfdrive/controls/lib/latcontrol_lqr.py
View File

@@ -53,7 +53,7 @@ class LatControlLQR():
# Subtract offset. Zero angle should correspond to zero torque
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors

2
selfdrive/controls/lib/latcontrol_pid.py
View File

@@ -21,7 +21,7 @@ class LatControlPID():
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
pid_log.steeringAngleDesiredDeg = angle_steers_des

70
selfdrive/controls/lib/tests/test_vehicle_model.py Executable file
View File

@@ -0,0 +1,70 @@
#!/usr/bin/env python3
import math
import unittest
import numpy as np
from control import StateSpace
from selfdrive.car.honda.interface import CarInterface
from selfdrive.car.honda.values import CAR
from selfdrive.controls.lib.vehicle_model import VehicleModel, dyn_ss_sol, create_dyn_state_matrices
class TestVehicleModel(unittest.TestCase):
def setUp(self):
CP = CarInterface.get_params(CAR.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)
self.assertAlmostEqual(sa, 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)
self.assertAlmostEqual(float(yr1), yr2)
def test_syn_ss_sol_simulate(self):
"""Verifies that dyn_ss_sol mathes 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
ss = StateSpace(A, B, np.eye(2), np.zeros((2, 2)))
ss = ss.sample(0.01)
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 = ss.A @ x1 + ss.B @ inp
# Compute steady state solution directly
x2 = dyn_ss_sol(sa, u, roll, self.VM)
np.testing.assert_almost_equal(x1, x2, decimal=3)
if __name__ == "__main__":
unittest.main()

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

@@ -5,7 +5,7 @@ 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]
The input u is the steering angle [rad], and roll [rad]
The system is defined by
x_dot = A*x + B*u
@@ -19,6 +19,9 @@ from numpy.linalg import solve
from cereal import car
ACCELERATION_DUE_TO_GRAVITY = 9.8
class VehicleModel:
def __init__(self, CP: car.CarParams):
"""
@@ -43,7 +46,7 @@ class VehicleModel:
self.cR = stiffness_factor * self.cR_orig
self.sR = steer_ratio
def steady_state_sol(self, sa: float, u: float) -> np.ndarray:
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),
@@ -52,26 +55,28 @@ class VehicleModel:
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, self)
return dyn_ss_sol(sa, u, roll, self)
else:
return kin_ss_sol(sa, u, self)
def calc_curvature(self, sa: float, u: float) -> float:
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
return (self.curvature_factor(u) * sa / self.sR) + self.roll_compensation(roll, u)
def curvature_factor(self, u: float) -> float:
"""Returns the curvature factor.
@@ -86,43 +91,63 @@ class VehicleModel:
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:
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.sR * 1.0 / self.curvature_factor(u)
return (curv - self.roll_compensation(roll, u)) * self.sR * 1.0 / self.curvature_factor(u)
def get_steer_from_yaw_rate(self, yaw_rate: float, u: float) -> float:
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)
return self.get_steer_from_curvature(curv, u, roll)
def yaw_rate(self, sa: float, u: float) -> float:
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) * u
return self.calc_curvature(sa, u, roll) * u
def kin_ss_sol(sa: float, u: float, VM: VehicleModel) -> np.ndarray:
@@ -152,7 +177,7 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
VM: Vehicle model
Returns:
A tuple with the 2x2 A matrix, and 2x1 B matrix
A tuple with the 2x2 A matrix, and 2x2 B matrix
Parameters in the vehicle model:
cF: Tire stiffness Front [N/rad]
@@ -165,30 +190,38 @@ def create_dyn_state_matrices(u: float, VM: VehicleModel) -> Tuple[np.ndarray, n
chi: Steer ratio rear [-]
"""
A = np.zeros((2, 2))
B = np.zeros((2, 1))
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, VM: VehicleModel) -> np.ndarray:
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)
return -solve(A, B) * sa
inp = np.array([[sa], [roll]])
return -solve(A, B) @ inp
def calc_slip_factor(VM):

19
selfdrive/locationd/models/car_kf.py
View File

@@ -5,6 +5,7 @@ from typing import Any, Dict
import numpy as np
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
from selfdrive.locationd.models.constants import ObservationKind
from selfdrive.swaglog import cloudlog
@@ -37,6 +38,7 @@ class States():
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
class CarKalman(KalmanFilter):
@@ -51,6 +53,7 @@ class CarKalman(KalmanFilter):
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
@@ -63,12 +66,14 @@ class CarKalman(KalmanFilter):
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
math.radians(1)**2,
])
P_initial = Q.copy()
obs_noise: Dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.01)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(5.0**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
@@ -87,7 +92,7 @@ class CarKalman(KalmanFilter):
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
# vehicle models comes from The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Model used is in 6.15 with formula from 6.198
@@ -106,6 +111,7 @@ class CarKalman(KalmanFilter):
cF, cR = x * cF_orig, x * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
@@ -122,8 +128,12 @@ class CarKalman(KalmanFilter):
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast)
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
@@ -145,11 +155,12 @@ class CarKalman(KalmanFilter):
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([x]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0): # pylint: disable=super-init-not-called
def __init__(self, generated_dir, steer_ratio=15, stiffness_factor=1, angle_offset=0, P_initial=None): # pylint: disable=super-init-not-called
dim_state = self.initial_x.shape[0]
dim_state_err = self.P_initial.shape[0]
x_init = self.initial_x
@@ -157,6 +168,8 @@ class CarKalman(KalmanFilter):
x_init[States.STIFFNESS] = stiffness_factor
x_init[States.ANGLE_OFFSET] = angle_offset
if P_initial is not None:
self.P_initial = P_initial
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, self.P_initial, dim_state, dim_state_err, global_vars=self.global_vars, logger=cloudlog)

3
selfdrive/locationd/models/constants.py
View File

@@ -38,6 +38,7 @@ class ObservationKind:
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
@@ -69,6 +70,8 @@ class ObservationKind:
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
]
@classmethod

38
selfdrive/locationd/paramsd.py
View File

@@ -16,10 +16,12 @@ from selfdrive.swaglog import cloudlog
MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s
ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits
ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10)
class ParamsLearner:
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset)
def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None):
self.kf = CarKalman(GENERATED_DIR, steer_ratio, stiffness_factor, angle_offset, P_initial)
self.kf.filter.set_global("mass", CP.mass)
self.kf.filter.set_global("rotational_inertia", CP.rotationalInertia)
@@ -30,9 +32,10 @@ class ParamsLearner:
self.active = False
self.speed = 0
self.speed = 0.0
self.roll = 0.0
self.steering_pressed = False
self.steering_angle = 0
self.steering_angle = 0.0
self.valid = True
@@ -41,16 +44,34 @@ class ParamsLearner:
yaw_rate = msg.angularVelocityCalibrated.value[2]
yaw_rate_std = msg.angularVelocityCalibrated.std[2]
localizer_roll = msg.orientationNED.value[0]
roll_valid = msg.orientationNED.valid and ROLL_MIN < localizer_roll < ROLL_MAX
if roll_valid:
roll = localizer_roll
roll_std = np.radians(1.0)
else:
# This is done to bound the road roll estimate when localizer values are invalid
roll = 0.0
roll_std = np.radians(10.0)
self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA)
yaw_rate_valid = msg.angularVelocityCalibrated.valid
yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s
yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s
if self.active:
if msg.inputsOK and msg.posenetOK and yaw_rate_valid:
if msg.inputsOK and msg.posenetOK:
if yaw_rate_valid:
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
self.kf.predict_and_observe(t,
ObservationKind.ROAD_FRAME_YAW_RATE,
np.array([[-yaw_rate]]),
np.array([np.atleast_2d(yaw_rate_std**2)]))
ObservationKind.ROAD_ROLL,
np.array([[self.roll]]),
np.array([np.atleast_2d(roll_std**2)]))
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]]))
elif which == 'carState':
@@ -152,6 +173,7 @@ def main(sm=None, pm=None):
msg.liveParameters.sensorValid = True
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.roll = float(x[States.ROAD_ROLL])
msg.liveParameters.angleOffsetAverageDeg = angle_offset_average
msg.liveParameters.angleOffsetDeg = angle_offset
msg.liveParameters.valid = all((

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
8118420b292f4c67ce7e196fc2121571da330e65
80430e515ea7ca44f2c73f047692d357856e74c1