2、无人驾驶--路径规划算法：Dijkstra

最新推荐文章于 2024-07-24 22:13:06 发布
最新推荐文章于 2024-07-24 22:13:06 发布
阅读量3k 收藏 52
点赞数 8
分类专栏： 无人驾驶学习笔记 文章标签： 算法 自动驾驶 人工智能
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_44705488/article/details/121306892
版权

目录

2、Dijkstra

声明：本文是学习古月居~基于栅格地图的机器人路径规划算法指南 • 黎万洪 后写的笔记，好记性不如烂笔头！方便自己日后的巩固与复习，这个教程讲的很高，值得推荐！

2.1、算法简介

迪杰斯特拉算法是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又称为狄克斯特拉算法；

它是从一个节点遍历其余各个节点的最短路径算法，解决的是有权图中最短路径问题。

主要特点：从起点开始，采用贪心算法的策略，每次遍历到初始点距离最近且未访问过的顶点的邻接点，直到扩展到终点为止。

2.2、算法思路

如上图G=(V,E)是一个带权有向图，把图中节点集合V分成两组：
第一组为已经求出最短路径的节点集合（用S表示，初始时S只有一个源点，即：D(0),之后每求得一条最短路径，就将该节点加入到集合S中，指导全部节点都加入到S中，算法就结束了）；第二组为其余未确定最短路径的节点集合（U表示），按最短路径长度的递增次序把第二组的节点加入S中。在加入的过程中，总保持从源点V到S中节点的最短路径长度不大于从源点V到U中任何节点的最短路径长度。
此外，每个节点对应一个距离,S中的节点的距离就是从V到此节点的最短路径长度,U中的节点的距离，是从V到此节点只包含S中的节点为中间节点的当前最短路径长度。

具体流程：

设D为起点，A为终点，找到D~A的最短路径
1、S中只包含起节点D（0）；U包含除S外的其它节点，如C节点，C与D相邻，所以C（3）表示C到D的距离为3，而F与D不相邻，所以设F到D的距离为∞。

2、在U中选出最短节点，这里为C（3），将C移到S中，并在U中删除。

3、按上述方法依次选出，在U中选出最短节点，此时为E（4），将E移到S中，并在U中删除。

4、此时就要对比DCB=13;DCF=9;DEF=6;DEG=12的距离，所以将F（6）移到S中，并在U中删除；

5、依次按照上述方法推算

6、直到U为空集，此时，可以得到最短距离：A(22) = DEF~A =4+2+16；

2.3、算法具体实现

defColorMap.m文件：

作用：生成栅格图

function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径

% 构建颜色MAP图
colormap(cmap);

% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);

% 障碍物区域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

getNeighborNodes.m文件：

作用：搭建个节点之间的关系；实现查找当前父节点临近的周围8个子节点

function neighborNodes = getNeighborNodes(rows, cols, lineIndex, field)
[row, col] = ind2sub([rows,cols], lineIndex);
neighborNodes = inf(8,2);

%% 查找当前父节点临近的周围8个子节点
% 左上节点
if row-1 > 0 && col-1 > 0
    child_node_sub = [row-1, col-1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(1,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(1,2) = cost;
    end
end

% 上节点
if row-1 > 0
    child_node_sub = [row-1, col];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(2,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(2,2) = cost;
    end
end

% 右上节点
if row-1 > 0 && col+1 <= cols
    child_node_sub = [row-1, col+1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(3,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(3,2) = cost;
    end
end

% 左节点
if  col-1 > 0
    child_node_sub = [row, col-1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(4,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(4,2) = cost;
    end
end

% 右节点
if  col+1 <= cols
    child_node_sub = [row, col+1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(5,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(5,2) = cost;
    end
end

% 左下节点
if row+1 <= rows && col-1 > 0
    child_node_sub = [row+1, col-1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(6,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(6,2) = cost;
    end
end

% 7.下节点
if row+1 <= rows
    child_node_sub = [row+1, col];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(7,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(7,2) = cost;
    end
end

% 8.右下节点
if row+1 <= rows && col+1 <= cols
    child_node_sub = [row+1, col+1];
    child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
    neighborNodes(8,1) = child_node_line;
    if field(child_node_sub(1), child_node_sub(2)) ~= 2
        cost = norm(child_node_sub - [row, col]);
        neighborNodes(8,2) = cost;
    end
end

Dijkstra.m文件：

具体的算法实现

% 基于栅格地图的机器人路径规划算法
% 第2节：Dijkstra算法
clc
clear
close all

%% 栅格界面、场景定义
% 行数和列数
rows = 10;
cols = 20;
[field,cmap] = defColorMap(rows, cols);

% 起点、终点、障碍物区域
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;

%% 算法初始化
% S/U的第一列表示栅格节点线性索引编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更；
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更
U(:,1) = (1: rows*cols)';
U(:,2) = inf;
S = [startPos, 0];
U(startPos,:) = [];

% 更新起点的邻节点及代价
neighborNodes = getNeighborNodes(rows, cols, startPos, field);
for i = 1:8
    childNode = neighborNodes(i,1);
    
    % 判断该子节点是否存在
    if ~isinf(childNode)
        idx = find(U(:,1) == childNode);
        U(idx,2) = neighborNodes(i,2);
    end
end



% S集合的最优路径集合
for i = 1:rows*cols
    path{i,1} = i;
end
for i = 1:8
    childNode =  neighborNodes(i,1);
    if ~isinf(neighborNodes(i,2))
        path{childNode,2} = [startPos,neighborNodes(i,1)];
    end
end


%% 循环遍历
while ~isempty(U)
    
    % 在U集合找出当前最小距离值的节点,视为父节点，并移除该节点至S集合中
    [dist_min, idx] = min(U(:,2));
    parentNode = U(idx, 1);
    S(end+1,:) = [parentNode, dist_min];
    U(idx,:) = [];
    
    % 获得当前节点的临近子节点
    neighborNodes = getNeighborNodes(rows, cols, parentNode, field);

    % 依次遍历邻近子节点，判断是否在U集合中更新邻节点的距离值
    for i = 1:8
        
        % 需要判断的子节点
        childNode = neighborNodes(i,1);
        cost = neighborNodes(i,2);
        if ~isinf(childNode)  && ~ismember(childNode, S)
            
            % 找出U集合中节点childNode的索引值
            idx_U = find(childNode == U(:,1));            
            
            % 判断是否更新
            if dist_min + cost < U(idx_U, 2)
                U(idx_U, 2) = dist_min + cost;
                
                % 更新最优路径
                path{childNode, 2} = [path{parentNode, 2}, childNode];
            end
        end
    end
end


%% 画栅格界面
% 找出目标最优路径
path_opt = path{goalPos,2};
field(path_opt(2:end-1)) = 6;

% 画栅格图
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
2.3.1、程序详解

①、 算法初始化

%% 算法初始化
% S/U的第一列表示栅格节点线性索引编号
% 对于S，第二列表示从源节点到本节点已求得的最小距离，不再变更；
% 对于U，第二列表示从源节点到本节点暂时求得的最小距离，可能会变更
U(:,1) = (1: rows*cols)';
U(:,2) = inf;
S = [startPos, 0];
U(startPos,:) = [];

对U集合进程初始化，生成如下表，第一列未线性索引值，第二列为距离都设为∞
U(:,1) = (1: rows*cols)';
U(:,2) = inf;


%将初始节点放入S集合中，并设置它的距离为0；再U集合中删除初始节点！
S = [startPos, 0];
U(startPos,:) = [];

②、更新起点的邻节点及代价

% 更新起点的邻节点及代价
neighborNodes = getNeighborNodes(rows, cols, startPos, field);
for i = 1:8
    childNode = neighborNodes(i,1);
    
    % 判断该子节点是否存在
    if ~isinf(childNode)
        idx = find(U(:,1) == childNode);
        U(idx,2) = neighborNodes(i,2);
    end
end

这里调用了getNeighborNodes函数实现查找当前父节点临近的周围8个子节点

有三种情况：
①、两列都为inf，即这个节点不存在；
②、两个都是数字，即此节点存在，且为自由空间（可以走的节点）；
③、第一列是数字，第二列是inf，即此节点为障碍物

for i = 1:8；之后进行8次循环，判断该子节点是否存在，如果子节点存在，则将从父节点到子节点的距离存放到U集合中；

如上图中D是父节点，它有两个子节点C和E，那么就要再U集合中更新D到这两个节点的距离为3和4。

③、S集合的最优路径集合

% S集合的最优路径集合
for i = 1:rows*cols
    path{i,1} = i;
end
for i = 1:8
    childNode =  neighborNodes(i,1);
    if ~isinf(neighborNodes(i,2))
        path{childNode,2} = [startPos,neighborNodes(i,1)];
    end
end

这里是生成初始节点的最优路径

④、进行循环，循环的条件是U集合不为空

while ~isempty(U)
    
    % 在U集合找出当前最小距离值的节点,视为父节点，并移除该节点至S集合中
    [dist_min, idx] = min(U(:,2));
    parentNode = U(idx, 1);
    S(end+1,:) = [parentNode, dist_min];
    U(idx,:) = [];
    
    % 获得当前节点的临近子节点
    neighborNodes = getNeighborNodes(rows, cols, parentNode, field);

    % 依次遍历邻近子节点，判断是否在U集合中更新邻节点的距离值
    for i = 1:8
        
        % 需要判断的子节点
        childNode = neighborNodes(i,1);
        cost = neighborNodes(i,2);
        if ~isinf(childNode)  && ~ismember(childNode, S)
            
            % 找出U集合中节点childNode的索引值
            idx_U = find(childNode == U(:,1));            
            
            % 判断是否更新
            if dist_min + cost < U(idx_U, 2)
                U(idx_U, 2) = dist_min + cost;
                
                % 更新最优路径
                path{childNode, 2} = [path{parentNode, 2}, childNode];
            end
        end
    end
end

得出个节点的距离值，进行排序后得到最优路径！

Dijkstra算法动态效果实现图：
在这里插入图片描述
想要获取工程源码，可以关注公众号：Kevin的学习站，后台回复：“Dijkstra”即可获取；希望此文对您有一定的帮助，整理不易，但您的点赞、关注、收藏就是对我最大的鼓励！

在这里插入图片描述

Kevin的学习站
关注
  • 8
    点赞
  • 踩
  • 52
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 4
    评论
专栏目录
自动驾驶-决策规划算法十五：人工蚁群算法(C++)
liuphahaha的专栏
04-24 1099
蚁群算法（Ant Colony Algorithm）最初于1992年由意大利学者M.Dorigo等人提出，它是一种模拟自然界中真实蚁群觅食行为的仿生优化算法。研究发现：每只蚂蚁觅食时在走过的路线上会留下一种称为信息素的物质，蚂蚁之间靠感知这种物质的浓度进行信息传递。蚂蚁在选择路径时总是倾向于朝信息索浓度高的方向移动，而距离短的路径上走过的蚂蚁多，留下的信息素也多，后续蚂蚁选择它的概率也会越大；其他路径上的信息素会随着时间的推移不断挥发，这样就形成了一种正反馈机制，最后整个蚁群聚集到最短路径上。
A star路径规划算法（含bmp位图，可直接运行）
12-02
适合路径规划、无人驾驶相关领域科研工作者。
059.迪杰斯特拉(Dijkstra)算法的原理以及解决最短路径问题
leon9dragon的博客
04-28 1510
<!-- TOC --> <ul> <li><a href="#1-%E8%BF%AA%E6%9D%B0%E6%96%AF%E7%89%B9%E6%8B%89dijkstra%E7%AE%97%E6
基于二次规划的路径规划算法
qq_40230900的博客
06-18 937
路径规划
3、无人驾驶--路径规划算法：Floyd算法
kevin的博客
03-04 3453
Floyd算法是一种动态规划算法，稠密图效果最佳，节点间的连接权值可正可负。此算法简单有效，由于三重循坏结构紧凑，对于稠密图，效果要高于Dijkstra算法。
自动驾驶-决策规划算法十六：Dijkstra算法(C++)
liuphahaha的专栏
04-24 722
本文介绍无人驾驶决策规划算法中的迪杰斯特拉（Dijkstra）算法，之前在数据结构与算法的文章中曾经介绍过。之前用的是C语言，这里用C++重写了一下，顺便修正了之前代码里的几个错误。如有错误或者遗漏之处，欢迎指出。
（2-5）A*算法：汽车导航路径规划系统
码农三叔
07-07 995
随着自动驾驶技术的迅猛发展，汽车导航路径规划成为了自动驾驶系统中不可或缺的关键技术之一。路径规划系统通过算法和技术，帮助车辆在复杂的道路网络中找到安全、高效的行驶路径，以实现驾驶员无人干预或辅助驾驶的目标。基于A*算法的路径规划系统，通过有效地结合启发式搜索和距离估计方法，能够在大规模地图数据和实时环境信息的基础上，快速响应并规划出最优路径，从而为自动驾驶车辆提供稳定、可靠的导航服务。
自动驾驶-决策规划算法十一：A*算法(C++)
liuphahaha的专栏
04-18 908
本文介绍无人驾驶决策规划算法中的A*算法，及A*与Dijkstra、BestFS算法的对比。和之前的算法文章中不同，为了方便表示格栅化地图，本文的地图用二维数组的数据结构表达，而为了方便表达路径点之间的关系，此关系用树（八叉树）的数据结构表达。语言用的是C++，图形基于easyX库输出。如有错误或者遗漏之处，欢迎指出。
自动驾驶路径规划——Dijkstra算法
sinat_52032317的博客
09-28 4180
本文介绍了深度优先、广度优先以及Dijkstra算法，并用MATLAB实现了Dijkstra算法。同时用贝塞尔曲线对路径进行优化。
自动驾驶之路径规划算法介绍
liuphahaha的专栏
04-18 1647
路径规划是指在一定的环境模型基础上，给定无人驾驶汽车起始点和目标点后，按照性能指标规划出一条无碰撞、能安全到达目标点的有效路径。路径规划主要包含两个步骤：建立包含障碍区域与自由区域的环境地图，以及在环境地图中选择合适的路径搜索算法，快速实时地搜索可行驶路径。路径规划结果对车辆行驶起着导航作用。它引导车辆从当前位置行驶到达目标位置。
无人驾驶汽车路径规划仿真分析
10-16
在全局路径规划算法中，A*算法因其高效和鲁棒性而被广泛应用。A*算法综合了Dijkstra算法的最短路径特性与最佳优先搜索算法的效率优势，通过估价函数平衡实际代价和预期代价，快速找到接近最优的路径。 然而，传统的...
无人驾驶中的路径规划算法论文集合
10-13
在无人驾驶技术领域，路径规划算法是核心技术之一，它关乎到自动驾驶车辆如何安全、高效地从起点到达终点。这个“无人驾驶中的路径规划算法论文集合”很可能包含了一系列深入研究此主题的学术文献，旨在探讨和改进...
无人车路径规划算法matlab+python代码.rar
07-09
本资源包含两个主要文件：一个RTF文档“无人车地面移动避障仿真图”和一个Python代码压缩包“无人车路径规划算法演示Python.zip”。下面将详细介绍这两个文件可能涉及的知识点。 1. **路径规划理论基础**： - **A*...
zero-autopliot-system-无人驾驶系统
07-16
零号自动驾驶系统（Zero-Autopilot System）是一款先进的无人驾驶技术解决方案，主要应用于智能交通领域。这个"第一版自动驾驶系统版本仓库"包含了该系统的核心组件和开发资源，为研究者和工程师提供了深入理解和...
自动驾驶-决策规划算法十二：动态规划算法(C++)
liuphahaha的专栏
04-20 439
本文介绍无人驾驶决策规划算法中的动态规划算法，动态规划是一种很经典的算法思路，通常用于求解多阶段决策过程最优化问题，应用非常广泛，在无人驾驶中通常用于在全局规划时求解最短路径问题。值得注意的是，动态规划的规划和无人驾驶中的路径规划，不是一个概念，但后者可以应用前者的思路。Floyd算法就是用的动态规划的思路，是求解一个给定图中任意两点间的最短路径，而本文的动态规划算法稍有区别，是用于求解一个给定图中，给定的起点和终点之间的最短路径，因为这更符合无人驾驶中全局路径规划的问题。
代码随想录算法训练营第22天-leetcode-二叉树08：669. 修剪二叉搜索树；108.将有序数组转换为二叉搜索树；538.把二叉搜索树转换为累加树
weixin_65180740的博客
07-24 916
你可能需要改变树的根节点，所以结果应当返回修剪好的二叉搜索树的新的根节点。给出二叉 搜索 树的根节点，该树的节点值各不相同，请你将其转换为累加树（Greater Sum Tree），使每个节点 node 的新值等于原树中大于或等于 node.val 的值之和。题目要求，实际上是每个节点加上中序排列后面的那个节点，故而可以反向中序排序，即RNL，用prior记录前一个节点的值，加上前一个节点的值即可。左右子树也必须是二叉搜索树。后序遍历每个节点，对于在范围外的节点，考虑保留其左子树/右子树（只可能保留一个）
2.6基本算法之动态规划6045:开餐馆
最新发布
Andy_SCP001的博客
07-24 226
北大信息学院的同学小明毕业之后打算创业开餐馆.现在共有n 个地点可供选择。我们用一个整数序列m1, m2, ... mn 来表示他们的相对位置。由于地段关系,开餐馆的利润会有所不同。为了避免自己的餐馆的内部竞争,餐馆之间的距离必须大于k。//f[i]:从第1个位置到开第i个位置开餐馆能获得的最大利润。j++){//从j个地点到第i个地点开餐馆。//n个地点开一家店。//每个餐馆开的位置。//边界条件:只有一个地点,只开一家店。//n个地点开很多家店。
算法训练营第47天|1143.最长公共子序列|1035.不相交的线|53. 最大子序和|392.判断子序列
bless_______的博客
07-24 214
如果text1[i - 1] 与 text2[j - 1]不相同，那就看看text1[0, i - 2]与text2[0, j - 1]的最长公共子序列 和 text1[0, i - 1]与text2[0, j - 2]的最长公共子序列，取最大的。如果text1[i - 1] 与 text2[j - 1]相同，那么找到了一个公共元素，所以dp[i][j] = dp[i - 1][j - 1] + 1;未想到特殊情况，即s或t为空串的情况，因为返回的是数组值，但是如果是空串就不会建立数组。
无人驾驶，局部路径规划算法
05-04
无人驾驶的局部路径规划算法可以分为基于图搜索的算法和基于采样的算法两类。 基于图搜索的算法，如Dijkstra算法、A*算法和RRT算法等，通过搜索已知的道路网络或者障碍物空间中的可行路径，以达到目标点。其中Dijkstra算法和A*算法都是基于最短路径的算法，但A*算法在计算时考虑了启发式信息，能够更快速地找到路径。RRT算法则是一种基于采样的树形搜索算法，它在障碍物空间中随机采样点，通过连接采样点和树节点的方式构建路径树，最终找到一条从起点到目标点的路径。 基于采样的算法，如RRT*算法和PRM算法等，通过对障碍物空间进行采样，生成一系列采样点，然后通过连接这些点来构建可行路径。RRT*算法在RRT算法基础上增加了重新连接策略，能够更好地优化路径；PRM算法则是一种基于全局路径规划的方法，通过构建路点图，在局部进行路径规划。 这些算法各有优缺点，需要根据具体应用场景进行选择。
评论 4
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Kevin的学习站

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值