mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-12 11:13:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

FrogPilot 0.9.7

Browse Source
This commit is contained in:
FrogAi
2024-06-27 10:22:08 -07:00
parent da00915ac8
commit b705b02e70
682 changed files with 181798 additions and 1348 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Executable → Regular
+55 -31
View File
@@ -9,7 +9,6 @@ from openpilot.common.swaglog import cloudlog
# WARNING: imports outside of constants will not trigger a rebuild
from openpilot.selfdrive.modeld.constants import index_function
from openpilot.selfdrive.car.interfaces import ACCEL_MIN
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@@ -43,6 +42,8 @@ CRASH_DISTANCE = .25
LEAD_DANGER_FACTOR = 0.75
LIMIT_COST = 1e6
ACADOS_SOLVER_TYPE = 'SQP_RTI'
# Default lead acceleration decay set to 50% at 1s
LEAD_ACCEL_TAU = 1.5
# Fewer timestamps don't hurt performance and lead to
@@ -57,26 +58,49 @@ T_DIFFS = np.diff(T_IDXS, prepend=[0.])
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0
def get_jerk_factor(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
return 1.0
elif personality==log.LongitudinalPersonality.standard:
return 1.0
elif personality==log.LongitudinalPersonality.aggressive:
return 0.5
def get_jerk_factor(aggressive_jerk_acceleration=0.5, aggressive_jerk_danger=0.5, aggressive_jerk_speed=0.5,
standard_jerk_acceleration=1.0, standard_jerk_danger=1.0, standard_jerk_speed=1.0,
relaxed_jerk_acceleration=1.0, relaxed_jerk_danger=1.0, relaxed_jerk_speed=1.0,
custom_personalities=False, personality=log.LongitudinalPersonality.standard):
if custom_personalities:
if personality==log.LongitudinalPersonality.relaxed:
return relaxed_jerk_acceleration, relaxed_jerk_danger, relaxed_jerk_speed
elif personality==log.LongitudinalPersonality.standard:
return standard_jerk_acceleration, standard_jerk_danger, standard_jerk_speed
elif personality==log.LongitudinalPersonality.aggressive:
return aggressive_jerk_acceleration, aggressive_jerk_danger, aggressive_jerk_speed
else:
raise NotImplementedError("Longitudinal personality not supported")
else:
raise NotImplementedError("Longitudinal personality not supported")
if personality==log.LongitudinalPersonality.relaxed:
return 1.0, 1.0, 1.0
elif personality==log.LongitudinalPersonality.standard:
return 1.0, 1.0, 1.0
elif personality==log.LongitudinalPersonality.aggressive:
return 0.5, 0.5, 0.5
else:
raise NotImplementedError("Longitudinal personality not supported")
def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard):
if personality==log.LongitudinalPersonality.relaxed:
return 1.75
elif personality==log.LongitudinalPersonality.standard:
return 1.45
elif personality==log.LongitudinalPersonality.aggressive:
return 1.25
def get_T_FOLLOW(aggressive_follow=1.25, standard_follow=1.45, relaxed_follow=1.75, custom_personalities=False, personality=log.LongitudinalPersonality.standard):
if custom_personalities:
if personality==log.LongitudinalPersonality.relaxed:
return relaxed_follow
elif personality==log.LongitudinalPersonality.standard:
return standard_follow
elif personality==log.LongitudinalPersonality.aggressive:
return aggressive_follow
else:
raise NotImplementedError("Longitudinal personality not supported")
else:
raise NotImplementedError("Longitudinal personality not supported")
if personality==log.LongitudinalPersonality.relaxed:
return 1.75
elif personality==log.LongitudinalPersonality.standard:
return 1.45
elif personality==log.LongitudinalPersonality.aggressive:
return 1.25
else:
raise NotImplementedError("Longitudinal personality not supported")
def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
@@ -273,12 +297,11 @@ class LongitudinalMpc:
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
jerk_factor = get_jerk_factor(personality)
def set_weights(self, acceleration_jerk=1.0, danger_jerk=1.0, speed_jerk=1.0, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard):
if self.mode == 'acc':
a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST]
a_change_cost = acceleration_jerk if prev_accel_constraint else 0
cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, a_change_cost, speed_jerk]
constraint_cost_weights = [LIMIT_COST, LIMIT_COST, LIMIT_COST, danger_jerk]
elif self.mode == 'blended':
a_change_cost = 40.0 if prev_accel_constraint else 0
cost_weights = [0., 0.1, 0.2, 5.0, a_change_cost, 1.0]
@@ -315,7 +338,7 @@ class LongitudinalMpc:
x_lead = 50.0
v_lead = v_ego + 10.0
a_lead = 0.0
a_lead_tau = _LEAD_ACCEL_TAU
a_lead_tau = LEAD_ACCEL_TAU
# MPC will not converge if immediate crash is expected
# Clip lead distance to what is still possible to brake for
@@ -332,13 +355,12 @@ class LongitudinalMpc:
self.cruise_min_a = min_a
self.max_a = max_a
def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):
t_follow = get_T_FOLLOW(personality)
def update(self, lead_one, lead_two, v_cruise, x, v, a, j, radarless_model, t_follow, trafficModeActive, personality=log.LongitudinalPersonality.standard):
v_ego = self.x0[1]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
self.status = lead_one.status or lead_two.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
lead_xv_1 = self.process_lead(radarstate.leadTwo)
lead_xv_0 = self.process_lead(lead_one)
lead_xv_1 = self.process_lead(lead_two)
# To estimate a safe distance from a moving lead, we calculate how much stopping
# distance that lead needs as a minimum. We can add that to the current distance
@@ -347,7 +369,8 @@ class LongitudinalMpc:
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
self.params[:,0] = ACCEL_MIN
self.params[:,1] = self.max_a
# negative accel constraint causes problems because negative speed is not allowed
self.params[:,1] = max(0.0, self.max_a)
# Update in ACC mode or ACC/e2e blend
if self.mode == 'acc':
@@ -356,6 +379,7 @@ class LongitudinalMpc:
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
# TODO does this make sense when max_a is negative?
v_upper = v_ego + (T_IDXS * self.max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
v_lower,
@@ -397,8 +421,8 @@ class LongitudinalMpc:
self.params[:,4] = t_follow
self.run()
if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and
radarstate.leadOne.modelProb > 0.9):
lead_probability = lead_one.prob if radarless_model else lead_one.modelProb
if (np.any(lead_xv_0[FCW_IDXS,0] - self.x_sol[FCW_IDXS,0] < CRASH_DISTANCE) and lead_probability > 0.9):
self.crash_cnt += 1
else:
self.crash_cnt = 0