基于动态窗口法的编队算法

#自己用动态窗口法写了一个算法，和长机-僚机法结合起来

动态窗口法的原理大致是在速度与加速度约束内，遍历选择所有可能的速度来进行预测未来秒的航迹，通过自己设置的航迹评价函数，例如与目标点距离，与障碍物距离，速度等多方面进行最优选择。

但是与传统优化算法类似，容易陷入局部最优解。表现就是不能避开与目标连线的障碍物。为避免动态窗口法陷入局部最优，提高复杂环境下的航 迹规划能力，在文献 的基础上，本文使用一种自适应航 迹评价函数权重的动态窗口法。采用两输入三输出的模糊控 制器优化权重系数，最终我编写代码出来的结果图如下：

整个算法的流程图如下：

部分代码如下“

function [ ] = formation_6 ()
close all;                 % 4follower and 1 leader
countmax=2000;
% stop_goal=[100 100];
 stop_goal=[40 40];
%% 初始化 位置pose、速度V、加速度控制量control
% start=[-8*2.5 -4*2.5 0 0 0;%%%[x y th]
%     -11*2.5 -2*2.5 0 0 0;
%         -7*2.5 -7*2.5 0 0 0;
%     -7*2.5 -4*2.5 0 0 0;
%     -3*1.5 -2*1.5 0 0 0];
% obstacle=[ 20 -20*rand(1);
%     60 100*rand(1);
%     120 50*rand(1);
%     250 50;
%     150 100
%     200 200
%     175 175];
obstacleR=0.5;% 冲突判定用的障碍物半径
%% 初始化 位置pose、速度V、加速度控制量control
%         start=[-7 -4 0 0 0;%%%[x y th]
%                 -6 -2 0 0 0; 
%                 -10 -3 0 0 0;
%                 -6 -4 0 0 0;
%                 -3 -2 0 0 0];   
start=[3 1 0 0 0;%%%[x y th]
    0.5 0.5 0 0 0;
    1 1 0 0 0;
   0 1 0 0 0;
    5 5 0 0 0];

     if ~isempty(traj5)
        for it=1:length(traj5(:,1))/5
            ind=1+(it-1)*5;
            plot(traj5(ind,:),traj5(ind+1,:),'-g');
        end
    end%画探索轨迹
        if norm(x5(1:2)-stop_goal')<0.5
            disp('Arrive Goal!!');
        end
    grid on;
    drawnow;

#本文的算法是自己写的小论文中的，其他博客会更新一些传统的轨迹更新方法，相关的代码也会放在代码仓库中promising76 · GitHub

欢迎各位提问探讨