1 简介
本文提出一种基于一致性算法和人工势场方法的无人机编队及其避障控制方法.通过引入与障碍物移动方向垂直的辅助牵引加速度信息,可消除局部优化解的影响.同时,为了实现无人机机群快速编队和避障,本文将有限时间一致性算法与人工势场方法相结合,提出一种编队及其避障控制方法.
2 部分代码
clc
clear all
close all
%%%%一阶算法仿真 非完整型约束
%%% 设置好重心偏移量和gama,权重,基本可行
%% 初始化 位置pose、速度V、加速度控制量control
init_f=[-3 -6 0; %%%[x y th]
-5 6 0;
2 4 pi/4;
5 -3 -pi/4;
3 0 pi/2];
pose_x=init_f(:,1);
pose_y=init_f(:,2);
pose_th=init_f(:,3);
%% follower相对leader的位置
delta_x=[-2 -6 -2 -6 0]; % 相对间隔误差
delta_y=[4 4 -4 -4 0]; %领航者与自己无误差
fol_num=4;
N=5; % 4follower and 1 leader
countmax=2000;
dt=0.1;
gama=3;
di=0.02; %%%重心偏移 单位m
K0=0;
%%% 直线K1 K2都设在0.2左右 k3=0
%%% 圆不考虑K2=
|