128 lines
4.1 KiB
Markdown
128 lines
4.1 KiB
Markdown
# 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̄
|
||
```
|