Bahnplanungsframework für ein autonomes Fahrzeug - oops ...

66 Suchalgorithmen
.
.
.
A
M
B
(Abb. 6.12): Satz des Thales: Alle Winkel in einem Halbkreisbogen sind rechte Winkel.
Es muss zunächst der Mittelpunkt ⃗M zwischen ⃗C Ω und ⃗b berechnet werden, der den Mittelpunkt
des Thaleskreises bildet und seinen Radius r T :
⃗M = ⃗ C Ω + ⃗b
2
r T = | ⃗C Ω − ⃗b|
2
Der Schnitt des Thaleskreises mit dem zuvor berechneten Kreis, gibt zwei Punkte, zu denen der
Punkt ⃗c gehört. Der richtige Punkt liegt in der gleichen Richtung, wie die zuvor berechnete Normale
⃗n des Fahrzeugs, somit muss das Skalarprodukt der beiden positiv sein, um den Punkt ⃗c zu identifizieren.
(⃗c − ⃗C Ω ) · ⃗n ≥ 0
Der Winkel β berechnet sich durch:
β = cos −1 (
)
(⃗a − ⃗C Ω ) · (⃗c − ⃗C Ω )
rmin
2
Die endgültige Länge des Weges, die zugleich die Heuristik darstellt berechnet sich schließlich:
h(⃗a,⃗b) = r min · β + |⃗b −⃗c|
Dabei ist zu beachten, dass β in Radians vorliegen muss.
Diese Heuristik ist größer oder gleich dem euklidischen Abstand, wobei Gleichheit nur dann vorliegt,
wenn das Fahrzeug exakt Richtung Ziel ausgerichtet ist. Dies bedeutet, dass man etwas besser
schätzt, als mit dem euklidischen Abstand, jedoch immer noch geringer, als der tatsächliche Weg.
∀x, y : d(x, y) ≤ h(x, y) ≤ g(x, y)