From 93f55f3ccf6b087e9e3a237fd813cec002530890 Mon Sep 17 00:00:00 2001 From: Vasily Tarasov Date: Thu, 15 Mar 2018 13:28:15 -0700 Subject: [PATCH] Interpolate ki/kp for steering PID loop (#200) * Interpolate ki/kp for steering PID loop Very much needed for the Volt port: car ping-pongs with low kp on high speeeds, and the loop is unstable with high kp on low speeds. Also, removes "number or array?" logic from PIController, now that all the callers use interpolation ofr ki/kp. * Pass speed to steering PID loop for ki/kp interpolation * Remove unused numbers import --- cereal/car.capnp | 8 ++++++-- selfdrive/car/honda/interface.py | 13 +++++++------ selfdrive/car/toyota/interface.py | 9 +++++---- selfdrive/controls/lib/latcontrol.py | 6 ++++-- selfdrive/controls/lib/pid.py | 15 ++------------- 5 files changed, 24 insertions(+), 27 deletions(-) diff --git a/cereal/car.capnp b/cereal/car.capnp index 14f6fc5d03..5e406d6e29 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -303,8 +303,12 @@ struct CarParams { tireStiffnessRear @14 :Float32; # [N/rad] rear tire coeff of stiff # Kp and Ki for the lateral control - steerKp @15 :Float32; - steerKi @16 :Float32; + steerKpBP @42 :List(Float32); + steerKpV @43 :List(Float32); + steerKiBP @44 :List(Float32); + steerKiV @45 :List(Float32); + steerKpDEPRECATED @15 :Float32; + steerKiDEPRECATED @16 :Float32; steerKf @25 :Float32; # Kp and Ki for the longitudinal control diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index adac338e60..18323b834c 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -151,6 +151,7 @@ class CarInterface(object): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] if candidate == CAR.CIVIC: stop_and_go = True ret.mass = mass_civic @@ -159,7 +160,7 @@ class CarInterface(object): ret.steerRatio = 13.0 # Civic at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e'] - ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] + ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [3.6, 2.4, 1.5] @@ -173,7 +174,7 @@ class CarInterface(object): ret.steerRatio = 15.3 # Acura at comma has modified steering FW, so different tuning for the Neo in that car is_fw_modified = os.getenv("DONGLE_ID") in ['85a6c74d4ad9c310'] - ret.steerKp, ret.steerKi = [0.4, 0.12] if is_fw_modified else [0.8, 0.24] + ret.steerKpV, ret.steerKiV = [[0.4], [0.12]] if is_fw_modified else [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -185,7 +186,7 @@ class CarInterface(object): ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 15.3 - ret.steerKp, ret.steerKi = 0.8, 0.24 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -197,7 +198,7 @@ class CarInterface(object): ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 ret.steerRatio = 15.0 - ret.steerKp, ret.steerKi = 0.8, 0.24 + ret.steerKpV, ret.steerKiV = [[0.8], [0.24]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -209,7 +210,7 @@ class CarInterface(object): ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 14.35 - ret.steerKp, ret.steerKi = 0.6, 0.18 + ret.steerKpV, ret.steerKiV = [[0.6], [0.18]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] @@ -221,7 +222,7 @@ class CarInterface(object): ret.wheelbase = 2.81 ret.centerToFront = ret.wheelbase * 0.41 ret.steerRatio = 16.0 - ret.steerKp, ret.steerKi = 0.38, 0.11 + ret.steerKpV, ret.steerKiV = [[0.38], [0.11]] ret.longitudinalKpBP = [0., 5., 35.] ret.longitudinalKpV = [1.2, 0.8, 0.5] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 53cfbb180a..acd22e9ae2 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -72,12 +72,13 @@ class CarInterface(object): tireStiffnessFront_civic = 85400 tireStiffnessRear_civic = 90000 + ret.steerKiBP, ret.steerKpBP = [[0.], [0.]] if candidate == CAR.PRIUS: ret.safetyParam = 66 # see conversion factor for STEER_TORQUE_EPS in dbc file ret.wheelbase = 2.70 ret.steerRatio = 14.5 # TODO: find exact value for Prius ret.mass = 3045./2.205 + std_cargo - ret.steerKp, ret.steerKi = 0.6, 0.05 + ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 2. elif candidate in [CAR.RAV4, CAR.RAV4H]: @@ -85,7 +86,7 @@ class CarInterface(object): ret.wheelbase = 2.65 ret.steerRatio = 14.5 # Rav4 2017 ret.mass = 3650./2.205 + std_cargo # mean between normal and hybrid - ret.steerKp, ret.steerKi = 0.6, 0.05 + ret.steerKpV, ret.steerKiV = [[0.6], [0.05]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. elif candidate == CAR.COROLLA: @@ -93,7 +94,7 @@ class CarInterface(object): ret.wheelbase = 2.70 ret.steerRatio = 17.8 ret.mass = 2860./2.205 + std_cargo # mean between normal and hybrid - ret.steerKp, ret.steerKi = 0.2, 0.05 + ret.steerKpV, ret.steerKiV = [[0.2], [0.05]] ret.steerKf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 ret.steerRateCost = 1. elif candidate == CAR.LEXUS_RXH: @@ -101,7 +102,7 @@ class CarInterface(object): ret.wheelbase = 2.79 ret.steerRatio = 16. # official specs say 14.8, but it does not seem right ret.mass = 4481./2.205 + std_cargo # mean between min and max - ret.steerKp, ret.steerKi = 0.6, 0.1 + ret.steerKpV, ret.steerKiV = [[0.6], [0.1]] ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerRateCost = .8 diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index 74020a5920..d2737a0496 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -26,7 +26,9 @@ def get_steer_max(CP, v_ego): class LatControl(object): def __init__(self, VM): - self.pid = PIController(VM.CP.steerKp, VM.CP.steerKi, k_f=VM.CP.steerKf, pos_limit=1.0) + self.pid = PIController((VM.CP.steerKpBP, VM.CP.steerKpV), + (VM.CP.steerKiBP, VM.CP.steerKiV), + k_f=VM.CP.steerKf, pos_limit=1.0) self.last_cloudlog_t = 0.0 self.setup_mpc(VM.CP.steerRateCost) @@ -103,7 +105,7 @@ class LatControl(object): self.pid.pos_limit = steers_max self.pid.neg_limit = -steers_max steer_feedforward = self.angle_steers_des * v_ego**2 # proportional to realigning tire momentum (~ lateral accel) - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward) + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, feedforward=steer_feedforward, speed=v_ego) self.sat_flag = self.pid.saturated return output_steer diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index 40ae836b9e..b25275d668 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -1,6 +1,5 @@ import numpy as np from common.numpy_fast import clip, interp -import numbers def apply_deadzone(error, deadzone): if error > deadzone: @@ -30,21 +29,11 @@ class PIController(object): @property def k_p(self): - if isinstance(self._k_p, numbers.Number): - kp = self._k_p - else: - kp = interp(self.speed, self._k_p[0], self._k_p[1]) - - return kp + return interp(self.speed, self._k_p[0], self._k_p[1]) @property def k_i(self): - if isinstance(self._k_i, numbers.Number): - ki = self._k_i - else: - ki = interp(self.speed, self._k_i[0], self._k_i[1]) - - return ki + return interp(self.speed, self._k_i[0], self._k_i[1]) def _check_saturation(self, control, override, error): saturated = (control < self.neg_limit) or (control > self.pos_limit)