Я бы порекомендовал альтернативный подход: быстро исследующее случайное дерево (RRT) . Одна крутая вещь об этом - вы можете заставить его идти по углам или взрываться во всех направлениях.
Алгоритм действительно прост:
// Returns a random tree containing the start and the goal.
// Grows the tree for a maximum number of iterations.
Tree RRT(Node start, Node goal, int maxIters)
{
// Initialize a tree with a root as the start node.
Tree t = new Tree();
t.Root = start;
bool reachedGoal = false;
int iter = 0;
// Keep growing the tree until it contains the goal and we've
// grown for the required number of iterations.
while (!reachedGoal || iter < maxIters)
{
// Get a random node somewhere near the goal
Node random = RandomSample(goal);
// Get the closest node in the tree to the sample.
Node closest = t.GetClosestNode(random);
// Create a new node between the closest node and the sample.
Node extension = ExtendToward(closest, random);
// If we managed to create a new node, add it to the tree.
if (extension)
{
closest.AddChild(extension);
// If we haven't yet reached the goal, and the new node
// is very near the goal, add the goal to the tree.
if(!reachedGoal && extension.IsNear(goal))
{
extension.AddChild(goal);
reachedGoal = true;
}
}
iter++;
}
return t;
}
Изменяя RandomSample
и ExtendToward
функции, вы можете получить очень разные деревья. Если RandomSample
повсюду будут одинаковые выборки, дерево будет расти равномерно во всех направлениях. Если оно смещено к цели, дерево будет стремиться расти к цели. Если это всегда выборка цели, дерево будет прямой линией от начала до цели.
ExtendToward
может позволить вам делать интересные вещи с деревом, а также. С одной стороны, если у вас есть препятствия (например, стены), вы можете заставить дерево расти вокруг них, просто отклоняя расширения, которые сталкиваются со стенами.
Вот как это выглядит, когда вы не смещаете выборку к цели:
(источник: uiuc.edu )
А вот как это выглядит со стенами
Некоторые интересные свойства RRT после его завершения:
- RRT никогда не пересечет себя
- RRT в конечном итоге покроет все пространство все меньшими и меньшими ветвями
- Путь от начала до цели может быть совершенно случайным и странным.