路径规划Dijkstra算法

Dijkstra搜索最短路径：

整体思路

从起始节点开始，将邻域节点进行遍历，标注好邻域节点最小的累计路径长度，直到遍历到终止节点。

算法复杂度

naive的方式，算法复杂度为O(|V|2)O(|V|^2)，其中|V||V|是节点数量
聪明的方式，使用优先队列，算法复杂度为O((|E|+|V|)log(|V|))O\left(\left(\left|E\right|+\left|V\right|\right)\log\left(\left|V\right|\right)\right)，其中|E||E|是边界数量，|V||V|是节点数量。

伪代码

对所有图中的节点n
- n.distance = Infinity
- n.parent = nil
创建一个List。
起始节点start.distance = 0, start.parent = nil，将start节点放入List
While(List 非空)
- 令current = List中distance最小的那个节点，然后将这个节点从List取出（表示这个节点已经完全访问过了，不需要以后在访问了）
- 对所有和current节点相邻的节点n
  - tempDistance = current.distance + length of edge from n to current
  - if(n.distance > tempDistance)
    - n.distance = tempDistance;
    - n.parent = current
    - 如果整个过程还没有完成，那么就将n加入到list里边去（或者更新list里边n的distance值和parent值）。

Matlab代码

本节来自Cousera的Robotics课程

function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords, drawMapEveryTime)
% Run Dijkstra's algorithm on a grid.
% Inputs :
%   input_map : a logical array where the freespace cells are false or 0 and
%   the obstacles are true or 1
%   start_coords and dest_coords : Coordinates of the start and end cell
%   respectively, the first entry is the row and the second the column.
% Output :
%    route : An array containing the linear indices of the cells along the
%    shortest route from start to dest or an empty array if there is no
%    route. This is a single dimensional vector
%    numExpanded: Remember to also return the total number of nodes
%    expanded during your search. Do not count the goal node as an expanded node.% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue  - on list
% 5 - green - start
% 6 - yellow - destinationcmap = [1 1 1; ...0 0 0; ...1 0 0; ...0 0 1; ...0 1 0; ...1 1 0; ...0.5 0.5 0.5];colormap(cmap);% variable to control if the map is being visualized on every
% iteration[nrows, ncols] = size(input_map);% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);map(~input_map) = 1;   % Mark free cells
map(input_map)  = 2;   % Mark obstacle cells% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));map(start_node) = 5;
map(dest_node)  = 6;% Initialize distance array
distanceFromStart = Inf(nrows,ncols);% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);distanceFromStart(start_node) = 0;% keep track of number of nodes expanded
numExpanded = 0;% Main Loop
while true% Draw current mapmap(start_node) = 5;map(dest_node) = 6;% make drawMapEveryTime = true if you want to see how the % nodes are expanded on the grid. if (drawMapEveryTime)image(1.5, 1.5, map);grid on;axis image;drawnow;end% Find the node with the minimum distance[min_dist, current] = min(distanceFromStart(:));if ((current == dest_node) || isinf(min_dist))break;end;% Update mapmap(current) = 3;         % mark current node as visiteddistanceFromStart(current) = Inf; % remove this node from further consideration% Compute row, column coordinates of current node[i, j] = ind2sub(size(distanceFromStart), current);% ********************************************************************* % YOUR CODE BETWEEN THESE LINES OF STARS% Visit each neighbor of the current node and update the map, distances% and parent tables appropriately.numExpanded = numExpanded + 1;if(i-1>=1) %upperid = sub2ind(size(map), i-1, j);    if((map(id) ~= 2) ... %if not obst&& (map(id) ~= 3) ... % if not visited&& (map(id) ~= 5)) ... % if not startif(distanceFromStart(id) >= min_dist + 1)distanceFromStart(id) = min_dist + 1;parent(id) = current;map(id) = 4;end            endendif(i+1 <= nrows) %lowerid = sub2ind(size(map), i+1, j);if((map(id) ~= 2) ... %if not obst&& (map(id) ~= 3) ... % if not visited&& (map(id) ~= 5)) ... % if not startif(distanceFromStart(id) >= min_dist + 1)distanceFromStart(id) = min_dist + 1;parent(id) = current;map(id) = 4;end            endendif(j-1 >= 1) %leftid = sub2ind(size(map), i, j-1);if((map(id) ~= 2) ... %if not obst&& (map(id) ~= 3) ... % if not visited&& (map(id) ~= 5)) ... % if not startif(distanceFromStart(id) >= min_dist + 1)distanceFromStart(id) = min_dist + 1;parent(id) = current;map(id) = 4;end            endendif(j+1 <= ncols) %leftid = sub2ind(size(map), i, j+1);if((map(id) ~= 2) ... %if not obst&& (map(id) ~= 3) ... % if not visited&& (map(id) ~= 5)) ... % if not startif(distanceFromStart(id) >= min_dist + 1)distanceFromStart(id) = min_dist + 1;parent(id) = current;map(id) = 4;end            endend%*********************************************************************end%% Construct route from start to dest by following the parent links
if (isinf(distanceFromStart(dest_node)))route = [];
elseroute = [dest_node];while (parent(route(1)) ~= 0)route = [parent(route(1)), route];end% Snippet of code used to visualize the map and the pathfor k = 2:length(route) - 1        map(route(k)) = 7;pause(0.1);image(1.5, 1.5, map);grid on;axis image;end
endend

然后执行代码

map = false(10); %Input Map Parameters
map (1:5, 6) = true; %Obstacle Declaration
start_coords = [6, 2]; %Starting Coordinates
dest_coords  = [8, 9]; %Destination Coordinates
drawMapEveryTime = false; %Display Outputs
[route, numExpanded] = DijkstraGrid(map,start_coords,dest_coords,drawMapEveryTime) %Implementation

最终结果：

路径规划Dijkstra算法相关推荐

自动驾驶路径规划——Dijkstra算法
目录前言 1. 深度优先(DFS)和广度优先(BFS) 2. 深度优先搜索(DFS) 2.1 算法基本思想 2.2 深度优先搜索算法(C) 3. 广度优先搜索(BFS) 3.1 算法基本思想 3.2 ...
1 基于搜索的路径规划 —— Dijkstra算法(python)
文章目录算法讲解重要说明栅格地图有权图 1 def main() 1.1 设置机器人的起点和终点,栅格大小,机器人半径 1.2 设置障碍物的位置 1.3 绘制步骤1和2的图 2 class D ...
路径规划—— A* 算法
参考原文:路径规划之 A* 算法 (weibo.com) A*(念做:A Star)算法是一种很常用的路径查找和图形遍历算法.它有较好的性能和准确度. 本... 路径规划之 A* 算法 A*(念做:A ...
改进后的A星三维路径规划完整算法(matlab语言)，包括障碍物模型优化
改进后的A星三维路径规划完整算法(matlab语言),包括障碍物模型优化,平滑处理,启发函数的改进,环境地图可以根据自己的实际情况进行改进,算法包含了非常详细的代码注释 ID:695006710638 ...
改进后的A星三维路径规划完整算法(matlab语言)
改进后的A星三维路径规划完整算法(matlab语言),包括障碍物模型优化,平滑处理,启发函数的改进,环境地图可以根据自己的实际情况进行改进,算法包含了非常详细的代码注释 YID:69500671063 ...
路径规划——CH算法
原文地址:https://zhuanlan.zhihu.com/p/110367906 1 CH算法的基本原理 CH(Contraction Hierarchies)算法是 Robert Geisbe ...
路径规划A*算法及SLAM自主地图创建导航算法
最近研究机器人自主路径规划,http://www.slamcn.org/index.php/首页.关注了「泡泡机器人SLAM」专栏平台,上面有很多公开的视频,涵盖多种SLAM技术及其算法的视频.PPT ...
牛客网短最优升级路径【Dijkstra算法】+【路径记录】
链接:https://www.nowcoder.com/questionTerminal/a7052c5bd8634edb9ccee711a5c1ea54 来源:牛客网短最优升级路径题目描述:游戏 ...
路径规划;a*算法 demo_路径规划A*算法
(做封面图片,可真是费了很长时间) 前言路径规划涉及到很多内容. 但路径规划的起点,应该都是A*这个来自斯坦福大学算法. 说起A*,网上有很多相关的内容.各种博客,各种解释. 但是我愣是没看明白. ...

路径规划Dijkstra算法

整体思路

算法复杂度

伪代码

Matlab代码

路径规划Dijkstra算法相关推荐

最新文章

热门文章