mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-09-18 05:24:02 +08:00
Code Issues Packages Projects Releases Wiki Activity

backport torque controller

Browse Source 
Co-authored-by: Copilot <copilot@github.com>
This commit is contained in:
Test User
2026-04-26 10:20:39 -05:00
committed by Comma Device
parent 0af68e13df
commit e1d3d08b26
8 changed files with 144 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/log.capnp
+2
View File
@@ -918,6 +918,8 @@ struct ControlsState @0x97ff69c53601abf1 {
saturated @7 :Bool;
actualLateralAccel @9 :Float32;
desiredLateralAccel @10 :Float32;
desiredLateralJerk @11 :Float32;
version @12 :Int32;
}
struct LateralLQRState {
common/pid.py
+21 -24
View File
@@ -2,7 +2,7 @@ import numpy as np
from numbers import Number
class PIDController:
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
def __init__(self, k_p, k_i, k_f=1., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
self._k_p = k_p
self._k_i = k_i
self._k_d = k_d
@@ -14,11 +14,9 @@ class PIDController:
if isinstance(self._k_d, Number):
self._k_d = [[0], [self._k_d]]
self.pos_limit = pos_limit
self.neg_limit = neg_limit
self.set_limits(pos_limit, neg_limit)
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.i_dt = 1.0 / rate
self.speed = 0.0
self.reset()
@@ -35,10 +33,6 @@ class PIDController:
def k_d(self):
return np.interp(self.speed, self._k_d[0], self._k_d[1])
@property
def error_integral(self):
return self.i/self.k_i
def reset(self):
self.p = 0.0
self.i = 0.0
@@ -46,25 +40,28 @@ class PIDController:
self.f = 0.0
self.control = 0
def update(self, error, error_rate=0.0, speed=0.0, override=False, feedforward=0., freeze_integrator=False):
def set_limits(self, pos_limit, neg_limit):
self.pos_limit = pos_limit
self.neg_limit = neg_limit
def update(self, error, error_rate=0.0, speed=0.0, feedforward=0., freeze_integrator=False):
self.speed = speed
self.p = self.k_p * float(error)
self.d = self.k_d * error_rate
self.f = self.k_f * feedforward
self.p = float(error) * self.k_p
self.f = feedforward * self.k_f
self.d = error_rate * self.k_d
i_candidate = self.i if freeze_integrator else self.i + self.k_i * self.i_dt * error
u = self.p + i_candidate + self.d + self.f
u_sat = np.clip(u, self.neg_limit, self.pos_limit)
if override:
self.i -= self.i_unwind_rate * float(np.sign(self.i))
if u == u_sat:
self.i = i_candidate
else:
if not freeze_integrator:
self.i = self.i + error * self.k_i * self.i_rate
# Clip i to prevent exceeding control limits
control_no_i = self.p + self.d + self.f
control_no_i = np.clip(control_no_i, self.neg_limit, self.pos_limit)
self.i = np.clip(self.i, self.neg_limit - control_no_i, self.pos_limit - control_no_i)
if u > self.pos_limit and error < 0:
self.i = i_candidate
elif u < self.neg_limit and error > 0:
self.i = i_candidate
control = self.p + self.i + self.d + self.f
self.control = np.clip(control, self.neg_limit, self.pos_limit)
return self.control
return self.control
opendbc_repo/opendbc/car/interfaces.py
+11 -4
View File
@@ -8,6 +8,7 @@ from enum import StrEnum
from typing import Any, NamedTuple
from collections.abc import Callable
from functools import cache
from cereal import car
from opendbc.car import DT_CTRL, apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG
from opendbc.car import structs
@@ -50,7 +51,8 @@ class LatControlInputs(NamedTuple):
aego: float
TorqueFromLateralAccelCallbackType = Callable[[LatControlInputs, structs.CarParams.LateralTorqueTuning, bool], float]
TorqueFromLateralAccelCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, bool], float]
LateralAccelFromTorqueCallbackType = Callable[[float, car.CarParams.LateralTorqueTuning, bool], float]
@cache
@@ -181,14 +183,19 @@ class CarInterfaceBase(ABC):
def get_steer_feedforward_function(self):
return self.get_steer_feedforward_default
def torque_from_lateral_accel_linear(self, latcontrol_inputs: LatControlInputs, torque_params: structs.CarParams.LateralTorqueTuning,
gravity_adjusted: bool) -> float:
def torque_from_lateral_accel_linear(self, lateral_acceleration: float, torque_params: car.CarParams.LateralTorqueTuning) -> float:
# The default is a linear relationship between torque and lateral acceleration (accounting for road roll and steering friction)
return latcontrol_inputs.lateral_acceleration / float(torque_params.latAccelFactor)
return lateral_acceleration / float(torque_params.latAccelFactor)
def torque_from_lateral_accel(self) -> TorqueFromLateralAccelCallbackType:
return self.torque_from_lateral_accel_linear
def lateral_accel_from_torque_linear(self, torque: float, torque_params: car.CarParams.LateralTorqueTuning) -> float:
return torque * float(torque_params.latAccelFactor)
def lateral_accel_from_torque(self) -> LateralAccelFromTorqueCallbackType:
return self.lateral_accel_from_torque_linear
# returns a set of default params to avoid repetition in car specific params
@staticmethod
def get_std_params(candidate: str) -> structs.CarParams:
selfdrive/controls/controlsd.py
+8 -7
View File
@@ -6,7 +6,7 @@ from cereal import car, log
import cereal.messaging as messaging
from openpilot.common.constants import CV
from openpilot.common.params import Params
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper
from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
from openpilot.common.swaglog import cloudlog
from opendbc.car.car_helpers import interfaces
@@ -17,6 +17,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.modeld.modeld import LAT_SMOOTH_SECONDS
from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
State = log.SelfdriveState.OpenpilotState
@@ -35,7 +36,7 @@ class Controls:
self.CI = interfaces[self.CP.carFingerprint](self.CP)
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState',
self.sm = messaging.SubMaster(['liveParameters', 'liveTorqueParameters', 'modelV2', 'selfdriveState', 'liveDelay',
'liveCalibration', 'livePose', 'longitudinalPlan', 'carState', 'carOutput',
'driverMonitoringState', 'onroadEvents', 'driverAssistance'], poll='selfdriveState')
self.pm = messaging.PubMaster(['carControl', 'controlsState'])
@@ -51,11 +52,11 @@ class Controls:
self.VM = VehicleModel(self.CP)
self.LaC: LatControl
if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
self.LaC = LatControlAngle(self.CP, self.CI)
self.LaC = LatControlAngle(self.CP, self.CI, DT_CTRL)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
self.LaC = LatControlPID(self.CP, self.CI, DT_CTRL)
elif self.CP.lateralTuning.which() == 'torque':
self.LaC = LatControlTorque(self.CP, self.CI)
self.LaC = LatControlTorque(self.CP, self.CI, DT_CTRL)
def update(self):
self.sm.update(15)
@@ -117,11 +118,11 @@ class Controls:
# Reset desired curvature to current to avoid violating the limits on engage
new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)
lat_delay = self.sm["liveDelay"].lateralDelay + LAT_SMOOTH_SECONDS
actuators.curvature = self.desired_curvature
steer, steeringAngleDeg, lac_log = self.LaC.update(CC.latActive, CS, self.VM, lp,
self.steer_limited_by_controls, self.desired_curvature,
curvature_limited) # TODO what if not available
curvature_limited, lat_delay)
actuators.torque = float(steer)
actuators.steeringAngleDeg = float(steeringAngleDeg)
# Ensure no NaNs/Infs
selfdrive/controls/lib/latcontrol.py
+11 -13
View File
@@ -1,31 +1,29 @@
import numpy as np
from abc import abstractmethod, ABC
from openpilot.common.realtime import DT_CTRL
class LatControl(ABC):
def __init__(self, CP, CI):
self.sat_count_rate = 1.0 * DT_CTRL
def __init__(self, CP, CI, dt):
self.dt = dt
self.sat_limit = CP.steerLimitTimer
self.sat_count = 0.
self.sat_time = 0.
self.sat_check_min_speed = 10.
# we define the steer torque scale as [-1.0...1.0]
self.steer_max = 1.0
@abstractmethod
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited):
def update(self, active: bool, CS, VM, params, steer_limited_by_safety: bool, desired_curvature: float, curvature_limited: bool, lat_delay: float):
pass
def reset(self):
self.sat_count = 0.
self.sat_time = 0.
def _check_saturation(self, saturated, CS, steer_limited_by_controls, curvature_limited):
def _check_saturation(self, saturated, CS, steer_limited_by_safety, curvature_limited):
# Saturated only if control output is not being limited by car torque/angle rate limits
if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_controls and not CS.steeringPressed:
self.sat_count += self.sat_count_rate
if (saturated or curvature_limited) and CS.vEgo > self.sat_check_min_speed and not steer_limited_by_safety and not CS.steeringPressed:
self.sat_time += self.dt
else:
self.sat_count -= self.sat_count_rate
self.sat_count = np.clip(self.sat_count, 0.0, self.sat_limit)
return self.sat_count > (self.sat_limit - 1e-3)
self.sat_time -= self.dt
self.sat_time = np.clip(self.sat_time, 0.0, self.sat_limit)
return self.sat_time > (self.sat_limit - 1e-3)
selfdrive/controls/lib/latcontrol_angle.py
+8 -7
View File
@@ -3,16 +3,17 @@ import math
from cereal import log
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
# TODO This is speed dependent
STEER_ANGLE_SATURATION_THRESHOLD = 2.5 # Degrees
class LatControlAngle(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
self.sat_check_min_speed = 5.
self.use_steer_limited_by_controls = CP.brand == "tesla"
self.use_steer_limited_by_safety = CP.brand == "tesla"
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited):
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
angle_log = log.ControlsState.LateralAngleState.new_message()
if not active:
@@ -23,9 +24,9 @@ class LatControlAngle(LatControl):
angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
angle_steers_des += params.angleOffsetDeg
if self.use_steer_limited_by_controls:
if self.use_steer_limited_by_safety:
# these cars' carcontrollers calculate max lateral accel and jerk, so we can rely on carOutput for saturation
angle_control_saturated = steer_limited_by_controls
angle_control_saturated = steer_limited_by_safety
else:
# for cars which use a method of limiting torque such as a torque signal (Nissan and Toyota)
# or relying on EPS (Ford Q3), carOutput does not capture maxing out torque # TODO: this can be improved
@@ -33,4 +34,4 @@ class LatControlAngle(LatControl):
angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
angle_log.steeringAngleDesiredDeg = angle_steers_des
return 0, float(angle_steers_des), angle_log
return 0, float(angle_steers_des), angle_log
selfdrive/controls/lib/latcontrol_pid.py
+17 -16
View File
@@ -6,18 +6,15 @@ from openpilot.common.pid import PIDController
class LatControlPID(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
self.pid = PIDController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.ff_factor = CP.lateralTuning.pid.kf
self.get_steer_feedforward = CI.get_steer_feedforward_function()
def reset(self):
super().reset()
self.pid.reset()
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited):
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
@@ -29,20 +26,24 @@ class LatControlPID(LatControl):
pid_log.steeringAngleDesiredDeg = angle_steers_des
pid_log.angleError = error
if not active:
output_steer = 0.0
output_torque = 0.0
pid_log.active = False
self.pid.reset()
else:
# offset does not contribute to resistive torque
steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
ff = self.ff_factor * self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(error,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
output_steer = self.pid.update(error, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo)
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.f = float(self.pid.f)
pid_log.output = float(output_steer)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited_by_controls, curvature_limited))
pid_log.output = float(output_torque)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
return output_steer, angle_steers_des, pid_log
return output_torque, angle_steers_des, pid_log
selfdrive/controls/lib/latcontrol_torque.py
+66 -40
View File
@@ -1,10 +1,11 @@
import math
import numpy as np
from collections import deque
from cereal import log
from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction
from opendbc.car.interfaces import LatControlInputs
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.selfdrive.controls.lib.latcontrol import LatControl
from openpilot.common.pid import PIDController
@@ -14,73 +15,98 @@ from openpilot.common.pid import PIDController
# 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.
# accelerations. To compensate for the low speed effects the
# proportional gain is increased at low speeds by the PID controller.
# 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_X = [0, 10, 20, 30]
LOW_SPEED_Y = [15, 13, 10, 5]
KP = 1.0
KI = 0.3
KD = 0.0
INTERP_SPEEDS = [1, 1.5, 2.0, 3.0, 5, 7.5, 10, 15, 30]
KP_INTERP = [250, 120, 65, 30, 11.5, 5.5, 3.5, 2.0, KP]
LP_FILTER_CUTOFF_HZ = 1.2
LAT_ACCEL_REQUEST_BUFFER_SECONDS = 1.0
VERSION = 0
class LatControlTorque(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
def __init__(self, CP, CI, dt):
super().__init__(CP, CI, dt)
self.torque_params = CP.lateralTuning.torque.as_builder()
self.pid = PIDController(self.torque_params.kp, self.torque_params.ki,
k_f=self.torque_params.kf, pos_limit=self.steer_max, neg_limit=-self.steer_max)
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
self.pid = PIDController([INTERP_SPEEDS, KP_INTERP], KI, KD, rate=1/self.dt)
self.update_limits()
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
self.lat_accel_request_buffer_len = int(LAT_ACCEL_REQUEST_BUFFER_SECONDS / self.dt)
self.lat_accel_request_buffer = deque([0.] * self.lat_accel_request_buffer_len , maxlen=self.lat_accel_request_buffer_len)
self.previous_measurement = 0.0
self.measurement_rate_filter = FirstOrderFilter(0.0, 1 / (2 * np.pi * LP_FILTER_CUTOFF_HZ), self.dt)
def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
self.torque_params.latAccelFactor = latAccelFactor
self.torque_params.latAccelOffset = latAccelOffset
self.torque_params.friction = friction
self.update_limits()
def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, curvature_limited):
def update_limits(self):
self.pid.set_limits(self.lateral_accel_from_torque(self.steer_max, self.torque_params),
self.lateral_accel_from_torque(-self.steer_max, self.torque_params))
def update(self, active, CS, VM, params, steer_limited_by_safety, desired_curvature, curvature_limited, lat_delay):
pid_log = log.ControlsState.LateralTorqueState.new_message()
pid_log.version = VERSION
if not active:
output_torque = 0.0
pid_log.active = False
else:
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
measured_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll)
roll_compensation = params.roll * ACCELERATION_DUE_TO_GRAVITY
curvature_deadzone = abs(VM.calc_curvature(math.radians(self.steering_angle_deadzone_deg), CS.vEgo, 0.0))
desired_lateral_accel = desired_curvature * CS.vEgo ** 2
# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
# desired_lateral_jerk = desired_curvature_rate * CS.vEgo ** 2
actual_lateral_accel = actual_curvature * CS.vEgo ** 2
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2
low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)**2
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
gravity_adjusted_lateral_accel = desired_lateral_accel - roll_compensation
torque_from_setpoint = self.torque_from_lateral_accel(LatControlInputs(setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
gravity_adjusted=False)
torque_from_measurement = self.torque_from_lateral_accel(LatControlInputs(measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
gravity_adjusted=False)
pid_log.error = float(torque_from_setpoint - torque_from_measurement)
ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params,
gravity_adjusted=True)
ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
delay_frames = int(np.clip(lat_delay / self.dt, 1, self.lat_accel_request_buffer_len))
expected_lateral_accel = self.lat_accel_request_buffer[-delay_frames]
# TODO factor out lateral jerk from error to later replace it with delay independent alternative
future_desired_lateral_accel = desired_curvature * CS.vEgo ** 2
self.lat_accel_request_buffer.append(future_desired_lateral_accel)
gravity_adjusted_future_lateral_accel = future_desired_lateral_accel - roll_compensation
desired_lateral_jerk = (future_desired_lateral_accel - expected_lateral_accel) / lat_delay
freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(pid_log.error,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
measurement = measured_curvature * CS.vEgo ** 2
measurement_rate = self.measurement_rate_filter.update((measurement - self.previous_measurement) / self.dt)
self.previous_measurement = measurement
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
error = setpoint - measurement
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
pid_log.error = float(error)
ff = gravity_adjusted_future_lateral_accel
# latAccelOffset corrects roll compensation bias from device roll misalignment relative to car roll
ff -= self.torque_params.latAccelOffset
# TODO jerk is weighted by lat_delay for legacy reasons, but should be made independent of it
ff += get_friction(error, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params)
freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5
output_lataccel = self.pid.update(pid_log.error,
-measurement_rate,
feedforward=ff,
speed=CS.vEgo,
freeze_integrator=freeze_integrator)
output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params)
pid_log.active = True
pid_log.p = float(self.pid.p)
pid_log.i = float(self.pid.i)
pid_log.d = float(self.pid.d)
pid_log.f = float(self.pid.f)
pid_log.output = float(-output_torque)
pid_log.actualLateralAccel = float(actual_lateral_accel)
pid_log.desiredLateralAccel = float(desired_lateral_accel)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_controls, curvature_limited))
pid_log.output = float(-output_torque) # TODO: log lat accel?
pid_log.actualLateralAccel = float(measurement)
pid_log.desiredLateralAccel = float(setpoint)
pid_log.desiredLateralJerk = float(desired_lateral_jerk)
pid_log.saturated = bool(self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS, steer_limited_by_safety, curvature_limited))
# TODO left is positive in this convention
return -output_torque, 0.0, pid_log
return -output_torque, 0.0, pid_log