mirror of
https://github.com/MoreTore/openpilot.git
synced 2026-10-01 03:43:51 +08:00
Lateral torque-based control with roll on TSS2 corolla and TSSP rav4 (#24260)
* Initial commit * Fix bugs * Need more torque rate * Cleanup cray cray control * Write nicely * Chiiil * Not relevant for cray cray control * Do some logging * Seems like it has more torque than I thought * Bit more feedforward * Tune change * Retune * Retune * Little more chill * Add coroll * Add corolla * Give craycray a good name * Update to proper logging * D to the PI * Should be in radians * Add d * Start oscillations * Add D term * Only change torque rate limits for new tune * Add d logging * Should be enough * Wrong sign in D * Downtune a little * Needed to prevent faults * Add lqr rav4 to tune * Try derivative again * Data based retune * Data based retune * add friction compensation * Doesnt need too much P with friction comp * remove lqr * Remove kd * Fix tests * fix tests * Too much error * Get roll induced error under 1cm/deg * Too much jitter * Do roll comp * Add ki * Final update * Update refs * Cleanup latcontrol_torque a little more
This commit is contained in:
HaraldSchafer
@@ -20,8 +20,8 @@ from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
|
||||
from selfdrive.controls.lib.longcontrol import LongControl
|
||||
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
||||
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
|
||||
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from selfdrive.controls.lib.events import Events, ET
|
||||
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
@@ -144,8 +144,8 @@ class Controls:
|
||||
self.LaC = LatControlPID(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'indi':
|
||||
self.LaC = LatControlINDI(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'lqr':
|
||||
self.LaC = LatControlLQR(self.CP, self.CI)
|
||||
elif self.CP.lateralTuning.which() == 'torque':
|
||||
self.LaC = LatControlTorque(self.CP, self.CI)
|
||||
|
||||
self.initialized = False
|
||||
self.state = State.disabled
|
||||
@@ -566,7 +566,7 @@ class Controls:
|
||||
lat_plan.curvatureRates)
|
||||
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.CP, self.VM,
|
||||
params, self.last_actuators, desired_curvature,
|
||||
desired_curvature_rate)
|
||||
desired_curvature_rate, self.sm['liveLocationKalman'])
|
||||
else:
|
||||
lac_log = log.ControlsState.LateralDebugState.new_message()
|
||||
if self.sm.rcv_frame['testJoystick'] > 0:
|
||||
@@ -730,8 +730,8 @@ class Controls:
|
||||
controlsState.lateralControlState.angleState = lac_log
|
||||
elif lat_tuning == 'pid':
|
||||
controlsState.lateralControlState.pidState = lac_log
|
||||
elif lat_tuning == 'lqr':
|
||||
controlsState.lateralControlState.lqrState = lac_log
|
||||
elif lat_tuning == 'torque':
|
||||
controlsState.lateralControlState.torqueState = lac_log
|
||||
elif lat_tuning == 'indi':
|
||||
controlsState.lateralControlState.indiState = lac_log
|
||||
|
||||
|
||||
@@ -16,7 +16,7 @@ class LatControl(ABC):
|
||||
self.steer_max = 1.0
|
||||
|
||||
@abstractmethod
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
|
||||
pass
|
||||
|
||||
def reset(self):
|
||||
|
||||
@@ -7,7 +7,7 @@ STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
|
||||
|
||||
|
||||
class LatControlAngle(LatControl):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
|
||||
angle_log = log.ControlsState.LateralAngleState.new_message()
|
||||
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
|
||||
@@ -63,7 +63,7 @@ class LatControlINDI(LatControl):
|
||||
self.steer_filter.x = 0.
|
||||
self.speed = 0.
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
|
||||
self.speed = CS.vEgo
|
||||
# Update Kalman filter
|
||||
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
|
||||
|
||||
@@ -1,84 +0,0 @@
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
from common.numpy_fast import clip
|
||||
from common.realtime import DT_CTRL
|
||||
from cereal import log
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
|
||||
|
||||
class LatControlLQR(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
self.scale = CP.lateralTuning.lqr.scale
|
||||
self.ki = CP.lateralTuning.lqr.ki
|
||||
|
||||
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
|
||||
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
|
||||
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
|
||||
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
|
||||
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
|
||||
self.dc_gain = CP.lateralTuning.lqr.dcGain
|
||||
|
||||
self.x_hat = np.array([[0], [0]])
|
||||
self.i_unwind_rate = 0.3 * DT_CTRL
|
||||
self.i_rate = 1.0 * DT_CTRL
|
||||
|
||||
self.reset()
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self.i_lqr = 0.0
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
lqr_log = log.ControlsState.LateralLQRState.new_message()
|
||||
|
||||
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
|
||||
|
||||
# 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, params.roll))
|
||||
|
||||
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
|
||||
desired_angle += instant_offset # Only add offset that originates from vehicle model errors
|
||||
lqr_log.steeringAngleDesiredDeg = desired_angle
|
||||
|
||||
# Update Kalman filter
|
||||
angle_steers_k = float(self.C.dot(self.x_hat))
|
||||
e = steering_angle_no_offset - angle_steers_k
|
||||
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)
|
||||
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
lqr_log.active = False
|
||||
lqr_output = 0.
|
||||
output_steer = 0.
|
||||
self.reset()
|
||||
else:
|
||||
lqr_log.active = True
|
||||
|
||||
# LQR
|
||||
u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
|
||||
lqr_output = torque_scale * u_lqr / self.scale
|
||||
|
||||
# Integrator
|
||||
if CS.steeringPressed:
|
||||
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
|
||||
else:
|
||||
error = desired_angle - angle_steers_k
|
||||
i = self.i_lqr + self.ki * self.i_rate * error
|
||||
control = lqr_output + i
|
||||
|
||||
if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
|
||||
(error <= 0 and (control >= -self.steer_max or i > 0.0)):
|
||||
self.i_lqr = i
|
||||
|
||||
output_steer = lqr_output + self.i_lqr
|
||||
output_steer = clip(output_steer, -self.steer_max, self.steer_max)
|
||||
|
||||
lqr_log.steeringAngleDeg = angle_steers_k
|
||||
lqr_log.i = self.i_lqr
|
||||
lqr_log.output = output_steer
|
||||
lqr_log.lqrOutput = lqr_output
|
||||
lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS)
|
||||
return output_steer, desired_angle, lqr_log
|
||||
@@ -17,7 +17,7 @@ class LatControlPID(LatControl):
|
||||
super().reset()
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
|
||||
pid_log = log.ControlsState.LateralPIDState.new_message()
|
||||
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
|
||||
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
|
||||
|
||||
@@ -0,0 +1,79 @@
|
||||
import math
|
||||
from selfdrive.controls.lib.pid import PIDController
|
||||
from common.numpy_fast import interp
|
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED
|
||||
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
from cereal import log
|
||||
|
||||
# At higher speeds (25+mph) we can assume:
|
||||
# Lateral acceleration achieved by a specific car correlates to
|
||||
# torque applied to the steering rack. It does not correlate to
|
||||
# wheel slip, or to speed.
|
||||
|
||||
# This controller applies torque to achieve desired lateral
|
||||
# accelerations. To compensate for the low speed effects we
|
||||
# use a LOW_SPEED_FACTOR in the error. Additionally there is
|
||||
# friction in the steering wheel that needs to be overcome to
|
||||
# move it at all, this is compensated for too.
|
||||
|
||||
|
||||
LOW_SPEED_FACTOR = 200
|
||||
JERK_THRESHOLD = 0.2
|
||||
|
||||
|
||||
class LatControlTorque(LatControl):
|
||||
def __init__(self, CP, CI):
|
||||
super().__init__(CP, CI)
|
||||
self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki,
|
||||
k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0)
|
||||
self.get_steer_feedforward = CI.get_steer_feedforward_function()
|
||||
self.steer_max = 1.0
|
||||
self.pid.pos_limit = self.steer_max
|
||||
self.pid.neg_limit = -self.steer_max
|
||||
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
|
||||
self.friction = CP.lateralTuning.torque.friction
|
||||
|
||||
def reset(self):
|
||||
super().reset()
|
||||
self.pid.reset()
|
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk):
|
||||
pid_log = log.ControlsState.LateralTorqueState.new_message()
|
||||
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active:
|
||||
output_torque = 0.0
|
||||
pid_log.active = False
|
||||
self.pid.reset()
|
||||
else:
|
||||
if self.use_steering_angle:
|
||||
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
|
||||
else:
|
||||
actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo
|
||||
desired_lateral_accel = desired_curvature * CS.vEgo**2
|
||||
desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2
|
||||
actual_lateral_accel = actual_curvature * CS.vEgo**2
|
||||
|
||||
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature
|
||||
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature
|
||||
error = setpoint - measurement
|
||||
pid_log.error = error
|
||||
|
||||
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
|
||||
output_torque = self.pid.update(error,
|
||||
override=CS.steeringPressed, feedforward=ff,
|
||||
speed=CS.vEgo,
|
||||
freeze_integrator=CS.steeringRateLimited)
|
||||
|
||||
friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction])
|
||||
output_torque += friction_compensation
|
||||
|
||||
pid_log.active = True
|
||||
pid_log.p = self.pid.p
|
||||
pid_log.i = self.pid.i
|
||||
pid_log.d = self.pid.d
|
||||
pid_log.f = self.pid.f
|
||||
pid_log.output = -output_torque
|
||||
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS)
|
||||
|
||||
#TODO left is positive in this convention
|
||||
return -output_torque, 0.0, pid_log
|
||||
@@ -9,7 +9,7 @@ from selfdrive.car.honda.values import CAR as HONDA
|
||||
from selfdrive.car.toyota.values import CAR as TOYOTA
|
||||
from selfdrive.car.nissan.values import CAR as NISSAN
|
||||
from selfdrive.controls.lib.latcontrol_pid import LatControlPID
|
||||
from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
|
||||
from selfdrive.controls.lib.latcontrol_torque import LatControlTorque
|
||||
from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
|
||||
from selfdrive.controls.lib.latcontrol_angle import LatControlAngle
|
||||
from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
@@ -17,7 +17,7 @@ from selfdrive.controls.lib.vehicle_model import VehicleModel
|
||||
|
||||
class TestLatControl(unittest.TestCase):
|
||||
|
||||
@parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlLQR), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
|
||||
@parameterized.expand([(HONDA.CIVIC, LatControlPID), (TOYOTA.RAV4, LatControlTorque), (TOYOTA.PRIUS, LatControlINDI), (NISSAN.LEAF, LatControlAngle)])
|
||||
def test_saturation(self, car_name, controller):
|
||||
CarInterface, CarController, CarState = interfaces[car_name]
|
||||
CP = CarInterface.get_params(car_name)
|
||||
|
||||
Reference in New Issue
Block a user