233 lines
7.7 KiB
Python
233 lines
7.7 KiB
Python
"""
|
||
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)
|