Robotics: computational motion planning 部分笔记—

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

news2024/9/20 20:34:38

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是col
    s_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:4
        if((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;
	            end
            end
        end
    end