Cx330_HomeWork/Graduation_Project
Cx330_HomeWork/Graduation_Project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

修复现存问题

Browse Source
This commit is contained in:
张梦南 2026-05-09 14:42:47 +08:00
parent 0b540c197f
commit c9958d43e3
1 changed files with 30 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
main/camera.py
View File

@ -16,18 +16,11 @@ FRAME_HEIGHT = 240
GUIDE_CENTER_X = 160 GUIDE_CENTER_X = 160
GUIDE_CENTER_Y = 120 GUIDE_CENTER_Y = 120
CONTROL_DEADZONE = 15 CONTROL_DEADZONE = 15
CONTROL_BASE_SPEED = 0.55
CONTROL_MAX_SPEED = 1.0
CONTROL_LOOKAHEAD_FRAMES = 1.4
CONTROL_LOOKAHEAD_GAIN = 0.06
MIN_CONTOUR_AREA = 90 MIN_CONTOUR_AREA = 90
MAX_CONTOUR_AREA = 6000 MAX_CONTOUR_AREA = 6000
EDGE_IGNORE = 12 EDGE_IGNORE = 12
TARGET_LOCK_DURATION = 3.0 TARGET_LOCK_DURATION = 3.0
TRACK_MAX_MISSES = 4
TRACK_FAST_MOVE_PX = 24.0
TRACK_CENTER_WINDOW = 28
@dataclass @dataclass
@ -259,38 +252,18 @@ def update_track_state(track_state, candidate):
bbox=candidate["bbox"], bbox=candidate["bbox"],
) )
raw_dx = candidate["cx"] - track_state.cx
raw_dy = candidate["cy"] - track_state.cy
motion_mag = math.hypot(raw_dx, raw_dy)
crossed_guide_x = (track_state.cx - GUIDE_CENTER_X) * (candidate["cx"] - GUIDE_CENTER_X) < 0
near_guide_x = (
abs(track_state.cx - GUIDE_CENTER_X) < TRACK_CENTER_WINDOW
or abs(candidate["cx"] - GUIDE_CENTER_X) < TRACK_CENTER_WINDOW
)
if track_state.misses > 0: if track_state.misses > 0:
smooth_prev = 0.30 smooth_prev = 0.35
else: else:
smooth_prev = 0.58 smooth_prev = 0.60
if motion_mag > TRACK_FAST_MOVE_PX:
smooth_prev = min(smooth_prev, 0.34)
if crossed_guide_x and near_guide_x:
smooth_prev = min(smooth_prev, 0.18)
new_cx = int(smooth_prev * track_state.cx + (1.0 - smooth_prev) * candidate["cx"]) new_cx = int(smooth_prev * track_state.cx + (1.0 - smooth_prev) * candidate["cx"])
new_cy = int(smooth_prev * track_state.cy + (1.0 - smooth_prev) * candidate["cy"]) new_cy = int(smooth_prev * track_state.cy + (1.0 - smooth_prev) * candidate["cy"])
raw_vx = new_cx - track_state.cx raw_vx = new_cx - track_state.cx
raw_vy = new_cy - track_state.cy raw_vy = new_cy - track_state.cy
velocity_keep = 0.60 new_vx = 0.6 * track_state.vx + 0.4 * raw_vx
if motion_mag > TRACK_FAST_MOVE_PX: new_vy = 0.6 * track_state.vy + 0.4 * raw_vy
velocity_keep = 0.42
if crossed_guide_x and near_guide_x:
velocity_keep = 0.30
new_vx = velocity_keep * track_state.vx + (1.0 - velocity_keep) * raw_vx
new_vy = velocity_keep * track_state.vy + (1.0 - velocity_keep) * raw_vy
track_state.cx = new_cx track_state.cx = new_cx
track_state.cy = new_cy track_state.cy = new_cy
@ -303,46 +276,6 @@ def update_track_state(track_state, candidate):
return track_state return track_state
def predict_track_state(track_state):
if track_state is None:
return None
x, y, w, h = track_state.bbox
next_cx = int(np.clip(track_state.cx + track_state.vx, 0, FRAME_WIDTH - 1))
next_cy = int(np.clip(track_state.cy + track_state.vy, 0, FRAME_HEIGHT - 1))
next_x = int(np.clip(x + track_state.vx, 0, max(0, FRAME_WIDTH - w)))
next_y = int(np.clip(y + track_state.vy, 0, max(0, FRAME_HEIGHT - h)))
track_state.cx = next_cx
track_state.cy = next_cy
track_state.bbox = (next_x, next_y, w, h)
track_state.vx *= 0.82
track_state.vy *= 0.82
track_state.misses += 1
track_state.visible = False
return track_state
def predicted_control_point(track_state):
if track_state is None:
return GUIDE_CENTER_X, GUIDE_CENTER_Y
speed = math.hypot(track_state.vx, track_state.vy)
lookahead = CONTROL_LOOKAHEAD_FRAMES + min(1.3, speed * CONTROL_LOOKAHEAD_GAIN)
px = int(np.clip(track_state.cx + track_state.vx * lookahead, 0, FRAME_WIDTH - 1))
py = int(np.clip(track_state.cy + track_state.vy * lookahead, 0, FRAME_HEIGHT - 1))
return px, py
def control_speed(error, velocity):
magnitude = abs(error) + abs(velocity) * 1.3
if magnitude <= CONTROL_DEADZONE:
return 0.0
normalized = min(1.0, (magnitude - CONTROL_DEADZONE) / 90.0)
return CONTROL_BASE_SPEED + normalized * (CONTROL_MAX_SPEED - CONTROL_BASE_SPEED)
def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remaining): def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remaining):
frame_width = frame.shape[1] frame_width = frame.shape[1]
cv2.circle(frame, (GUIDE_CENTER_X, GUIDE_CENTER_Y), 5, (255, 0, 0), -1) cv2.circle(frame, (GUIDE_CENTER_X, GUIDE_CENTER_Y), 5, (255, 0, 0), -1)
@ -375,8 +308,6 @@ def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remain
color = (0, 255, 0) if track_state.visible else (0, 165, 255) color = (0, 255, 0) if track_state.visible else (0, 165, 255)
cv2.rectangle(frame, (x, y), (x + w, y + h), color, 2) cv2.rectangle(frame, (x, y), (x + w, y + h), color, 2)
cv2.circle(frame, (track_state.cx, track_state.cy), 5, (0, 0, 255), -1) cv2.circle(frame, (track_state.cx, track_state.cy), 5, (0, 0, 255), -1)
predicted_x, predicted_y = predicted_control_point(track_state)
cv2.circle(frame, (predicted_x, predicted_y), 4, (0, 255, 255), -1)
status = "track" if track_state.visible else f"hold:{track_state.misses}" status = "track" if track_state.visible else f"hold:{track_state.misses}"
cv2.putText( cv2.putText(
@ -397,38 +328,34 @@ def draw_debug(frame, mask, track_state, motion_level, inlier_count, lock_remain
def drive_to_target(track_state, lock_active=False): def drive_to_target(track_state, lock_active=False):
if lock_active or track_state is None: if lock_active or track_state is None or not track_state.visible:
motor.stop() motor.stop()
return return
target_x, target_y = predicted_control_point(track_state) dx = track_state.cx - GUIDE_CENTER_X
dx = target_x - GUIDE_CENTER_X dy = track_state.cy - GUIDE_CENTER_Y
dy = target_y - GUIDE_CENTER_Y
speed_x = control_speed(dx, track_state.vx)
speed_y = control_speed(dy, track_state.vy)
if speed_x == 0.0 and speed_y == 0.0: if abs(dx) < CONTROL_DEADZONE and abs(dy) < CONTROL_DEADZONE:
motor.stop() motor.stop()
elif speed_x == 0.0: elif abs(dx) < CONTROL_DEADZONE:
if dy > 0: if dy > CONTROL_DEADZONE:
motor.move_right(speed=speed_y) motor.move_right(speed=1.0)
else: else:
motor.move_left(speed=speed_y) motor.move_left(speed=1.0)
elif speed_y == 0.0: elif abs(dy) < CONTROL_DEADZONE:
if dx > 0: if dx > CONTROL_DEADZONE:
motor.backward(speed=speed_x) motor.forward(speed=1.0)
else: else:
motor.forward(speed=speed_x) motor.backward(speed=1.0)
else: else:
speed = max(speed_x, speed_y) if dx > CONTROL_DEADZONE and dy > CONTROL_DEADZONE:
if dx > 0 and dy > 0: motor.move_right_backward(speed=1.0)
motor.move_right_backward(speed=speed) elif dx > CONTROL_DEADZONE and dy < -CONTROL_DEADZONE:
elif dx > 0 and dy < 0: motor.move_left_backward(speed=1.0)
motor.move_left_backward(speed=speed) elif dx < -CONTROL_DEADZONE and dy > CONTROL_DEADZONE:
elif dx < 0 and dy > 0: motor.move_right_forward(speed=1.0)
motor.move_right_forward(speed=speed)
else: else:
motor.move_left_forward(speed=speed) motor.move_left_forward(speed=1.0)
print("Starting camera tracking...") print("Starting camera tracking...")
@ -485,18 +412,14 @@ while True:
selected = None selected = None
else: else:
selected = select_target(candidates, track_state) selected = select_target(candidates, track_state)
if selected is not None: if track_state is not None and selected is None:
track_state = update_track_state(track_state, selected)
elif track_state is not None:
track_state = predict_track_state(track_state)
if track_state.misses > TRACK_MAX_MISSES:
motor.stop() motor.stop()
track_state = None track_state = None
lock_until = now + TARGET_LOCK_DURATION lock_until = now + TARGET_LOCK_DURATION
lock_remaining = TARGET_LOCK_DURATION lock_remaining = TARGET_LOCK_DURATION
lock_active = True lock_active = True
else: else:
track_state = None track_state = update_track_state(track_state, selected)
draw_debug(frame, motion_mask, track_state, motion_level, inlier_count, lock_remaining) draw_debug(frame, motion_mask, track_state, motion_level, inlier_count, lock_remaining)
drive_to_target(track_state, lock_active=lock_active) drive_to_target(track_state, lock_active=lock_active)