""" collision.py — Robot Tracker v010 Stabiler Differentialantrieb-Regler mit Heading-Filter. Kernverbesserungen gegenüber v009: 1. Kraftvektor wird geglättet (EMA) — Kamerarauschen triggert keinen Spin mehr 2. SPIN_THRESHOLD auf 120° erhöht — Kurvenfahrt bis 120° möglich 3. Hindernisabstoßung sanfter — kein abruptes Umkehren des Zielvektors 4. Heading-Deadband: kleine Winkelfehler (<8°) werden ignoriert → keine Zitterkorrekturen Gewünschtes Verhalten: - Ausrichten, dann geradeaus fahren - Kurskorrektur nur wenn Abweichung wirklich signifikant - Hindernisse smooth umkurven, nicht drum herumdrehen """ import math import logging logger = logging.getLogger(__name__) # ----------------------------------------------------------------------- # Regler-Parameter # ----------------------------------------------------------------------- # Spin: nur bei wirklich falscher Richtung SPIN_THRESHOLD = 90.0 # zurück auf 90° — 120° war zu hoch, Endlos-Spin SPIN_STOP = 35.0 # Spin beenden SPIN_SLOWDOWN = 60.0 # ab hier Spin abbremsen HEADING_DEADBAND = 6.0 # Winkelfehler < 6° → geradeaus SLOWDOWN_DIST = 150.0 SLOWDOWN_MIN = 0.35 # Kraft-Glättung: höher als 0.15 damit Spin-Korrektur schnell ankommt # 0.15 war zu träge — Roboter überschoss immer und kam nie unter SPIN_STOP FORCE_ALPHA = 0.35 # Zustand pro Roboter _state: dict = {} # robot_id → {"spinning": bool, "fx": float, "fy": float} def _get_state(robot_id: int) -> dict: if robot_id not in _state: _state[robot_id] = {"spinning": False, "fx": 0.0, "fy": 0.0} return _state[robot_id] def compute_avoidance( robot_x: float, robot_y: float, goal_x: float, goal_y: float, obstacles: list, safe_distance: float, repulsion_strength: float, ) -> tuple: """ Potentialfeld: Zielanziehung + sanfte Hindernisabstoßung. Gibt (fx, fy) als normalisierten Richtungsvektor zurück. Änderungen v010: - Abstoßung verwendet kubische Abschwächung (nicht quadratisch) → sanfterer Verlauf, kein abrupter Richtungswechsel - Abstoßung skaliert mit Zieldistanz: weit weg = Hindernis weniger relevant """ dx_goal = goal_x - robot_x dy_goal = goal_y - robot_y dist_goal = math.sqrt(dx_goal**2 + dy_goal**2) if dist_goal < 1: return 0.0, 0.0 fx = dx_goal / dist_goal fy = dy_goal / dist_goal for obs in obstacles: dx = robot_x - obs["x"] dy = robot_y - obs["y"] dist = math.sqrt(dx**2 + dy**2) if dist < 1 or dist >= safe_distance: continue # Kubische Abschwächung: stärker nah, aber kein harter Sprung factor = ((safe_distance - dist) / safe_distance) ** 2 repulsion = repulsion_strength * factor fx += (dx / dist) * repulsion fy += (dy / dist) * repulsion magnitude = math.sqrt(fx**2 + fy**2) if magnitude < 1e-6: return 0.0, 0.0 return fx / magnitude, fy / magnitude def force_to_speeds( force_x: float, force_y: float, robot_theta: float, speed_base: float, speed_turn: float, speed_min: float, speed_max: float, angle_threshold: float, # nicht mehr genutzt axle_width: float, robot_id: int = 0, dist_to_goal: float = 9999.0, ) -> dict: """ Stabiler Differentialantrieb-Regler mit Heading-Filter. Der Kraftvektor wird geglättet bevor der Fehler berechnet wird. Das eliminiert Rauschen-getriebene Spin-Trigger und Zitterkorrekturen. """ st = _get_state(robot_id) # ----------------------------------------------------------------------- # Schritt 1: Kraftvektor glätten (EMA) # Neue Kameramessung fließt nur zu FORCE_ALPHA ein. # Ergebnis: kein abrupter Richtungswechsel durch Pixelrauschen # ----------------------------------------------------------------------- prev_fx = st["fx"] prev_fy = st["fy"] if prev_fx == 0.0 and prev_fy == 0.0: # Erster Aufruf: direkt setzen st["fx"] = force_x st["fy"] = force_y else: st["fx"] = prev_fx + FORCE_ALPHA * (force_x - prev_fx) st["fy"] = prev_fy + FORCE_ALPHA * (force_y - prev_fy) # Geglätteter Vektor normalisieren mag = math.sqrt(st["fx"]**2 + st["fy"]**2) if mag < 1e-6: return {"speed_l": 0.0, "speed_r": 0.0, "status": "idle"} sfx = st["fx"] / mag sfy = st["fy"] / mag # ----------------------------------------------------------------------- # Schritt 2: Winkelfehler aus geglättetem Vektor # ----------------------------------------------------------------------- target_angle = math.degrees(math.atan2(sfx, -sfy)) angle_error = (target_angle - robot_theta + 180) % 360 - 180 abs_error = abs(angle_error) sign = 1.0 if angle_error > 0 else -1.0 # ----------------------------------------------------------------------- # Schritt 3: Spin-Entscheidung # Kein Spin wenn nah am Ziel (immer vorwärts einfahren) # ----------------------------------------------------------------------- near_goal = dist_to_goal < SLOWDOWN_DIST if near_goal: st["spinning"] = False elif st["spinning"]: if abs_error < SPIN_STOP: st["spinning"] = False else: if abs_error > SPIN_THRESHOLD: st["spinning"] = True # ----------------------------------------------------------------------- # Zone A: Spin auf der Stelle (nur bei >120° Fehler) # ----------------------------------------------------------------------- if st["spinning"]: if abs_error > SPIN_SLOWDOWN: spin_speed = min(speed_turn, max(speed_min, abs_error * 0.8)) else: t_slow = (abs_error - SPIN_STOP) / max(1.0, SPIN_SLOWDOWN - SPIN_STOP) t_slow = max(0.0, min(1.0, t_slow)) spin_speed = speed_min + t_slow * (speed_turn - speed_min) return { "speed_l": round(spin_speed * sign, 1), "speed_r": round(-spin_speed * sign, 1), "status": "spinning", } # ----------------------------------------------------------------------- # Zone B: Kurvenfahrt (0–120°) # Differentialantrieb: Außenrad voll, Innenrad reduziert # # Deadband: bei < HEADING_DEADBAND geradeaus (kein Zittern) # ----------------------------------------------------------------------- # Verlangsamung bei Zielanfahrt if dist_to_goal < SLOWDOWN_DIST: slowdown = max(SLOWDOWN_MIN, dist_to_goal / SLOWDOWN_DIST) base = speed_base * slowdown base = max(speed_min, base) else: base = speed_base if abs_error < HEADING_DEADBAND: # Geradeaus — kein Lenken speed_l = base speed_r = base status = "straight" else: # Normalisierter Fehler: 0 bei DEADBAND, 1.0 bei SPIN_THRESHOLD effective_error = abs_error - HEADING_DEADBAND effective_range = SPIN_THRESHOLD - HEADING_DEADBAND t = min(1.0, effective_error / effective_range) # Außenrad: voll outer = base # Innenrad: von base (t=0) bis -base (t=1) # Bei t=0.5 (~64°): Innenrad = 0 (enge Kurve) inner = base * (1.0 - 2.0 * t) if angle_error > 0: speed_l = inner speed_r = outer else: speed_l = outer speed_r = inner status = "curving" speed_l = max(-speed_max, min(speed_max, speed_l)) speed_r = max(-speed_max, min(speed_max, speed_r)) return {"speed_l": round(speed_l, 1), "speed_r": round(speed_r, 1), "status": status} def reset_robot(robot_id: int) -> None: """Zustand für einen Roboter zurücksetzen (z.B. bei Verbindungsabbruch).""" _state.pop(robot_id, None)