【飞行器】基于matlab多源信息融合算法多旋翼无人机组合导航系统【含Matlab源码 1267期】

一、四旋翼飞行器简介

0 引 言
四旋翼飞行器由于具有可垂直起降、机动性强、操作方便等诸多优点,在军事和民用场合得到广泛应用,从而成为众多学者的研究热点。四旋翼飞行器是具有四输入、六输出的欠驱动、非线性、强耦合系统。其姿态控制精度和抗干扰问题一直是研究重点。目前国内较普遍的飞行器控制算法主要包括:反步法、自适应控制、H控制、滑模控制、自抗扰控制等,对实现四旋翼飞行器的姿态控制具有重要的理论和实践意义。文献提出应用反步算法为飞行器上下、前后、左右、偏航4个子系统配置控制律,实现了四旋翼飞行器对设定轨迹的精确跟踪。该算法在构造Lyapunov函数的过程中,是以其导数小于零为前提,因此应用受到局限。文献针对传统的离散线性滑模应用于四旋翼飞行器控制具有跟踪误差大、响应速度慢、不能有限时间收敛等问题,提出了干扰观测器补偿的自适应离散终端滑模控制,使响应时间更快、跟踪效果更理想、鲁棒性更强。文献利用线性扩张状态观测器对四旋翼飞行器内部不确定干扰和外部干扰进行实时估计,进而采取线性状态反馈控制对扰动的估计值进行在线补偿,以实现四旋翼飞行器的姿态控制。

1 四旋翼飞行器动力学模型的建立
1. 1 四旋翼飞行器受力分析
对于飞行器的每个旋翼,剖面呈非对称,一旦旋翼旋转,由于上表面空气流速比下表面快,故上表面受到的空气压力小于下表面,上下表面受到的压差形成升力,如图1所示。旋翼1、3逆时针旋转,旋翼2、4顺时针旋转。由叶素动量理论可知,每个旋翼产生的升力Fi与电机转速ωi的平方成正比,即Fi=kFω2i(i=1,2,3,4),其中kF为升力系数。

图1 四旋翼飞行器受力分析图
当旋翼旋转时,空气阻力会阻碍其旋转。这种阻力形成施加在机体上的反扭转力矩,当4个旋翼转速相等时,旋翼产生的反扭矩作用相互抵消。四旋翼飞行器通过改变2对正反螺旋桨的转速实现对其运动控制。在4个旋翼转速相等的情况下,同时增加或者减少4个旋翼转速,可以实现飞行器上升或者下降。如果4个旋翼产生的升力之和等于机体重力,可以实现飞行器空中悬停。保持旋翼2、4的转速不变,改变旋翼1或者旋翼3的转速,飞行器在力矩l(F3-F1)或l(F1-F3)的作用下(其中l为电机轴线到飞行器中心距离),可实现俯仰运动。保持旋翼1和旋翼3的转速不变,改变旋翼2或者旋翼4的转速,飞行器在力矩l(F4-F2)或l(F2-F4)的作用下,可实现横滚运动。如果同时改变旋翼1和旋翼3的转速,或者同时改变旋翼2和旋翼4的转速,并保持飞行器总升力与机体重力相等,飞行器会在反扭矩的作用下实现偏航运动。由此可见,实现飞行器垂直运动的升力,以及实现俯仰、横滚、偏航运动的旋转力矩可以表示为

式中: F——飞行器垂直运动的升力;
Mx、My、Mz——四旋翼俯仰、横滚和偏航运动的旋转力矩;
kF——升力系数;
kM——旋转力矩系数。

1. 2 动力学模型建立
为了描述飞行器的姿态和运动状态,需要引入地理坐标系n(X,Y,Z)与载体坐标系b(x,y,z)。地理坐标系又称为东北天坐标系,载体坐标系与飞行器固连,原点为飞行器中心。将地理坐标系与载体坐标系原点重合,并将地理坐标系分别绕X、Y、Z轴旋转3次之后可得到载体坐标系,地理坐标系到载体坐标系的转换矩阵可表示为


由式(5)和式(2)可求得地理坐标系n中飞行器在X、Y、Z轴方向所受旋翼升力向量:


在低速飞行过程中,角速度矢量较小,式(8)中左边第二项可近似认为为零,则式(8)可化简为

将式(10)转换可得:

由式(7)和式(11)可得四旋翼飞行器在低速飞行情况下的非线性动力学模型:

由式(12)可知,四旋翼飞行器线运动不影响角运动,但是角运动会影响线运动。以u1、u2、u3、u4为系统输入,通过改变这4个输入变量的值,可以改变飞行器的3个线位移和3个角位移,从而实现对飞行器的运动控制。

2 四旋翼飞行器的控制系统构建与仿真
经典PID算法结构简单,基于偏差设计反馈律,不依赖受控对象的具体数学模型,在很多过程控制中均有良好表现。尽管各种新的控制算法不断涌现,但是并没有改变PID控制算法在工业控制中的主导地位。本文根据四旋翼在飞行过程中经常会遇到不确定外界干扰等情况,设计了基于小扰动的PID控制器,如图2所示。

图2 PID控制器结构图

二、部分源代码

clear all

demo_0 = 1;
rad = pi/180;
deg = 180/pi;

pos = [30*pi/180, 120*pi/180, 200]';
vn = [10, 10, 10];


att0 = [10, 10, 10]'*rad;
v0 = [10, 20, 30]';
% ans = euler2dcm(att0)

b = fir1(20, 0.01, 'low');
b = b/sum(b);
% x = repmat([att0; vby]', length(b), 1);

% rv = [10, 2, 1]'*pi/180;
% sqrt(rv'*rv)

wm = [1, 1, 1
      2, 2, 2
      3, 3, 3
      4, 4, 4
      5, 5, 5];
%   
% wmm = wm(1:2, :)
% wm1 = wm(2:3, :)
% cross(wmm,wm1, 2)
% cs = [ [   2,    0,    0,    0,     0]/3
%        [   9,   27,    0,    0,     0]/20
%        [  54,   92,  214,    0,     0]/105
%        [ 250,  525,  650, 1375,     0]/504
%        [2315, 4558, 7296, 7834, 15797]/4620 ];
%    
%     cs(3, 1:3)*wm(1:3, :)
%     wm(1:3, :)

% eth = earth(pos, vn)
% skew(v0)


% u = [1, 2, 3]';
% u = u/norm(u);
% theta = 0.8*pi;
% 
% phi1 = theta;
% phi2 = -(2*pi - theta) ;
% 
% PHI1 = phi1*u;
% PHI2 = phi2*u;
% % norm(PHI)
% qq1 = rv2q(PHI1)'
% qq2 = rv2q(PHI2)'
% 
% PHI1'*PHI1
% rk = [[0.1; 0.1; 0.1]; [10; 10; 10]]';
% Rk = diag(rk)^2;

% R =1 + 0.1.*randn(10000, 1);
% mean(R)
% var(R)
% 
% clearvars -except R
% imu_err = imuerror();
name = 'selfdefine';
exist('name', 'var') && ~strcmp(name, '') && ~strcmp(name, 'zero')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 名称：Opitiaml Based Alignment (GPS velocity aided) version 1.0
% 功能：
%
% 缩写的含义：
% msr: measurement; 
% ref: reference; 特指真实值
% sv: save; 
% prv: previous; 对应变量在上次解算后的值或在本次解算开始时刻的值
% opt: optimal

% Author: Kun Gan, Tongji University
% Email : ciaotigre@126.com
% Date  : 2020/12/1
%%
% 全局变量
close all
gvar_earth;

% ** 下载数据 **
% 数据包含 
% 1. trajectory_ref.(pos, vn, att) 
% 2. imu.ref.(acc, gyr)
% 轨迹仿真程序生成的数据，trajectory的长度会比imu长1。trajectory首个数据为
% t0时刻的avp_ref, imu首个数据为理想imu在t1时刻输出的(t0, t1]内角度和速
% 度增量。
load('trj_and_imu.mat');

% ** 与测量数据相关的量 **
% imu输出频率 (Hz)
f_imu = 100;
ts_imu = 1/f_imu;
% gps输出频率 (Hz)
f_gps = f_imu;
ts_gps = 1/f_gps;
% 总数据长度 (个)
num_imu_data = size(imu_ref.acc, 1);

% *** 给imu数据添加误差 ***
imu_err = imuerrorset('selfdefine');
% add imu error
[imu_msr.gyr, imu_msr.acc] = imuadderr(imu_ref.gyr, imu_ref.acc, ...
                imu_err.eb, imu_err.web, imu_err.db, imu_err.wdb, ts_imu);

% ** 设置全局变量 **
% 第一个"读入"的数据所对应的时刻以及在imu_ref和trajectory_ref中的位置 (s)
% 可以认为导航系统是在第start_time秒启动的
start_time = 0;
first_data_index = round(start_time/ts_imu) + 1;
% 对准时间长度 (s)
total_alignment_time = 92;

% 更新算法子样数 (个)
num_subsample = 2;
nts = num_subsample*ts_imu;

% 初始位置、速度、姿态
pos0 = trajectory_ref.pos(first_data_index, :)';
vn0 = trajectory_ref.vn(first_data_index, :)';
att0 = trajectory_ref.att(first_data_index, :)';

att0_ref = att0;
Cbn0_ref = a2mat(att0);
q0_ref = a2qua(att0);

% *** 一些变量在上一时刻的值 ***
% 速度、位置、姿态 
% 目前还没有准备对初始时刻值添加误差
pos_prv = pos0;
vn_prv = vn0;
att_prv = att0;

% eth保存惯导解算需要的变量，例如wien, winn等。
eth_prv = earth(pos_prv, vn_prv);

% bt系和nt系相对惯性空间变化量
Cntn0_prv = eye(3);
Cbtb0_prv = eye(3);
qbtb0 = [1, 0, 0, 0]';


% 分配动态变量存储空间
phi_sv = zeros(round(total_alignment_time/ts_imu), 3);
phi0_sv = zeros(round(total_alignment_time/ts_imu), 3);

% 计数变量
% 当前时刻 s
current = 0;
% 当前循环时第i次循环
i = 0;
% index
k = 0;
%%
% 我们的终极目标是让算法能够在实际中运行，实际中情况大概是这样的：
% 1. t=0，惯导开机
% 2. t=ts_imu，imu在此时输出第一组速度增量vm(1)和角增量wm(1)
% 3. t=nts,imu输出凑足了执行一次惯导更新所需要的子样数，导航计算机立即执行
%    惯导更新程序并输出导航结果p(t),vn(t),att(t)
% 4. 导航计算机继续等待imu测得n组测量信息再进行惯导更新
while current < total_alignment_time  
    % 每过nts秒，imu就会测量出num_subsample组采样结果。获得足够imu输出后立
    % 即在current时刻（亦称：当前时刻）进行导航更新
    current = current + nts;
    

    
    % current时刻，载体的位置、速度、姿态参考值
    pos_ref = trajectory_ref.pos(k, :)';
    vn_ref = trajectory_ref.vn(k, :)';
    att_ref = trajectory_ref.att(k, :)';
    qbn_ref = a2qua(att_ref);
    
    % 用参考速度模拟当前时刻GPS速度,其中位置信息暂时不加误差
    vn_gps = vn_ref + 0.*randn(3, 1);
    pos_gps = pos_ref;
    
    vn_gps_sv(i, :) = vn_gps;
    % 用gps输出的速度作为组合导航系统给出的速度
    vn = vn_gps;
    pos = pos_gps;
    
    % 从imu_ref中读出(current - nts, current]这段时间内imu输出的n组量测信息
    wm = imu_msr.gyr(k-num_subsample+1 : k, :);
    vm = imu_msr.acc(k-num_subsample+1 : k, :);
    
    wm_sv(2*i-1:2*i, :) = wm;
    vm_sv(2*i-1:2*i, :) = vm;
    % 步长圆锥/划桨误差
    [phim, dvbm] = cnscl(wm, vm);

    
    % *** 计算alpha(n0)和beta(b0) ***
    % alpha(n0)
    % 1. 目前用vn0_ref计算alpha, 暂时不考虑滑动窗口。
    % 2. 用gps输出的位置计算wien和gn
    alpha_sigma = alpha_sigma + ... 
                  Cntn0_prv*(cross(eth_prv.wien, vn_prv) - eth_prv.gn)*nts;
    alpha = Cntn0*vn - vn0 + alpha_sigma;
    % beta(b0)
    beta = beta_prv + Cbtb0_prv*dvbm;
    
    % QUEST 方法计算qbn0
    [ qbn0, K ] = QUEST( beta, alpha, K_prv );
    
    % 姿态解算
    Cbn0 = q2mat(qbn0);
    Cbn = Cntn0'*Cbn0*Cbtb0;
    att = m2att(Cbn);
    phi0_sv(i, :) = atterrnorml(q2att(qbn0) - att0_ref)*deg;
    phi_sv(i, :) = atterrnorml(att - att_ref)*deg;
    
    % 计算导航解算时所需要的相关参数
    eth = earth(pos, vn);
    
    % 将本次更新后的导航参数作为下次更新初值
    pos_prv = pos;
    vn_prv = vn;
    att_prv = att;
    eth_prv = eth;
    Cntn0_prv = Cntn0;
    Cbtb0_prv = Cbtb0;
    alpha_prv = alpha;
    beta_prv = beta;
    K_prv = K;
    
end

% 注意：如果end前面加了空格就会出错!
phi_sv(i+1:end, :) = [];
phi0_sv(i+1:end, :) = [];
%% 绘图
time_axis = (1:1:i)*nts;
time_axis_imu = (1:1:2*i)*ts_imu;

% Cbn误差
msplot(311, time_axis, phi_sv(:, 1), 'pitch error / \circ');
msplot(312, time_axis, phi_sv(:, 2), 'roll error / \circ');
msplot(313, time_axis, phi_sv(:, 3), 'yaw error / \circ');

%Cbn0 误差
msplot(311, time_axis, phi0_sv(:, 1), 'pitch error / \circ');
msplot(312, time_axis, phi0_sv(:, 2), 'roll error / \circ');
msplot(313, time_axis, phi0_sv(:, 3), 'yaw error / \circ');





  

 

  
1
  
2
  
3
  
4
  
5
  
6
  
7
  
8
  
9
  
10
  
11
  
12
  
13
  
14
  
15
  
16
  
17
  
18
  
19
  
20
  
21
  
22
  
23
  
24
  
25
  
26
  
27
  
28
  
29
  
30
  
31
  
32
  
33
  
34
  
35
  
36
  
37
  
38
  
39
  
40
  
41
  
42
  
43
  
44
  
45
  
46
  
47
  
48
  
49
  
50
  
51
  
52
  
53
  
54
  
55
  
56
  
57
  
58
  
59
  
60
  
61
  
62
  
63
  
64
  
65
  
66
  
67
  
68
  
69
  
70
  
71
  
72
  
73
  
74
  
75
  
76
  
77
  
78
  
79
  
80
  
81
  
82
  
83
  
84
  
85
  
86
  
87
  
88
  
89
  
90
  
91
  
92
  
93
  
94
  
95
  
96
  
97
  
98
  
99
  
100
  
101
  
102
  
103
  
104
  
105
  
106
  
107
  
108
  
109
  
110
  
111
  
112
  
113
  
114
  
115
  
116
  
117
  
118
  
119
  
120
  
121
  
122
  
123
  
124
  
125
  
126
  
127
  
128
  
129
  
130
  
131
  
132
  
133
  
134
  
135
  
136
  
137
  
138
  
139
  
140
  
141
  
142
  
143
  
144
  
145
  
146
  
147
  
148
  
149
  
150
  
151
  
152
  
153
  
154
  
155
  
156
  
157
  
158
  
159
  
160
  
161
  
162
  
163
  
164
  
165
  
166
  
167
  
168
  
169
  
170
  
171
  
172
  
173
  
174
  
175
  
176
  
177
  
178
  
179
  
180
  
181
  
182
  
183
  
184
  
185
  
186
  
187
  
188
  
189
  
190
  
191
  
192
  
193
  
194
  
195
  
196
  
197
  
198
  
199
  
200
  
201
  
202
  
203
  
204
  
205
  
206
  
207
  
208
  
209
  
210
  
211
  
212
  
213
  
214
  
215
  
216
  
217
  
218
  
219
  
220
  
221
  
222
  
223
  
224
  
225
  
226
  
227
  
228
  
229
  
230
  
231
  
232
  
233
  
234
  
235
  
236
  
237
  
238
  
239
  
240
  
241
  
242
  
243
  
244
  
245
  
246
  
247
  
248
  
249
  
250
  
251
  
252
  
253
  
254
  
255
  
256
 

三、运行结果

四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]张萍.四旋翼飞行器姿态控制建模与仿真[J].电机与控制应用. 2019,46(12)
[4]刘岩,杨牧.四旋翼飞行器飞行控制系统研究与设计[J].山东工业技术. 2019,(07)

文章来源: qq912100926.blog.csdn.net，作者：海神之光，版权归原作者所有，如需转载，请联系作者。

原文链接：qq912100926.blog.csdn.net/article/details/119952515