自動(dòng)駕駛路徑規(guī)劃：A*（Astar）算法

3. 其他Astar算法

4. MATLAB實(shí)現(xiàn)Astar算法

function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ...    % 1-白色-空地
  0 0 0; ...      % 2-黑色-靜態(tài)障礙
  1 0 0; ...      % 3-紅色-動(dòng)態(tài)障礙
  1 1 0;...      % 4-黃色-起始點(diǎn) 
  1 0 1;...      % 5-品紅-目標(biāo)點(diǎn)
  0 1 0; ...      % 6-綠色-到目標(biāo)點(diǎn)的規(guī)劃路徑  
  0 1 1];       % 7-青色-動(dòng)態(tài)規(guī)劃的路徑


% 構(gòu)建顏色MAP圖
colormap(cmap);


% 定義柵格地圖全域，并初始化空白區(qū)域
field = ones(rows, cols);


% 障礙物區(qū)域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

function childNodes = getChildNode(field,closeList, parentNode)
% 選取父節(jié)點(diǎn)周邊8個(gè)節(jié)點(diǎn)作為備選子節(jié)點(diǎn)，線性化坐標(biāo)
% 排除超過邊界之外的、位于障礙區(qū)的、位于closeList中的


[rows, cols] = size(field);
[row_parentNode, col_parentNode] = ind2sub([rows, cols], parentNode);
childNodes = [];
closeList = closeList(:,1);


% 第1個(gè)子節(jié)點(diǎn)(右節(jié)點(diǎn))
childNode = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end


% 第2個(gè)子節(jié)點(diǎn)（右上節(jié)點(diǎn)）
childNode = [row_parentNode-1, col_parentNode+1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end  
  end
end




% 第3個(gè)子節(jié)點(diǎn)（上節(jié)點(diǎn)）
childNode = [row_parentNode-1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end




% 第4個(gè)子節(jié)點(diǎn)（左上）
childNode = [row_parentNode-1, col_parentNode-1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end 
  end
end




% 第5個(gè)子節(jié)點(diǎn)(左節(jié)點(diǎn))
childNode = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end




% 第6個(gè)子節(jié)點(diǎn)（左下）
childNode = [row_parentNode+1, col_parentNode-1];
  child_brother_node_sub1 = [row_parentNode, col_parentNode-1];
  child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end 
  end
end




% 第7個(gè)子節(jié)點(diǎn)（下）
childNode = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end




% 第8個(gè)子節(jié)點(diǎn)（右下）
childNode = [row_parentNode+1, col_parentNode+1];
  child_brother_node_sub1 = [row_parentNode, col_parentNode+1];
  child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2 
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2)); 
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end
  end
end

% 基于柵格地圖的機(jī)器人路徑規(guī)劃算法
% A*算法
clc
clear
close all


%% 柵格界面、場(chǎng)景定義
% 行數(shù)和列數(shù)
rows = 20;
cols = 20;
[field,cmap] = defColorMap(rows, cols);


% 起點(diǎn)、終點(diǎn)、障礙物區(qū)域
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;


%% 預(yù)處理


% 初始化closeList
parentNode = startPos;
closeList = [startPos,0];


% 初始化openList
openList = struct;
childNodes = getChildNode(field,closeList,parentNode);
for i = 1:length(childNodes)
  [row_startPos,col_startPos] = ind2sub([rows, cols],startPos);
  [row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);  
  [row,col] = ind2sub([rows, cols],childNodes(i));


  % 存入openList結(jié)構(gòu)體
  openList(i).node = childNodes(i);
  openList(i).g = norm([row_startPos,col_startPos] - [row,col]); % 實(shí)際代價(jià)采用歐式距離
  openList(i).h = abs(row_goalPos - row) + abs(col_goalPos - col); % 估計(jì)代價(jià)采用曼哈頓距離
  openList(i).f = openList(i).g + openList(i).h;
end


% 初始化path
for i = 1:rows*cols
  path{i,1} = i; % 線性索引值
end
for i = 1:length(openList)
  node = openList(i).node;
  path{node,2} = [startPos,node];
end 


%% 開始搜索
% 從openList開始搜索移動(dòng)代價(jià)最小的節(jié)點(diǎn)
[~, idx_min] = min([openList.f]); 
parentNode = openList(idx_min).node;


% 進(jìn)入循環(huán)
while true 
  
  % 找出父節(jié)點(diǎn)的8個(gè)子節(jié)點(diǎn)，障礙物節(jié)點(diǎn)用inf，
  childNodes = getChildNode(field, closeList,parentNode);
  
  % 判斷這些子節(jié)點(diǎn)是否在openList中，若在，則比較更新；沒在則追加到openList中
  for i = 1:length(childNodes)
    
    % 需要判斷的子節(jié)點(diǎn)
    childNode = childNodes(i);
    [in_flag,idx] = ismember(childNode, [openList.node]);
      
    % 計(jì)算代價(jià)函數(shù)
    [row_parentNode,col_parentNode] = ind2sub([rows, cols], parentNode);
    [row_childNode,col_childNode] = ind2sub([rows, cols], childNode);
    [row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);
    g = openList(idx_min).g + norm( [row_parentNode,col_parentNode] -...
      [row_childNode,col_childNode]);
    h = abs(row_goalPos - row_childNode) + abs(col_goalPos - col_childNode); % 采用曼哈頓距離進(jìn)行代價(jià)估計(jì)
    f = g + h;
    
    if in_flag  % 若在，比較更新g和f    
      if f < openList(idx).f
        openList(idx).g = g;
        openList(idx).h = h;
        openList(idx).f = f;
        path{childNode,2} = [path{parentNode,2}, childNode];
      end
    else     % 若不在，追加到openList
      openList(end+1).node = childNode;
      openList(end).g = g;
      openList(end).h = h;
      openList(end).f = f;
      path{childNode,2} = [path{parentNode,2}, childNode];
    end
  end
    
  % 從openList移出移動(dòng)代價(jià)最小的節(jié)點(diǎn)到closeList
  closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
  openList(idx_min)= [];


  % 重新搜索：從openList搜索移動(dòng)代價(jià)最小的節(jié)點(diǎn)
  [~, idx_min] = min([openList.f]);
  parentNode = openList(idx_min).node;
  
  % 判斷是否搜索到終點(diǎn)
  if parentNode == goalPos
    closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
    break
  end
end


%% 畫路徑
% 找出目標(biāo)最優(yōu)路徑
path_target = path{goalPos,2};
for i = 1:rows*cols
   if ~isempty(path{i,2}) 
    field((path{i,1})) = 7;
   end
end
field(startPos) = 4;
field(goalPos) = 5;
field(path_target(2:end-1)) = 6;


% 畫柵格圖
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
hold on;
[y0,x0] = ind2sub([rows,cols], path_target);
y = y0 + 0.5;
x = x0 + 0.5;
plot(x,y,'-','Color','r','LineWidth',2.5);
hold on;
points = [x',y'];
M = 10;
[x1,y1] = bezir_n(points, M);
plot(x1,y1,'-','Color','y','LineWidth',2.5);
hold on;
values = spcrv([[x(1) x x(end)];[y(1) y y(end)]],3);
plot(values(1,:),values(2,:),'b','LineWidth',2.5);

5. python實(shí)現(xiàn)Astar算法

make an openlist containing only the starting node
make an empty closed list
  while (the destination node has not been reached):
    consider the node with the lowest f score in the open list
    if (this node is our destination node) :
      we are finished 
    if not:
      put the current node in the closed list and look at all of its neighbors
      for (each neighbor of the current node):
        if (neighbor has lower g value than current and is in the closed list) :
          replace the neighbor with the new, lower, g value 
          current node is now the neighbor's parent      
        else if (current g value is lower and this neighbor is in the open list ) :
          replace the neighbor with the new, lower, g value 
          change the neighbor's parent to our current node


        else if this neighbor is not in both lists:
          add it to the open list and set its g

6. Java實(shí)現(xiàn)Astar算法

7. 實(shí)踐案例—基于ROS實(shí)現(xiàn)Astar與DWA算法