Callibration Mathe
This commit is contained in:
chk
303
doc/Kalibrierung.md
Normal file
303
doc/Kalibrierung.md
Normal file
@@ -0,0 +1,303 @@
|
||||
# Kalibrierung – appRobotHoming
|
||||
|
||||
> Stand: 2026-06-14
|
||||
> 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
|
||||
```
|
||||
|
||||
| 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` | ✅ |
|
||||
| Arm-Marker eintragen | — | `links.Arm1/Ellbow/Arm2/Hand.markers` | 🔶 Nutzer |
|
||||
|
||||
---
|
||||
|
||||
## [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
|
||||
|
||||
---
|
||||
|
||||
### 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.**
|
||||
|
||||
---
|
||||
|
||||
## 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)
|
||||
...
|
||||
```
|
||||
Reference in New Issue
Block a user