python路径规划算法_路径规划算法学习笔记（一）：A*算法

一、Dijkstra算法

Dijkstra算法从物体所在的初始点开始，访问图中的结点。它迭代检查待检查结点集中的结点，并把和该结点最靠近的尚未检查的结点加入待检查结点集。该结点集从初始结点向外扩展，直到到达目标结点。Dijkstra算法保证能找到一条从初始点到目标点的最短路径，只要所有的边都有一个非负的代价值。

1.1 算法原理与效果图

Dijkstra算法采用贪心算法的思想，解决的问题可以描述为：在无向图 G=(V,E) 中，假设每条边 E[i] 的长度为 w[i]，找到由顶点vs到其余各点的最短路径。

通过Dijkstra计算图G中的最短路径时，需要指定起点vs(即从顶点vs开始计算)。此外，引进两个集合S和U。S的作用是记录已求出最短路径的顶点(以及相应的最短路径长度)，而U则是记录还未求出最短路径的顶点(以及该顶点到起点vs的距离)。初始时，S中只有起点vs；U中是除vs之外的顶点，并且U中顶点的路径是"起点vs到该顶点的路径"。然后，从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。 然后，再从U中找出路径最短的顶点，并将其加入到S中；接着，更新U中的顶点和顶点对应的路径。重复该操作，直到遍历完所有顶点。

1.2 源码分析

算法实现的主代码分析：

defdijkstra_planning(sx, sy, gx, gy, ox, oy, reso, rr):"""sx: start x position [m]

sy: start y position [m]　　　　　　　　　　#起始点坐标

gx: goal x position [m]

gy: goal y position [m] 　　　　　　　　　　#目标点坐标

ox: x position list of Obstacles [m]

oy: y position list of Obstacles [m]　　　#障碍点坐标

reso: grid resolution [m]　　　　　　　　　 #栅格地图分辨率

rr: robot radius[m] #机器人的半径"""nstart= Node(round(sx / reso), round(sy / reso), 0.0, -1)　　# 同除地图分辨率，坐标四舍五入后归一化

ngoal= Node(round(gx / reso), round(gy / reso), 0.0, -1)

ox= [iox / reso for iox inox]

oy= [ioy / reso for ioy inoy]

obmap, minx, miny, maxx, maxy, xw, yw=calc_obstacle_map(ox, oy, reso, rr)　　# 障碍物地图范围计算、保存

motion=get_motion_model()　　# 节点移动向量共8组对应8个移动方向(x,y,cost)</