Splajn sferyczny na S² — implementacja w geomstats
W poprzednim artykule wyjaśniliśmy geometryczną przyczynę szarpania na rozmaitości — nieciągłość wektora prędkości w punkcie złączenia dwóch geodezyjnych (kink). Teraz pokażemy jak skonstruować gładką alternatywę używając biblioteki geomstats i algorytmu de Casteljau na sferze.
Biblioteka geomstats #
Geomstats to biblioteka Pythona do obliczeń, statystyki i uczenia maszynowego na rozmaitościach. Powstała w 2018 roku — jej autorką jest Nina Miolane (UC Santa Barbara). Obsługuje NumPy, PyTorch i Autograd jako backendy, a jej moduł geometry implementuje między innymi sfery, grupy Liego SO(n), SE(3), rozmaitości Stieflera i Grassmanna oraz macierze SPD.
Dla naszego problemu kluczowe są dwie operacje z modułu HypersphereMetric:
metric.geodesic(initial_point=p, end_point=q) # funkcja t ∈ [0,1] → punkt na sferze
metric.log(q, p) # wektor styczny z p do q
metric.dist(p, q) # odległość geodezyjna
Instalacja:
pip install "numpy<2.0" geomstats matplotlib
Uwaga: geomstats 2.8.0 wymaga numpy w wersji poniżej 2.0. Przy nowszym numpy import zakończy się błędem
cannot import name 'trapz'. Downgrade numpy rozwiązuje problem.
Uruchamiamy z backendem numpy:
GEOMSTATS_BACKEND=numpy python skrypt.py
Problem: trasa A→W→B #
Mamy trzy punkty na sferze S²:
import os; os.environ['GEOMSTATS_BACKEND'] = 'numpy'
import numpy as np
from geomstats.geometry.hypersphere import Hypersphere
sphere = Hypersphere(dim=2)
metric = sphere.metric
p_A = np.array([0.0, 0.0, 1.0]); p_A /= np.linalg.norm(p_A) # biegun północny
p_B = np.array([1.0, 0.0, -0.2]); p_B /= np.linalg.norm(p_B)
p_W = np.array([0.3, 0.95, 0.3]); p_W /= np.linalg.norm(p_W) # waypoint boczny
Punkt W leży z dala od geodezyjnej A→B — to wymusza kink przy naiwnym złączeniu.
Trasa 1: dwie złączone geodezyjne #
Geomstats zwraca geodezyjną jako funkcję parametru t ∈ [0,1]:
N = 40
t = np.linspace(0, 1, N)
geo_AW = metric.geodesic(initial_point=p_A, end_point=p_W)(t) # (N, 3)
geo_WB = metric.geodesic(initial_point=p_W, end_point=p_B)(t) # (N, 3)
# Łączymy — pomijamy zduplikowany punkt W
piecewise = np.vstack([geo_AW, geo_WB[1:]]) # (2N-1, 3)
Każda geodezyjna jest gładka wewnątrz. Problem jest wyłącznie w punkcie W, gdzie v⁻ ≠ v⁺.
Trasa 2: algorytm de Casteljau na sferze #
Klasyczna krzywa Béziera w R^n budowana jest algorytmem de Casteljau: interpolacja liniowa między punktami kontrolnymi, rekurencyjnie. Na rozmaitości interpolację liniową zastępuje interpolacja geodezyjna — przejście po najkrótszej krzywej między dwoma punktami.
Funkcja pomocnicza — jeden krok interpolacji:
def geo_pt(p, q, tt):
"""Punkt na geodezyjnej p→q w chwili tt ∈ [0,1]."""
return metric.geodesic(initial_point=p, end_point=q)(float(tt))[0]
Rekurencja de Casteljau:
def casteljau(points, t):
"""
Algorytm de Casteljau na sferze.
points: lista punktów kontrolnych na S²
t: parametr ∈ [0,1]
"""
pts = [np.array(p, dtype=float) for p in points]
while len(pts) > 1:
pts = [geo_pt(pts[i], pts[i+1], t) for i in range(len(pts)-1)]
return pts[0]
Dla każdej pary sąsiednich punktów kontrolnych wykonujemy interpolację geodezyjną, skracamy listę o jeden element, i powtarzamy aż zostanie jeden punkt. Dokładnie jak klasyczny de Casteljau, ale każda „prosta" jest krzywą na sferze.
Wybór punktów kontrolnych #
Pięć punktów kontrolnych: A, punkt na geodezyjnej A→W w t=0.6, W, punkt na geodezyjnej W→B w t=0.4, B:
c1 = geo_pt(p_A, p_W, 0.6) # punkt kontrolny przed W
c2 = geo_pt(p_W, p_B, 0.4) # punkt kontrolny za W
control_points = [p_A, c1, p_W, c2, p_B]
Symetria punktów kontrolnych wokół W (0.6 i 0.4) gwarantuje że krzywa wchodzi i wychodzi z W pod zbliżonymi kątami — to jest warunek ciągłości pierwszej pochodnej w punkcie W.
Obliczenie krzywej #
t2 = np.linspace(0, 1, 2*N - 1)
spline = np.array([casteljau(control_points, tt) for tt in t2])
Pomiar szarpania #
Szarpanie mierzymy jako normę drugiej różnicy skończonej pozycji — dyskretną aproksymację drugiej pochodnej trajektorii:
def accel(pts):
v = np.diff(pts, axis=0) # Δp — prędkość
a = np.diff(v, axis=0) # Δ²p — przyspieszenie
return np.linalg.norm(a, axis=1) # norma w każdym kroku
pw_accel = accel(piecewise)
spl_accel = accel(spline)
print(f'Kink — max szarpanie: {pw_accel.max():.4f}')
print(f'Splajn — max szarpanie: {spl_accel.max():.4f}')
print(f'Redukcja: {(1 - spl_accel.max()/pw_accel.max())*100:.0f}%')
Wynik:
Kink — max szarpanie: 0.0403
Splajn — max szarpanie: 0.0017
Redukcja: 96%
Splajn redukuje maksymalne szarpanie o 96% w porównaniu ze złączonymi geodezyjnymi.
Wizualizacja #
Na wykresie prędkości (środkowy panel) widać wyraźnie skok przy t=0.5 dla złączonych geodezyjnych — to jest kink. Splajn przebiega płynnie przez ten sam punkt W.
Kompletny skrypt #
import os; os.environ['GEOMSTATS_BACKEND'] = 'numpy'
import numpy as np
import matplotlib.pyplot as plt
import matplotlib.gridspec as gridspec
from geomstats.geometry.hypersphere import Hypersphere
sphere = Hypersphere(dim=2)
metric = sphere.metric
# Punkty trasy
p_A = np.array([0.0, 0.0, 1.0]); p_A /= np.linalg.norm(p_A)
p_B = np.array([1.0, 0.0, -0.2]); p_B /= np.linalg.norm(p_B)
p_W = np.array([0.3, 0.95, 0.3]); p_W /= np.linalg.norm(p_W)
N = 40
t = np.linspace(0, 1, N)
# Trasa 1: złączone geodezyjne
geo_AW = metric.geodesic(initial_point=p_A, end_point=p_W)(t)
geo_WB = metric.geodesic(initial_point=p_W, end_point=p_B)(t)
piecewise = np.vstack([geo_AW, geo_WB[1:]])
# Trasa 2: splajn de Casteljau
def geo_pt(p, q, tt):
return metric.geodesic(initial_point=p, end_point=q)(float(tt))[0]
def casteljau(points, t):
pts = [np.array(p, dtype=float) for p in points]
while len(pts) > 1:
pts = [geo_pt(pts[i], pts[i+1], t) for i in range(len(pts)-1)]
return pts[0]
c1 = geo_pt(p_A, p_W, 0.6)
c2 = geo_pt(p_W, p_B, 0.4)
ctrl = [p_A, c1, p_W, c2, p_B]
t2 = np.linspace(0, 1, 2*N - 1)
spline = np.array([casteljau(ctrl, tt) for tt in t2])
# Metryki
def accel(pts):
return np.linalg.norm(np.diff(np.diff(pts, axis=0), axis=0), axis=1)
def speed(pts):
return np.linalg.norm(np.diff(pts, axis=0), axis=1)
pw_a = accel(piecewise); spl_a = accel(spline)
pw_s = speed(piecewise); spl_s = speed(spline)
t_a = np.linspace(0, 1, len(pw_a))
t_s = np.linspace(0, 1, len(pw_s))
print(f'Max szarpanie — kink: {pw_a.max():.4f}')
print(f'Max szarpanie — splajn: {spl_a.max():.4f}')
print(f'Redukcja: {(1 - spl_a.max()/pw_a.max())*100:.0f}%')
Gdzie to działa w praktyce #
Algorytm de Casteljau na rozmaitości jest podstawą wielu praktycznych zastosowań. W robotyce pozwala planować trajektorie ramienia przez zadane pozycje pośrednie bez szarpania w przegubach. W systemach nawigacji dronów umożliwia oblot punktów kontrolnych po gładkiej krzywej na SO(3). W sterowaniu silnikiem profile prędkości budowane jako splajny na S¹ eliminują szarpanie przy zmianach wartości zadanej.
Wspólny mianownik: wszędzie tam gdzie przestrzeń stanu ma strukturę rozmaitości i zależy nam na gładkości trajektorii, algorytm de Casteljau daje eleganckie i numerycznie stabilne rozwiązanie — dostępne wprost w geomstats.
Kod przetestowany z geomstats 2.8.0 i numpy 1.26.4. Biblioteka geomstats — autorzy: Nina Miolane i współpracownicy, licencja MIT, geomstats.ai.