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 __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 __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 __init__(self, CP): self.LP = LanePlanner() self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost self.setup_mpc() self.solution_invalid_cnt = 0 self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False self.lane_hugging = LaneHugging() self.op_params = opParams() self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True) self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0)
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
class PathPlanner(): def __init__(self, CP): self.LP = LanePlanner() self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost self.setup_mpc() self.solution_invalid_cnt = 0 self.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False self.lane_hugging = LaneHugging() self.op_params = opParams() self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True) self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0) def setup_mpc(self): self.libmpc = libmpc_py.libmpc self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") self.cur_state[0].x = 0.0 self.cur_state[0].y = 0.0 self.cur_state[0].psi = 0.0 self.cur_state[0].delta = 0.0 self.angle_steers_des = 0.0 self.angle_steers_des_mpc = 0.0 self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 def update(self, sm, pm, CP, VM): v_ego = sm['carState'].vEgo angle_steers = sm['carState'].steeringAngle active = sm['controlsState'].active angle_offset = sm['liveParameters'].angleOffset # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) curvature_factor = VM.curvature_factor(v_ego) self.LP.parse_model(sm['model']) # Lane change logic one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker below_lane_change_speed = v_ego < self.alca_min_speed * CV.MPH_TO_MS if sm['carState'].leftBlinker: self.lane_change_direction = LaneChangeDirection.left elif sm['carState'].rightBlinker: self.lane_change_direction = LaneChangeDirection.right if (not active) or (self.lane_change_timer > LANE_CHANGE_TIME_MAX) or (not one_blinker) or (not self.lane_change_enabled): self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none else: torque_applied = True if self.alca_nudge_required: torque_applied = sm['carState'].steeringPressed and \ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or \ (sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right)) lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 # pre elif self.lane_change_state == LaneChangeState.preLaneChange: if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off elif torque_applied: self.lane_change_state = LaneChangeState.laneChangeStarting # starting elif self.lane_change_state == LaneChangeState.laneChangeStarting: # fade out lanelines over 1s self.lane_change_ll_prob = max(self.lane_change_ll_prob - DT_MDL, 0.0) # 98% certainty if lane_change_prob < 0.02 and self.lane_change_ll_prob < 0.01: self.lane_change_state = LaneChangeState.laneChangeFinishing # finishing elif self.lane_change_state == LaneChangeState.laneChangeFinishing: # fade in laneline over 1s self.lane_change_ll_prob = min(self.lane_change_ll_prob + DT_MDL, 1.0) if one_blinker and self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.preLaneChange elif self.lane_change_ll_prob > 0.99: self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: self.lane_change_timer = 0.0 else: self.lane_change_timer += DT_MDL self.prev_one_blinker = one_blinker desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Turn off lanes during lane change if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: self.LP.l_prob *= self.lane_change_ll_prob self.LP.r_prob *= self.lane_change_ll_prob self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) else: self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) self.LP.update_d_poly(v_ego) # account for actuation delay angle_offset = self.lane_hugging.modify_offset(float(angle_offset), self.lane_change_direction, self.lane_change_state) self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers - angle_offset, curvature_factor, VM.sR, CP.steerActuatorDelay) v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed self.libmpc.run_mpc(self.cur_state, self.mpc_solution, list(self.LP.l_poly), list(self.LP.r_poly), list(self.LP.d_poly), self.LP.l_prob, self.LP.r_prob, curvature_factor, v_ego_mpc, self.LP.lane_width) # reset to current steer angle if not active or overriding if active: delta_desired = self.mpc_solution[0].delta[1] rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR) else: delta_desired = math.radians(angle_steers - angle_offset) / VM.sR rate_desired = 0.0 self.cur_state[0].delta = delta_desired self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset) # Check for infeasable MPC solution mpc_nans = any(math.isnan(x) for x in self.mpc_solution[0].delta) t = sec_since_boot() if mpc_nans: self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost) self.cur_state[0].delta = math.radians(angle_steers - angle_offset) / VM.sR if t > self.last_cloudlog_t + 5.0: self.last_cloudlog_t = t cloudlog.warning("Lateral mpc - nan: True") if self.mpc_solution[0].cost > 20000. or mpc_nans: # TODO: find a better way to detect when MPC did not converge self.solution_invalid_cnt += 1 else: self.solution_invalid_cnt = 0 plan_solution_valid = self.solution_invalid_cnt < 2 plan_send = messaging.new_message('pathPlan') plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'liveParameters', 'model']) plan_send.pathPlan.laneWidth = float(self.LP.lane_width) plan_send.pathPlan.dPoly = [float(x) for x in self.LP.d_poly] plan_send.pathPlan.lPoly = [float(x) for x in self.LP.l_poly] plan_send.pathPlan.lProb = float(self.LP.l_prob) plan_send.pathPlan.rPoly = [float(x) for x in self.LP.r_poly] plan_send.pathPlan.rProb = float(self.LP.r_prob) plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc) plan_send.pathPlan.rateSteers = float(rate_desired) plan_send.pathPlan.angleOffset = self.lane_hugging.modify_offset(float(sm['liveParameters'].angleOffsetAverage), self.lane_change_direction, self.lane_change_state) plan_send.pathPlan.mpcSolutionValid = bool(plan_solution_valid) plan_send.pathPlan.paramsValid = bool(sm['liveParameters'].valid) plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid) plan_send.pathPlan.desire = desire plan_send.pathPlan.laneChangeState = self.lane_change_state plan_send.pathPlan.laneChangeDirection = self.lane_change_direction pm.send('pathPlan', plan_send) if LOG_MPC: dat = messaging.new_message('liveMpc') dat.liveMpc.x = list(self.mpc_solution[0].x) dat.liveMpc.y = list(self.mpc_solution[0].y) dat.liveMpc.psi = list(self.mpc_solution[0].psi) dat.liveMpc.delta = list(self.mpc_solution[0].delta) dat.liveMpc.cost = self.mpc_solution[0].cost pm.send('liveMpc', dat)