pull/27343/head
Bruce Wayne 3 years ago
parent 76a3aa2556
commit 64744758f0
  1. 4
      selfdrive/controls/lib/drive_helpers.py
  2. 6
      selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
  3. 25
      selfdrive/controls/lib/lateral_planner.py

@ -17,9 +17,9 @@ V_CRUISE_INITIAL_EXPERIMENTAL_MODE = 105
IMPERIAL_INCREMENT = 1.6 # should be CV.MPH_TO_KPH, but this causes rounding errors
MIN_SPEED = 1.0
LAT_MPC_N = 32
LAT_MPC_N = 16
LON_MPC_N = 32
CONTROL_N = 33
CONTROL_N = 17
CAR_ROTATION_RADIUS = 0.0
# EU guidelines

@ -168,14 +168,14 @@ class LateralMpc():
self.solver.constraints_set(0, "lbx", x0_cp)
self.solver.constraints_set(0, "ubx", x0_cp)
self.yref[:,0] = y_pts
v_ego = p_cp[0]
v_ego = p_cp[0, 0]
# rotation_radius = p_cp[1]
self.yref[:,1] = heading_pts * (v_ego + SPEED_OFFSET)
self.yref[:,2] = yaw_rate_pts * (v_ego + SPEED_OFFSET)
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.set(i, "p", p_cp)
self.solver.set(N, "p", p_cp)
self.solver.set(i, "p", p_cp[i])
self.solver.set(N, "p", p_cp[N])
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
t = sec_since_boot()

@ -2,6 +2,7 @@ import numpy as np
from common.realtime import sec_since_boot, DT_MDL
from common.numpy_fast import interp
from system.swaglog import cloudlog
from selfdrive.modeld.constants import T_IDXS
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import N as LAT_MPC_N
from selfdrive.controls.lib.drive_helpers import CONTROL_N, MIN_SPEED
@ -35,6 +36,7 @@ class LateralPlanner:
self.solution_invalid_cnt = 0
self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.velocity_xyz = np.zeros((TRAJECTORY_SIZE, 3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.plan_yaw_rate = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
@ -47,9 +49,13 @@ class LateralPlanner:
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
def update(self, sm):
def update(self, sm, v_plan):
# clip speed , lateral planning is not possible at 0 speed
self.v_ego = max(MIN_SPEED, sm['carState'].vEgo)
v_plan = self.v_ego * np.ones(len(T_IDXS))
self.v_plan = v_plan
plan_odo = np.concatenate(([0], np.cumsum(((v_plan[0:-1] + v_plan[1:])/2) * np.diff(T_IDXS))))
plan_odo = np.clip(plan_odo, MIN_SPEED, np.inf)[:LAT_MPC_N + 1]
measured_curvature = sm['controlsState'].curvature
# Parse model predictions
@ -59,6 +65,9 @@ class LateralPlanner:
self.t_idxs = np.array(md.position.t)
self.plan_yaw = np.array(md.orientation.z)
self.plan_yaw_rate = np.array(md.orientationRate.z)
self.velocity_xyz = np.column_stack([md.velocity.x, md.velocity.y, md.velocity.z])
car_speed = np.linalg.norm(self.velocity_xyz, axis=1)
self.model_odo = np.concatenate(([0], np.cumsum(((car_speed[0:-1] + car_speed[1:])/2) * np.diff(self.t_idxs))))
# Lane change logic
desire_state = md.meta.desireState
@ -68,21 +77,23 @@ class LateralPlanner:
lane_change_prob = self.l_lane_change_prob + self.r_lane_change_prob
self.DH.update(sm['carState'], sm['carControl'].latActive, lane_change_prob)
d_path_xyz = self.path_xyz
self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST,
LATERAL_ACCEL_COST, LATERAL_JERK_COST,
STEERING_RATE_COST)
y_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
yaw_rate_pts = np.interp(self.v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
#y_pts = np.interp(plan_odo, self.model_odo, self.path_xyz[:, 1])
#heading_pts = np.interp(plan_odo, self.model_odo, self.plan_yaw)
#yaw_rate_pts = np.interp(plan_odo, self.model_odo, self.plan_yaw_rate)
y_pts = np.interp(plan_odo, np.linalg.norm(self.path_xyz, axis=1), self.path_xyz[:, 1])
heading_pts = np.interp(plan_odo, np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
yaw_rate_pts = np.interp(plan_odo, np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw_rate)
self.y_pts = y_pts
assert len(y_pts) == LAT_MPC_N + 1
assert len(heading_pts) == LAT_MPC_N + 1
assert len(yaw_rate_pts) == LAT_MPC_N + 1
lateral_factor = max(0, self.factor1 - (self.factor2 * self.v_ego**2))
p = np.array([self.v_ego, lateral_factor])
lateral_factor = np.clip(self.factor1 - (self.factor2 * self.v_plan**2), 0.0, np.inf)
p = np.column_stack([self.v_plan, lateral_factor])
self.lat_mpc.run(self.x0,
p,
y_pts,

Loading…
Cancel
Save