Witamy w cudownym świecie niehonomicznego planowania ruchu. Polecam to zrobić za pomocą planera ścieżki siatki kratowej . Inne alternatywy obejmują kinodynamiczną RRT i optymalizację trajektorii . Systemy niehonomiczne obejmują samochody, łodzie, monocykle lub cokolwiek, w którym pojazd nie może jechać w dowolnym kierunku. Planowanie tych systemów jest znacznie trudniejsze niż systemy holonomiczne i do ~ 2000 roku znajdowało się na krawędzi badań akademickich. Obecnie istnieje wiele algorytmów do wyboru, z których działa przyzwoicie.
Oto jak to działa.
Stan
Konfiguracja twojego samochodu q jest w rzeczywistości stanem 3D zawierającym pozycję x, y i jego orientację t . Węzły w algorytmie A * są w rzeczywistości wektorami 3D.
class Node
{
// The position and orientation of the car.
float x, y, theta;
}
działania
A co z krawędziami?
To nieco trudniejsze, ponieważ twój samochód może faktycznie wybrać nieskończoną liczbę sposobów obracania koła. Tak, możemy zrobić to dostępne do planowania sieci kratowej poprzez ograniczenie liczby działań samochód może podjąć w celu dyskretnego zbioru, A . Dla uproszczenia załóżmy, że samochód nie przyspiesza, ale może natychmiast zmienić prędkość. W naszym przypadku A może wyglądać następująco:
class Action
{
// The direction of the steering wheel.
float wheelDirection;
// The speed to go at in m/s.
float speed;
// The time that it takes to complete an action in seconds.
float dt;
}
Teraz możemy stworzyć dyskretny zestaw działań, które samochód może podjąć w dowolnym momencie. Na przykład twarde prawe naciśnięcie gazu do końca przez 0,5 sekundy wyglądałoby tak:
Action turnRight;
turnRight.speed = 1;
turnRight.wheelDirection = 1;
turnRight.dt = 0.5;
Włączenie cofania i cofanie samochodu wyglądałoby następująco:
Action reverse;
reverse.speed = -1;
reverse.wheelDirection = 0;
reverse.dt = 0.5;
Twoja lista działań wyglądałaby następująco:
List<Action> actions = { turnRight, turnLeft, goStraight, reverse ...}
Potrzebny jest także sposób zdefiniowania, w jaki sposób działanie podjęte w węźle powoduje powstanie nowego węzła. Nazywa się to dynamiką naprzód systemu.
// These forward dynamics are for a dubin's car that can change its
// course instantaneously.
Node forwardIntegrate(Node start, Action action)
{
// the speed of the car in theta, x and y.
float thetaDot = action.wheelDirection * TURNING_RADIUS;
// the discrete timestep in seconds that we integrate at.
float timestep = 0.001;
float x = start.x;
float y = start.y;
float theta = start.theta;
// Discrete Euler integration over the length of the action.
for (float t = 0; t < action.dt; t += timestep)
{
theta += timestep * thetaDot;
float xDot = action.speed * cos(theta);
float yDot = action.speed * sin(theta);
x += timestep * xDot;
y += timestep * yDot;
}
return Node(x, y, theta);
}
Dyskretne komórki siatki
Teraz, aby zbudować siatkę kratową, wszystko, co musimy zrobić, to hash stany samochodu w dyskretne komórki siatki. To zamienia je w odrębne węzły, po których może następować A *. Jest to bardzo ważne, ponieważ w przeciwnym razie A * nie miałby możliwości dowiedzenia się, czy dwa stany samochodu są takie same, aby je porównać. Przez mieszanie z wartościami całkowitymi komórek siatki staje się to trywialne.
GridCell hashNode(Node node)
{
GridCell cell;
cell.x = round(node.x / X_RESOLUTION);
cell.y = round(node.y / Y_RESOLUTION);
cell.theta = round(node.theta / THETA_RESOLUTION);
return cell;
}
Teraz możemy zrobić plan A *, w którym GridCells są węzłami, Działania są krawędziami między węzłami, a Start i Cel są wyrażone w postaci GridCells. Heurystyka między dwoma komórkami siatki to odległość w xiy plus odległość kątowa w theta.
Podążając ścieżką
Teraz, gdy mamy ścieżkę pod względem GridCells i akcji między nimi, możemy napisać obserwatora ścieżki dla samochodu. Ponieważ komórki siatki są dyskretne, samochód przeskakuje między komórkami. Będziemy musieli wygładzić ruch samochodu wzdłuż ścieżki. Jeśli gra korzysta z silnika fizyki, można to osiągnąć, pisząc kontroler kierowania, który stara się utrzymywać samochód jak najbliżej ścieżki. W przeciwnym razie możesz animować ścieżkę za pomocą krzywych Beziera lub po prostu uśredniając kilka najbliższych punktów na ścieżce.