深蓝路径规划作业-第二章

一、在matlab中实现A*算法

二、在ROS环境中实现JPS

一、在matlab中实现A*算法

A*算法主要是在Dijkstra算法上增加了启发式搜索功能，其表达式可以写成

$f(n)=g(n)+h(n)$ ,

其中g(n)表示当前点的cost，h(n)表示对终点的预测，这个预测可以用0、1、2和∞范数，但L1范数（曼哈顿/出租车距离）无法保证其最优性。其算法流程可以用下图表示：

那么在代码中该如何实现呢？本节作业主要是完成A_star_search.m，在随机的地图中生成一条安全的A*路径。

首先最先想到的是我们要有一张地图，那么需要有一定的长度和宽度（二维）、离散的障碍物。因此我们定义一个地图，并设置起始点和终止点。

% Environment map in 2D space 
% 生成地图
xStart = 1.0;%起始点横坐标
yStart = 1.0;
xTarget = 9.0;%目标点横坐标
yTarget = 9.0;
MAX_X = 10;%地图宽度
MAX_Y = 10;
map = obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y); % 返回障碍物出现的地方

这个obstacle_map主要是生成地图上的障碍，当rand小于0.45并且不是起始点和终止点的时候产生障碍map点。

 function map = obstacle_map(xStart,yStart,xTarget,yTarget,MAX_X,MAX_Y)
%This function returns a map contains random distribution obstacles.
    rand_map = rand(MAX_X,MAX_Y);
    map = [];
    % 第一个障碍物为起点
    map(1,1) = xStart;
    map(1,2) = yStart;
    k=2;
    % 地图中的障碍物比例
    obstacle_ratio = 0.45;
    for i = 1:1:MAX_X
        for j = 1:1:MAX_Y
            if( (rand_map(i,j) < obstacle_ratio) && (i~= xStart || j~=yStart) && (i~= xTarget || j~=yTarget))
                map(k,1) = i;
                map(k,2) = j;
                k=k+1;
            end    
        end
    end
    % 最后一个障碍物为终点
    map(k,1) = xTarget;
    map(k,2) = yTarget;
end

然后就需要执行生成A*路径的算法了：

% Waypoint Generator Using the A* 
% 路径搜索
path = A_star_search(map, MAX_X,MAX_Y);

其中在A*寻找这个函数中，首先将障碍物、目标和起始点进行定义：Obstacle=-1, Target = 0, Start=1，并将这个地图乘以一个系数k。根据A*算法伪代码，需要维护两个数组，一个是oprn list，一个是clsed list，其中第一步要将所有障碍物加入closed list：

  %Put all obstacles on the Closed list
    % 将障碍物加入close list
    k=1;%Dummy counter
    for i=1:MAX_X
        for j=1:MAX_Y
            if(MAP(i,j) == -1)
                CLOSED(k,1)=i;
                CLOSED(k,2)=j;
                k=k+1;
            end
        end
    end

之后的功能主要是维护OPEN这个数组，将数组里的数弹出比较，直到扩展到目标节点，生成path

以 10X10的地图为例，最后的效果如下：

在生成地图的过程中，因为障碍是随机生成的，可能并没有路径出现。此外，A*算法扩展效率慢，当地图变大的时候，效率很低，计算耗时很大。

二、在ROS环境中实现JPS和A*对比

实验结果如下，具体步骤参考第一章流程：深蓝路径规划作业-第一章