一、背景

编队控制代码地址

主要实现控制多架无人机从任意随机初始位置，运动成规则编队。需要安装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: /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 </licenses/>.%addpath('Helpers');%% Simulation parameters for triangle formation% Desired formation coordinates% qs = [0 -2 -2 -4 -4 -4% 0 -11 -202]*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 = [ 011000% 101110% 110011% 010010% 011101% 001010];%% Simulation parameters for square formation% Desired formation coordinates 10 所需编队坐标 （10个无人机所形成的形状）qs = [000 -1 -1 -1 -2 -2 -20 -1 -20 -1 -20 -1 -2]*15;%一字型% qs = [0 -1 -2 -3% 0 -1 -2 -3]*15;% Random initial positions 1.5q0 = [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 anglestheta0 = [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 matrixadj = [ 010100000 %第一个无人机和第2第4个101010000010001000100010100010101010001010001000100010000010101000001010];%针对一字型，新通信矩阵% adj = [ 0101%1010%0101%1010];%% ParametersT = [0, 30]; % Simulation time interval 模拟时间vSat = [3, 5]; % Speed range 3 5omegaSat = [-pi/4, pi/4];% Allowed heading angle rate of changep = [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 modelTvec = linspace(T(1), T(2), 50); %用于产生T1,T2之间的50点行矢量 也就是取0~30s内50个插值点state0 = [q0(:); theta0]; % Initial state 初始位置和初始方位角 27*1% Parameters passed down to the ODE solverpar.n = n;%无人机数目par.A = A;%增益矩阵par.p = p;%行进方向par.vSat= vSat;%速度范围par.omegaSat = omegaSat;%方向角度变化范围% Simulate the dynamic systemopt = odeset('AbsTol', 1.0e-06, 'RelTol', 1.0e-06);%用参数名以及参数值设定解法器的参数[t,stateMat] = ode45(@PlaneSys_Ver3_2, Tvec, state0, opt, par);%数值分析中数值求解微分方程组的一种方法，4阶五级Runge-Kutta算法%t 0-30s内的50个插值%% Make movie and plot the resultsclose all% Simulation parametersplotParam.adj = adj;plotParam.N = n;plotParam.stateMat = stateMat;%50*27包含所有无人机的位置信息plotParam.trace= 50; % Trace length 跟踪范围大小plotParam.qs = qs;%编队形状% Make movie and snapshotsfileName = 'UAVSim';MoviePlaneVer2_1(fileName, plotParam)%用来绘制结果

三、思路解读

首先每个无人机包含的信息有(x,y)坐标以及方向角yaw，程序开始前需要给这些无人机初始一些位置信息和角度信息，也就是代码中的q0位置信息和theta0方位角信息，然后确定无人机群的飞行方向，也就是p=[1,1]，表示向右上角飞。无人机编队最后的形状，由矩阵qs确定，通过(x,y)坐标确定无人机编队的最终形状。

以上初始化信息确定好后，需要确定仿真时间，仿真关键点个数，也就对应的是

Tvec = linspace(T(1), T(2), 50); %用于产生T1,T2之间的50点行矢量 也就是取0~30s内50个插值点

接着就是使用微分方程求解编队按着预设方向飞行的时候，各个无人机位置信息和角度信息。关键函数为ode45，返回值stateMat大小为50*27，每一行数据代表一个时刻点无人机的位置和角度信息，前18个分别为x,y坐标信息，后9个为方位角yaw的信息。