Реализация алгоритма Дейкстры для поиска оптимального пути между двумя точками на поверхности и обхода препятствий16.11.2022 15:47

Приветствую Вас, Хабровчане!

Я думаю, по названию статьи и так понятно о чем я Вам буду рассказывать в этой работе. Поэтому не вижу смысла в длинных преамбулах и аннотациях. Ну, а для тех, кто совсем не в теме и фамилию известного нидерландского ученого видят впервые — отправляю Вас на википедию с его биографией и алгоритмом, о котором далее пойдет речь.

И да, я прекрасно понимаю, что я далеко не первый и не последний кто находит в себе силы попробовать на практике реализовать этот алгоритм. Как говорится, повторение — мать учения. Поэтому не нужно меня за это закидывать камнями, палками и чем бы то ни было. Я лишь предложу свой вариант реализации и продемонстрирую результаты. Как Вы уже догадались, писать будем на языке C#. Исходники найдете здесь.

Поехали!

Содержание

Математическая постановка задачи

В декартовой прямоугольной системе координат на плоскости задана равномерная сетка:

$\{ x_i = i \cdot dx, \ \ i = 0, \ldots, N-1 \} \\ \{ y_j = j \cdot d y,\ \ j = 0, \ldots, M-1 \}$

где (x_i, y_j) — узлы сетки; dx, dy — шаги сетки; N, M — количество точек по осии, соответственно. В каждом узле сетки задано значение $z_{i, j} = z (x_i, y_j)$ , представляющее собой высоту карты (ландшафта) местности в рассматриваемой точке. Значения $z_{i, j}$ , образующие исследуемую поверхность, могут быть отрицательными — в таком случае высоту карты следует понимать как глубину относительно некоторой нулевой отметки z=0 .

Заданы стартовая и целевая точка искомого оптимального маршрутаи, рисунок 1.

Рисунок 1. Пример ландшафта местности — исследуемая поверхность, заданная множеством точек с заданными началом, A, и концом, B, искомого маршрута.

На выходе мы должны получить конечный список (массив) координат (x_k, y_k) , следуя по которым мы доберемся из точкив точкунаиболее оптимальным образом.

Также в алгоритме будет учитываться параметр, задающий максимальный угол уклона (в градусах) для поиска подходящих соседей.

Так, ну, а при чем тут графы и какой-то алгоритм ?!

Графы и алгоритм Дейкстры

Алгоритм Дейкстры представляет собой глобальный алгоритм поиска кратчайших путей на взвешенном графе. О том, что такое граф, и с чем его едят, что такое вершина, ребро, какие графы бывают — я здесь Вам рассказывать не буду. На тему графов написано немало хороших книг, на одну из которых я дам ссылку в конце статьи. А пока я буду предполагать, что Вы уже знакомы с основными понятиями теории графов. Хотя, как по мне, большинство терминов, которые будут описаны далее в статье должны быть понятны и ежу на интуитивном уровне.

Как я упомянул выше, алгоритм Дейкстры является глобальным, я бы даже сказал самым глобальным из всех подобных. Имеются и другие алгоритмы, которые в ряде случаев оптимизируют и ускоряют поиск, например, такие как алгоритм Ли, алгоритм A* и др. Очень хороший обзор на алгоритмы поиска (в т.ч. и Дейкстры) да еще и с примерами кода можете найти тут.

Ну, а я просто захотел попробовать сделать это своими силами. Как говорится, хочешь сделать что-то хорошо — сделай это сам! =)

Так в чем же именно глобальность алгоритма Дейкстры?! Глобальность в том, что даже если нам будет необходимо найти кратчайший путь между двумя смежными вершинами графа, в котором 1000 вершин, нам так или иначе придется перебрать абсолютно все 1000 вершин.

Поясню это на примере взвешенного графа с 6-ю вершинами, рисунок 2.

Рисунок 2. Взвешенный граф с 6-ю вершинами. Синим цветом обозначен вес ребра. Рисунок 2. Взвешенный граф с 6-ю вершинами. Синим цветом обозначен вес ребра.

Найдем кратчайший путь из вершиныдо вершины. Даже не «запуская» на этом графе алгоритм Дейкстры, очевидно, что кратчайший путь изв будет путь $A \rightarrow C \rightarrow E \rightarrow F \rightarrow D$ , длина которого равна 1+3+2+5=11 . Так вот, несмотря на то, что вершиныисмежные — нам все равно пришлось рассматривать ВСЕ вершины этого графа. В этом и заключается глобальность, и в то же время сложность этого алгоритма (я специально подобрал весовые коэффициенты для некоторых ребер намного большими чем другие).

Превращаем поверхность в граф

Чтобы решить нашу задачу с помощью алгоритма Дейкстры нам нужно каким-то образом «превратить» нашу исследуемую поверхность во взвешенный граф. Делать будем это так, — посмотрим на нашу поверхность сверху (перпендикулярно плоскости Oxy ), вид будет примерно такой, рисунок 3:

Рисунок 3. Граф, построенный по исследуемой поверхности. В него добавлены «диагональные» ребра, позволяющие алгоритму двигаться в более широком диапазоне направлений.

Точки поверхности будут спроецированы на плоскость Oxy — это и будут вершины нашего графа. Но откуда появились эти «крестики» в каждом квадратике, спросите Вы ?! Эти крестики — тоже ребра графа, которые добавлены для того, чтобы у нас была возможность ходить не только по горизонтали и вертикали, но и по диагонали. Да, это сильно увеличит сложность алгоритма и время расчета, но зато путь будет еще более оптимальным.

Таким образом любую вершину подобного графа можно идентифицировать с помощью координаты (i, j) , что нам очень пригодится в дальнейшем для хранения всех вершин графа в виде матрицы.

Наш граф должен быть взвешенным. Вот тут, конечно, можно поиграться и задавать весовые коэффициенты любым изощренным способом. Но я пока поступлю просто — вес (W) ребра, соединяющего смежные вершины V_1 и V_2 , заданные своими координатами (i, j) будет не что иное как расстояние в пространстве между соответствующими точками поверхности, из которой наш граф был построен:

$W(V_1, V_2) = \sqrt{(x_2 - x_1)^2+(y_2 - y_1)^2 + (z_2 - z_1)^2}$

Рассматриваемый граф является связным (причем степень связности достаточно неплохая!, особенно после добавления диагональных ребер), поэтому «тупиковых» ситуаций возникнуть не должно и если все учтено правильно, алгоритм все-таки после долгих блужданий доберется из стартовой в целевую вершину и на своем пути «переберет» абсолютно все вершины графа.

Ну все…осталось запустить алгоритм Дейкстры!

Само собой при численной реализации этого алгоритма возникнут определенные сложности много костылей и вопросы. Их я уже буду комментировать в процессе объяснения написанного кода.

Численная реализация

Кодить, как я уже говорил будем на C#, поэтому постараемся взять все самое лучшее из возможностей этого языка. А возможностей у него немало.

Я создам в Visual Studio консольный проект (.NET 4.7.2). Ключевыми классами будут Point2D.cs, Vertex.cs и Graph.cs. Начнем с самого простого.

Класс Point2D будет содержать всего два свойства для хранения координат вершины графа:

public class Point2D
    {
        public int i { get; }
        public int j { get; }

        public Point2D(int i, int j)
        {
            this.i = i;
            this.j = j;
        }
    }

С классом Vertex уже чуть поинтереснее:

public class Vertex
    {
        public Point2D Coordinate { get; set; }
        public double Height { get; set; }
        public Point2D CameFrom { get; set; }
        public double Label { get; set; }
        public bool IsVisited { get; set; }
        public bool IsGoal { get; set; }
        public bool IsObstacle { get; set; }

        public Vertex(int i, int j, Point2D CameFrom = null, double Height = 0.0, double Label = double.MaxValue, bool IsVisited = false, bool IsGoal = false, bool IsObstacle = false)
        {
            Coordinate = new Point2D(i, j);
            this.CameFrom = CameFrom;
            this.Height = Height;           
            this.Label = Label;
            this.IsVisited = IsVisited;
            this.IsGoal = IsGoal;
            this.IsObstacle = IsObstacle;
        }
    }

Опишу кратко свойства.

Первое из них — это координаты вершины графа; Height — высота, значение функции z(x, y) в точке вершины, необходимое для расчета весового коэффициента и величины уклона между двумя смежными вершинами; CameFrom — здесь в процессе работы алгоритма будут храниться координаты вершины из которой мы попали в текущую вершину — на основании этого свойства мы в конце работы алгоритма сформируем наш искомый маршрут; Label — метка, хранящая значение длины пути из стартовой вершины в текущую; IsVisited — говорит нам посетили ли мы данную вершину в процессе расчета или нет; IsGoal — данное свойство будет истинным только для целевой вершины графа (та вершина, оптимальный путь к которой мы ищем). Это свойство понадобилось мне для того, чтобы алгоритм Дейкстры не завершился раньше времени и мы обошли абсолютно все вершины графа; IsObstacle — наш алгоритм будет также уметь обходить препятствия, например, искать кратчайший путь в лабиринте, это свойство позволяет задать определенные вершины как вершины-препятствия, чтобы выбрасывать их из рассмотрения наряду с уже посещенными.

Теперь о классе Graph.

Свойства класса Graph:

 public class Graph
    {
        /// 
        /// Шаг сетки по оси Ox
        /// 
        public double dx { get; }
        /// 
        /// Шаг сетки по оси Oy
        /// 
        public double dy { get; }
        /// 
        /// Количество вершин по оси Ox
        /// 
        public int N { get; }
        /// 
        /// Количество вершин по оси Oy
        /// 
        public int M { get; }
        /// 
        /// Матрица вершин графа
        /// 
        public Vertex[,] Vertices { get; }
        /// 
        /// Предельная величина уклона, необходимая для обхода препятствий, в градусах
        /// 
        public double MaxSlope { get; }
	}

Для работы алгоритма реализуем несколько вспомогательных методов.

Для расчета весов и уклона нам нужно будет получать «реальные» координаты вершины графа на плоскости Oxy (с учетом величины шагов и ):

(double, double) GetRealXY(Vertex vertex)
        {
            double x = vertex.Coordinate.i * dx;
            double y = vertex.Coordinate.j * dy;

            return (x, y);
        }

Вес ребра между смежными вершинами (расстояние между точками поверхности) находим так:

double Weight(Vertex v1, Vertex v2)
        {
            (double, double) x1y1 = GetRealXY(v1);
            (double, double) x2y2 = GetRealXY(v2);

            double xDiff = x1y1.Item1 - x2y2.Item1;
            double yDiff = x1y1.Item2 - x2y2.Item2;
            double zDiff = v1.Height - v2.Height;

            double sumOfSquares = Math.Pow(xDiff, 2.0) + Math.Pow(yDiff, 2.0) + Math.Pow(zDiff, 2.0);

            return Math.Sqrt(sumOfSquares);
        }

Следующий метод возвращает величину уклона между смежными вершинами (в градусах):

private double Slope(Vertex v1, Vertex v2)
        {
            double hypotenuse = Weight(v1, v2); // Вес ребра - это и есть по факту расстояние между точками
            double zDiffAbs = Math.Abs(v1.Height - v2.Height); // Модуль разности по высоте

            return Math.Asin(zDiffAbs / hypotenuse) * 180.0 / Math.PI; // Переводим радианы в градусы
        }

Для удобства реализуем 8 методов для получения соседней вершины в зависимости от направления (я привел один из них):

private Vertex GetTopVertex(Vertex v) => Vertices[v.Coordinate.i, v.Coordinate.j + 1];

И еще 8 методов для определения принадлежности вершины той или иной части сетки (здесь я привел два из них):

private bool IsTopRightVertex(Vertex v1) => v1.Coordinate.i == N - 1 && v1.Coordinate.j == M - 1;

private bool IsVertexOnTheRightSide(Vertex v1) => v1.Coordinate.i == N - 1;

Эти методы понадобятся для получения всех смежных вершин для рассматриваемой вершины. Вот тут нам придется нарубить if-ов, т.к. количество смежных вершин не всегда одно и то же:

Метод GetAllAdjacentVertices (Vertex vertex) для получения всех смежных вершин

private List GetAllAdjacentVertices(Vertex vertex)
        {
            #region Рассматриваем угловые вершины

            if (IsTopRightVertex(vertex))
                return new List
                {
                    GetLeftVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetBottomVertex(vertex)
                };

            if (IsBottomRightVertex(vertex))
                return new List
                {
                    GetTopVertex(vertex),
                    GetTopLeftVertex(vertex),
                    GetLeftVertex(vertex)
                };

            if (IsBottomLeftVertex(vertex))
                return new List
                {
                    GetTopVertex(vertex),
                    GetTopRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            if (IsTopLeftVertex(vertex))
                return new List
                {
                    GetBottomVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            #endregion

            #region Рассматриваем боковые вершины

            if (IsVertexOnTheTopSide(vertex))
                return new List
                {
                    GetLeftVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetBottomVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            if (IsVertexOnTheRightSide(vertex))
                return new List
                {
                    GetTopVertex(vertex),
                    GetTopLeftVertex(vertex),
                    GetLeftVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetBottomVertex(vertex)
                };

            if (IsVertexOnTheBottomSide(vertex))
                return new List
                {
                    GetLeftVertex(vertex),
                    GetTopLeftVertex(vertex),
                    GetTopVertex(vertex),
                    GetTopRightVertex(vertex),
                    GetRightVertex(vertex)
                };

            if (IsVertexOnTheLeftSide(vertex))
                return new List
                {
                    GetTopVertex(vertex),
                    GetTopRightVertex(vertex),
                    GetRightVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetBottomVertex(vertex)
                };

            #endregion

            // Иначе вершина лежит "в середине карты" и нужно вернуть все 8 смежных вершин
            return new List
                {
                    GetTopVertex(vertex),
                    GetRightVertex(vertex),
                    GetBottomVertex(vertex),
                    GetLeftVertex(vertex),

                    GetTopRightVertex(vertex),
                    GetBottomRightVertex(vertex),
                    GetBottomLeftVertex(vertex),
                    GetTopLeftVertex(vertex)
                };
        }

Напишем метод, который будет «отсеивать неподходящих» соседей:

private List GetValidNeighbors(Vertex current)
        {
            // Из всех смежных вершин оставляем только те, которые 
            // 1. Еще не посещены
            // 2. Не являются вершинами-препятствиями
            // 3. Наклон к которым меньше заданной величины (например, 30 градусов)
            return GetAllAdjacentVertices(current).Where(v => !v.IsVisited && !v.IsObstacle && Slope(v, current) < MaxSlope).ToList();
        }

и метод, который из оставшихся соседей будет отсеивать целевую вершину и сообщать осталось ли что-то после отсеивания:

private bool HasValidAndNotGoalNeighbors(Vertex vertex, out List validAndNotGoalNeighbors)
        {
            validAndNotGoalNeighbors = GetValidNeighbors(vertex).Where(v => !v.IsGoal).ToList();
            return validAndNotGoalNeighbors.Any();
        }

Именно этот метод (в основном) и нужен для того, чтобы мы перебрали абсолютно все вершины графа (помните о глобальности, да?!). Так как бывают кейсы, когда мы натыкаемся на целевую вершину «раньше срока» (раньше того, как алгоритм пробежит по всем вершинам графа и найдет нам оптимальный маршрут).

Также в основном while-цикле метода по поиску пути (скоро мы уже дойдем до него =)) я буду накапливать в список посещенные вершины. Это тоже относится к замечанию выше и будет гарантировать нам «полный обход» всех вершин графа. Этот список посещенных вершин я буду использовать в следующем методе.

Метод для получения новой «текущей» вершины:

GetCurrent (List visitedVertices)

private Vertex GetCurrent(List visitedVertices)
        {
            List validAndNotGoalNeighbors = new List();

            foreach (Vertex v in visitedVertices)
                if (HasValidAndNotGoalNeighbors(v, out validAndNotGoalNeighbors))
                    break;

            // Если не нашлось ни одного подходящего соседа, значит мы дошли 
            // до целевой вершины. Алгоритм завершен
            if (!validAndNotGoalNeighbors.Any())
                return null;

            // Иначе находим и возвращаем соседа с минимальной меткой
            double minLabel = validAndNotGoalNeighbors.Min(v => v.Label);
            Vertex newCurrent = validAndNotGoalNeighbors.First(v => v.Label == minLabel);

            return newCurrent;
        }

И вот он! Метод для поиска кратчайшего (оптимального) пути (и его длины):

public List FindShortestPathAndLength(Point2D startPoint, Point2D goalPoint, out double shortestPathLength)
        {
            shortestPathLength = 0.0;
            // Стартовой вершине присваиваем нулевую метку
            Vertex start = Vertices[startPoint.i, startPoint.j];
            start.Label = 0.0;
            // Целевую вершину пометим, что она целевая
            Vertex goal = Vertices[goalPoint.i, goalPoint.j];
            goal.IsGoal = true;
            // Помечаем стартовую вершину как текущую
            Vertex current = start;
  
            // В этом списке будем копить посещенные вершины
            List visitedVertices = new List();
  
            while (current != null)
            {
                // Находим подходящих (годных) соседей: которые еще не посещены, не являются препятствиями и т.п.
                List neighbors = GetValidNeighbors(current);

                foreach (Vertex neighbor in neighbors)
                {
                    double currentWeight = current.Label + Weight(current, neighbor);
                    if (currentWeight < neighbor.Label)
                    {
                        neighbor.Label = currentWeight;
                        neighbor.CameFrom = current.Coordinate;
                    }                    
                }

                // После того как все подходящие соседи рассмотрены (им расставлены метки), помечаем текущую вершину как посещенную
                current.IsVisited = true;
                // и добавляем ее в список посещенных вершин
                visitedVertices.Add(current);
                // Используем этот список посещенных вершин для поиска новой текущей вершины
                current = GetCurrent(visitedVertices);
            }

            // В конце работы алгоритма в целевой вершине в свойстве Label будет находиться длина искомого пути
            shortestPathLength = goal.Label;
            // Основываясь на свойстве CameFrom сформируем и вернем сам искомый путь
            return GetShortestPath(goal);
        }

Следующий метод на основе свойства CameFrom класса Vertex «собирает» и возвращает нам искомый путь:

GetShortestPath (Vertex goal)

private List GetShortestPath(Vertex goal)
        {
            List path = new List();
            
            path.Add(goal.Coordinate);
            Point2D cameFrom = goal.CameFrom;

            while (cameFrom != null)
            {
                Vertex vertex = Vertices[cameFrom.i, cameFrom.j];
                path.Add(vertex.Coordinate);
                cameFrom = vertex.CameFrom;
            }

            return path;
        }

Надо заметить, что массив, содержащий координаты искомого маршрута будет сформирован «задом на перед». Т.е. первым элементом массива будут координаты целевой вершины, а последним элементом — координаты стартовой.

Ну вот, в принципе, и все!

Весь остальной код, классы и методы, — являются вспомогательными (вычисление двумерного Гауссиана, запись/чтение из файла и т.п.). Если Вы дошли до этого абзаца и хорошо понимаете, что происходит, остальную часть, я думаю, понять Вам не составит труда. Поэтому не буду на нем останавливаться, тем более что код подробно прокомментирован.

Стоит, наверное, все-таки упомянуть о двух перегруженных конструкторах класса Graph, один из которых инициализирует граф с помощью поверхности (для расчетов я использовал только Гауссиан, можете поиграться с другими поверхностями), а другой конструктор инициализирует граф с помощью матрицы из нулей и единичек (матрица-препятствий) для создания сложных лабиринтов.

Давайте наконец посмотрим на результаты!

Результаты расчетов. Простые препятствия

Начнем с простого примера. Создадим на сетке небольшое вертикальное препятствие, рисунок 4, и позволим алгоритму обойти его:

Рисунок 4. Поиск кратчайшего маршрута с вертикальным препятствием. Шаги dx = dy = 1. Количество вершин графа — 15×10 = 150. Черные вершины — препятствия, красные — кратчайший путь.

Вершины графа, которые попадают под черную линию имеют свойство IsObstacle = true, чтобы пометить их как вершины-препятствия. Параметр уклона MaxSlope здесь не имеет смысла, т.к. алгоритм работает в одной плоскости. Как видите, результат очень правдоподобный.

Из архива. Как я формировал это препятствие в csv-файле. Координаты пути

Для чтения матрицы из csv-файла и других вспомогательных действий создан класс Obstacle.

Рассмотрим следующий пример, рисунок 5:

Рисунок 5. Проход «свозь» препятствие.

В данном случае я бы не сказал, что это является ошибкой, ведь алгоритм учитывает как препятствие только те вершины графа, которые непосредственно помечены как вершины-препятствия (IsObstacle = true).

А вот если мы сформируем препятствие вот так, тогда алгоритм никуда не денется и ему придется его обойти, рисунок 6:

Рисунок 6. Обход ступенчатого препятствия.

Лабиринты

Усложним работу для алгоритма и сформируем для него целый лабиринт препятствий, рисунок 7:

Рисунок 7. Поиск кратчайшего пути между двумя заданными точками в лабиринте. Красным отмечен кратчайший путь из вершины A в вершину B.

Увеличим лабиринт и изменим целевую вершину, рисунок 8:

Рисунок 8. Поиск кратчайшего пути в большом лабиринте.

Все исходные файлы по которым были построены все вышеперечисленные препятствия и лабиринты, а также координаты найденных путей находятся в проекте в соответствующих папках.

Поиск оптимального пути на поверхности

Ну и в конце приведу пример поиска оптимального пути между двумя точками на сложной поверхности. Именно оптимального, т.к. нельзя сказать, что он кратчайший, рисунок 9:

Рисунок 9. Поиска оптимального пути между двумя точками поверхности. Параметр MaxSlope= 20 градусов.

Для имитации холмов и оврага были использованы двумерные функции Гаусса с различными параметрами. Для имитации искусственных сооружений в класс Graph был добавлен метод:

CreateBuilding ()

public void CreateBuilding(Point2D bottomLeftCoordinate, int width, int length, double height)
        {
            for (int i = bottomLeftCoordinate.i; i < bottomLeftCoordinate.i + width; i++)
            {
                for (int j = bottomLeftCoordinate.j; j < bottomLeftCoordinate.j + length; j++)
                    Vertices[i, j].Height = height;
            }
        }

Замечу, что здесь ни одна вершина графа не помечена как препятствие, алгоритм находит оптимальный путь благодаря заданию параметра MaxSlope. Увеличивая этот параметр алгоритм может пойти прямиком через горы.

И еще картинки с других ракурсов.

Рисунок 10. Вид с другого ракурса. Без осей координат. Рисунок 10. Вид с другого ракурса. Без осей координат.Рисунок 11. Вид сверху.

Заключение

Спасибо за прочтение!

Надеюсь статья была интересной и полезной. Все доработки и оптимизацию алгоритма оставляю Вам.

Литература:

Робин Уилсон. Введение в теорию графов. Пятое издание. 2019

Всем добра и качественного кода!