sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Long MPC cleanup (#22462)

Browse Source 
* cleaner extrapolation

* some comments

* new ref

* more comments

* new ref
old-commit-hash: a4bc1bbb74
commatwo_master
HaraldSchafer 4 years ago committed by GitHub
parent 9d25936fed
commit 5a1e44c417
2 changed files with 45 additions and 44 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 87
      selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
  2. 2
      selfdrive/test/process_replay/ref_commit

87
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
Unescape View File

@ -1,6 +1,5 @@
#!/usr/bin/env python3
import os
import math
import numpy as np
from common.realtime import sec_since_boot
@ -24,8 +23,6 @@ U_DIM = 1
COST_E_DIM = 3
COST_DIM = COST_E_DIM + 1
CONSTR_DIM = 4
MIN_ACCEL = -3.5
X_EGO_COST = 3.
X_EGO_E2E_COST = 10.
@ -34,8 +31,11 @@ J_EGO_COST = 10.
DANGER_ZONE_COST = 100.
CRASH_DISTANCE = .5
LIMIT_COST = 1e6
T_IDXS = np.array(T_IDXS_LST)
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
MIN_ACCEL = -3.5
T_REACT = 1.8
MAX_BRAKE = 9.81
@ -113,6 +113,10 @@ def gen_long_mpc_solver():
desired_dist_comfort = get_safe_obstacle_distance(v_ego)
# The main cost in normal operation is how close you are to the "desired" distance
# from an obstacle at every timestep. This obstacle can be a lead car
# or other object. In e2e mode we can use x_position targets as a cost
# instead.
costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
x_ego,
a_ego,
@ -120,6 +124,9 @@ def gen_long_mpc_solver():
ocp.model.cost_y_expr = vertcat(*costs)
ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
# Constraints on speed, acceleration and desired distance to
# the obstacle, which is treated as a slack constraint so it
# behaves like an assymetrical cost.
constraints = vertcat((v_ego),
(a_ego - a_min),
(a_max - a_ego),
@ -131,10 +138,7 @@ def gen_long_mpc_solver():
ocp.constraints.x0 = x0
ocp.parameter_values = np.array([-1.2, 1.2, 0.0])
# These constraints put hard limits on speed and
# acceleration as well as giving an assymetrical
# cost on approaching a lead. We only use lower
# bounds with an L2 cost.
# We put all constraint cost weights to 0 and only set them at runtime
cost_weights = np.zeros(CONSTR_DIM)
ocp.cost.zl = cost_weights
ocp.cost.Zl = cost_weights
@ -147,12 +151,17 @@ def gen_long_mpc_solver():
ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
ocp.constraints.idxsh = np.arange(CONSTR_DIM)
# The HPIPM solver can give decent solutions even when it is stopped early
# Which is critical for our purpose where the compute time is strictly bounded
# We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
# does not cause issues since the problem is well bounded.
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
# More iterations take too much time and less lead to inaccurate convergence in
# some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
ocp.solver_options.qp_solver_iter_max = 4
# set prediction horizon
@ -202,9 +211,11 @@ class LongitudinalMpc():
W = np.diag([X_EGO_COST, 0.0, A_EGO_COST, J_EGO_COST])
Ws = np.tile(W[None], reps=(N,1,1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3./5.)*np.copy(W[:COST_E_DIM, :COST_E_DIM]))
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
# Set L2 slack cost on lower bound constraints
Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST])
Zls = np.tile(Zl[None], reps=(N+1,1,1))
self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
@ -213,8 +224,11 @@ class LongitudinalMpc():
W = np.diag([0.0, X_EGO_E2E_COST, 0., J_EGO_COST])
Ws = np.tile(W[None], reps=(N,1,1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
# Set L2 slack cost on lower bound constraints
Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
Zls = np.tile(Zl[None], reps=(N+1,1,1))
self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
@ -229,47 +243,34 @@ class LongitudinalMpc():
self.x0[1] = v
self.x0[2] = a
def extrapolate_lead(self, x_lead, v_lead, a_lead_0, a_lead_tau):
lead_xv = np.zeros((N+1,2))
lead_xv[0, 0], lead_xv[0, 1] = x_lead, v_lead
for i in range(1, N+1):
dt = T_IDXS[i] - T_IDXS[i-1]
a_lead = a_lead_0 * math.exp(-a_lead_tau * (T_IDXS[i]**2)/2.)
x_lead += v_lead * dt
v_lead += a_lead * dt
if v_lead < 0.0:
a_lead = 0.0
v_lead = 0.0
lead_xv[i, 0], lead_xv[i, 1] = x_lead, v_lead
def extrapolate_lead(self, x_lead, v_lead, a_lead, a_lead_tau):
a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
return lead_xv
def process_lead(self, lead):
v_ego = self.x0[1]
if lead is not None and lead.status:
x_lead = lead.dRel
v_lead = max(0.0, lead.vLead)
a_lead = clip(lead.aLeadK, -10.0, 5.0)
# MPC will not converge if immidiate crash is expected
# Clip lead distance to what is still possible to brake for
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
if x_lead < min_x_lead:
x_lead = min_x_lead
if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
v_lead = 0.0
a_lead = 0.0
self.a_lead_tau = lead.aLeadTau
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_tau)
v_lead = lead.vLead
a_lead = lead.aLeadK
a_lead_tau = lead.aLeadTau
else:
# Fake a fast lead car, so mpc can keep running in the same mode
x_lead = 50.0
v_lead = v_ego + 10.0
a_lead = 0.0
self.a_lead_tau = _LEAD_ACCEL_TAU
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, self.a_lead_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
min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
x_lead = clip(x_lead, min_x_lead, 1e8)
v_lead = clip(v_lead, 0.0, 1e8)
a_lead = clip(a_lead, -10., 5.)
lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
return lead_xv
def set_accel_limits(self, min_a, max_a):
@ -287,14 +288,14 @@ class LongitudinalMpc():
self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
self.params[:,1] = self.cruise_max_a
# To consider a safe distance from a moving lead, we calculate how much stopping
# 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])
# Fake an obstacle for cruise
# TODO find cleaner way to write hacky fake cruise obstacle
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
cruise_lower_bound = v_ego + (3/4) * self.cruise_min_a * T_IDXS
cruise_upper_bound = v_ego + (3/4) * self.cruise_max_a * T_IDXS
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
d643d6ff47522e00d06035ab0cb9e14d1c0c0ae0
641884dca2102fe74e3164f8ce001cf3294b3255
Write Preview
Loading…
Cancel
Save