基于Dijkstras最短路径算法的栅格地图避障路线规划matlab仿真

目录

1.栅格地图表示

2.Dijkstra算法原理

初始化

算法步骤

3.MATLAB程序

4.仿真结果

---

       Dijkstra算法是一种经典的图论算法，用于解决从图中的一个源节点到其他所有节点的最短路径问题。当应用于栅格地图上的避障路线规划时，该算法能够有效地找到从起点到终点，同时避开障碍物的最短路径。

1.栅格地图表示

       首先，将实际地形抽象成一个二维栅格地图，其中每个单元格可以表示为可通行（通常标记为0）或不可通行（障碍物，标记为1）。设地图的大小为M×N，每个单元格的位置由坐标(i,j)标识，其中1≤i≤M,1≤j≤N。

       将栅格地图视为无向图G=(V,E)，其中顶点集V对应于地图上的每个可通行单元格，边集E包含每对相邻可通行单元格之间的连接。如果两个相邻的单元格(i,j)和(i′,j′)都是可通行的，则它们之间有一条无权边，权重通常设定为1，表示移动成本。

2.Dijkstra算法原理

       Dijkstra是路径规划算法中非常经典的一种算法，在很多地方都会用到，特别是在机器人的路径规划中，基本学习机器人运动相关的都会接触到该算法。Dijkstra算法本身的原理是基于贪心思想实现的，首先把起点到所有点的距离存下来找个最短的，然后松弛一次再找出最短的，所谓的松弛操作就是，遍历一遍看通过刚刚找到的距离最短的点作为中转站会不会更近，如果更近了就更新距离，这样把所有的点找遍之后就存下了起点到其他所有点的最短距离。

初始化

源节点：选择一个起点s作为源节点。

距离集合：为图中的每个顶点v设置一个初始距离值d(v)，其中d(s)=0，对于所有其他节点v，若v是可达的则d(v)=∞，表示尚未发现到达v的路径。

已访问集合：初始化为空集Vvisited​。

未访问集合：开始时包含除了源节点之外的所有节点。

算法步骤

选择未访问节点中距离最小的节点：设当前选择的节点为u。

u=argminv∈V−Vvisited​​d(v)

考察节点u的邻居：对于u的每个邻接节点v（与u相邻且未访问过），计算通过u到达v的新路径长度。

alt=d(u)+cost(u,v)

其中，cost(u,v)是从U到v的移动成本，在栅格地图上通常为1。

更新距离：如果通过u到达v的路径比当前已知的路径更短，则更新v的距离。

d(v)=min(d(v),alt)

标记节点为已访问：将节点u加入已访问集合Vvisited​。

其伪代码如下：

3.MATLAB程序

%%%%%%%%%%%%%%%%%%%%%dijkstra算法实现%%%%%%%%%%%%%%%%%%%%%
function [Cost, Route] = Dijkstras( Graph, SourceNode, TerminalNode )

PathToNode = cell(size(Graph,1),1);

% Initialize all Node costs to infinity except for the source node
NodeCost = Inf.*ones(1,size(Graph,1));
NodeCost(SourceNode) = 0;

% Initialize the Current Node to be the Source Node
CurrentNode = SourceNode;

% Initialize the set of Visited and Unvisited Nodes
VisitedNodes = SourceNode;
UnvisitedNodes = 1:size(Graph,2);
UnvisitedNodes = UnvisitedNodes(UnvisitedNodes ~= VisitedNodes);

while (CurrentNode ~= TerminalNode)
    % Extract the Costs/Path Lengths to each node from the current node
    CostVector = Graph(CurrentNode, :);
    % Only look at valid neighbors ie. those nodes which are unvisited
    UnvisitedNeighborsCostVector = CostVector(UnvisitedNodes);
    % Extract the cost to get to the Current Node
    CurrentNodeCost = NodeCost(CurrentNode);
    % Extract the path to the current node
    PathToCurrentNode = PathToNode{CurrentNode};
    % Iterate through the Unvisited Neighbors assigning them a new tentative cost
    for i = 1:length(UnvisitedNeighborsCostVector)
       if UnvisitedNeighborsCostVector(i) ~= Inf % Only Check for update if non-infinite
           tempCost = CurrentNodeCost + UnvisitedNeighborsCostVector(i); % The tentative cost to get to the neighbor through the current node
           % Compare the tentative cost to the currently assigned cost and
           % assign the minimum
           if tempCost < NodeCost(UnvisitedNodes(i))
               NewPathToNeighbor = [PathToCurrentNode(PathToCurrentNode~=0) CurrentNode]; % The new path to get to the neighbor
               NewPath = [NewPathToNeighbor zeros(1,size(Graph,1)-size(NewPathToNeighbor,2))];
               PathToNode{UnvisitedNodes(i)}(:) = NewPath;
               NodeCost(UnvisitedNodes(i)) = tempCost;
           end
       end
    end
    % Search for the smallest cost remaining that is in the unvisited set
    RemainingCosts = NodeCost(UnvisitedNodes);
    [MIN, MIN_IND] = min(RemainingCosts);
    
    % If the smallest remaining cost amongst the unvisited set of nodes is
    % infinite then there is no valid path from the source node to the
    % terminal node. 
    if MIN == Inf
       fprintf('There is no valid path from the source node to the');
       fprintf('terminal node. Please check your graph.\n')
       return;
    end
    
    % Update the Visited and Unvisited Nodes
    VisitedNodes = [VisitedNodes CurrentNode];
    CurrentNode = UnvisitedNodes(MIN_IND);
    UnvisitedNodes = UnvisitedNodes(UnvisitedNodes~=CurrentNode);
end

Route = PathToNode{TerminalNode};
Route = Route(Route~=0);
Route = [Route TerminalNode];
Cost = NodeCost(TerminalNode);
end
up4117

4.仿真结果

       基于Dijkstra算法的栅格地图避障路线规划，实质上是将实际问题转化为图论问题，并利用Dijkstra算法高效地求解最短路径。该方法适用于静态环境下的路径规划，尤其是当所有移动成本相等时最为有效。然而，对于动态环境或存在不同成本的路径，可能需要考虑其他变种，如A*算法，它通过引入启发式信息来加速搜索过程。