mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-20 07:43:48 +08:00
Code Issues Packages Projects Releases Wiki Activity

smooth center

Browse Source
This commit is contained in:
firestar5683
2026-08-06 20:57:29 -05:00
parent 8f95c92891
commit eac56eea22
2 changed files with 30 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/lib/lane_centering.py
+12 -5
View File
@@ -16,10 +16,12 @@ _MAX_RAW_CORRECTION = 0.004
_MAX_GAIN = 0.30
_SMOOTH_TAU = 0.4
_SIGNAL_RELEASE_TAU = 0.20
_CONFIDENCE_RELEASE_TAU = 0.20
_CENTER_ERROR_DEADBAND = 0.08
_E2E_MAX_PATH_STD = 0.35
_E2E_BREAK_IN_START = 0.25
_E2E_BREAK_IN_FULL = 0.75
_E2E_BREAK_IN_START = 0.15
_E2E_BREAK_IN_FULL = 0.50
class LaneCenteringController:
@@ -68,8 +70,8 @@ class LaneCenteringController:
float(np.clip(e2e_authority, 0.0, 1.0)),
)
if not valid:
self.reset()
return model_curvature
self._correction = float(smooth_value(0.0, self._correction, _CONFIDENCE_RELEASE_TAU, dt=DT_CTRL))
return model_curvature + self._correction
target = float(np.clip(raw_correction, -_MAX_RAW_CORRECTION, _MAX_RAW_CORRECTION)) * _MAX_GAIN
self._correction = float(smooth_value(target, self._correction, _SMOOTH_TAU, dt=DT_CTRL))
@@ -120,6 +122,11 @@ class LaneCenteringController:
target_y = 0.5 * (left + right) + float(np.clip(offset, -max_safe_offset, max_safe_offset))
model_y = float(np.interp(lookahead, pos_x, pos_y))
error = target_y - model_y
error_abs = abs(error)
if error_abs <= _CENTER_ERROR_DEADBAND:
error = 0.0
else:
error = np.copysign(error_abs - _CENTER_ERROR_DEADBAND, error)
try:
pos_y_std = np.asarray(model_v2.position.yStd, dtype=float)
@@ -127,7 +134,7 @@ class LaneCenteringController:
path_std = float(np.interp(lookahead, pos_x, pos_y_std))
if 0.0 <= path_std <= _E2E_MAX_PATH_STD:
break_in = np.clip(
(abs(error) - _E2E_BREAK_IN_START) / (_E2E_BREAK_IN_FULL - _E2E_BREAK_IN_START),
(error_abs - _E2E_BREAK_IN_START) / (_E2E_BREAK_IN_FULL - _E2E_BREAK_IN_START),
0.0,
1.0,
)
selfdrive/controls/tests/test_lane_centering.py
+18 -1
View File
@@ -108,6 +108,11 @@ def test_lane_center_error_steers_toward_center():
assert left < 0.0
def test_small_center_error_does_not_chatter():
_, output = _converge(_model(left=-1.75, right=1.85), authority=0.0)
assert output == 0.0
def test_offset_direction():
_, right = _converge(_model(), offset=0.2, authority=0.0)
_, left = _converge(_model(), offset=-0.2, authority=0.0)
@@ -144,10 +149,22 @@ def test_e2e_authority_blends_lane_correction():
assert lane_only > blended > e2e >= 0.0
def test_confident_e2e_authority_starts_before_large_offset():
model = _model(left=-1.7, right=2.1, model_y=0.0, path_std=0.1)
_, lane_only = _converge(model, authority=0.0)
_, e2e = _converge(model, authority=1.0)
assert lane_only > e2e > 0.0
def test_confidence_loss_drops_filtered_correction():
controller, output = _converge(_model(left=-1.5, right=2.1), authority=0.0)
assert output > 0.0
assert _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0) == 0.0
fading = _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0)
assert 0.0 < fading < output
for _ in range(300):
fading = _update(controller, _model(left=-1.5, right=2.1, lane_prob=0.2), authority=0.0)
assert abs(fading) < 1e-6
def test_correction_is_smoothed_and_capped():