Rerevert torque control (#24649)
* Revert "Revert torque control (#24565)"
This reverts commit 93f434d047a92366c4ccefeece86fe101ed98d02.
* Move tune out of car specific stuff
* Update ref commit
old-commit-hash: d928972150
taco
HaraldSchafer
3 years ago
committed by
GitHub
parent
8d83408ace
commit
39886246c9
11 changed files with 24 additions and 125 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, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): |
|
||||||
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 |
|
||||||
@ -1 +1 @@ |
|||||||
b8c35486e8354713221d4237e97e5abced6f5228 |
336d77ad17b90af17b7eb24cc832e80b62d05a24 |
||||||
|
|||||||
Loading…
Reference in new issue