4.4.c Dijkstra代码解析

category
type
status
slug
date
summary
tags
password
icon
有些鸟儿是注定不会被关在牢笼里的，它们的每一片羽毛都闪耀着自由的光辉。
—《肖申克的救赎》
🏰代码及环境配置：请参考 环境配置和代码运行!
本节提供了Dijkstra算法的代码测试：

4.4.c.1 构图的代码实现

基于图搜的运动规划中最重要的一步是构图，构建的图比较简单，主要包含map border和obstacles，读者也可根据需求修改构图方式。

4.4.c.2 Dijkstra的代码实现

在Dijkstra算法中，首先我们定义了节点Node ，这是图的基础元素。其中（x, y）表示节点的位置，cost表示从源节点到当前节点的cost，parent 表示当前节点的父节点。
下面是Dijkstra的核心算法，基于环境信息，起始点位置，目标点位置搜到最优路径，其中：
sx:起始点的x坐标的值
sy:起始点的y坐标的值
gx:目标点的x坐标的值
gy:目标点的y坐标的值
最终返回一条路径：rx, ry

4.4.c.3 Dijkstra的代码测试

在main 函数中设置起始点，目标点，grid的分辨率和机器人的半径，在创建grid map之后，并运行Dijkstra算法，即可找到最优路径。如下图所示，红色路径即为最终Dijkstra搜出来的最优路径。
notion image
 
上一篇
动手学控制理论
下一篇
端到端-理论与实战视频课程
Loading...