一、背景
編隊控制代碼地址
?主要實現(xiàn)控制多架無人機從任意隨機初始位置，運動成規(guī)則編隊。需要安裝cvx工具包CVX: Matlab Software for Disciplined Convex Programming | CVX Research, Inc.
二、代碼
% This script simulates formation control of a group of UAVs.
%
% -------> Scale of the formation is NOT controlled in this demo!
%
% -------> IMPORTANT: CVX must be installed before running!
%
% -------> Download CVX from: http://cvxr.com/cvx/
%
%
% This program is a free software: you can redistribute it and/or modify it
% under the terms of the GNU lesser General Public License,
% either version 3, or any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY. See the GNU Lesser General Public License
% for more details <http://www.gnu.org/licenses/>.
%
addpath('Helpers');
%% Simulation parameters for triangle formation
% Desired formation coordinates
% qs = [0 -2 -2 -4 -4 -4
% 0 -1 1 -2 0 2]*sqrt(5)*2;
%
% % Random initial positions
% q0 = [ 12.9329 8.2202 10.2059 1.1734 0.7176 0.5700
% 6.6439 8.5029 5.5707 6.8453 11.0739 14.3136];
%
% % Random initial heading angles
% theta0 = [5.6645 4.2256 1.8902 4.5136 3.6334 5.7688].';
%
% n = size(qs,2); % Number of agents
%
% % Graph adjacency matrix
% adj = [ 0 1 1 0 0 0
% 1 0 1 1 1 0
% 1 1 0 0 1 1
% 0 1 0 0 1 0
% 0 1 1 1 0 1
% 0 0 1 0 1 0];
%% Simulation parameters for square formation
% Desired formation coordinates 10 所需編隊坐標 （10個無人機所形成的形狀）
qs = [0 0 0 -1 -1 -1 -2 -2 -2
0 -1 -2 0 -1 -2 0 -1 -2]*15;
%一字型
% qs = [0 -1 -2 -3
% 0 -1 -2 -3]*15;
% Random initial positions 1.5
q0 = [18.2114 14.9169 7.6661 11.5099 5.5014 9.0328 16.0890 0.5998 1.7415;
16.0112 16.2623 12.3456 10.6010 4.9726 4.5543 19.7221 10.7133 16.0418]*1.5;
%一字形
% q0 = [ 27.3171 11.4992 13.5492 2.6122;
% 24.0168 18.5184 6.8314 24.0627]*1.5;
% Random initial heading angles
theta0 = [6.2150 0.4206 5.9024 0.1142 4.2967 4.9244 3.3561 5.5629 5.6486].';
% theta0 = [6.2150 0.4206 5.9024 0.1142].';
n = size(qs,2); % Number of agents
%theta0 = 2*pi*rand(n,1);
% Graph adjacency matrix
adj = [ 0 1 0 1 0 0 0 0 0 %第一個無人機和第2第4個
1 0 1 0 1 0 0 0 0
0 1 0 0 0 1 0 0 0
1 0 0 0 1 0 1 0 0
0 1 0 1 0 1 0 1 0
0 0 1 0 1 0 0 0 1
0 0 0 1 0 0 0 1 0
0 0 0 0 1 0 1 0 1
0 0 0 0 0 1 0 1 0];
%針對一字型，新通信矩陣
% adj = [ 0 1 0 1
% 1 0 1 0
% 0 1 0 1
% 1 0 1 0];
%% Parameters
T = [0, 30]; % Simulation time interval 模擬時間
vSat = [3, 5]; % Speed range 3 5
omegaSat = [-pi/4, pi/4]; % Allowed heading angle rate of change
p = [1; 1]; % Desired travel direction 行進方向
%% Computing formation control gains
% Find stabilizing control gains (Needs CVX) 增益控制矩陣
A = FindGains(qs(:), adj); %無人機形狀 無人機間連接矩陣
% % If optimization failed, perturbe 'qs' slightly:
% A = FindGains(qs(:)+0.0001*rand(2*n,1), adj);
%% Simulate the model
Tvec = linspace(T(1), T(2), 50); %用于產(chǎn)生T1,T2之間的50點行矢量 也就是取0~30s內50個插值點
state0 = [q0(:); theta0]; % Initial state 初始位置和初始方位角 27*1
% Parameters passed down to the ODE solver
par.n = n;%無人機數(shù)目
par.A = A;%增益矩陣
par.p = p;%行進方向
par.vSat = vSat;%速度范圍
par.omegaSat = omegaSat;%方向角度變化范圍
% Simulate the dynamic system
opt = odeset('AbsTol', 1.0e-06, 'RelTol', 1.0e-06);%用參數(shù)名以及參數(shù)值設定解法器的參數(shù)
[t,stateMat] = ode45(@PlaneSys_Ver3_2, Tvec, state0, opt, par);%數(shù)值分析中數(shù)值求解微分方程組的一種方法，4階五級Runge-Kutta算法
%t 0-30s內的50個插值
%% Make movie and plot the results
close all
% Simulation parameters
plotParam.adj = adj;
plotParam.N = n;
plotParam.stateMat = stateMat;%50*27包含所有無人機的位置信息
plotParam.trace = 50; % Trace length 跟蹤范圍大小
plotParam.qs = qs;%編隊形狀
% Make movie and snapshots
fileName = 'UAVSim';
MoviePlaneVer2_1(fileName, plotParam)%用來繪制結果
?
三、思路解讀
首先每個無人機包含的信息有(x,y)坐標以及方向角yaw，程序開始前需要給這些無人機初始一些位置信息和角度信息，也就是代碼中的q0位置信息和theta0方位角信息，然后確定無人機群的飛行方向，也就是p=[1,1]，表示向右上角飛。無人機編隊最后的形狀，由矩陣qs確定，通過(x,y)坐標確定無人機編隊的最終形狀。
以上初始化信息確定好后，需要確定仿真時間，仿真關鍵點個數(shù)，也就對應的是
Tvec = linspace(T(1), T(2), 50); %用于產(chǎn)生T1,T2之間的50點行矢量 ?也就是取0~30s內50個插值點文章來源：http://www.zghlxwxcb.cn/news/detail-744051.html
接著就是使用微分方程求解編隊按著預設方向飛行的時候，各個無人機位置信息和角度信息。?關鍵函數(shù)為ode45，返回值stateMat大小為50*27，每一行數(shù)據(jù)代表一個時刻點無人機的位置和角度信息，前18個分別為x,y坐標信息，后9個為方位角yaw的信息。文章來源地址http://www.zghlxwxcb.cn/news/detail-744051.html
到了這里，關于無人機集群編隊控制的文章就介紹完了。如果您還想了解更多內容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
