643 lines
23 KiB
Markdown
643 lines
23 KiB
Markdown
# HoloDeck — AprilTag-Robotersteuerung für den Unterricht
|
||
|
||
**Pädagogische Hochschule Weingarten · FrankeLab**
|
||
|
||
Ein overhead-kamerabasiertes System zur autonomen Navigation physischer Differentialantriebs-Roboter in Klassenzimmern. Schülerinnen und Schüler programmieren die Roboter über eine grafische Blockly-Oberfläche.
|
||
|
||
---
|
||
|
||
## Inhaltsverzeichnis
|
||
|
||
1. [Systemübersicht](#1-systemübersicht)
|
||
2. [Hardware-Komponenten](#2-hardware-komponenten)
|
||
3. [Verdrahtung & Schaltplan](#3-verdrahtung--schaltplan)
|
||
4. [Netzwerk & Infrastruktur](#4-netzwerk--infrastruktur)
|
||
5. [AprilTag-Layout](#5-apriltag-layout)
|
||
6. [Software-Architektur](#6-software-architektur)
|
||
7. [Entwicklungsgeschichte (v005–v011)](#7-entwicklungsgeschichte-v005v011)
|
||
8. [Deployment (Server)](#8-deployment-server)
|
||
9. [Firmware flashen (ESP32)](#9-firmware-flashen-esp32)
|
||
10. [Konfiguration (config.yaml)](#10-konfiguration-configyaml)
|
||
11. [Kommunikationsprotokoll](#11-kommunikationsprotokoll)
|
||
12. [Regler-Tuning](#12-regler-tuning)
|
||
13. [Bekannte Probleme & Workarounds](#13-bekannte-probleme--workarounds)
|
||
14. [Roadmap](#14-roadmap)
|
||
|
||
---
|
||
|
||
## 1. Systemübersicht
|
||
|
||
```
|
||
┌─────────────────────────────────────────────────────────────┐
|
||
│ Overhead-Kamera (HuskyLens 2) │
|
||
│ RTSP → 192.168.0.30:8554/live │
|
||
└────────────────────┬────────────────────────────────────────┘
|
||
│ RTSP (UDP, 1280×720)
|
||
▼
|
||
┌─────────────────────────────────────────────────────────────┐
|
||
│ Ubuntu-Server 192.168.0.250 │
|
||
│ /opt/robot-tracker-v010/ │
|
||
│ ┌──────────┐ ┌───────────┐ ┌──────────────┐ │
|
||
│ │ tracker │ │ collision │ │ navlogger │ │
|
||
│ │ AprilTag │→ │ Potential │→ │ CSV-Logging │ │
|
||
│ │ Detektor │ │ -feld PID │ │ │ │
|
||
│ └──────────┘ └───────────┘ └──────────────┘ │
|
||
│ FastAPI / uvicorn Port 80 │
|
||
│ TCP-Server Port 8765 │
|
||
└────────────────┬────────────────────────────────────────────┘
|
||
│ TCP (JSON, newline-delimited)
|
||
▼
|
||
┌─────────────────────────────────────────────────────────────┐
|
||
│ Roboter (Freenove ESP32-S3 + MicroPython v1.23) │
|
||
│ WiFi FrankeLab → 192.168.0.250:8765 │
|
||
│ ┌──────────┐ ┌────────────┐ ┌────────────┐ │
|
||
│ │ PID-Loop │ │ Encoder │ │ Motoren │ │
|
||
│ │ 20 Hz │← │ IRQ 50ms │ │ L9110S │ │
|
||
│ └──────────┘ └────────────┘ └────────────┘ │
|
||
└─────────────────────────────────────────────────────────────┘
|
||
```
|
||
|
||
---
|
||
|
||
## 2. Hardware-Komponenten
|
||
|
||
### Server / Kamera
|
||
|
||
| Komponente | Details |
|
||
|---|---|
|
||
| Server | Ubuntu 24.04, statisch 192.168.0.250 |
|
||
| Kamera | HuskyLens 2 (RTSP-Modus) |
|
||
| Kamera-Mount | Overhead-Befestigung zentral über der Spielfläche |
|
||
|
||
### Roboter (ein Exemplar, erweiterbar)
|
||
|
||
| Komponente | Bezeichnung / Bestellnummer |
|
||
|---|---|
|
||
| Chassis | DollaTek 2WD Roboter-Chassis-Kit |
|
||
| Mikrocontroller | Freenove ESP32-S3 WROOM Board Lite |
|
||
| Breakout-Board | Freenove ESP32-S3 Breakout Board (Schraubklemmen) |
|
||
| Motortreiber | L9110S-Modul (Schraubklemmen, 2-Kanal) |
|
||
| Encoder-Sensoren | Optokoppler-Geschwindigkeitssensor (74HC14D-basiert, 3,3–5V) |
|
||
| Powerbank | Intenso XS5000 (USB-A + USB-C Ausgang) |
|
||
| AprilTag (Roboter) | Tag36h11, ID 91, auf Aufkleber am Roboter |
|
||
|
||
> **Hinweis Motortereiber-Geschichte:** Zunächst wurde ein handgelöteter TB6612FNG auf Lochraster verwendet. Dieser wurde durch das deutlich handhabbarere L9110S-Schraubklemmen-Modul ersetzt.
|
||
|
||
> **WARNUNG:** Ein früheres AZDelivery-ESP32-Board wurde wahrscheinlich durch gleichzeitigen USB+Akku-Betrieb beschädigt. **Niemals** gleichzeitig USB und Powerbank anlegen wenn GPIO-Pins des Motortreibers auf HIGH liegen!
|
||
|
||
---
|
||
|
||
## 3. Verdrahtung & Schaltplan
|
||
|
||
### Stromversorgung — ZWEI GETRENNTE RAILS (kritisch!)
|
||
|
||
Das System verwendet **zwei physisch getrennte Stromversorgungskreise**, weil WiFi-Peak (200–400 mA) + zwei Motoren den 5V-Rail eines einzigen USB-Anschlusses überlasten:
|
||
|
||
```
|
||
┌─────────────────────┐ ┌──────────────────────────────────┐
|
||
│ Intenso XS5000 │ │ Intenso XS5000 │
|
||
│ USB-C Ausgang │ │ USB-A Ausgang │
|
||
│ 5V → ESP32 (VIN) │ │ Kabel aufschneiden: │
|
||
│ │ │ Roter Draht → VM (Motortreiber) │
|
||
└─────────────────────┘ └──────────────────────────────────┘
|
||
│ │
|
||
│ USB-C Kabel │ Modifiziertes USB-A-Kabel
|
||
▼ ▼
|
||
ESP32-S3 (USB-C) VM-Pin L9110S
|
||
Logikversorgung Motorversorgung
|
||
|
||
GND verbunden: ESP32 GND ←——→ L9110S GND (gemeinsame Masse!)
|
||
```
|
||
|
||
**Wichtig:** Das schwarze (GND) Kabel des USB-A-Kabels wird ebenfalls mit dem ESP32-GND verbunden. Die Datenleitungen (grün/weiß) bleiben unbenutzt / abgeklebt.
|
||
|
||
---
|
||
|
||
### GPIO-Belegung ESP32-S3
|
||
|
||
| GPIO | Funktion | Verbindung |
|
||
|---|---|---|
|
||
| GPIO 17 | AIN1 (Motor A Richtung 1) | L9110S Pin IA1 |
|
||
| GPIO 18 | AIN2 (Motor A Richtung 2) | L9110S Pin IA2 |
|
||
| GPIO 8 | PWMA (Motor A Geschwindigkeit) | L9110S Pin — via Jumper oder direkt |
|
||
| GPIO 9 | BIN1 (Motor B Richtung 1) | L9110S Pin IB1 |
|
||
| GPIO 10 | BIN2 (Motor B Richtung 2) | L9110S Pin IB2 |
|
||
| GPIO 11 | PWMB (Motor B Geschwindigkeit) | L9110S Pin — |
|
||
| GPIO 6 | Encoder Links (Interrupt) | Optokoppler OUT |
|
||
| GPIO 7 | Encoder Rechts (Interrupt) | Optokoppler OUT |
|
||
| 3V3 | Encoder VCC | Optokoppler VCC |
|
||
| GND | Gemeinsame Masse | L9110S GND, Encoder GND |
|
||
|
||
> **Achtung GPIO 12:** GPIO 12 ist am ESP32(-S3 nicht betroffen, aber klassischer ESP32) ein Strapping-Pin und verursacht Boot-Probleme. Wird hier nicht verwendet.
|
||
|
||
---
|
||
|
||
### L9110S Motor-Treiber Anschlussschema
|
||
|
||
```
|
||
ESP32-S3 L9110S Modul
|
||
───────── ────────────
|
||
GPIO 17 ──────────────────→ IA1 OA1 ──→ Motor A (links) Klemme 1
|
||
GPIO 18 ──────────────────→ IA2 OA2 ──→ Motor A (links) Klemme 2
|
||
GPIO 8 ──────── (PWM) ──→ IA1 oder Jumper (je nach Modul)
|
||
GPIO 9 ──────────────────→ IB1 OB1 ──→ Motor B (rechts) Klemme 1
|
||
GPIO 10 ──────────────────→ IB2 OB2 ──→ Motor B (rechts) Klemme 2
|
||
GPIO 11 ──────── (PWM) ──→ IB1 oder Jumper
|
||
3V3 ──────────────────→ VCC (Logik)
|
||
GND ──────────────────→ GND
|
||
USB-A rot ────────────────→ VM (Motorversorgung 5V)
|
||
```
|
||
|
||
> **Hinweis:** Das L9110S-Modul vereint Richtungssteuerung und PWM auf zwei Eingangspins pro Kanal (IA1/IA2). Die Drehrichtung ergibt sich aus dem Muster HIGH/LOW bzw. PWM/GND. Konkrete Zuordnung in `main.py → set_motor()`.
|
||
|
||
---
|
||
|
||
### Encoder-Sensor Anschluss
|
||
|
||
```
|
||
Optokoppler-Modul (74HC14D) ESP32-S3
|
||
─────────────────────────── ─────────
|
||
VCC ──────────────────────→ 3V3
|
||
GND ──────────────────────→ GND
|
||
OUT (linker Sensor) ──────→ GPIO 6
|
||
OUT (rechter Sensor) ──────→ GPIO 7
|
||
|
||
Lochscheibe: 20 Löcher/Umdrehung → 20 Pulse/Rev
|
||
```
|
||
|
||
---
|
||
|
||
### Befestigung
|
||
|
||
- Alle Elektronikmodule mit **Heißkleber** auf dem Chassis fixiert (sicher, rückbaubar)
|
||
- Encoder-Sensoren justiert auf Lochscheibe der Motorachsen
|
||
- AprilTag (ID 91) flach auf der Oberseite des Roboters aufgeklebt, muss von oben sichtbar sein
|
||
|
||
---
|
||
|
||
## 4. Netzwerk & Infrastruktur
|
||
|
||
| Komponente | IP-Adresse | Port |
|
||
|---|---|---|
|
||
| Ubuntu-Server | 192.168.0.250 (statisch) | — |
|
||
| FastAPI/Webinterface | 192.168.0.250 | 80 |
|
||
| TCP Robot-Server | 192.168.0.250 | 8765 |
|
||
| HuskyLens 2 (RTSP) | 192.168.0.30 | 8554 |
|
||
| Roboter (ESP32) | DHCP | — |
|
||
|
||
**WLAN:** `FrankeLab` (Passwort in `main.py` und `WIFI_PSW` Variable)
|
||
|
||
**Python-Umgebung Server:**
|
||
```bash
|
||
/opt/vision-venv/ # Virtuelle Umgebung
|
||
/opt/robot-tracker-v010/ # Aktueller Produktionsstand
|
||
```
|
||
|
||
---
|
||
|
||
## 5. AprilTag-Layout
|
||
|
||
Alle Tags aus der Familie **tag36h11**.
|
||
|
||
### Kalibrierungs-Tags (Referenzpunkte)
|
||
|
||
Auf einem A4-Blatt, fixiert im Sichtfeld der Kamera:
|
||
|
||
| Tag-ID | Position (mm) |
|
||
|---|---|
|
||
| 0 | (0.0, 0.0) — Ursprung |
|
||
| 1 | (100.5, 0.0) |
|
||
| 2 | (0.0, 181.0) |
|
||
| 3 | (100.5, 181.0) |
|
||
|
||
### Roboter-Tags
|
||
|
||
| Tag-ID | Bedeutung |
|
||
|---|---|
|
||
| 91 | Roboter 1 |
|
||
|
||
### Hindernis-Tags
|
||
|
||
| Tag-IDs |
|
||
|---|
|
||
| 11, 12, 21, 22, 31, 32, 41, 42, 51, 52, 61, 62, 71, 72, 81, 82, 92 |
|
||
|
||
Hindernisse werden als AprilTags auf dem Boden oder an Objekten platziert. Der Server berechnet automatisch Abstoßungskräfte (Potentialfeld).
|
||
|
||
---
|
||
|
||
## 6. Software-Architektur
|
||
|
||
### Server-Stack
|
||
|
||
```
|
||
app.py FastAPI-App, 20-Hz-Steuerungsloop, TCP-Server, WebSocket-Broadcast
|
||
tracker.py AprilTag-Detektion via pupil-apriltags, EMA-Filter, Dead Reckoning
|
||
collision.py Potentialfeld-Navigation: Zielanziehung + Hindernisabstoßung
|
||
navlogger.py CSV-Logging aller Navigationsdaten → logs/nav_YYYYMMDD_HHMMSS.csv
|
||
calibration.py Homographie-Kalibrierung (Kamerabild → Weltkoordinaten mm)
|
||
config.yaml Alle Parameter externalisiert
|
||
static/ Browser-Frontend (Live-Kamerabild + AprilTag-Overlay)
|
||
```
|
||
|
||
### ESP32-Firmware
|
||
|
||
```
|
||
main.py Komplette Roboter-Firmware (MicroPython)
|
||
├── WiFi-Verbindung mit Auto-Reconnect
|
||
├── TCP-Client → Server 192.168.0.250:8765
|
||
├── Encoder-Interrupts (GPIO 6, 7)
|
||
├── PID-Loop 20 Hz (asyncio)
|
||
├── Telemetrie-Loop 10 Hz (asyncio)
|
||
└── Feedforward-Architektur (v011, aktuell deaktiviert = Option B)
|
||
```
|
||
|
||
---
|
||
|
||
## 7. Entwicklungsgeschichte v005–v011
|
||
|
||
### v005 — Stabile Tracking-Basis (Server only)
|
||
- **Kamera-I/O** auf dedizierten Background-Thread via `asyncio.Queue(maxsize=1)`
|
||
- **ffmpeg-Watchdog** mit Auto-Reconnect bei Kameraausfall
|
||
- **config.yaml** — alle Parameter externalisiert, kein Hardcoding mehr
|
||
- **Velocity-Vektor-Visualisierung** im Browser-Frontend
|
||
- Bestätigt: Merklich flüssiger als v004
|
||
|
||
### v006 — Grundlegende Navigation
|
||
- Erste TCP-Kommunikation mit ESP32
|
||
- Einfache Richtungs-Befehle (links/rechts/vorwärts)
|
||
- Kein Encoder-Feedback
|
||
|
||
### v007 — Homographie-Kalibrierung
|
||
- `calibration.py` eingeführt: 4 Referenz-Tags → Kamerabild-zu-Weltkoordinaten-Mapping
|
||
- Kamerakoordinaten erstmals in echte mm umgerechnet
|
||
- A4-Kalibrierblatt mit Tags 0–3 als Referenz
|
||
|
||
### v008 — Erste Hindernisvermeidung
|
||
- `collision.py` eingeführt: Potentialfeld-Ansatz
|
||
- Zielanziehung + Hindernisabstoßung
|
||
- Noch instabil: Spin-Probleme, zu aggressiv
|
||
|
||
### v008_log — Navigation-Logging
|
||
- `navlogger.py` hinzugefügt
|
||
- CSV-Logging aller Navigationsdaten für spätere Analyse
|
||
- Log-Dateien in `logs/nav_DATUM_UHRZEIT.csv`
|
||
|
||
### v009 — Encoder-PID + Erste stabile Navigation
|
||
**Meilenstein: Erster Roboter fährt autonom zum Ziel**
|
||
|
||
- **Encoder-Interrupts** auf GPIO 6/7
|
||
- **PID-Regler** für Radgeschwindigkeit: `KP=1.2, KI=0.3, KD=0.05`
|
||
- **Kommunikationsprotokoll** auf mm/s umgestellt (server→robot: `{"speed_l": N, "speed_r": N}`)
|
||
- **TCP statt WebSocket** für Robot-Kommunikation (simpler, stabiler)
|
||
- **Dead Reckoning**: Bei Tag-Verlust schätzt ESP32 Position aus Encoder-Daten
|
||
- Hardware-Parameter in config.yaml: `mm_per_pulse=10.525`, `axle_width_mm=121.0`
|
||
- **PWM-Minimum 150** damit Motoren sicher anlaufen
|
||
|
||
### v009_fix → v009_spin_fix → v009_goal_fix
|
||
Iterative Fixes basierend auf Log-Analyse:
|
||
- Spin-Schwelle angepasst
|
||
- Ziel-Erreicht-Erkennung verbessert
|
||
- Repulsionskraft von 5000 → 1500 (war zu aggressiv)
|
||
|
||
### v010 — Stabilisierte Navigation (aktueller Produktionsstand)
|
||
**Serverseitige Verbesserungen:**
|
||
|
||
| Parameter | Alt | Neu | Grund |
|
||
|---|---|---|---|
|
||
| `FORCE_ALPHA` | 0.15 | 0.35 | Zu träge → Endlos-Spin |
|
||
| `SPIN_THRESHOLD` | 120° | 90° | 120° zu hoch, Roboter überschoss |
|
||
| `SPIN_STOP` | — | 35° | Sauberes Spin-Ende |
|
||
| `SPIN_SLOWDOWN` | — | 60° | Sanftes Abbremsen |
|
||
| `HEADING_DEADBAND` | — | 6° | Unter 6° kein Kurskorrektur |
|
||
| `repulsion_strength` | 5000 | 1500 | Sanftere Abstoßung |
|
||
| Controller-Frequenz | 10 Hz | 20 Hz | Doppelte Reaktionsgeschwindigkeit |
|
||
| `reset_robot()` | — | ✓ | Roboter stoppt bei Verbindungsabbruch |
|
||
|
||
**Bug behoben:** `FORCE_ALPHA=0.15` war zu träge — der geglättete Kraftvektor folgte der echten Richtung so langsam, dass der Roboter beim Drehen immer wieder die Stopp-Schwelle überschoss und neu anfing zu drehen → Endlos-Spin.
|
||
|
||
### v011 — Feedforward-Architektur (aktuell auf ESP32)
|
||
**Firmware-Verbesserungen:**
|
||
|
||
Neue Feedforward-Architektur: `u = kS·sign(v) + kV·|v| + PID(error)`
|
||
|
||
- **kS (Haftreibungskompensation):** Fester PWM-Wert um Haftreibung zu überwinden
|
||
- **kV (Kennlinien-Linearisierung):** PWM proportional zur Sollgeschwindigkeit
|
||
- **PID regelt nur noch den kleinen Restfehler** statt alles alleine
|
||
- **Gleitendes Messfenster** `SPEED_WINDOW=3` (150ms) für Istgeschwindigkeit — bei 20 Pulsen/Rev ist 50ms zu grob (quantisiertes Rauschen)
|
||
- **Anti-Windup:** Integral einfrieren bei PWM-Sättigung
|
||
- **Option B (aktuell aktiv):** `KS=0, KV=0` → reiner PID wie v009, aber mit Messfenster und Anti-Windup
|
||
|
||
**Bugs gefunden und behoben in v011:**
|
||
|
||
*Bug 1: Motoren drehten nicht*
|
||
- Ursache: In v011 wird `current_pwm = ff + pid_out · sign` **direkt gesetzt** statt aufaddiert
|
||
- Mit `KS=KV=0` gibt feedforward() 0 zurück → PWM bleibt auf der ersten Stufe, zu klein für Motoranlauf
|
||
- Fix: Wenn `KS=KV=0` → aufaddieren wie v009 (`current_pwm += pid_out · sign`) + PWM_MIN erzwingen
|
||
|
||
```python
|
||
if KS_L == 0.0 and KV_L == 0.0:
|
||
current_pwm_l += pid_out_l * sign_l # aufaddieren
|
||
if sign_l > 0 and current_pwm_l < PWM_MIN:
|
||
current_pwm_l = float(PWM_MIN) # Mindest-PWM
|
||
else:
|
||
current_pwm_l = ff_l + pid_out_l * sign_l # Feedforward-Modus
|
||
```
|
||
|
||
---
|
||
|
||
## 8. Deployment (Server)
|
||
|
||
### Voraussetzungen
|
||
|
||
```bash
|
||
# Python-Umgebung
|
||
python3 -m venv /opt/vision-venv
|
||
source /opt/vision-venv/bin/activate
|
||
pip install fastapi uvicorn pupil-apriltags opencv-python-headless numpy pyyaml
|
||
```
|
||
|
||
### Aktuellen Stand deployen
|
||
|
||
```bash
|
||
# Dateien kopieren
|
||
scp -r v010/* root@192.168.0.250:/opt/robot-tracker-v010/
|
||
|
||
# Server starten
|
||
ssh root@192.168.0.250
|
||
cd /opt/robot-tracker-v010
|
||
/opt/vision-venv/bin/python -m uvicorn app:app --host 0.0.0.0 --port 80
|
||
```
|
||
|
||
### Schnell neu starten
|
||
|
||
```bash
|
||
ssh root@192.168.0.250 "pkill -f uvicorn; sleep 1; \
|
||
cd /opt/robot-tracker-v010 && \
|
||
/opt/vision-venv/bin/python -m uvicorn app:app --host 0.0.0.0 --port 80"
|
||
```
|
||
|
||
### Nur collision.py neu laden (kein Neustart nötig bei Code-Änderungen)
|
||
|
||
```bash
|
||
scp collision.py root@192.168.0.250:/opt/robot-tracker-v010/
|
||
# dann Server neu starten
|
||
```
|
||
|
||
### Logs einsehen
|
||
|
||
```bash
|
||
ssh root@192.168.0.250
|
||
ls -lt /opt/robot-tracker-v010/logs/
|
||
cat /opt/robot-tracker-v010/logs/nav_20260401_192534.csv | head -5
|
||
```
|
||
|
||
### Log-Format
|
||
|
||
```
|
||
timestamp, elapsed_s, robot_x_mm, robot_y_mm, robot_theta,
|
||
goal_x_mm, goal_y_mm, dist_to_goal_mm, angle_error_deg,
|
||
cmd_speed_l, cmd_speed_r, cmd_status,
|
||
num_obstacles, nearest_obstacle_id, nearest_obstacle_dist_mm,
|
||
all_obstacle_dists
|
||
```
|
||
|
||
`cmd_status` kann sein: `straight`, `curving`, `spinning`
|
||
|
||
---
|
||
|
||
## 9. Firmware flashen (ESP32)
|
||
|
||
### Voraussetzungen
|
||
|
||
```bash
|
||
# esptool aus Freenove-Toolchain
|
||
~/Downloads/Freenove_ESP32_S3_WROOM_Board_Lite-main/Python/Python_Firmware/
|
||
|
||
# mpremote für REPL-Zugriff
|
||
pip install mpremote
|
||
```
|
||
|
||
### Gerät erkennen
|
||
|
||
Der ESP32-S3 erscheint unter Ubuntu als `/dev/ttyACM0` (native USB, **nicht** `/dev/ttyUSB*`).
|
||
|
||
```bash
|
||
ls /dev/ttyACM*
|
||
```
|
||
|
||
### MicroPython flashen (nur wenn nötig)
|
||
|
||
```bash
|
||
# Flash löschen
|
||
python esptool.py --chip esp32s3 --port /dev/ttyACM0 erase_flash
|
||
|
||
# MicroPython flashen (v1.23.0)
|
||
python esptool.py --chip esp32s3 --port /dev/ttyACM0 write_flash \
|
||
-z 0x0 ESP32_GENERIC_S3-SPIRAM_OCT-20240602-v1.23.0.bin
|
||
```
|
||
|
||
### Firmware-Datei übertragen
|
||
|
||
```bash
|
||
# main.py auf ESP32 kopieren
|
||
mpremote connect /dev/ttyACM0 fs cp main.py :main.py
|
||
|
||
# Neustart
|
||
mpremote connect /dev/ttyACM0 reset
|
||
```
|
||
|
||
### REPL für Debugging
|
||
|
||
```bash
|
||
mpremote connect /dev/ttyACM0
|
||
# Dann Ctrl+C → interaktive Python-Konsole
|
||
```
|
||
|
||
---
|
||
|
||
## 10. Konfiguration (config.yaml)
|
||
|
||
```yaml
|
||
camera:
|
||
rtsp_url: "rtsp://192.168.0.30:8554/live"
|
||
width: 1280
|
||
height: 720
|
||
rtsp_transport: "udp"
|
||
reconnect_delay: 3.0
|
||
|
||
server:
|
||
host: "0.0.0.0"
|
||
port: 80
|
||
broadcast_hz: 30
|
||
|
||
tracker:
|
||
tag_family: "tag36h11"
|
||
detector_threads: 2
|
||
quad_decimate: 1.0
|
||
ema_alpha_pos: 0.35 # Positions-Glättung (0=träge, 1=roh)
|
||
ema_alpha_ang: 0.35 # Winkel-Glättung
|
||
ema_alpha_vel: 0.25 # Geschwindigkeits-Glättung
|
||
hold_seconds: 0.80 # Tag-Verlust Toleranz
|
||
predict_seconds: 0.25 # Prediction-Horizont
|
||
|
||
robots:
|
||
tcp_port: 8765
|
||
robot_tag_ids: [91]
|
||
goal_radius: 80 # mm — Ziel gilt als erreicht
|
||
speed_base: 160 # mm/s — Fahrgeschwindigkeit
|
||
speed_turn: 90 # mm/s — Spin-Geschwindigkeit
|
||
speed_min: 50
|
||
speed_max: 350
|
||
angle_threshold: 120 # Grad — nur noch für Spin-Trigger
|
||
|
||
calibration:
|
||
reference_tags:
|
||
0: [0.0, 0.0 ]
|
||
1: [100.5, 0.0 ]
|
||
2: [0.0, 181.0]
|
||
3: [100.5, 181.0]
|
||
|
||
obstacles:
|
||
safe_distance: 150 # mm — Abstoßungsbereich
|
||
repulsion_strength: 1500 # Stärke der Abstoßung
|
||
obstacle_tag_ids: [11,12,21,22,31,32,41,42,51,52,61,62,71,72,81,82,92]
|
||
|
||
robot_hardware:
|
||
wheel_diameter_mm: 67.0
|
||
wheel_circumference_mm: 210.5
|
||
encoder_pulses_per_rev: 20
|
||
mm_per_pulse: 10.525 # 210.5 / 20
|
||
axle_width_mm: 121.0 # Radmitte links → Radmitte rechts
|
||
pulses_per_360deg: 36.1 # π × axle_width / mm_per_pulse
|
||
```
|
||
|
||
---
|
||
|
||
## 11. Kommunikationsprotokoll
|
||
|
||
### Server → Roboter (TCP, newline-delimited JSON)
|
||
|
||
```json
|
||
{"speed_l": 160.0, "speed_r": 160.0}
|
||
```
|
||
|
||
Beide Werte in **mm/s**. Negativ = rückwärts. `0.0` = Stop.
|
||
|
||
### Roboter → Server (alle 100 ms)
|
||
|
||
```json
|
||
{
|
||
"robot_id": 91,
|
||
"enc_l": 1234,
|
||
"enc_r": 1235,
|
||
"speed_l": 158.3,
|
||
"speed_r": 161.1,
|
||
"pwm_l": 420,
|
||
"pwm_r": 435
|
||
}
|
||
```
|
||
|
||
| Feld | Bedeutung |
|
||
|---|---|
|
||
| `enc_l/r` | Absoluter Encoder-Zähler (monoton steigend) |
|
||
| `speed_l/r` | Istgeschwindigkeit in mm/s (aus gleitendem Fenster) |
|
||
| `pwm_l/r` | Aktueller PWM-Wert (0–950) |
|
||
|
||
---
|
||
|
||
## 12. Regler-Tuning
|
||
|
||
### Serverseitig (collision.py)
|
||
|
||
| Parameter | Aktuell | Wirkung |
|
||
|---|---|---|
|
||
| `SPIN_THRESHOLD` | 90° | Ab diesem Winkel dreht Roboter auf der Stelle |
|
||
| `SPIN_STOP` | 35° | Unterhalb: Spin beendet, Kurvenfahrt beginnt |
|
||
| `SPIN_SLOWDOWN` | 60° | Spin wird sanfter |
|
||
| `FORCE_ALPHA` | 0.35 | EMA-Glättung des Kraftvektors (0=träge, 1=roh) |
|
||
| `HEADING_DEADBAND` | 6° | Winkeltoleranz für Geradeausfahrt |
|
||
| `repulsion_strength` | 1500 | Abstoßungsstärke von Hindernissen |
|
||
| `goal_radius` | 80 mm | Ziel gilt als erreicht |
|
||
|
||
### ESP32-seitig (main.py) — Option A: Feedforward aktivieren
|
||
|
||
```
|
||
Schritt 1 — kS bestimmen (Haftreibung):
|
||
→ speed_base sehr klein setzen (z.B. 20 mm/s)
|
||
→ KS_L/R erhöhen bis Räder gerade anfangen zu drehen
|
||
→ Typisch: 130–170
|
||
|
||
Schritt 2 — kV bestimmen (Kennlinie):
|
||
→ speed_base = 160 mm/s, KS bereits gesetzt
|
||
→ Log beobachten: pwm_l/pwm_r bei Geradeausfahrt ablesen
|
||
→ kV = (PWM_gemessen - kS) / 160
|
||
→ Typisch: 2.0–3.5
|
||
|
||
Schritt 3 — PID reduzieren:
|
||
→ KP von 1.2 auf ~0.4 reduzieren
|
||
→ KI von 0.3 auf ~0.08 reduzieren
|
||
→ KD bleibt ~0.02
|
||
|
||
Schritt 4 — Links/Rechts-Asymmetrie:
|
||
→ kS_L ≠ kS_R fein abgleichen bis Geradeausfahrt ohne Drift
|
||
```
|
||
|
||
### ESP32-seitig — Option B (aktuell): Reiner PID
|
||
|
||
```python
|
||
KS_L = KS_R = 0.0 # Feedforward aus
|
||
KV_L = KV_R = 0.0
|
||
KP = 1.2
|
||
KI = 0.3
|
||
KD = 0.05
|
||
PWM_MIN = 150 # Erzwungenes Mindest-PWM
|
||
```
|
||
|
||
---
|
||
|
||
## 13. Bekannte Probleme & Workarounds
|
||
|
||
| Problem | Ursache | Workaround / Fix |
|
||
|---|---|---|
|
||
| Endlos-Spin | `FORCE_ALPHA` zu klein, Kraftvektor folgt zu langsam | `FORCE_ALPHA=0.35`, `SPIN_THRESHOLD=90°` |
|
||
| Motoren laufen nicht an | PWM_MIN fehlt im direkten Feedforward-Pfad | `PWM_MIN=150` erzwingen wenn `KS=0` |
|
||
| ESP32 bootet nicht | GPIO 12 belegt (Strapping-Pin) | GPIO 12 nicht verwenden |
|
||
| ESP32 beschädigt | Gleichzeitig USB + Powerbank + Motoren | Stets separate Stromkreise! |
|
||
| Kamera-Freeze | ffmpeg verliert RTSP-Stream | Watchdog mit Auto-Reconnect in v005+ |
|
||
| Tag nicht erkannt | Schlechte Lichtverhältnisse | Gleichmäßige Beleuchtung, möglichst kein direktes Sonnenlicht |
|
||
| Ziel nie erreicht | `goal_radius` zu klein, Kamera-Jitter | `goal_radius` auf 100–120 mm erhöhen |
|
||
| Ziel wird weggedrückt | Hindernis-Repulsion nahe am Ziel | `repulsion_strength` bei kleiner `dist_to_goal` abschwächen (TODO) |
|
||
|
||
---
|
||
|
||
## 14. Roadmap
|
||
|
||
### Kurzfristig (Tuning)
|
||
- [ ] Feedforward-Koeffizienten `kS`, `kV` experimentell bestimmen und aktivieren
|
||
- [ ] `goal_radius` auf 100 mm erhöhen
|
||
- [ ] Repulsion nahe am Ziel abschwächen
|
||
- [ ] `axle_width_mm` und `pulses_per_360deg` exakt nachmessen
|
||
|
||
### Mittelfristig (Architektur)
|
||
- [ ] v/ω-Regler statt direktem Innen-/Außenrad-Schema (`collision.py`)
|
||
- [ ] Weiches Dead-Reckoning/Vision-Fusion (kein harter Sprung bei Tag-Wiedersehen)
|
||
- [ ] Mehrere Roboter gleichzeitig (IDs 91, 92, ...)
|
||
- [ ] Seeed Grove I2C TB6612 für einfachere Multi-Roboter-Verdrahtung
|
||
|
||
### Langfristig (Unterrichts-Interface)
|
||
- [ ] **Blockly-Interface** für Schülerinnen und Schüler
|
||
- [ ] Ziel per Klick im Browser setzen (bereits teilweise vorhanden)
|
||
- [ ] Mehrere Ziele als Route definieren
|
||
- [ ] Begegnungs-/Kooperationsszenarien
|
||
|
||
---
|
||
|
||
## Lizenz
|
||
|
||
Entwickelt für Unterrichtszwecke an der Pädagogischen Hochschule Weingarten.
|
||
Internes Projekt — nicht zur öffentlichen Weiterverbreitung bestimmt.
|
||
|
||
---
|
||
|
||
*Zuletzt aktualisiert: April 2026 — v011 Firmware, v010 Server*
|