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 morepull/24265/head
HaraldSchafer
3 years ago
committed by
GitHub
parent
6877059b45
commit
fe0bcdaef6
16 changed files with 120 additions and 123 deletions
@ -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 |
||||
@ -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 |
||||
@ -1 +1 @@ |
||||
22356d49a926a62c01d698d77c1f323016b68fd8 |
||||
185f5f9c8d878ad4b98664afc7147400476208cc |
||||
Loading…
Reference in new issue