From f9b65f9189579523df22d8b27acfd90c211fa674 Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Tue, 11 Oct 2022 14:53:43 -0700 Subject: [PATCH] Low speed lateral like before (#26022) * Add explicit cost on steering wheel movement * Laxer low speed control * Laxer low speed control * Lower min speed now there is a cost * 3m/s * Similar to old master * Add cost * Crazy high * Update ref * comment old-commit-hash: 2c9b150761f533a6132fac3639df24bb286386bb --- selfdrive/controls/lib/latcontrol_torque.py | 14 ++++++---- .../controls/lib/lateral_mpc_lib/lat_mpc.py | 28 +++++++++++++------ selfdrive/controls/lib/lateral_planner.py | 10 +++++-- selfdrive/controls/tests/test_lateral_mpc.py | 6 ++-- selfdrive/test/process_replay/ref_commit | 2 +- 5 files changed, 41 insertions(+), 19 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index fa1bb480f1..7d656b55a9 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -17,6 +17,8 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY # friction in the steering wheel that needs to be overcome to # move it at all, this is compensated for too. +LOW_SPEED_FACTOR = 100 + class LatControlTorque(LatControl): def __init__(self, CP, CI): @@ -55,13 +57,15 @@ class LatControlTorque(LatControl): actual_lateral_accel = actual_curvature * CS.vEgo ** 2 lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2 - low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200]) - setpoint = desired_lateral_accel + low_speed_factor * desired_curvature - measurement = actual_lateral_accel + low_speed_factor * actual_curvature + setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature + measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature error = setpoint - measurement gravity_adjusted_lateral_accel = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY - pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, lateral_accel_deadzone, friction_compensation=False) - ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, error, lateral_accel_deadzone, friction_compensation=True) + pid_log.error = self.torque_from_lateral_accel(error, self.torque_params, error, + lateral_accel_deadzone, friction_compensation=False) + ff = self.torque_from_lateral_accel(gravity_adjusted_lateral_accel, self.torque_params, + desired_lateral_accel - actual_lateral_accel, + lateral_accel_deadzone, friction_compensation=True) freeze_integrator = steer_limited or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py index 0cbd576341..9607532ace 100755 --- a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py +++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py @@ -19,7 +19,7 @@ X_DIM = 4 P_DIM = 2 N = 16 COST_E_DIM = 3 -COST_DIM = COST_E_DIM + 1 +COST_DIM = COST_E_DIM + 2 SPEED_OFFSET = 10.0 MODEL_NAME = 'lat' ACADOS_SOLVER_TYPE = 'SQP_RTI' @@ -89,13 +89,21 @@ def gen_lat_ocp(): ocp.cost.yref = np.zeros((COST_DIM, )) ocp.cost.yref_e = np.zeros((COST_E_DIM, )) + # Add offset to smooth out low speed control + # TODO unclear if this right solution long term + v_ego_offset = v_ego + SPEED_OFFSET + # TODO there are two costs on psi_rate_ego_dot, one + # is correlated to jerk the other to steering wheel movement + # the steering wheel movement cost is added to prevent excessive + # wheel movements ocp.model.cost_y_expr = vertcat(y_ego, - ((v_ego + SPEED_OFFSET) * psi_ego), - ((v_ego + SPEED_OFFSET) * psi_rate_ego), - ((v_ego + SPEED_OFFSET) * psi_rate_ego_dot)) + v_ego_offset * psi_ego, + v_ego_offset * psi_rate_ego, + v_ego_offset * psi_rate_ego_dot, + psi_rate_ego_dot / (v_ego + 0.1)) ocp.model.cost_y_expr_e = vertcat(y_ego, - ((v_ego + SPEED_OFFSET) * psi_ego), - ((v_ego + SPEED_OFFSET) * psi_rate_ego)) + v_ego_offset * psi_ego, + v_ego_offset * psi_rate_ego) # set constraints ocp.constraints.constr_type = 'BGH' @@ -144,8 +152,12 @@ class LateralMpc(): self.solve_time = 0.0 self.cost = 0 - def set_weights(self, path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost): - W = np.asfortranarray(np.diag([path_weight, heading_weight, yaw_rate_weight, yaw_accel_cost])) + def set_weights(self, path_weight, heading_weight, + lat_accel_weight, lat_jerk_weight, + steering_rate_weight): + W = np.asfortranarray(np.diag([path_weight, heading_weight, + lat_accel_weight, lat_jerk_weight, + steering_rate_weight])) for i in range(N): self.solver.cost_set(i, 'W', W) self.solver.cost_set(N, 'W', W[:COST_E_DIM,:COST_E_DIM]) diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index d4c832b53b..9fbfd4a11f 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -17,8 +17,13 @@ PATH_COST = 1.0 LATERAL_MOTION_COST = 0.11 LATERAL_ACCEL_COST = 0.0 LATERAL_JERK_COST = 0.05 +# Extreme steering rate is unpleasant, even +# when it does not cause bad jerk. +# TODO this cost should be lowered when low +# speed lateral control is stable on all cars +STEERING_RATE_COST = 800.0 -MIN_SPEED = 1.5 +MIN_SPEED = .3 class LateralPlanner: @@ -67,7 +72,8 @@ class LateralPlanner: d_path_xyz = self.path_xyz self.lat_mpc.set_weights(PATH_COST, LATERAL_MOTION_COST, - LATERAL_ACCEL_COST, LATERAL_JERK_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) diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py index 9b986c053d..df5154b2b4 100644 --- a/selfdrive/controls/tests/test_lateral_mpc.py +++ b/selfdrive/controls/tests/test_lateral_mpc.py @@ -10,7 +10,7 @@ def run_mpc(lat_mpc=None, v_ref=30., x_init=0., y_init=0., psi_init=0., curvatur if lat_mpc is None: lat_mpc = LateralMpc() - lat_mpc.set_weights(1., 1., 0.0, 1.) + lat_mpc.set_weights(1., .1, 0.0, .05, 800) y_pts = poly_shift * np.ones(LAT_MPC_N + 1) heading_pts = np.zeros(LAT_MPC_N + 1) @@ -77,9 +77,9 @@ class TestLateralMpc(unittest.TestCase): def test_switch_convergence(self): lat_mpc = LateralMpc() - sol = run_mpc(lat_mpc=lat_mpc, poly_shift=30.0, v_ref=7.0) + sol = run_mpc(lat_mpc=lat_mpc, poly_shift=3.0, v_ref=7.0) right_psi_deg = np.degrees(sol[:,2]) - sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-30.0, v_ref=7.0) + sol = run_mpc(lat_mpc=lat_mpc, poly_shift=-3.0, v_ref=7.0) left_psi_deg = np.degrees(sol[:,2]) np.testing.assert_almost_equal(right_psi_deg, -left_psi_deg, decimal=3) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a4a24ca603..0d806ae5f1 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -f636c68e2b4ed88d3731930cf15b6dee984eb6dd +0f0a9aa8fed425468c488a4f8b7581c48d724e67