Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Longitudinal MPC: dont creep #23838

Closed
wants to merge 2 commits into from
Closed

Longitudinal MPC: dont creep #23838

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
53d94ed
longitudinal mpc: weigh a_change in 1 place only
FreyJo Feb 23, 2022
52f5561
longitudinal MPC: modify get_safe_obstacle_distance with smooth trans…
FreyJo Feb 23, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 9 additions & 3 deletions selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@
V_EGO_COST = 0.
A_EGO_COST = 0.
J_EGO_COST = 5.0
A_CHANGE_COST = .5
A_CHANGE_COST = 200.
DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .5
LIMIT_COST = 1e6
Expand All @@ -57,7 +57,13 @@ def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)

def get_safe_obstacle_distance(v_ego):
return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * v_ego + STOP_DISTANCE
v_min = .5 # below this speed the follow distance does not decrease further
eps = 1e-3
# smooth_max(v_min, v_ego)
vego_smooth_max = (v_ego + v_min + np.sqrt(eps+ (v_ego-v_min)**2))/2
# NOTE: one could add "-v_min * T_FOLLOW"
# to achieve desired_follow_distance(0, 0) = STOP_DISTANCE
return (v_ego**2) / (2 * COMFORT_BRAKE) + T_FOLLOW * (vego_smooth_max) + STOP_DISTANCE

def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
Expand Down Expand Up @@ -136,7 +142,7 @@ def gen_long_mpc_solver():
x_ego,
v_ego,
a_ego,
20*(a_ego - prev_a),
FreyJo marked this conversation as resolved.
Show resolved Hide resolved
a_ego - prev_a,
j_ego]
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
Expand Down