无人机集群编队控制_基于vicon的无人机编队分布式控制代码

一、背景

编队控制代码地址

 主要实现控制多架无人机从任意随机初始位置，运动成规则编队。需要安装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); %用于产生T1,T2之间的50点行矢量  也就是取0~30s内50个插值点
state0 = [q0(:); theta0];  % Initial state  初始位置和初始方位角 27*1
 
% Parameters passed down to the ODE solver
par.n        = n;%无人机数目
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);%用参数名以及参数值设定解法器的参数
[t,stateMat] = ode45(@PlaneSys_Ver3_2, Tvec, state0, opt, par);%数值分析中数值求解微分方程组的一种方法，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)坐标确定无人机编队的最终形状。

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

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

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