Matlab多无人艇协同避碰情况下的仿真探索(三)

最新推荐文章于 2024-04-24 12:21:59 发布

chenxiang12159

最新推荐文章于 2024-04-24 12:21:59 发布

阅读量2.7k

点赞数 10

本文链接：https://blog.csdn.net/chenxiang12159/article/details/121504328

版权

多无人艇仿真及避碰专栏收录该内容

4 篇文章 31 订阅

订阅专栏

距离上一次笔者更新已经过去了3个多月，这段时间主要忙于挑战杯国赛及开题报告等等事情。所以科研被耽误了很多。最近又有了一些进展，所以放这些放上来。作为记录，也供大家参考。

前提：笔者选择的反向是无人艇路径的优化与决策部分，涉及无人艇的路径规划与障碍物避碰。（在此不考虑艇本身的运动学特性，仅仅当一个壳子）

笔者的主要思路是无人艇的编队路径规划，具体分为全局路径规划和局部路径规划。

何为全局路径规划，即在无人艇编队未出发前已经总体的环境布局，已知静态障碍物的位置和可航区域。进行路径的决定。（默认知道环境中所有静态障碍物的信息）笔者采用的是通过无人艇与终点的连接获得一条虚拟路线，由该路径顺时针与逆时针旋转确定是否遇到障碍物及障碍物的边界位置，根据障碍物的边界位置确定无人艇的子目标点路径，全局情况下，每个无人艇的子目标点连接起来就是该无人艇的整体全局路径。

全局路径选择的代码可直接运行看效果

% 全局路径函数的编写
%   主要由顺逆时针的转动及确定障碍物的位置及可行走的目标路径点构成
% 重新计划避碰轨迹，虚拟领航者需要去寻找新的障碍物避碰点来达到避碰，首先知道总的目标点，(终点)。新的wp应该是目标船有航向角，
%  ...障碍物通过 寻找相切点——(加膨胀的半径) 来确定路径点


clear;
goal=[120 120];  % 终点
init_f=        [4 3 pi/2;%%%[x y th]
                -5 1 pi/4; 
                5 0 -pi/4;      %跟随者与领导者的初始位置所在
                6 4 pi/2;
                -2 -1 0];
pose_x=init_f(:,1);            % x都取第一个位置的量
pose_y=init_f(:,2);  
    pose_quan_x=init_f(:,1); 
    pose_quan_y=init_f(:,2);
obs(:,:,1)=[20 10;40 10;40 40;20 40]; %初始障碍物位置信息
obs(:,:,2)=[30 60;50 60;50 100;30 100];
N=5;
l=0.5;
sub_point=0;
goal_quanju=goal;
    [c_obs,r_obs]=clock_search_method_single(obs);   
    for i_quanju=1:1000
        
        for i_quan_num=1:N
                                                   
            
            dis_goal_os_x=goal_quanju(1,1)-pose_quan_x(i_quan_num,i_quanju);
            dis_goal_os_y=goal_quanju(1,2)-pose_quan_y(i_quan_num,i_quanju);
  
  
                if dis_goal_os_x>=0&&dis_goal_os_y>=0
                        theta_goal(i_quan_num,i_quanju)=atan(dis_goal_os_y/dis_goal_os_x);
                elseif dis_goal_os_y>=0&&dis_goal_os_x<0
                        theta_goal(i_quan_num,i_quanju)=pi-atan(abs(dis_goal_os_y/dis_goal_os_x));
                elseif dis_goal_os_y<0&&dis_goal_os_x<0
                        theta_goal(i_quan_num,i_quanju)=pi+atan(abs(dis_goal_os_y/dis_goal_os_x));
                elseif dis_goal_os_y<0&&dis_goal_os_x>0
                        theta_goal(i_quan_num,i_quanju)=2*pi-atan(abs(dis_goal_os_y/dis_goal_os_x));
                end
  % 计算各个障碍物点与本船形成的夹角
  % input(pose_x,pose_y  )
  
  %% 此处应该设置一个判断是否经过障碍物的函数，用来使船舶持续向目的地挺进
  % 不管路径是上还是下，总归还是一直在向前前进的对吧
  
  %% 计算多次 计算各个障碍物点与本船形成的夹角
  
  for i_obs_num=1:size(obs,3)  % 也是没问题的
      for i_deg_obs=1:size(obs,1)
            dis_obs_os_x=obs(i_deg_obs,1,i_obs_num)-pose_quan_x(i_quan_num,i_quanju);
            dis_obs_os_y=obs(i_deg_obs,2,i_obs_num)-pose_quan_y(i_quan_num,i_quanju);
            
            if dis_obs_os_x>=0&&dis_obs_os_y>=0
                theta_obs(i_quan_num,i_quanju,i_obs_num,i_deg_obs)=atan(dis_obs_os_y/dis_obs_os_x);
            elseif dis_obs_os_y>=0&&dis_obs_os_x<0
                theta_obs(i_quan_num,i_quanju,i_obs_num,i_deg_obs)=pi-atan(abs(dis_obs_os_y/dis_obs_os_x));
            elseif dis_obs_os_y<0&&dis_obs_os_x<0
                theta_obs(i_quan_num,i_quanju,i_obs_num,i_deg_obs)=pi+atan(abs(dis_obs_os_y/dis_obs_os_x));
            elseif dis_obs_os_y<0&&dis_obs_os_x>0
                theta_obs(i_quan_num,i_quanju,i_obs_num,i_deg_obs)=2*pi-atan(abs(dis_obs_os_y/dis_obs_os_x));
            end
      end
      
  end

 
% 这里每次计算的时候，同样是从首个障碍物开始算起的。
  for i_check=1:size(obs,3)
%       count_obs_num=count_obs_num+1;
      if (min(theta_obs(i_quan_num,i_quanju,i_check,:))<=theta_goal(i_quan_num,i_quanju))&&(theta_goal(i_quan_num,i_quanju)<=max(theta_obs(i_quan_num,i_quanju,i_check,:)))  %theta_goal 是指当前情况下与目标点的位置
          disp("产生子路径点")
          
          Way_change=1;  %进入子目标点函数的信号
          break;
          
      else
%           disp("无碰撞路径")
          Way_change=0;
      end
      
  end
% 根据Way_change的值来产生子目标点
% for i =1:1000
if Way_change==1   % 找两个变量
    theta_goal1(i_quan_num,i_quanju)=theta_goal(i_quan_num,i_quanju);
    theta_goal2(i_quan_num,i_quanju)=theta_goal(i_quan_num,i_quanju);

while ((theta_goal1(i_quan_num,i_quanju)>min(theta_obs(i_quan_num,i_quanju,i_check,:)))||(theta_goal2(i_quan_num,i_quanju)<max(theta_obs(i_quan_num,i_quanju,i_check,:))))
if (theta_goal1(i_quan_num,i_quanju)>min(theta_obs(i_quan_num,i_quanju,i_check,:)))
% 角：每一次调整5度，这个值可以调
    theta_goal1(i_quan_num,i_quanju)=theta_goal1(i_quan_num,i_quanju)-pi/90;
end
if(theta_goal2(i_quan_num,i_quanju)<max(theta_obs(i_quan_num,i_quanju,i_check,:)))
    
    theta_goal2(i_quan_num,i_quanju)=theta_goal2(i_quan_num,i_quanju)+pi/90;
end
end

     [obsmin_col]=find(theta_obs(i_quan_num,i_quanju,i_check,:)==min(theta_obs(i_quan_num,i_quanju,i_check,:)));
    
    obs_x1=obs(obsmin_col,1,i_check);
    obs_y1=obs(obsmin_col,2,i_check);
    theta_obs_d1=atan((obs_y1-pose_quan_y(i_quan_num,i_quanju))/(obs_x1-pose_quan_x(i_quan_num,i_quanju)));
    

    
    %假设找到了最远障碍物的边界点
    obs_ob_d1(i_quan_num,i_quanju)= sqrt((obs_x1-pose_quan_x(i_quan_num,i_quanju))^2+(obs_y1-pose_quan_y(i_quan_num,i_quanju))^2); % obs_x1,obs_y1指得是障碍物的位置

    obs_ob_d_long1(i_quan_num,i_quanju)=obs_ob_d1(i_quan_num,i_quanju)*cos(abs(theta_obs_d1-theta_goal(i_quan_num,i_quanju)));    % obs_ob_d_long 表示三角形中长边，和位置。
    obs_ob_d_f1(i_quan_num,i_quanju)=obs_ob_d_long1(i_quan_num,i_quanju)/(cos(abs(theta_goal1(i_quan_num,i_quanju)-theta_goal(i_quan_num,i_quanju)))); % obs_ob_d_f 表示最长边，就是我们要求的点
    
    x_wp1(i_quan_num,i_quanju)=pose_quan_x(i_quan_num,i_quanju)+obs_ob_d_f1(i_quan_num,i_quanju)*cos(theta_goal1(i_quan_num,i_quanju));
    y_wp1(i_quan_num,i_quanju)=pose_quan_y(i_quan_num,i_quanju)+obs_ob_d_f1(i_quan_num,i_quanju)*sin(theta_goal1(i_quan_num,i_quanju));

    [obsmax_row,obsmax_col,max_theta]=find(theta_obs(i_quan_num,i_quanju,i_check,:)==max(theta_obs(i_quan_num,i_quanju,i_check,:)));

    
    obs_x2=obs(obsmax_col,1,i_check);
    obs_y2=obs(obsmax_col,2,i_check);  

    theta_obs_d2=atan((obs_y2-pose_quan_y(i_quan_num,i_quanju))/(obs_x2-pose_quan_x(i_quan_num,i_quanju)));
    
    obs_ob_d2(i_quan_num,i_quanju)= sqrt((obs_x2-pose_quan_x(i_quan_num,i_quanju))^2+(obs_y2-pose_quan_y(i_quan_num,i_quanju))^2);
    obs_ob_d_long2(i_quan_num,i_quanju)=obs_ob_d2(i_quan_num,i_quanju)*cos(abs(theta_obs_d2-theta_goal(i_quan_num,i_quanju)));
    
    obs_ob_d_f2(i_quan_num,i_quanju)=obs_ob_d_long2(i_quan_num,i_quanju)/(cos(abs(theta_goal2(i_quan_num,i_quanju)-theta_goal(i_quan_num,i_quanju))));
    obs_ob_d_f2_record(i_quan_num,i_quanju)=obs_ob_d_f2(i_quan_num,i_quanju);

    x_wp2(i_quan_num,i_quanju)=pose_quan_x(i_quan_num,i_quanju)+obs_ob_d_f2(i_quan_num,i_quanju)*cos(theta_goal2(i_quan_num,i_quanju));
    y_wp2(i_quan_num,i_quanju)=pose_quan_y(i_quan_num,i_quanju)+obs_ob_d_f2(i_quan_num,i_quanju)*sin(theta_goal2(i_quan_num,i_quanju));



% 选择路径点
        a_ship=rand;
        if a_ship>0.5

            wp_x(i_quan_num,i_quanju)=x_wp1(i_quan_num,i_quanju);
            wp_y(i_quan_num,i_quanju)=y_wp1(i_quan_num,i_quanju);
        else

            wp_x(i_quan_num,i_quanju)=x_wp2(i_quan_num,i_quanju);
            wp_y(i_quan_num,i_quanju)=y_wp2(i_quan_num,i_quanju);
        end

else
    %说明船位与终点的连线没有障碍物，则按照原有的路径继续行进

            wp_x(i_quan_num,i_quanju)=goal_quanju(1,1);
            wp_y(i_quan_num,i_quanju)=goal_quanju(1,2);
end
     
        pose_quan_x(i_quan_num,i_quanju+1)=wp_x(i_quan_num,i_quanju); 
        pose_quan_y(i_quan_num,i_quanju+1)=wp_y(i_quan_num,i_quanju);
        
                    sub_point=sub_point+1;

           sub_goal(sub_point,i_quan_num,1)=wp_x(i_quan_num,i_quanju);
           sub_goal(sub_point,i_quan_num,2)=wp_y(i_quan_num,i_quanju);
           
           
      end


       if ((abs(pose_quan_x(1,i_quanju) - goal_quanju(1,1)) < 2*l)&&(abs(pose_quan_y(1,i_quanju) - goal_quanju(1,2))< 2*l))...
          &&((abs(pose_quan_x(2,i_quanju) - goal_quanju(1,1)) < 2*l)&&(abs(pose_quan_y(2,i_quanju) - goal_quanju(1,2))< 2*l))...
          &&((abs(pose_quan_x(3,i_quanju) - goal_quanju(1,1)) < 2*l)&&(abs(pose_quan_y(3,i_quanju) - goal_quanju(1,2))< 2*l))...
          &&((abs(pose_quan_x(4,i_quanju) - goal_quanju(1,1)) < 2*l)&&(abs(pose_quan_y(4,i_quanju) - goal_quanju(1,2))< 2*l))...
          &&((abs(pose_quan_x(5,i_quanju) - goal_quanju(1,1)) < 2*l)&&(abs(pose_quan_y(5,i_quanju) - goal_quanju(1,2))< 2*l))
           %             Change_wp=1;
%             disp('Change waypoint');
                    disp('达到目标点'); 
            break;  
       end
       
    end

% 画图
figure;
for i_obs=1:4
    for i_obs_num=1:2
%         axis([0 120 0 120])
        scatter(goal(1,1),goal(1,2),'v');
        hold on;
        if i_obs_num==1
        scatter(obs(i_obs,1,i_obs_num),obs(i_obs,2,i_obs_num),'r');
        else
        scatter(obs(i_obs,1,i_obs_num),obs(i_obs,2,i_obs_num),'g');  
        end
        
        hold on;
        
    end
end
hold on;


for i_usv=1:N
%     for i_waypoint=1:size(pose_quan_x,2)
        plot(pose_quan_x(i_usv,:),pose_quan_y(i_usv,:));
        hold on;
%     end
end

出来的图是下图

可以看出来，五个不同初始位置的艇，它们的终点相同，但是在碰到障碍物时选择了不同的路径。这就是全局路径。

然后就是将全局路劲的代码结合到局部路径上。这部分可参考我之前做的

Matlab多无人艇协同避碰情况下的仿真探索(二)

Matlab多无人艇协同避碰情况下的仿真探索

结合的时候碰到的困难主要是，障碍物的替换，因为我局部路径选择的避碰方法是人工势场法(APF),这个方法是每个点具有自己的势场，所以我的障碍物在全局路径上仅仅是障碍物的四个顶点就可以达到障碍物的效果，而在局部路径规划中需要将障碍物具体到许多点才能达到。

处理后的图：

该图为全局+局部之后的编队实际行进图

编队内各个艇与虚拟领导者之间的位置误差(中间误差过大是因为该艇选择了不同的避碰路径，可发现最后边对接基本恢复了队形)

编队内部各个艇的速度示意图，由于速度补偿措施的存在可使得艇掉队后能够快速跟上

这里显示的跟随艇的x与y方向的速度补偿

下一步，就是完成多个障碍物地形下的编队重构想法，目前还没实现，顺便优化一下我的图像。

欢迎大家加笔者qq：1131445521 或者邮箱chenxiang12159@tju.deu.cn 相互沟通学习。

chenxiang12159

关注

10
点赞
踩
38

收藏

觉得还不错? 一键收藏
4
评论
Matlab多无人艇协同避碰情况下的仿真探索(三)

距离上一次笔者更新已经过去了3个多月，这段时间主要忙于挑战杯国赛及开题报告等等事情。所以科研被耽误了很多。最近又有了一些进展，所以放这些放上来。作为记录，也供大家参考。前提：笔者选择的反向是无人艇路径的优化与决策部分，涉及无人艇的路径规划与障碍物避碰。（在此不考虑艇本身的运动学特性，仅仅当一个壳子）笔者的主要思路是无人艇的编队路径规划，具体分为全局路径规划和局部路径规划。何为全局路径规划，即在无人艇编队未出发前已经总体的环境布局，已知静态障碍物的位置和可航区域。进行路径的决定。（默认知道环境
复制链接

扫一扫