class LatControlPID():
def __init__(self, CP):
self.pid = PIController(
(CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
(CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
k_f=CP.lateralTuning.pid.kf,
pos_limit=1.0)
self.angle_steers_des = 0.
self.lane_hugging = LaneHugging()
def reset(self):
self.pid.reset()
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, CP, path_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(angle_steers)
pid_log.steerRate = float(angle_steers_rate)
if v_ego < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = self.lane_hugging.lane_hug(
path_plan.angleSteers) # get from MPC/PathPlanner
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.01
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,
deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p
pid_log.i = self.pid.i
pid_log.f = self.pid.f
pid_log.output = output_steer
pid_log.saturated = bool(self.pid.saturated)
self.sat_flag = self.pid.saturated
return output_steer, float(self.angle_steers_des), pid_log
class LatControlLQR():
def __init__(self, CP, rate=100):
self.sat_flag = False
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 / rate
self.i_rate = 1.0 / rate
self.lane_hugging = LaneHugging()
self.reset()
def reset(self):
self.i_lqr = 0.0
self.output_steer = 0.0
def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, v_ego)
torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = self.lane_hugging.lane_hug(path_plan.angleSteers - path_plan.angleOffset)
angle_steers -= path_plan.angleOffset
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = angle_steers - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(eps_torque / torque_scale) + self.L.dot(e)
if v_ego < 0.3 or not active:
lqr_log.active = False
lqr_output = 0.
self.reset()
else:
lqr_log.active = True
# LQR
u_lqr = float(self.angle_steers_des / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale
# Integrator
if steer_override:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = self.angle_steers_des - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i
if ((error >= 0 and (control <= steers_max or i < 0.0)) or \
(error <= 0 and (control >= -steers_max or i > 0.0))):
self.i_lqr = i
self.output_steer = lqr_output + self.i_lqr
self.output_steer = clip(self.output_steer*0.7, -steers_max, steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output
return self.output_steer, float(self.angle_steers_des), lqr_log
class LatControlINDI():
def __init__(self, CP):
self.angle_steers_des = 0.
A = np.matrix([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])
# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.matrix([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.matrix([[0.], [0.], [0.]])
self.enfore_rate_limit = CP.carName == "toyota"
self.RC = CP.lateralTuning.indi.timeConstant
self.G = CP.lateralTuning.indi.actuatorEffectiveness
self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain
self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain
self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
self.lane_hugging = LaneHugging()
self.reset()
def reset(self):
self.delayed_output = 0.
self.output_steer = 0.
self.counter = 0
def update(self, active, v_ego, angle_steers, angle_steers_rate,
eps_torque, steer_override, CP, path_plan):
# Update Kalman filter
y = np.matrix([[math.radians(angle_steers)],
[math.radians(angle_steers_rate)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
if v_ego < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = self.lane_hugging.lane_hug(
path_plan.angleSteers)
self.rate_steers_des = path_plan.rateSteers
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)
# Expected actuator value
self.delayed_output = self.delayed_output * self.alpha + self.output_steer * (
1. - self.alpha)
# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des -
self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]
# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error
# Enforce rate limit
if self.enfore_rate_limit:
steer_max = float(SteerLimitParams.STEER_MAX)
new_output_steer_cmd = steer_max * (self.delayed_output +
delta_u)
prev_output_steer_cmd = steer_max * self.output_steer
new_output_steer_cmd = apply_toyota_steer_torque_limits(
new_output_steer_cmd, prev_output_steer_cmd,
prev_output_steer_cmd, SteerLimitParams)
self.output_steer = new_output_steer_cmd / steer_max
else:
self.output_steer = self.delayed_output + delta_u
steers_max = get_steer_max(CP, v_ego)
self.output_steer = clip(self.output_steer, -steers_max,
steers_max)
indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.delayed_output)
indi_log.delta = float(delta_u)
indi_log.output = float(self.output_steer)
self.sat_flag = False
return float(self.output_steer), float(self.angle_steers_des), indi_log