mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-25 16:02:14 +08:00
Code Issues Packages Projects Releases Wiki Activity

Low speed lateral like before (#26022)

Browse Source 
* 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
This commit is contained in:
HaraldSchafer
2022-10-11 14:53:43 -07:00
committed by GitHub
parent dbdb3a02a8
commit 2c9b150761
5 changed files with 41 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/latcontrol_torque.py
+9 -5
View File
@@ -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,
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+20 -8
View File
@@ -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])
selfdrive/controls/lib/lateral_planner.py
+8 -2
View File
@@ -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)
selfdrive/controls/tests/test_lateral_mpc.py
+3 -3
View File
@@ -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)
selfdrive/test/process_replay/ref_commit
+1 -1
View File
@@ -1 +1 @@
f636c68e2b4ed88d3731930cf15b6dee984eb6dd
0f0a9aa8fed425468c488a4f8b7581c48d724e67