机器人路径规划:基于迪杰斯特拉算法(Dijkstra)的机器人路径规划(提供Python代码)

迪杰斯特拉算法(Dijkstra)是由荷兰计算机科学家狄克斯特拉于1959年提出的,因此又叫狄克斯特拉算法。是从一个顶点到其余各顶点的最短路径算法,解决的是有权图中最短路径问题。迪杰斯特拉算法主要特点是从起始点开始,采用贪心算法策略,每次遍历到始点距离最近且未访问过的顶点的邻接节点,直到扩展到终点为止。它可以找到从一个起始节点到其他所有节点的最短路径。

一、算法介绍

Dijkstra算法采用贪心策略,通过逐步扩展已知最短路径集合来逐步确定最短路径。它使用一个距离数组来记录从起始节点到其他节点的当前最短距离,并通过不断更新距离数组来逐步确定最短路径。

二、算法描述

  1. 创建一个距离数组dist[],用于记录起始节点到其他节点的当前最短距离。初始化dist[],将起始节点的距离设为0,其他节点的距离设为无穷大。

  2. 创建一个集合visited[],用于记录已经确定最短路径的节点。

  3. 重复以下步骤,直到visited[]包含所有节点:

a. 从未访问的节点中选择一个距离最小的节点u,将其加入visited[]。

b. 对于节点u的所有邻居节点v,更新其距离数组dist[]:

  • 如果通过节点u可以获得更短的路径,则更新dist[v]为新的最短距离。
  1. 最终,dist[]数组中记录了起始节点到其他所有节点的最短距离。

三、算法流程

  1. 初始化dist[]数组和visited[]集合。

  2. 将起始节点的距离设为0。

  3. 重复以下步骤,直到visited[]包含所有节点:

a. 从未访问的节点中选择一个距离最小的节点u。

b. 将节点u加入visited[]。

c. 对于节点u的所有邻居节点v,更新其距离数组dist[]:

  • 如果通过节点u可以获得更短的路径,则更新dist[v]为新的最短距离。
  1. 返回dist[]数组作为最短路径结果。

四、部分代码

复制代码
import matplotlib.pyplot as plt
import math

show_animation = True


class Dijkstra:

    def __init__(self, ox, oy, resolution, robot_radius):
        """
        Initialize map for a star planning

        ox: x position list of Obstacles [m]
        oy: y position list of Obstacles [m]
        resolution: grid resolution [m]
        rr: robot radius[m]
        """

        self.min_x = None
        self.min_y = None
        self.max_x = None
        self.max_y = None
        self.x_width = None
        self.y_width = None
        self.obstacle_map = None

        self.resolution = resolution
        self.robot_radius = robot_radius
        self.calc_obstacle_map(ox, oy)
        self.motion = self.get_motion_model()

    class Node:
        def __init__(self, x, y, cost, parent_index):
            self.x = x  # index of grid
            self.y = y  # index of grid
            self.cost = cost
            self.parent_index = parent_index  # index of previous Node

        def __str__(self):
            return str(self.x) + "," + str(self.y) + "," + str(
                self.cost) + "," + str(self.parent_index)

五、部分结果

六、完整Python代码

见下方联系方式

相关推荐
胡小禾2 小时前
JDK17和JDK8的 G1
jvm·算法
胖咕噜的稞达鸭4 小时前
算法入门:专题攻克一---双指针(3)有效三角形的个数 查找总价格为目标值的两个商品(剑指offer题目)
算法
Rock_yzh5 小时前
AI学习日记——参数的初始化
人工智能·python·深度学习·学习·机器学习
青衫客366 小时前
基于 Python 构建的安全 gRPC 服务——TLS、mTLS 与 Casbin 授权实战
python·安全·微服务
-dzk-7 小时前
【3DGS复现】Autodl服务器复现3DGS《简单快速》《一次成功》《新手练习复现必备》
运维·服务器·python·计算机视觉·3d·三维重建·三维
逻辑留白陈8 小时前
Adaboost进阶:与主流集成算法对比+工业级案例+未来方向
算法
Learn Beyond Limits8 小时前
Mean Normalization|均值归一化
人工智能·神经网络·算法·机器学习·均值算法·ai·吴恩达
摩羯座-185690305948 小时前
爬坑 10 年!京东店铺全量商品接口实战开发:从分页优化、SKU 关联到数据完整性闭环
linux·网络·数据库·windows·爬虫·python
ACERT3338 小时前
5.吴恩达机器学习—神经网络的基本使用
人工智能·python·神经网络·机器学习
天选之女wow9 小时前
【代码随想录算法训练营——Day28】贪心算法——134.加油站、135.分发糖果、860.柠檬水找零、406.根据身高重建队列
算法·leetcode·贪心算法