diff --git a/doc/04_y_achse.md b/doc/04_y_achse.md new file mode 100644 index 0000000..68d9bf6 --- /dev/null +++ b/doc/04_y_achse.md @@ -0,0 +1,127 @@ +# Bestimmung der Y-Rotationsachse aus Marker-Positionen + +## Problem + +Wenn der Roboter um seine Y-Achse rotiert, bewegt sich jeder erkannte Marker auf einem **Kreisbogen** im 3D-Raum. Ziel ist es, Lage und Richtung dieser Rotationsachse aus den beobachteten Marker-Positionen (x, y, z) zu mehreren Zeitstempeln zu berechnen. + +--- + +## Reichen zwei Positionen? + +**Nein.** Zwei Punkte P₁, P₂ desselben Markers definieren nur eine Strecke. Die Rotationsachse muss durch die senkrechte Mittelebene dieser Strecke laufen – aber *wo* genau in dieser Ebene und in welcher Richtung bleibt unbestimmt. Das System ist unterbestimmt. + +| Beobachtungen | Bestimmbar? | Begründung | +|---|---|---| +| 1 Marker, 2 Positionen | Nein | Achse liegt irgendwo auf einer Halbebene | +| 1 Marker, 3 Positionen | **Ja** | Eindeutiger Umkreis → eindeutige Achse | +| N Marker, je ≥ 3 Positionen | Ja, überbestimmt | Least-Squares, robuster gegenüber Messrauschen | + +--- + +## Mathematik: 1 Marker, 3 Positionen + +Gegeben: P₁, P₂, P₃ ∈ ℝ³ (derselbe Marker zu drei verschiedenen Rotationswinkeln). + +Da die Rotation starr ist, liegen P₁, P₂, P₃ auf einem **Kreis**, dessen Ebene senkrecht zur Rotationsachse steht. + +### Schritt 1 – Achsenrichtung (Normalenvektor der Kreisebene) + +``` +v₁ = P₂ − P₁ +v₂ = P₃ − P₁ +n = (v₁ × v₂) / |v₁ × v₂| ← Einheitsvektor entlang der Rotationsachse +``` + +### Schritt 2 – Mittelpunkt des Kreises (Umkreismittelpunkt) + +Der Umkreismittelpunkt C liegt in der Ebene der drei Punkte und ist von allen drei Punkten gleich weit entfernt. Er ist ein Punkt **auf der Rotationsachse**. + +Berechnung über Baryzentrischen Koordinaten: + +``` +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₃) +``` + +### Schritt 3 – Rotationsachse + +``` +Achse: r(t) = C + t·n, t ∈ ℝ +``` + +--- + +## Mathematik: N Marker, je ≥ 3 Positionen (Least Squares) + +Jeder Marker i liefert: +- einen Schätzwert **Cᵢ** (Umkreismittelpunkt, Punkt auf der Achse) +- einen Schätzwert **nᵢ** (Normalenvektor, Achsenrichtung) + +### Achsenrichtung (gemittelt / PCA) + +Wegen möglichem Vorzeichenambiguität alle nᵢ auf dasselbe Halbraum ausrichten, dann: + +``` +n̄ = mean(nᵢ) / |mean(nᵢ)| +``` + +Robuster: **PCA** über die Matrix der nᵢ-Vektoren → erster Hauptkomponentenvektor. + +### Achsenposition (Least Squares) + +Minimiere die quadratischen Abstände aller Umkreismittelpunkte Cᵢ zur gesuchten Geraden `r(t) = A + t·n̄`: + +``` +Abstand² von Cᵢ zur Achse = |Cᵢ − A|² − ((Cᵢ − A)·n̄)² +``` + +Ableitung null setzen liefert: + +``` +A = C̄ − (C̄·n̄)·n̄ + (C̄·n̄)·n̄ = C̄ (Referenzpunkt = Schwerpunkt der Cᵢ) +``` + +d. h., der **Schwerpunkt der Umkreismittelpunkte** ist der optimale Referenzpunkt auf der Achse. Die Achsenrichtung n̄ bleibt unverändert. + +**Verbleibender Fehler** (Residuum pro Marker): + +``` +εᵢ = |(Cᵢ − C̄) − ((Cᵢ − C̄)·n̄)·n̄| +``` + +Große εᵢ deuten auf einen fehlerhaften Marker oder eine nicht-rotatorische Bewegungskomponente hin. + +--- + +## Praktische Empfehlung + +- **Mindestens 3 Zeitstempel** pro Marker, verteilt über einen Winkelbereich von ≥ 30° (sonst ist der Kreis numerisch schlecht konditioniert). +- **3–4 Marker** mit je 3 Positionen sind ausreichend für eine stabile Schätzung. +- Die Rotationswinkel müssen nicht bekannt sein – nur die 3D-Koordinaten der Marker. +- Bei sehr kleinen Winkeln (< 10°) ist die Bestimmung numerisch instabil; größere Drehungen bevorzugen. + +--- + +## Zusammenfassung + +``` +Eingabe: Marker-Positionen P[marker][timestamp] ∈ ℝ³ + (mind. 3 Timestamps pro Marker, mind. 1 Marker) + +Pro Marker: + 1. n_i = normalize((P2-P1) × (P3-P1)) ← Achsenrichtung + 2. C_i = Umkreismittelpunkt(P1, P2, P3) ← Punkt auf Achse + +Kombination: + 3. n̄ = normalize(mean(n_i)) ← beste Achsenrichtung + 4. C̄ = mean(C_i) ← bester Referenzpunkt + +Ergebnis: Rotationsachse r(t) = C̄ + t·n̄ +```