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̄

4.1 KiB Raw Blame History Unescape Escape