路径规划——Dijkstra算法

算法原理

Dijkstra算法采用贪心算法的思想，解决的问题可以描述为：在无向图G=(V,E)中，假设每条边E[i] 的长度为 w[i]，找到由顶点vs到其余各点的最短路径。

通过Dijkstra计算图G中的最短路径时，需要指定起点vs(即从顶点vs开始计算)。此外，引进两个集合S和U。S的作用是记录已求出最短路径的顶点（以及相应的最短路径长度），而U则是记录还未求出最短路径的顶点（以及该顶点到起点vs的距离）。

初始时，S中只有起点vs；U中是除vs之外的顶点，并且U中顶点的路径是“起点vs到该顶点的路径”。然后，从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。然后，再从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。重复该操作，直到遍历完所有顶点。

时间复杂度为O(V^2)其中V为顶点数，但可以通过优先队列实现最小堆来优化时间复杂度。

所谓贪心是指每一点都存储该点到起点的最短路径。

本文基于栅格地图做Dijkstra路径规划，寻找下一个节点有8个方向：上、下、左、右、左上、右上、右下、左下，其距离/代价/权重分别为1、1、1、1、sqrt(2)、sqrt(2)、sqrt(2)、sqrt(2)。

算法实现

C++算法核心：

std::tuple<vector<pair<int, int>>, vector<double>> Dijkstra::get_neighbors(pair<int, int> node) {
    vector<pair<int, int>> neighbors;
    vector<double> distances;
    vector<PlanNode> nexts = {PlanNode({-1, 0}, 1), PlanNode({-1, 1}, sqrt(2)), PlanNode({0, 1}, 1), PlanNode({1, 1}, sqrt(2)),
                          PlanNode({1, 0}, 1), PlanNode({1, -1}, sqrt(2)), PlanNode({0, -1}, 1), PlanNode({-1, -1}, sqrt(2))};
    for (auto& next : nexts) {
        pair<int, int> neighbor = {node.first + next.directions.first, node.second + next.directions.second};
        if (0 <= neighbor.first && neighbor.first < grid.size() && 0 <= neighbor.second && neighbor.second < grid[0].size()) {
            if (grid[neighbor.first][neighbor.second] == 0) {
                if (next.directions.first != 0 && next.directions.second != 0) {
                    if (grid[node.first + next.directions.first][node.second] == 0 && grid[node.first][node.second + next.directions.second] == 0) {
                        neighbors.push_back(neighbor);
                        distances.push_back(next.cost);
                    }
                } else {
                    neighbors.push_back(neighbor);
                    distances.push_back(next.cost);
                }
            }
        }
    }
    return make_tuple(neighbors,distances);
}

double Dijkstra::plan() {
    priority_queue<pair<double, pair<int, int>>, vector<pair<double, pair<int, int>>>, greater<>> priority_queue;
    priority_queue.push({0, start});
    map<pair<int, int>, double> costs;
    map<pair<int, int>, pair<int, int>> previous_nodes;
    costs[start] = 0;

    while (!priority_queue.empty()) {
        auto [current_cost, current_node] = priority_queue.top();
        priority_queue.pop();

        if (visited.find(current_node) != visited.end()) continue;

        visited.insert(current_node);
        visit_order.push_back(current_node);

        if (current_node == goal) break;

        vector<pair<int, int>> neighbors;
        vector<double> distances;
        tie(neighbors,distances) = get_neighbors(current_node);

        for (size_t i=0; i<neighbors.size();++i) {
            pair<int, int> neighbor = neighbors[i];
            double distance = distances[i];
            double cost = current_cost + distance;
            // double cost = current_cost + sqrt(pow(neighbor.first - current_node.first, 2) + pow(neighbor.second - current_node.second, 2));
            if (costs.find(neighbor) == costs.end() || cost < costs[neighbor]) {
                costs[neighbor] = cost;
                previous_nodes[neighbor] = current_node;
                priority_queue.push({cost, neighbor});
            }
        }
    }

    path.clear();
    pair<int, int> current_node = goal;
    while (current_node != start) {
        path.push_back(current_node);
        current_node = previous_nodes[current_node];
        std::cout<<"node: "<<current_node.first<<","<<current_node.second<<" ";
        printf("cost:%lf\n",costs[current_node]);
    }
    path.push_back(start);
    reverse(path.begin(), path.end());

    return costs[goal];
}

算法中使用创建最小堆来优化： 

priority_queue<pair<double, pair<int, int>>, vector<pair<double, pair<int, int>>>, greater<>> priority_queue;

1.pair<double, pair<int, int>>：优先队列中的元素类型是一个 pair，它包含一个 double 和另一个包含两个 int 的 pair。

2.vector<pair<double, pair<int, int>>>：底层容器是一个 vector，用来存储这些 pair 元素。

3.greater<>：这是用于比较的函数对象。greater<> 是标准库中的一个函数对象，用于执行“大于”比较。这意味着，优先队列会将优先级最低的元素（在这里即 double 值最小的元素）放在队头，因此构成了一个最小堆。

因此，这个优先队列将按照 pair<double, pair<int, int>> 中 double 的值的升序排序，从而构成最小堆。

然后使用top()访问队头元素即值最小的元素，再使用pop()移除队头元素。

Python算法核心：

class Node:
    def __init__(self,directions,cost):
        self.directions = directions
        self.cost = cost

def get_neighbors(self, node):
    neighbors = []
    distances = []
    nexts = [self.Node((-1, 0),1), self.Node((0, 1),1), self.Node((0, -1),1), self.Node((1,0),1),
            self.Node((-1,1),math.sqrt(2)), self.Node((1,1),math.sqrt(2)),self.Node((1, -1),math.sqrt(2)), self.Node((-1,-1),math.sqrt(2))]
    for next in nexts:
        neighbor = (node[0] + next.directions[0], node[1] + next.directions[1])
        if self.board_size <= neighbor[0] < len(self.grid)-self.board_size and self.board_size <= neighbor[1] < len(self.grid[0])-self.board_size:
            if self.grid[neighbor[0]][neighbor[1]] == 0:
                if next.directions[0] != 0 and next.directions[1] != 0:  # 对角线方向
                    if self.grid[node[0] + next.directions[0]][node[1]] == 0 and self.grid[node[0]][node[1] + next.directions[1]] == 0:
                        neighbors.append(neighbor)
                        distances.append(next.cost)
                else:
                    neighbors.append(neighbor)
                    distances.append(next.cost)

    return neighbors,distances

def plan(self):
    priority_queue = []
    heapq.heappush(priority_queue,(0,self.start))
    costs = {self.start: 0}
    previous_nodes = {self.start: None}
    self.visited = set()
    self.visit_order = []

    while priority_queue:
        current_cost, current_node = heapq.heappop(priority_queue)
        # Determines whether the current node has already been visited
        if current_node in self.visited:
            continue

        self.visited.add(current_node)
        self.visit_order.append(current_node)
        if current_node == self.goal:
            break
        # Find passable neighbors
        neighbors, distances = self.get_neighbors(current_node)
        for neighbor,distance in zip(neighbors,distances):
            if neighbor[0] == -1:
                continue
            # Compute the cost from the start point
            cost = current_cost + distance
            # Store cost and update to minimum value
            if neighbor not in costs or cost < costs[neighbor]:
                costs[neighbor] = cost
                # The parent node of neighbor is current_node
                previous_nodes[neighbor] = current_node
                # Push the node into priority_queue
                heapq.heappush(priority_queue,(cost, neighbor))
    
    self.path = []
    current_node = self.goal
    while current_node is not None:
        self.path.append(current_node)
        current_node = previous_nodes.get(current_node)
    self.path = self.path[::-1]

    return costs[self.goal]

 在Python中同样使用优先队列实现最小堆来优化：

priority_queue = []
heapq.heappush(priority_queue,(0,self.start))

在 Python 中，heapq 模块提供了堆队列算法的实现，也称为优先队列算法。heapq 模块提供的堆是一个最小堆，即堆顶元素是最小的。 

 

路径规划——dijkstra(迪杰斯特拉)算法

欢迎Star和Follow:GitHub - Benxiaogu/PathPlanning: Path planning and Navigation