Skip to content

Longitudinal MPC: reduce costs at low speeds for quicker starts #550

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

Open
wants to merge 3 commits into
base: SA-master
Choose a base branch
from
Open

Longitudinal MPC: reduce costs at low speeds for quicker starts #550

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d03f9d4
Use vel_diff to alter follow; Lower J and X Costs at low speeds
krkeegan Dec 31, 2021
e9015cc
remove function
sshane Jan 8, 2022
773cce5
clean up function
sshane Jan 8, 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
16 changes: 11 additions & 5 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 @@ -55,14 +55,20 @@
COMFORT_BRAKE = 2.5
STOP_DISTANCE = 6.0

def get_stopped_equivalence_factor(v_lead):
return (v_lead**2) / (2 * COMFORT_BRAKE)
def get_stopped_equivalence_factor(v_lead, v_ego):
# KRK add linear offset based on speed differential in attempt to
# rectify sluggish start issue
v_diff_offset = v_lead - v_ego # Linear offset
v_diff_offset *= (10 - v_ego) / 10 # Offset tapers off ending at v_ego of 10m/s
v_diff_offset = min(max(v_diff_offset, 0), STOP_DISTANCE) # Clip offset
distance = (v_lead ** 2) / (2 * COMFORT_BRAKE) + v_diff_offset
return distance

def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW):
return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + STOP_DISTANCE

def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW):
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead)
return get_safe_obstacle_distance(v_ego, t_follow) - get_stopped_equivalence_factor(v_lead, v_ego)


def gen_long_model():
Expand Down Expand Up @@ -343,8 +349,8 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
# 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
# and then treat that as a stopped car/obstacle at this new distance.
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], v_ego)
lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], v_ego)

# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
Expand Down