forked from Dan4ick/Lidar_Muxa
134 lines
6.2 KiB
Python
134 lines
6.2 KiB
Python
"""DESCENDING NEURONS — решение.
|
||
|
||
Весь разбор сцены у мухи сходится на нескольких десятках нисходящих нейронов,
|
||
идущих в грудной ганглий. Два из них работают как раз по надвигающемуся объекту:
|
||
|
||
* **Giant Fiber (DNp01)** — один толстый аксон с высоким порогом. Срабатывает
|
||
только на близкое и быстрое надвигание и запускает немедленный аварийный
|
||
взлёт, жертвуя устойчивостью ради скорости.
|
||
* **DNp02/DNp11** — порог ниже, реакция раньше и мягче: муха успевает
|
||
подготовиться, не срываясь в паническое движение.
|
||
|
||
Поезду нужна ровно такая же пара уровней: заблаговременное предупреждение с
|
||
запасом по дальности и экстренное торможение по надёжному близкому объекту.
|
||
Гистерезис здесь — не украшение: без него трек на пороге даёт дребезг, а
|
||
дребезжащая команда торможения хуже её отсутствия.
|
||
"""
|
||
from __future__ import annotations
|
||
|
||
from dataclasses import dataclass, field
|
||
|
||
from .central_complex import CentralComplex, Track
|
||
|
||
BRAKING_DECEL = 1.0 # м/с², служебное торможение метропоезда (оценка)
|
||
REACTION_TIME = 1.5 # с, задержка канала «решение → тормоз»
|
||
|
||
|
||
@dataclass
|
||
class DetectedObject:
|
||
distance: float
|
||
lateral: float
|
||
height: float
|
||
width: float
|
||
size_v: float
|
||
confidence: float
|
||
novelty: float
|
||
n_rays: int
|
||
track_id: int
|
||
ttc: float
|
||
|
||
|
||
@dataclass
|
||
class Decision:
|
||
"""Выход системы за один кадр."""
|
||
|
||
detected: bool = False
|
||
emergency: bool = False
|
||
distance: float = float("inf")
|
||
ttc: float = float("inf")
|
||
confidence: float = 0.0
|
||
stopping_distance: float = 0.0
|
||
objects: list[DetectedObject] = field(default_factory=list)
|
||
speed: float = 0.0
|
||
|
||
@property
|
||
def clear(self) -> bool:
|
||
return not self.detected
|
||
|
||
|
||
class DescendingNeurons:
|
||
"""Два порога с гистерезисом поверх подтверждённых треков."""
|
||
|
||
def __init__(self, *, warn_evidence: float = 0.5, clear_evidence: float = 0.3,
|
||
emergency_evidence: float = 0.75, min_hits: int = 2,
|
||
novelty_floor: float = 0.10, max_range: float = 200.0,
|
||
warn_far: float | None = None, warn_far_from: float = 90.0):
|
||
self.warn_evidence = warn_evidence
|
||
self.clear_evidence = clear_evidence
|
||
self.emergency_evidence = emergency_evidence
|
||
self.min_hits = min_hits
|
||
self.novelty_floor = novelty_floor
|
||
self.max_range = max_range
|
||
# Порог предупреждения, ослабевающий с дальностью. Пара нисходящих
|
||
# нейронов у мухи не равноправна: DNp02/DNp11 срабатывают рано и
|
||
# мягко, Giant Fiber — поздно и жёстко. Здесь то же самое по
|
||
# дальности: далёкая улика физически слабее (4…8 лучей вместо
|
||
# сотни), а цена ошибки ниже — до предмета ещё сотня метров и
|
||
# десятки кадров на подтверждение. Экстренное торможение своего
|
||
# порога не снижает. None — послабления нет.
|
||
self.warn_far = warn_evidence if warn_far is None else warn_far
|
||
self.warn_far_from = warn_far_from
|
||
self._latched: set[int] = set()
|
||
|
||
@property
|
||
def _hyst(self) -> float:
|
||
return self.clear_evidence / max(self.warn_evidence, 1e-6)
|
||
|
||
def _warn_at(self, d: float) -> float:
|
||
"""Порог тревоги на дальности d: линейно от warn до warn_far."""
|
||
span = self.max_range - self.warn_far_from
|
||
if span <= 0 or d <= self.warn_far_from:
|
||
return self.warn_evidence
|
||
f = min((d - self.warn_far_from) / span, 1.0)
|
||
return self.warn_evidence + (self.warn_far - self.warn_evidence) * f
|
||
|
||
def decide(self, cx: CentralComplex, speed: float) -> Decision:
|
||
out = Decision(speed=speed)
|
||
stop = speed * REACTION_TIME + speed * speed / (2 * BRAKING_DECEL)
|
||
out.stopping_distance = stop
|
||
|
||
live: list[tuple[Track, float]] = []
|
||
for t in cx.tracks:
|
||
d = t.distance(cx.s_world)
|
||
if not (0.0 < d <= self.max_range):
|
||
self._latched.discard(t.id)
|
||
continue
|
||
warn = self._warn_at(d)
|
||
# гистерезис: попавший в тревогу трек держится до нижнего порога
|
||
on = warn if t.id not in self._latched else warn * self._hyst
|
||
if t.evidence < on or t.hits < self.min_hits or t.novelty < self.novelty_floor:
|
||
self._latched.discard(t.id)
|
||
continue
|
||
self._latched.add(t.id)
|
||
live.append((t, d))
|
||
|
||
if not live:
|
||
return out
|
||
|
||
live.sort(key=lambda p: p[1])
|
||
for t, d in live:
|
||
ttc = d / speed if speed > 0.5 else float("inf")
|
||
out.objects.append(DetectedObject(
|
||
distance=d, lateral=t.u, height=t.h, width=t.width, size_v=t.height,
|
||
confidence=min(1.0, t.evidence * t.novelty + 0.0),
|
||
novelty=t.novelty, n_rays=t.last_n_rays, track_id=t.id, ttc=ttc))
|
||
|
||
nearest, d0 = live[0]
|
||
out.detected = True
|
||
out.distance = d0
|
||
out.ttc = d0 / speed if speed > 0.5 else float("inf")
|
||
out.confidence = max(o.confidence for o in out.objects)
|
||
out.emergency = any(
|
||
t.evidence >= self.emergency_evidence and (d <= max(stop, 25.0))
|
||
for t, d in live)
|
||
return out
|