Robotics: computational motion planning 部分笔记—

Robotics: computational motion planning 部分笔记—— week 1 graph-based

grassfire algorithm

四周扩散性；从终点开始按照相邻最小距离格子移动
在这里插入图片描述

Dijkstra’s Algorithm

标明从起点开始的所有点的最短距离（从上一节点继承），直到终点
在这里插入图片描述

伪代码

A* Algorithm

带有启发性的，给出距离估计，朝向终点的搜索。
其实就是引入了一个启发式函数 $H (n)$ ，用来估计节点 n 和终点 g 之间的距离。
算法的改进在选点（current）上，列表是以 f 值从小到大排序的，f 值就是已经走过的距离值加上 $H (n)$ 。这样做会优先探索离目标点近的节点，提高搜索效率。
在这里插入图片描述
作业代码：

    numExpanded = numExpanded + 1; current_node=[i,j];%i是row j是cols_node=[i+1,j];n_node=[i-1,j];e_node=[i,j+1];w_node=[i,j-1];neighbourhood=[s_node;n_node;e_node;w_node];step =0;step =step+1; %记录已经走过的步数for k=1:4if((neighbourhood(k,1)>0 && neighbourhood(k,2)>0) && (neighbourhood(k,1)<=nrows && neighbourhood(k,2)<=ncols))%关于未出界的判定if(map(neighbourhood(k,1),neighbourhood(k,2)) ~=3 && map(neighbourhood(k,1),neighbourhood(k,2)) ~=2 && map(neighbourhood(k,1),neighbourhood(k,2)) ~=5 )g(neighbourhood(k,1),neighbourhood(k,2)) = step;if(f(neighbourhood(k,1),neighbourhood(k,2))>  step+H(neighbourhood(k,1),neighbourhood(k,2)) )    f(neighbourhood(k,1),neighbourhood(k,2))=g(neighbourhood(k,1),neighbourhood(k,2))+H(neighbourhood(k,1),neighbourhood(k,2));parent(neighbourhood(k,1),neighbourhood(k,2))=sub2ind(size(map),current);%把现在的邻居点加入到父列表中map(neighbourhood(k,1),neighbourhood(k,2))=3;endendendend