348 lines
15 KiB
Markdown
348 lines
15 KiB
Markdown
# Kalibrierung – appRobotHoming
|
||
|
||
> Stand: 2026-06-15
|
||
> Einmaliger Vorgang — nur nach mechanischen Änderungen wiederholen.
|
||
> Jede Stufe baut auf der vorherigen auf.
|
||
|
||
---
|
||
|
||
## Übersicht
|
||
|
||
```
|
||
[1] Camera NPZ → [2] Board → [3] X-Achse → [4] Arm1 Y-Achse → [5] Arm-Marker
|
||
```
|
||
|
||
| Schritt | UI-Tab | Ergebnis | Status |
|
||
|---------|--------|----------|--------|
|
||
| [1] Camera NPZ | Camera NPZ | `data/calibration/{session}/{cam}_calibration.npz` | ✅ |
|
||
| [2] Board | Board | `links.Board.markers[].position` in `robot.json` | ✅ |
|
||
| [3] X-Achse | Robot X Axis | Alle Marker-Positionen in `robot.json` rotiert | ✅ |
|
||
| [4] Arm1 Y-Achse | Arm1 – Y | `links.Arm1.jointToParent.origin[1,2]` in `robot.json` | ✅ |
|
||
| [5] Arm-Marker | Marker | `links.*.markers[].{position,spin}` visuell prüfen und korrigieren | ✅ |
|
||
|
||
---
|
||
|
||
## [1] Camera NPZ — Kameraparameter
|
||
|
||
**Ziel:** Intrinsische Parameter (Brennweite, Verzerrungskoeffizienten, Kameramatrix)
|
||
für jede Kamera als `.npz`-Datei speichern.
|
||
|
||
**Ablauf:**
|
||
1. ChArUco-Board aus verschiedenen Winkeln fotografieren (mehrere Posen)
|
||
2. OpenCV `calibrateCamera()` berechnet Kameraparameter
|
||
3. Ergebnis als `.npz` speichern und an Webcam-Service übertragen
|
||
|
||
**Speichert:** `data/calibration/{session}/{cam}_calibration.npz`
|
||
|
||
**Wird verwendet von:** Script `1_detect_aruco_observations.py` beim Board-Run und Homing
|
||
(Argument `-npz`).
|
||
|
||
---
|
||
|
||
## [2] Board — Referenz-Marker-Positionen
|
||
|
||
**Ziel:** Absolute 3D-Positionen aller Board-ArUco-Marker im Welt-Koordinatensystem
|
||
bestimmen und in `robot.json` speichern.
|
||
|
||
**Ablauf:**
|
||
1. Foto mit allen Kameras (Snapshot)
|
||
2. Script `1_detect_aruco_observations.py` → `{cam}_aruco_detection.json`
|
||
3. Script `2_estimate_camera_from_observations.py` → `{cam}_camera_pose.json`
|
||
4. Script `3b_corner_marker_poses.py` → `aruco_marker_poses.json` (Triangulierung)
|
||
5. Positionen in `robot.json` unter `links.Board.markers` eintragen / bestätigen
|
||
|
||
**Aktionen im Board-Tab:**
|
||
- **Board erkennen**: startet den vollständigen Foto+Script-Durchlauf (SSE-Stream)
|
||
- **Marker zuordnen** (Z-Bereich, Set, Link): Marker manuell kategorisieren
|
||
- **Sets justieren** (Kabsch 2D+Z): Zwei Marker-Sets aufeinander ausrichten
|
||
- **Marker ID zuordnen / entfernen**: einzelne Marker-Korrekturen
|
||
|
||
**Speichert:** `links.Board.markers` in `robot.json`
|
||
|
||
---
|
||
|
||
## [3] X-Achse — Schieberichtung des Sliders
|
||
|
||
**Ziel:** X-Achse des Roboters (Slider-Richtung `Base → Arm1`) im Board-Koordinatensystem
|
||
ausrichten, sodass `[1, 0, 0]` der realen Bewegungsrichtung entspricht.
|
||
|
||
**Ablauf:**
|
||
1. Roboter auf Position A fahren → Board erkennen
|
||
2. Roboter auf Position B fahren (mind. 20 mm Unterschied) → Board erkennen
|
||
3. Board-Viewer berechnet den Richtungsvektor der Markerbewegungen
|
||
4. „X-Achse übernehmen": alle Marker-Positionen in `robot.json` werden rotiert,
|
||
sodass die gemessene Richtung zur neuen X-Achse `[1, 0, 0]` wird
|
||
|
||
**Implementierung:** `editRobot.js` → `adoptXAxis()` — rotiert alle `links.*.markers[].position`
|
||
um den A0-Schwerpunkt als Ursprung.
|
||
|
||
**Speichert:** alle `links.*.markers[].position` in `robot.json` (rotiert)
|
||
|
||
---
|
||
|
||
## [4] Arm1 — Y-Rotationsachse (Schultergelenk)
|
||
|
||
**Ziel:** Lage und Richtung der Rotationsachse zwischen `Base` und `Arm1` bestimmen
|
||
und als `jointToParent.origin` in `robot.json` speichern.
|
||
|
||
**Ablauf:**
|
||
1. Board erkennen mit Arm in **Position A**
|
||
2. Arm1 um ≥ 15° drehen
|
||
3. Board erkennen (**Position B**)
|
||
4. Arm1 nochmals ≥ 15° drehen
|
||
5. Board erkennen (**Position C**)
|
||
6. Board-Viewer berechnet automatisch die Rotationsachse (magenta Linie im 3D-Viewer)
|
||
7. Aktion **„Joint-Origin Y/Z übernehmen"**: speichert Y/Z des Achspunkts in `robot.json`
|
||
|
||
**Optional — Aktion „Fixe Marker → Link Base":**
|
||
Marker mit Bewegung < 10 mm über alle drei Positionen sind physisch am Basis-Körper
|
||
befestigt. Diese werden in `links.Base.markers` eingetragen.
|
||
|
||
**Speichert:**
|
||
- `links.Arm1.jointToParent.origin[1]` (Y) und `[2]` (Z) in `robot.json`
|
||
- Optional: `links.Base.markers` ergänzt
|
||
|
||
### Alternative/Ergänzung — `pose_estimation.fit_origin_link` (`5_pose_estimation.py`)
|
||
|
||
🔶 *Experimentell, noch nicht in dieses UI eingebunden* — Details, Befund und
|
||
Vergleich zu Verfahren B: [`doc/Homing_5_Pose.md`](Homing_5_Pose.md) (Abschnitt
|
||
„Kalibrier-Switch: Gelenk-Origin").
|
||
|
||
Ein Schalter in `robot.json` (`pose_estimation.fit_origin_link: "Arm1"`, aktuell
|
||
gesetzt): bestimmt `origin[1,2]` (Y,Z) **zusammen mit** der Gelenkvariable in
|
||
derselben Pose-Schätzung — aus einer einzelnen vorhandenen Homing-Aufnahme
|
||
(Position + gemessene Normale aller Arm1-Marker), keine eigene Mess-Session
|
||
nötig. Bei Erfolg **automatisch übernommen**: jeder Lauf schreibt den neuen
|
||
Wert direkt in `robot.json` zurück (wie der „Joint-Origin Y/Z übernehmen"-Button,
|
||
nur automatisch statt per Klick). Auf drei realen Captures ergab sich eine
|
||
konsistente, sich einschwingende Korrektur von ca. **+6 bis +7 mm (Y) / −19 bis
|
||
−20 mm (Z)** gegenüber dem ursprünglichen Wert — bisher **nicht** gegen eine
|
||
frische Messung mit Verfahren B gegengeprüft.
|
||
|
||
---
|
||
|
||
### Mathematik: Bestimmung der Rotationsachse
|
||
|
||
Bei einer Rotation um die Y-Achse bewegt sich jeder erkannte Marker auf einem
|
||
**Kreisbogen** im 3D-Raum. Aus den beobachteten Marker-Positionen zu mehreren
|
||
Zeitstempeln lassen sich Lage und Richtung der Rotationsachse berechnen.
|
||
|
||
#### Wie viele Beobachtungen sind nötig?
|
||
|
||
Entscheidend ist nicht „2 oder 3 Positionen" allein, sondern die Kombination aus
|
||
Anzahl Marker und Anzahl Positionen:
|
||
|
||
| Beobachtungen | Bestimmbar? | Begründung |
|
||
|---|---|---|
|
||
| 1 Marker, 2 Positionen | **Nein** | Achse liegt *irgendwo* in der Mittelsenkrechten-Ebene der Sehne; Richtung und Lage bleiben unterbestimmt |
|
||
| **2 Marker, je 2 Positionen** | **Ja** | **Verfahren A**: Richtung aus d₁ × d₂, Lage aus Schnitt zweier Mittelsenkrechten-Ebenen |
|
||
| 1 Marker, 3 Positionen | **Ja** | **Verfahren B**: drei Punkte definieren einen eindeutigen Umkreis → eindeutige Achse |
|
||
| N Marker, je ≥ 2 bzw. ≥ 3 Positionen | Ja, überbestimmt | Least-Squares, robust gegenüber Messrauschen, erlaubt Fehlerabschätzung |
|
||
|
||
Die häufige Kurzbehauptung „zwei Positionen reichen nicht" gilt nur für **einen
|
||
einzigen** Marker. Sobald **zwei** (nicht-entartete) Marker vorliegen, genügen
|
||
zwei Positionen — siehe Verfahren A.
|
||
|
||
> Die aktuelle Implementierung (`public/yAxisCompute.js`) verwendet **Verfahren B**.
|
||
|
||
#### Verfahren A — Zwei Marker, je zwei Positionen
|
||
|
||
Gegeben: M₁ₐ, M₁ᵦ (Marker 1 zu Zeit a, b) und M₂ₐ, M₂ᵦ (Marker 2 zu Zeit a, b).
|
||
Beide Marker sitzen am selben starren Körper, der zwischen a und b um die gesuchte
|
||
Achse rotiert. Verschiebungsvektoren (Sehnen der Kreisbögen):
|
||
|
||
```
|
||
d₁ = M₁ᵦ − M₁ₐ
|
||
d₂ = M₂ᵦ − M₂ₐ
|
||
```
|
||
|
||
**Schritt 1 – Achsenrichtung.** Jeder Punkt bewegt sich in einer Ebene **senkrecht**
|
||
zur Achse; die Sehne liegt in dieser Ebene, also `d₁ ⊥ n` und `d₂ ⊥ n`:
|
||
|
||
```
|
||
n = (d₁ × d₂) / |d₁ × d₂| ← Einheitsvektor entlang der Rotationsachse
|
||
```
|
||
|
||
**Schritt 2 – Achsenlage.** Der Kreismittelpunkt eines Markers ist von Start- und
|
||
Endposition gleich weit entfernt, liegt also in der **Mittelsenkrechten-Ebene** der
|
||
Sehne. Da zusätzlich `n ⊥ dᵢ`, liegt die **gesamte Achse** in dieser Ebene.
|
||
|
||
```
|
||
Ebene 1: d₁ · (x − P₁) = 0, P₁ = (M₁ₐ + M₁ᵦ) / 2
|
||
Ebene 2: d₂ · (x − P₂) = 0, P₂ = (M₂ₐ + M₂ᵦ) / 2
|
||
```
|
||
|
||
Die Achse ist die **Schnittgerade** beider Ebenen mit Richtung `n`. Ein Punkt A auf
|
||
ihr (Ansatz A = α·d₁ + β·d₂, Komponente entlang n zu 0 gesetzt):
|
||
|
||
```
|
||
c₁ = d₁ · P₁, c₂ = d₂ · P₂
|
||
D = |d₁|²·|d₂|² − (d₁ · d₂)² (= |d₁ × d₂|²)
|
||
|
||
α = (c₁·|d₂|² − c₂·(d₁ · d₂)) / D
|
||
β = (c₂·|d₁|² − c₁·(d₁ · d₂)) / D
|
||
|
||
A = α·d₁ + β·d₂ ← Referenzpunkt auf der Achse
|
||
```
|
||
|
||
Ergebnis: `r(t) = A + t·n`.
|
||
|
||
**Entartung:** `D = 0` ⟺ `d₁ ∥ d₂` ⟺ beide Marker liegen **mit der Achse in einer
|
||
gemeinsamen Ebene**. Dann sind die Mittelsenkrechten-Ebenen parallel und schneiden
|
||
sich nicht eindeutig. Erkennung über `|d₁ × d₂| / (|d₁|·|d₂|) ≈ 0`; auch ein Marker
|
||
direkt auf der Achse liefert `dᵢ ≈ 0`. → anderen Marker wählen oder Verfahren B.
|
||
|
||
#### Verfahren B — Ein Marker, drei Positionen *(implementiert)*
|
||
|
||
Gegeben P₁, P₂, P₃ desselben Markers zu drei Drehwinkeln. Die drei Punkte liegen
|
||
auf einem Kreis, dessen Ebene senkrecht zur Achse steht.
|
||
|
||
**Schritt 1 — Achsenrichtung** (Normalenvektor der Kreisebene):
|
||
```
|
||
v₁ = P₂ − P₁
|
||
v₂ = P₃ − P₁
|
||
n = normalize(v₁ × v₂)
|
||
```
|
||
|
||
**Schritt 2 — Umkreismittelpunkt** (Punkt auf der Achse), über baryzentrische Gewichte:
|
||
```
|
||
a² = |P₂ − P₃|², b² = |P₁ − P₃|², c² = |P₁ − P₂|²
|
||
|
||
w₁ = a²·(b² + c² − a²)
|
||
w₂ = b²·(a² + c² − b²)
|
||
w₃ = c²·(a² + b² − c²)
|
||
|
||
C = (w₁·P₁ + w₂·P₂ + w₃·P₃) / (w₁ + w₂ + w₃)
|
||
```
|
||
|
||
Ergebnis: `r(t) = C + t·n`. **Entartung:** P₁,P₂,P₃ kollinear (zu kleiner Drehwinkel)
|
||
→ `|v₁ × v₂| ≈ 0`, Umkreis numerisch schlecht bestimmt.
|
||
|
||
#### Kombination über N Marker (Least Squares)
|
||
|
||
Beide Verfahren lassen sich über mehrere Marker mitteln — robuster und mit
|
||
Fehlerabschätzung.
|
||
|
||
**Achsenrichtung gemeinsam.** Aus `dᵢ ⊥ n` (bzw. Ebenen-Normalen) minimiert die beste
|
||
Richtung `Σ (dᵢ · n)²` unter `|n| = 1`:
|
||
```
|
||
M = Σ dᵢ · dᵢᵀ (3×3-Matrix)
|
||
n = Eigenvektor von M zum kleinsten Eigenwert
|
||
```
|
||
(Verallgemeinert das Kreuzprodukt. Verfahren B alternativ: alle Normalen nᵢ aufs
|
||
gleiche Halbraum ausrichten und mitteln.)
|
||
|
||
**Achsenlage gemeinsam.** Für Verfahren B (Umkreismittelpunkte Cᵢ) ergibt sich der
|
||
einfache Schwerpunkt:
|
||
```
|
||
axisDir = normalize( mean(nᵢ) ) ← Vorzeichen vorher angleichen
|
||
axisPoint = mean(Cᵢ) ← Schwerpunkt der Umkreismittelpunkte
|
||
```
|
||
|
||
**Residuum pro Marker** (Verfahren B, Fehler- / Ausreißer-Erkennung):
|
||
```
|
||
εᵢ = |(Cᵢ − C̄) − ((Cᵢ − C̄)·n̄)·n̄|
|
||
```
|
||
Große εᵢ deuten auf einen fehlerhaften Marker oder eine nicht-rotatorische
|
||
Bewegungskomponente hin.
|
||
|
||
Die Y/Z-Koordinaten von `axisPoint` geben an, wo die Rotationsachse durch die
|
||
YZ-Ebene geht — das ist der gesuchte `jointToParent.origin`.
|
||
|
||
#### Genauigkeit & Verfahrenswahl
|
||
|
||
Beide Verfahren sind im **rauschfreien** Fall exakt; die Unterschiede liegen in
|
||
Robustheit und Aufwand:
|
||
|
||
| Kriterium | Verfahren A (2 Marker, 2 Bilder) | Verfahren B (1 Marker, 3 Bilder) |
|
||
|---|---|---|
|
||
| Mindest-Aufnahmen | 2 Bilder | 3 Bilder |
|
||
| Mindest-Marker | 2 (gut separiert) | 1 |
|
||
| Eigenständige Fehler-Schätzung | nur über mehrere Marker | pro Marker (Residuum εᵢ) |
|
||
| Kritische Entartung | d₁ ∥ d₂ (Marker koplanar mit Achse) | P₁,P₂,P₃ kollinear (kleiner Winkel) |
|
||
| Empfindlichkeit | hoch, wenn Sehnen fast parallel oder Drehwinkel klein | hoch, wenn Drehwinkel klein (Bogen fast gerade) |
|
||
|
||
**Beide** brauchen einen ausreichend großen Drehwinkel: kleine Winkel liefern kurze
|
||
Sehnen bzw. fast gerade Bögen, in denen das Messrauschen dominiert.
|
||
|
||
- **Verfahren A** ist schneller (zwei statt drei Aufnahmen), sinnvoll bei ≥ 2 gut
|
||
getrennten Markern.
|
||
- **Verfahren B** ist robuster für die **Fehlerrechnung**: jeder Marker liefert
|
||
unabhängig eine vollständige Achsen-Schätzung, das Residuum εᵢ erlaubt
|
||
Ausreißer-Erkennung, keine Entartung zwischen Markern. Für belastbare Genauigkeit
|
||
daher vorzuziehen — und der Grund, weshalb die Implementierung Verfahren B nutzt.
|
||
|
||
**Praktische Werte (beide Verfahren):**
|
||
- Drehwinkel zwischen den Positionen ≥ 15° (besser ≥ 30°), sonst numerisch instabil.
|
||
- 3–4 Marker mit guter räumlicher Verteilung um die Achse.
|
||
- Die Rotationswinkel müssen **nicht** bekannt sein – nur die 3D-Koordinaten.
|
||
|
||
**Marker-Filter (`min_movement_mm = 10`):**
|
||
Marker, die sich zwischen A/B/C weniger als 10 mm bewegen, sind nicht am rotierenden
|
||
Teil befestigt und werden aus der Achsenberechnung ausgeschlossen. Ihre Position A
|
||
wird für die optionale Base-Link-Zuweisung gespeichert.
|
||
|
||
---
|
||
|
||
### Implementierungsnachweis
|
||
|
||
Die Implementierung in `public/yAxisCompute.js` setzt **Verfahren B** um:
|
||
|
||
| Schritt | Implementierung | Verifikation |
|
||
|---------|----------------|--------------|
|
||
| Normalenvektor | `yAxisCompute.js` Z. 51–61 | Kreuzprodukt v₁×v₂, normiert ✓ |
|
||
| Umkreismittelpunkt | `yAxisCompute.js` Z. 63–76 | Baryzentrische Gewichte ✓ |
|
||
| Mittelung Normalen | `yAxisCompute.js` Z. 153–162 | Vorzeichen-Alignment + mean + renormieren ✓ |
|
||
| Mittelung Achspunkte | `yAxisCompute.js` Z. 164–167 | Schwerpunkt der Cᵢ ✓ |
|
||
| Origin speichern | `calibration.js` → `setOriginBtn` sendet `axisPoint[1,2]` | `editRobot.js` schreibt `origin[1,2]` ✓ |
|
||
|
||
**Ergebnis: Implementierung entspricht der beschriebenen Mathematik (Verfahren B) vollständig.**
|
||
|
||
---
|
||
|
||
## [5] Arm-Marker — Spin-Verifikation und Korrektur
|
||
|
||
**Ziel:** Sicherstellen, dass die in `robot.json` eingetragenen Arm-Marker
|
||
(Position, Normale, Spin) mit dem realen Aufkleber übereinstimmen — insbesondere
|
||
die `spin`-Orientierung, die der Homing-Viewer korrekt darstellen muss.
|
||
|
||
**Ablauf:**
|
||
1. Arm-Marker physisch aufkleben und Position/Normale grob in `robot.json` eintragen
|
||
2. Homing-Run starten (Tab „Homing" oder `homing.html`)
|
||
3. Kalibrierung → Tab **„Marker"** öffnen:
|
||
- Tabelle zeigt alle Arm-Marker mit aktuellem `spin`-Wert
|
||
- Viewer zeigt 3D-Skeleton mit spin-orientiertem Marker-Quadrat und Orientierungszeiger (→ Ecke 0)
|
||
4. Visuelle Prüfung: zeigt die Viewer-Grafik dieselbe Orientierung wie der echte Aufkleber?
|
||
5. Falls nicht: Link und Marker-ID wählen → **−90° / +90° / 180°** klicken → Viewer lädt neu
|
||
6. Wiederholen bis Modell und Realität übereinstimmen
|
||
|
||
**Aktionen im Marker-Tab:**
|
||
- **Link-Dropdown + ID-Dropdown**: Marker auswählen
|
||
- **Spin-Buttons** (−90°, +90°, 180°): Spin in `robot.json` korrigieren und Viewer neu laden
|
||
- **Viewer** (boardViewer.html?mode=homing): zeigt Skeleton mit Marker-Quadraten und Orientierungszeigern
|
||
|
||
**Speichert:** `links.{Link}.markers[].spin` in `robot.json` (normalisiert auf [0, 360))
|
||
|
||
**Details und Roadmap:** [`doc/Kalibrierung_Marker.md`](Kalibrierung_Marker.md)
|
||
|
||
---
|
||
|
||
## Dateistruktur
|
||
|
||
```
|
||
data/
|
||
calibration/
|
||
{session}/
|
||
meta.json
|
||
{cam}_calibration.npz ← Kameraparameter
|
||
{cam}_aruco_detection.json ← Marker-Erkennung
|
||
{cam}_camera_pose.json ← Kamera-Pose
|
||
scripts/
|
||
robot_1781069752019.json ← robot.json (Haupt-Konfiguration)
|
||
links.Board.markers ← Board-Marker-Positionen
|
||
links.Base.markers ← fixe Basis-Marker (nach Y-Kalibrierung)
|
||
links.Arm1.jointToParent.origin ← Schultergelenk-Position (nach Y-Kalibrierung)
|
||
links.Arm1.markers ← Arm1-Marker (Nutzer eingetragen)
|
||
...
|
||
```
|