mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-29 02:22:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Interpolate ki/kp for steering PID loop (#200)

Browse Source 
* 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

old-commit-hash: 93f55f3ccf
This commit is contained in:
Vasily Tarasov
2018-03-15 13:28:15 -07:00
committed by rbiasini
parent 60dc707715
commit e9ef18d088
5 changed files with 24 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/car.capnp
+6 -2
View File
@@ -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
selfdrive/car/honda/interface.py
+7 -6
View File
@@ -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]
selfdrive/car/toyota/interface.py
+5 -4
View File
@@ -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
selfdrive/controls/lib/latcontrol.py
+4 -2
View File
@@ -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
selfdrive/controls/lib/pid.py
+2 -13
View File
@@ -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)