阅读 123

【路径规划】基于matlab A_star算法多机器人路径规划【含Matlab源码 1251期】

一、A_star算法简介

0 引言 随着现代技术的发展,飞行器种类不断变多,应用也日趋专一化、完善化,如专门用作植保的大疆PS-X625无人机,用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机,以及用作水下救援的白鲨MIX水下无人机等,决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言,在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作,可能会对操作员心理以及技术提出极高的要求,为了避免个人操作失误,进而造成飞行器损坏的危险,一种解决问题的方法就是对飞行器进行航迹规划。 飞行器的测量精度,航迹路径的合理规划,飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务,并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。常见的航迹规划算法如图1所示。 在这里插入图片描述 图1 常见路径规划算法 文中主要对无人机巡航阶段的航迹规划进行研究,假设无人机在飞行中维持高度与速度不变,那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单,容易实现。在改进A算法基础上,提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化,节点扩展只限于栅格线的交叉点,在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化,然后分别用两段上的相应路径规划点作为切点,找到相对应的组成内切圆的圆心,然后作弧,并求出相对应的两切点之间的弧所对应的圆心角,根据下式计算出弧线的长度 在这里插入图片描述 式中:R———内切圆的半径; α———切点之间弧线对应的圆心角。

1 A*算法概述 A算法是在Dijstar算法的基础上引入的启发式函数,通过定义的代价函数来评估代价大小,从而确定最优路径。A算法的代价函数 在这里插入图片描述 式中:f(x,y)———初始状态X0(x0,y0)到达目标状态X1(x1,y1)的代价估计; g(x,y)———状态空间中从初始状态X0(x0,y0)到状态N(x1,y1)的实际代价; h(x,y)———从状态N(x1,y1)到目标状态X1(x1,y1)最佳路径的估计代价。 要找到最短路径的实质是找到f(x,y)的最小值,其中在式(2)中寻找最短路径的关键在于求估计代价h (x,y)值。设系数λ表示状态N(x1,y1)到X1(x1,y1)最优距离,如果λ<h(x,y),搜索范围小,不能保证得到最优解;λ>h(x,y),搜索范围大,费时,但能找到最优解;λ=h(x,y),搜索到最短路径。其中h(x,y)一般用欧几里德距离(式(3))或者绝对值距离(式(4))计算。 在这里插入图片描述 A算法是以起始点为中心,周围8个栅格的中心为下一步预选,并不断地计算预选位置的f(x,y)值,其中f(x,y)值最小的作为当前位置,依次逐层比较,直到当前位置的临近点出现目标点为止,其最小单元如图2所示。 在这里插入图片描述 图2 最小单元 A算法的流程如下: 1)创建开始节点START,目标节点TARGET、OPEN列表、CLOSE列表、CLOSE列表初始为空; 2)将START加入到OPEN列表; 3)检查OPEN列表中的节点,若列表为空,则无可行路径;若不为空,选择使f(x,y)值最小的节点k; 4)将节点k从OPEN中去除,并将其添加到CLOSE中,判断节点k是否目标节点TARGET,若是,则说明找到路径;若不是,则继续扩展节点k,生成k节点的子节点集,设q为k的子节点集,对所有节点q计算相应的f(x,y)值,并选择f(x,y)值最小的节点,将该节点放入CLOSE列表中; 5)跳到3),直到算法获得可行路径或无解退出。

二、部分源代码

%% 使用A *搜索演示

%% 参数的设定
% 定义二维地图框架
MAX_X = 10;
MAX_Y = 10;
MAP = zeros(MAX_X,MAX_Y) % 此数组存储地图的坐标和每个坐标中的对象
% 获得障碍,目标和机器人位置
% 使用输入值初始化MAP
% 障碍= -1,目标= 1,起点= 1,空格= 0
j=0;
x_val = 1;
y_val = 1;
axis([1 MAX_X+1 1 MAX_Y+1])  % 框图的大小
grid on;                     % 加入网格
hold on;                     % 与新图共存
n=0;                         % 障碍物的数量

 for BB = 1:3
%% 确定目标点
pause(1);                           % 一般是为了动态观察变化过程 pause(a)暂停a秒后执行下一条指令
h=msgbox('请使用鼠标左键选择目标');  % 显示提示窗口
uiwait(h,5);
if ishandle(h) == 1
    delete(h);
end
xlabel('请使用鼠标左键选择目标','Color','black');
but=0;                               % 这个地方可以设置成1,这样的话,统一目标点
while (but ~= 1)                     % 重复,直到没有单击“向左”按钮
    [xval,yval,but]=ginput(1);
end
xval=floor(xval);
yval=floor(yval);
xTarget=xval;                        % X目标的坐标
yTarget=yval;                        % Y目标的坐标
MAP(xval,yval)=1;                    % 初始化地图中的目标位置
plot(xval+.5,yval+.5,'gd');
text(xval+1,yval+.5,'目标点')

%% 确定障碍物
pause(2);
h=msgbox('使用鼠标左键选择障碍物,使用右键选择最后一个障碍物');
  xlabel('使用鼠标左键选择障碍物,使用右键选择最后一个障碍物','Color','blue');
uiwait(h,10);
if ishandle(h) == 1
    delete(h);
end
while but == 1
    [xval,yval,but] = ginput(1);
    xval=floor(xval);
    yval=floor(yval);
    MAP(xval,yval)=-1;             %Put on the closed list as well
    plot(xval+.5,yval+.5,'ro');
end

%% 确定起点
pause(1);
h=msgbox('请使用鼠标左键选择初始位置');
uiwait(h,5);
if ishandle(h) == 1
    delete(h);
end
xlabel('请选择初始位置','Color','black');
but=0;
while (but ~= 1)                    % 重复,直到没有单击“向左”按钮
    [xval,yval,but]=ginput(1);
    xval=floor(xval);
    yval=floor(yval);
end
xStart=xval;                       % 起始位置
yStart=yval;                    
MAP(xval,yval)=1;
 plot(xval+.5,yval+.5,'bo');
% End of obstacle-Target pickup

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM  用于算法的列表
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE       开放列表结构
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
OPEN=[];
%CLOSED LIST STRUCTURE 封闭的列表结构
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED=[];

%Put all obstacles on the Closed 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
CLOSED_COUNT=size(CLOSED,1);
%set the starting node as the first node  将起始节点设置为第一个节点
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;
goal_distance=distance(xNode,yNode,xTarget,yTarget);
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);
OPEN(OPEN_COUNT,1)=0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED(CLOSED_COUNT,1)=xNode;
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1)
%  plot(xNode+.5,yNode+.5,'go');
 exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);
 exp_count=size(exp_array,1);
 %UPDATE LIST OPEN WITH THE SUCCESSOR NODES  更新列表打开SUCCESSOR NODES
 %OPEN LIST FORMAT
 %--------------------------------------------------------------------------
 %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
 %--------------------------------------------------------------------------
 %EXPANDED ARRAY FORMAT
 %--------------------------------
 %|X val |Y val ||h(n) |g(n)|f(n)|
 %--------------------------------
 for i=1:exp_count
    flag=0;
    for j=1:OPEN_COUNT
        if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) )
            OPEN(j,8)=min(OPEN(j,8),exp_array(i,5)); %#ok<*SAGROW>
            if OPEN(j,8)== exp_array(i,5)
                %UPDATE PARENTS,gn,hn
                OPEN(j,4)=xNode;
                OPEN(j,5)=yNode;
                OPEN(j,6)=exp_array(i,3);
                OPEN(j,7)=exp_array(i,4);
            end;%End of minimum fn check
            flag=1;
        end;%End of node check
%         if flag == 1
%             break;
    end;%End of j for
    if flag == 0
        OPEN_COUNT = OPEN_COUNT+1;
        OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5));
     end;%End of insert new element into the OPEN list
 end;%End of i for
 %END OF WHILE LOOP

 % Find out the node with the smallest fn  找出fn最小的节点
  index_min_node = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);
  if (index_min_node ~= -1)    
   %Set xNode and yNode to the node with minimum fn
   xNode=OPEN(index_min_node,2);
   yNode=OPEN(index_min_node,3);
   path_cost=OPEN(index_min_node,6);  % Update the cost of reaching the parent node 更新到达父节点的成本
  %Move the Node to list CLOSED     将节点移动到列表CLOSED
  CLOSED_COUNT=CLOSED_COUNT+1;
  CLOSED(CLOSED_COUNT,1)=xNode;
  CLOSED(CLOSED_COUNT,2)=yNode;
  OPEN(index_min_node,1)=0;
  else
                  % No path exists to the Target!!
      NoPath=0;   % Exits the loop!
  end;            % End of index_min_node check
end;
% 算法运行后,通过从最后一个节点(如果它是目标节点)开始生成最佳路径,然后识别其父节点,直到它到达起始节点。这是最佳路径

i=size(CLOSED,1);
Optimal_path=[];
xval=CLOSED(i,1);
yval=CLOSED(i,2);
i=1;
Optimal_path(i,1)=xval;
Optimal_path(i,2)=yval;

i=i+1;
if ( (xval == xTarget) && (yval == yTarget))
    inode=0;
   % Traverse OPEN and determine the parent nodes 遍历OPEN并确定父节点
   parent_x=OPEN(node_index(OPEN,xval,yval),4); % node_index returns the index of the node     node_index返回节点的索引
   parent_y=OPEN(node_index(OPEN,xval,yval),5);
   
   while( parent_x ~= xStart || parent_y ~= yStart)
           Optimal_path(i,1) = parent_x;
           Optimal_path(i,2) = parent_y;
           %Get the grandparents:-)
           inode=node_index(OPEN,parent_x,parent_y);
           parent_x=OPEN(inode,4);    % node_index returns the index of the node  node_index返回节点的索引
           parent_y=OPEN(inode,5);
           i=i+1;
    end;
 j=size(Optimal_path,1);
 % Plot the Optimal Path!  % 绘制最佳路径!
 p=plot(Optimal_path(j,1)+.5,Optimal_path(j,2)+.5,'bo');
 j=j-1;
 for i=j:-1:1
 
 h=msgbox('不好意思,没有找到目标的路径!','warn');
 uiwait(h,5);
end
Optimal_path
[m,n]=size(Optimal_path);
for ii=1:m;
%     for jj=1:1
       a=Optimal_path(ii,1)
       b=Optimal_path(ii,2)
        MAP(a,b)=1
       c=a+0.5;
       d=b+0.5;
%         Optimal_path(i,j)=1;
%     end
plot(c,d,'*k');hold on;
end



% MAP(Optimal_path(2,1),Optimal_path(2,2))=1
MAP1=rot90(MAP)
number = size(Optimal_path,1)

if (BB==1)
Optimal_path1 = Optimal_path
        hold on;
else if (BB==2)
        Optimal_path2 = Optimal_path
        hold on;
    else
        Optimal_path3 = Optimal_path
    end
end

 end
    




复制代码

三、运行结果

在这里插入图片描述

四、matlab版本及参考文献

1 matlab版本 2014a

2 参考文献 [1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例(第2版)[M].电子工业出版社,2016. [2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社,2017.

文章分类
人工智能
文章标签