文章目录
- 5.10 飞行控制——自稳飞行
- 5.10.1 数学模型——三轴角度系统
- （1）三轴角度系统微分方程
- （2）状态空间方程的建立
- 5.10.2 A1软件设计
- 5.10.3 A1运行与调试
- 5.10.4 三轴角度串级PID控制器
- 5.10.5 A2软件设计
- 5.10.6 A2运行与调试
总目录：http://t.csdnimg.cn/YDe8m
5.10 飞行控制——自稳飞行
四旋翼无人机机通常具有三种基本的飞行模式，即自稳飞行、定高飞行、定点飞行。在自稳飞行模式下，可人为控制无人机三个姿态角的大小，并合其在无外部命令情况下三个姿态角都为零。
为了在Matlab上实现自稳飞行的仿真，首先，需要建立起四旋翼三轴角度系统状态空间方程形式的数据模式；其次，则需要设计一种可以同时控制四旋翼三轴角度与角速度的串级PID控制器。
5.10.1 数学模型——三轴角度系统
本小节第一个例程的功能为：建立四旋翼三轴角度系统状态空间方程形式的数学模型，并仿真其对初始状态的响应。其具体的原理如下。
（1）三轴角度系统微分方程
已知四旋翼x轴角度系统的数学模型，即：
I
x
A
x
¨
(
t
)
+
K
1
A
x
˙
(
t
)
=
M
x
(
5.10.1.
a
)
I_x\ddot{A_x}(t) + K_1 \dot{A_x}(t) = M_x \qquad (5.10.1.a)
IxAx¨(t)+K1Ax˙(t)=Mx(5.10.1.a)
y轴与z轴角度系统的数学模型与之类似，即：
I
y
A
y
¨
(
t
)
+
K
2
A
y
˙
(
t
)
=
M
y
(
5.10.1.
b
)
I
z
A
z
¨
(
t
)
+
K
3
A
z
˙
(
t
)
=
M
z
(
5.10.1.
c
)
\begin{aligned} I_y\ddot{A_y}(t) + K_2 \dot{A_y}(t) = M_y \qquad (5.10.1.b) \\ I_z\ddot{A_z}(t) + K_3 \dot{A_z}(t) = M_z \qquad (5.10.1.c) \end{aligned}
IyAy¨(t)+K2Ay˙(t)=My(5.10.1.b)IzAz¨(t)+K3Az˙(t)=Mz(5.10.1.c)
其中：
I
y
、
I
z
I_y、I_z
Iy、Iz——机体坐标系下，四旋翼对y轴、z轴的转动惯量。
A
y
(
t
)
、
A
z
(
t
)
A_y(t)、A_z(t)
Ay(t)、Az(t)——y、z轴对应姿态角，也即俯仰角（pit）、横滚角（rol）。
K
2
、
K
3
K_2、K_3
K2、K3——y、z轴旋阻系数，与空气阻力相关。
M
y
、
M
z
M_y、M_z
My、Mz——电机产生的y、z轴力矩。
三式合在一直，便是四旋翼三轴角度系统微分方程形式的数学模型。
（2）状态空间方程的建立
1）设计输入量、状态变量、输出量
设计系统输入为：
u
1
(
t
)
=
M
x
(
t
)
(
5.10.2.
a
)
u
2
(
t
)
=
M
y
(
t
)
(
5.10.2.
b
)
u
3
(
t
)
=
M
z
(
t
)
(
5.10.2.
c
)
\begin{aligned} &u_1(t) = M_x(t) \qquad & (5.10.2.a) \\ &u_2(t) = M_y(t) \qquad & (5.10.2.b) \\ &u_3(t) = M_z(t) \qquad & (5.10.2.c) \\ \end{aligned}
u1(t)=Mx(t)u2(t)=My(t)u3(t)=Mz(t)(5.10.2.a)(5.10.2.b)(5.10.2.c)
设计状态变量：
x
1
(
t
)
=
A
x
(
t
)
(
5.10.2.
d
)
x
2
(
t
)
=
W
x
(
t
)
(
5.10.2.
e
)
x
3
(
t
)
=
A
y
(
t
)
(
5.10.2.
f
)
x
4
(
t
)
=
W
y
(
t
)
(
5.10.2.
g
)
x
5
(
t
)
=
A
z
(
t
)
(
5.10.2.
h
)
x
6
(
t
)
=
W
z
(
t
)
(
5.10.2.
i
)
\begin{aligned} &x_1(t) = A_x(t) \qquad &(5.10.2.d) \\ &x_2(t) = W_x(t) \qquad &(5.10.2.e) \\ &x_3(t) = A_y(t) \qquad &(5.10.2.f) \\ &x_4(t) = W_y(t) \qquad &(5.10.2.g) \\ &x_5(t) = A_z(t) \qquad &(5.10.2.h) \\ &x_6(t) = W_z(t) \qquad &(5.10.2.i) \\ \end{aligned}
x1(t)=Ax(t)x2(t)=Wx(t)x3(t)=Ay(t)x4(t)=Wy(t)x5(t)=Az(t)x6(t)=Wz(t)(5.10.2.d)(5.10.2.e)(5.10.2.f)(5.10.2.g)(5.10.2.h)(5.10.2.i)
设计输出量：
y
1
(
t
)
=
x
1
(
t
)
(
5.10.2.
j
)
y
2
(
t
)
=
x
2
(
t
)
(
5.10.2.
k
)
y
3
(
t
)
=
x
3
(
t
)
(
5.10.2.
l
)
y
4
(
t
)
=
x
4
(
t
)
(
5.10.2.
m
)
y
5
(
t
)
=
x
5
(
t
)
(
5.10.2.
n
)
y
6
(
t
)
=
x
6
(
t
)
(
5.10.2.
o
)
\begin{aligned} &y_1(t) = x_1(t) \qquad &(5.10.2.j) \\ &y_2(t) = x_2(t) \qquad &(5.10.2.k) \\ &y_3(t) = x_3(t) \qquad &(5.10.2.l) \\ &y_4(t) = x_4(t) \qquad &(5.10.2.m) \\ &y_5(t) = x_5(t) \qquad &(5.10.2.n) \\ &y_6(t) = x_6(t) \qquad &(5.10.2.o) \\ \end{aligned}
y1(t)=x1(t)y2(t)=x2(t)y3(t)=x3(t)y4(t)=x4(t)y5(t)=x5(t)y6(t)=x6(t)(5.10.2.j)(5.10.2.k)(5.10.2.l)(5.10.2.m)(5.10.2.n)(5.10.2.o)
2）列写一阶微分方程
式（5.10.1）可以拆分成一系列的一阶微分方程：
d
A
x
(
t
)
d
t
=
W
x
(
t
)
(
5.10.3.
a
)
d
W
x
(
t
)
d
t
=
−
K
1
I
x
W
x
(
t
)
+
1
I
x
u
1
(
t
)
(
5.10.3.
b
)
d
A
y
(
t
)
d
t
=
W
y
(
t
)
(
5.10.3.
e
)
d
W
y
(
t
)
d
t
=
−
K
2
I
y
W
y
(
t
)
+
1
I
y
u
1
(
t
)
(
5.10.3.
f
)
d
A
z
(
t
)
d
t
=
W
z
(
t
)
(
5.10.3.
g
)
d
W
z
(
t
)
d
t
=
−
K
3
I
z
W
z
(
t
)
+
1
I
z
u
1
(
t
)
(
5.10.3.
h
)
\begin{aligned} &\frac{\mathrm{d}A_x(t)}{\mathrm{d}t} = W_x(t) \qquad &(5.10.3.a) \\ & \frac{\mathrm{d}W_x(t)}{\mathrm{d}t} = - \frac{K_1}{I_x}W_x(t) + \frac{\mathrm{1}}{I_x} u_1(t) \qquad &(5.10.3.b) \\ &\frac{\mathrm{d}A_y(t)}{\mathrm{d}t} = W_y(t) \qquad &(5.10.3.e) \\ & \frac{\mathrm{d}W_y(t)}{\mathrm{d}t} = - \frac{K_2}{I_y}W_y(t) + \frac{\mathrm{1}}{I_y} u_1(t) \qquad &(5.10.3.f) \\ &\frac{\mathrm{d}A_z(t)}{\mathrm{d}t} = W_z(t) \qquad &(5.10.3.g) \\ & \frac{\mathrm{d}W_z(t)}{\mathrm{d}t} = - \frac{K_3}{I_z}W_z(t) + \frac{\mathrm{1}}{I_z} u_1(t) \qquad &(5.10.3.h) \\ \end{aligned}
dtdAx(t)=Wx(t)dtdWx(t)=−IxK1Wx(t)+Ix1u1(t)dtdAy(t)=Wy(t)dtdWy(t)=−IyK2Wy(t)+Iy1u1(t)dtdAz(t)=Wz(t)dtdWz(t)=−IzK3Wz(t)+Iz1u1(t)(5.10.3.a)(5.10.3.b)(5.10.3.e)(5.10.3.f)(5.10.3.g)(5.10.3.h)
3）整理成标准形式
将式（5.10.2）代入式（5.10.3）得：
d
x
1
(
t
)
d
t
=
x
2
(
t
)
(
5.10.4.
a
)
d
x
2
(
t
)
d
t
=
−
K
1
I
x
x
2
(
t
)
+
1
I
x
u
1
(
t
)
(
5.10.4.
b
)
d
x
3
(
t
)
d
t
=
x
4
(
t
)
(
5.10.4.
e
)
d
x
4
(
t
)
d
t
=
−
K
2
I
y
x
4
(
t
)
+
1
I
y
u
1
(
t
)
(
5.10.4.
f
)
d
x
5
(
t
)
d
t
=
x
6
(
t
)
(
5.10.4.
g
)
d
x
6
(
t
)
d
t
=
−
K
3
I
z
x
6
(
t
)
+
1
I
z
u
1
(
t
)
(
5.10.4.
h
)
\begin{aligned} &\frac{\mathrm{d}x_1(t)}{\mathrm{d}t} = x_2(t) \qquad &(5.10.4.a) \\ & \frac{\mathrm{d}x_2(t)}{\mathrm{d}t} = - \frac{K_1}{I_x}x_2(t) + \frac{\mathrm{1}}{I_x} u_1(t) \qquad &(5.10.4.b) \\ &\frac{\mathrm{d}x_3(t)}{\mathrm{d}t} = x_4(t) \qquad &(5.10.4.e) \\ & \frac{\mathrm{d}x_4(t)}{\mathrm{d}t} = - \frac{K_2}{I_y}x_4(t) + \frac{\mathrm{1}}{I_y} u_1(t) \qquad &(5.10.4.f) \\ &\frac{\mathrm{d}x_5(t)}{\mathrm{d}t} = x_6(t) \qquad &(5.10.4.g) \\ & \frac{\mathrm{d}x_6(t)}{\mathrm{d}t} = - \frac{K_3}{I_z}x_6(t) + \frac{\mathrm{1}}{I_z} u_1(t) \qquad &(5.10.4.h) \\ \end{aligned}
dtdx1(t)=x2(t)dtdx2(t)=−IxK1x2(t)+Ix1u1(t)dtdx3(t)=x4(t)dtdx4(t)=−IyK2x4(t)+Iy1u1(t)dtdx5(t)=x6(t)dtdx6(t)=−IzK3x6(t)+Iz1u1(t)(5.10.4.a)(5.10.4.b)(5.10.4.e)(5.10.4.f)(5.10.4.g)(5.10.4.h)
4）转化为矩阵形式
d d t [ x 1 ( t ) x 2 ( t ) x 3 ( t ) x 4 ( t ) x 5 ( t ) x 6 ( t ) ] = [ 0 1 0 − K 1 I x 0 1 0 − K 2 I y 0 1 0 − K 3 I z ] [ x 1 ( t ) x 2 ( t ) x 3 ( t ) x 4 ( t ) x 5 ( t ) x 6 ( t ) ] + [ 0 1 I x 0 1 I y 0 1 I z ] [ u 1 ( t ) u 2 ( t ) u 3 ( t ) u 4 ( t ) u 5 ( t ) u 6 ( t ) ] ( 5.10.5. a ) [ y 1 ( t ) y 2 ( t ) y 3 ( t ) y 4 ( t ) y 5 ( t ) y 6 ( t ) ] = I 6 × 6 [ x 1 ( t ) x 2 ( t ) x 3 ( t ) x 4 ( t ) x 5 ( t ) x 6 ( t ) ] + O 6 × 3 [ u 1 ( t ) u 2 ( t ) u 3 ( t ) u 4 ( t ) u 5 ( t ) u 6 ( t ) ] ( 5.10.5. b ) \begin{aligned} &\frac{\mathrm{d}}{\mathrm{d}t} \begin{bmatrix} x_1(t) \\ x_2(t) \\ x_3(t) \\ x_4(t) \\ x_5(t) \\ x_6(t) \end{bmatrix} = \begin{bmatrix} 0 &1 \\ 0 &-\frac{K_1}{I_x} \\ & &0 &1 && \\ & &0 &-\frac{K_2}{I_y} \\ &&& &0 &1 \\ &&& &0 &-\frac{K_3}{I_z} \end{bmatrix} \begin{bmatrix} x_1(t)\\ x_2(t)\\ x_3(t)\\ x_4(t)\\ x_5(t)\\ x_6(t) \end{bmatrix} + \begin{bmatrix} 0 & & \\ \frac{\mathrm{1}}{I_x} & & \\ &0 & \\ &\frac{\mathrm{1}}{I_y} & \\ & &0 \\ & &\frac{\mathrm{1}}{I_z} \\ \end{bmatrix} \begin{bmatrix} u_1(t) \\ u_2(t) \\ u_3(t) \\ u_4(t) \\ u_5(t) \\ u_6(t) \end{bmatrix}&(5.10.5.a) \\ &\begin{bmatrix} y_1(t) \\ y_2(t) \\ y_3(t) \\ y_4(t) \\ y_5(t) \\ y_6(t) \end{bmatrix} = \boldsymbol{I}_{6\times 6} \begin{bmatrix} x_1(t)\\ x_2(t)\\ x_3(t)\\ x_4(t)\\ x_5(t)\\ x_6(t) \end{bmatrix} + \boldsymbol{O}_{6\times 3} \begin{bmatrix} u_1(t) \\ u_2(t) \\ u_3(t) \\ u_4(t) \\ u_5(t) \\ u_6(t) \end{bmatrix} &(5.10.5.b) \end{aligned} dtd x1(t)x2(t)x3(t)x4(t)x5(t)x6(t) = 001−IxK1001−IyK2001−IzK3 x1(t)x2(t)x3(t)x4(t)x5(t)x6(t) + 0Ix10Iy10Iz1 u1(t)u2(t)u3(t)u4(t)u5(t)u6(t) y1(t)y2(t)y3(t)y4(t)y5(t)y6(t) =I6×6 x1(t)x2(t)x3(t)x4(t)x5(t)x6(t) +O6×3 u1(t)u2(t)u3(t)u4(t)u5(t)u6(t) (5.10.5.a)(5.10.5.b)
式中，矩阵中空缺的部分为0； I 6 × 6 \boldsymbol{I}_{6\times 6} I6×6为 6 × 6 6\times6 6×6单位矩阵； O 6 × 3 \boldsymbol{O}_{6\times 3} O6×3为 6 × 3 6\times 3 6×3零矩阵。
5）提取矩阵
A = [ 0 1 0 − K 1 I x 0 1 0 − K 2 I y 0 1 0 − K 3 I z ] ( 5.10.6. a ) B = [ 0 1 I x 0 1 I y 0 1 I z ] ( 5.10.6. b ) C = I 6 × 6 ( 5.10.6. c ) D = O 6 × 3 ( 5.10.6. d ) u ( t ) = [ u 1 ( t ) u 2 ( t ) ⋯ u 6 ( t ) ] T ( 5.10.6. e ) x ( t ) = [ x 1 ( t ) x 2 ( t ) ⋯ x 6 ( t ) ] T ( 5.10.6. f ) d x ( t ) d t = A x ( t ) + B u ( t ) ( 5.10.6. g ) y ( t ) = C x ( t ) + D u ( t ) ( 5.10.6. h ) \begin{aligned} &\boldsymbol{A} = \begin{bmatrix} 0 &1 \\ 0 &-\frac{K_1}{I_x} \\ & &0 &1 && \\ & &0 &-\frac{K_2}{I_y} \\ &&& &0 &1 \\ &&& &0 &-\frac{K_3}{I_z} \end{bmatrix} &(5.10.6.a) \\ & \boldsymbol{B} = \begin{bmatrix} 0 & & \\ \frac{\mathrm{1}}{I_x} & & \\ &0 & \\ &\frac{\mathrm{1}}{I_y} & \\ & &0 \\ & &\frac{\mathrm{1}}{I_z} \\ \end{bmatrix} &(5.10.6.b) \\ & \boldsymbol{C} = \boldsymbol{I}_{6\times 6} & (5.10.6.c) \\ & \boldsymbol{D} = \boldsymbol{O}_{6\times 3} & (5.10.6.d) \\ &\boldsymbol{u}(t) = \begin{bmatrix} u_1(t) &u_2(t) &\cdots & u_6(t) \end{bmatrix}^{\mathrm{T}} & (5.10.6.e) \\ &\boldsymbol{x}(t) = \begin{bmatrix} x_1(t) & x_2(t) &\cdots &x_6(t) \end{bmatrix}^{\mathrm{T}} & (5.10.6.f) \\ &\frac{\mathrm{d}\boldsymbol{x}(t)}{\mathrm{d}t} = \boldsymbol{A}\boldsymbol{x}(t) + \boldsymbol{B}\boldsymbol{u}(t) & (5.10.6.g) \\ & \boldsymbol{y}(t) = \boldsymbol{C}\boldsymbol{x}(t) + \boldsymbol{D}\boldsymbol{u}(t) &(5.10.6.h) \end{aligned} A= 001−IxK1001−IyK2001−IzK3 B= 0Ix10Iy10Iz1 C=I6×6D=O6×3u(t)=[u1(t)u2(t)⋯u6(t)]Tx(t)=[x1(t)x2(t)⋯x6(t)]Tdtdx(t)=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t)(5.10.6.a)(5.10.6.b)(5.10.6.c)(5.10.6.d)(5.10.6.e)(5.10.6.f)(5.10.6.g)(5.10.6.h)
5.10.2 A1软件设计
本小节例程位于“2、飞控例程\Matlab\（3）飞行控制——自稳飞行\A1_三轴角度模型”中。
例程功能为：建立四旋翼三轴角度系统状态空间方程形式的数学模型，并仿真其对初始状态的响应。
例程中的文件如图5.10.1所示，主要修改了“M_Angle.m”、“P_plot_all.m”、“输入_系统参数.xlsx”等文件。
图5.10.1
M_Angle.m 文件
修改了初始化部分，运行部分不变。
%% 四旋转子 角度系统 动态模型
% 输入：
% s_A.u 四旋转子 角度系统 输入
%
% 输出
% y 四旋转子 角度系统 输出
%%
if init == 0 %未初始化
%四旋转子 转速系统 系统参数
s_A.I = data_i(9, 4:6); %转动惯量
s_A.K = data_i(11, 4:6); %旋阻系数
%过程噪声方差、数学期望期望
s_A.Q_d = [0 0 0 0 0 0]; %数学期望期望为0
s_A.Q_c = data_i(13, 4:9); %过程噪声方差 角度、角速度
s_A.Q_c = s_A.Q_c*pi/180; %转化单位为弧度
% 定义系统状态与输出
s_A.S_n = 6; %状态变量数
s_A.S_m = 6; %输出变量数
s_A.S_p = 3; %输入变量数
s_A.x_last = zeros(s_A.S_n, 1); %上次状态变量
s_A.x = zeros(s_A.S_n, 1); %当前状态变量
s_A.x_last(:, 1) = [4; 3.5; 3;
-2; 2; 1]; %初始状态 角度、角速度
s_A.x_last = s_A.x_last.*pi/180; %转化单位为弧度
s_A.y = zeros(s_A.S_n, 1); %系数输出
% 定义状态空间方程
s_A.A = zeros(s_A.S_n, s_A.S_n);
s_A.B = zeros(s_A.S_n, s_A.S_p);
s_A.C = eye(s_A.S_n); %生成一个 S_n 阶的单位矩阵
s_A.D = zeros(s_A.S_m, s_A.S_p);
s_A.A(1:2, 1:2) = [0, 1; 0, -s_A.K(1)/s_A.I(1)];
s_A.A(3:4, 3:4) = [0, 1; 0, -s_A.K(2)/s_A.I(2)];
s_A.A(5:6, 5:6) = [0, 1; 0, -s_A.K(3)/s_A.I(3)];
s_A.B(2, 1) = 1/s_A.I(1);
s_A.B(4, 2) = 1/s_A.I(2);
s_A.B(6, 3) = 1/s_A.I(3);
s_A.sys_d = c2d(ss(s_A.A, s_A.B, s_A.C, s_A.D),Ts);
% 提取离散系统A矩阵
s_A.A = s_A.sys_d.a;
% 提取离散系统B矩阵
s_A.B = s_A.sys_d.b;
% 提取离散系统B矩阵
s_A.C = s_A.sys_d.c;
% 提取离散系统B矩阵
s_A.D = s_A.sys_d.d;
else %计算系统响应
......
end
- 行12~13
从Excel数组中获取三轴转动惯量、旋阻系数等参数。 - 行20~23
赋值输入、状态变量、输出的元素个数。 - 行39~41
对状态空间方程的A矩阵赋值，对应式（5.10.6.a）。 - 行43~45
对状态空间方程的B矩阵赋值，对应式（5.10.6.b）
P_plot_all.m 文件
%% 绘图文件
......
%% 四旋转子系统输入、输出，
......
% 三轴角度
subplot(3, 1, 2);
plot(t, s_History.ya(:, 1), 'r');
hold on;
plot(t, s_History.ya(:, 3), 'g');
hold on;
plot(t, s_History.ya(:, 5), 'b');
hold on;
legend('A_x', 'A_y', 'A_z');
xlabel('时间(s)');
ylabel('角度（deg）');
grid on;
hold off;
% 三轴角速度
subplot(3, 1, 3);
plot(t, s_History.ya(:, 2), 'r');
hold on;
plot(t, s_History.ya(:, 4), 'g');
hold on;
plot(t, s_History.ya(:, 6), 'b');
hold on;
legend('W_x', 'W_y', 'W_z');
xlabel('时间(s)');
ylabel('角速度（deg/s）');
grid on;
hold off;
-
行13~16
绘制y、z轴角度曲线图。 -
行31~34
绘制y、z轴角速度曲线图。
5.10.3 A1运行与调试
双击进入“A1_main.mlx”文件，点击上方“实时编辑器”中的“运行”按钮即可运行例程。运行结果如图5.10.2所示，例程当前只控制了x轴的角度与角速度，其它状态变量。
（a）
（b）
图 5.10.2
5.10.4 三轴角度串级PID控制器
y轴、z轴的串级PID控制与x轴类似，只是期望、测量值等变为了y、z轴，此处不再赘述。
5.10.5 A2软件设计
本小节例程位于“2、飞控例程\Matlab\（3）飞行控制——自稳飞行\A2_三轴串级PID控制角度”中。
例程功能为：使用3组（共6环）的串级PID对四旋翼三轴的角度、角速度进行控制。
例程中的文件如图5.10.3所示，主要修改了“C_Cascade_PID.m”文件。
图5.10.3
C_Cascade_PID.m 文件
%% 四旋翼 串级PID 控制器
% 输入：
% s_A.y 角度系统的输出
%
% 输出
% s_mot.u_all 四个电机的功率
%%
if init == 0 %未初始化
% 变量默认顺序，Ax、Wx、Ay、Wy、Az、Wz
s_pid.d = data_i(26, 4:9); %串级PID期望，三轴角度、角速度，单位：deg、deg/s
s_pid.d = s_pid.d *pi/180; %转化单位为弧度
s_pid.K_pid = data_i(14:19, 4:6); %PID参数
s_pid.e_last = zeros(6, 1);
s_pid.Ei = zeros(6, 1);
s_pid.y = zeros(6, 1);
% PID输出转电机功率 矩阵
s_pid.Out_M = [ -1 -1 1 1;
-1 1 -1 1;
1 -1 -1 1;
1 1 1 1;]; % PID输出转电机功率 矩阵
else %进行控制
k = 1;
for PID_index = 1:3
% % 角度环
% Sys_e(k) = Expect(PID_index) - y(k, 1); %角度 期望与测量误差
% [Sys_u(k), Sys_Ei(k)] = C_PID(Ts, K_pid(k, :), Sys_e(k), Sys_e_last(k), Sys_Ei(k));
% Sys_e_last(k) = Sys_e(k); %保存本次误差
% 角速度环
s_pid.e(k) = s_pid.d(k) - s_A.y(k, 1); %角度 期望与测量误差
[s_pid.y(k), s_pid.Ei(k)] = C_PID(Ts, s_pid.K_pid(k, :), s_pid.e(k), s_pid.e_last(k), s_pid.Ei(k));
s_pid.e_last(k) = s_pid.e(k); %保存本次误差
k = k+1;
% % 角速度环
% Sys_e(k) = Sys_u(k-1) - y(k, 1); %角速度 角度环输入与测量误 之差
% [Sys_u(k), Sys_Ei(k)] = C_PID(Ts, K_pid(k, :), Sys_e(k), Sys_e_last(k), Sys_Ei(k));
% Sys_e_last(k) = Sys_e(k); %保存本次误差
% 角速度环
s_pid.e(k) = ( s_pid.d(k) + s_pid.y(k-1) ) - s_A.y(k, 1); %角速度 期望与测量误差
[s_pid.y(k), s_pid.Ei(k)] = C_PID(Ts, s_pid.K_pid(k, :), s_pid.e(k), s_pid.e_last(k), s_pid.Ei(k));
s_pid.e_last(k) = s_pid.e(k); %保存本次误差
k = k+1;
end
% % 角速度环
% s_pid.e(1) = s_pid.d(1) - s_A.y(3, 1); %角度 期望与测量误差
% [s_pid.y(1), s_pid.Ei(1)] = C_PID(Ts, s_pid.K_pid(1, :), s_pid.e(1), s_pid.e_last(1), s_pid.Ei(1));
% s_pid.e_last(1) = s_pid.e(1); %保存本次误差
%
% % 角速度环
% s_pid.e(2) = ( s_pid.d(2) + s_pid.y(1) ) - s_A.y(4, 1); %角速度 期望与测量误差
% [s_pid.y(2), s_pid.Ei(2)] = C_PID(Ts, s_pid.K_pid(2, :), s_pid.e(2), s_pid.e_last(2), s_pid.Ei(2));
% s_pid.e_last(2) = s_pid.e(2); %保存本次误差
% PID输出转化为各电机功率
s_pid.PID_out = [s_pid.y(2) s_pid.y(4) s_pid.y(6) 500]'; %x、y、z三轴角速度环输出、z轴速度环输出
s_mot.u_all = s_pid.Out_M*s_pid.PID_out; %PID输出 转 电机控制功率
% 电机功率限幅
s_mot.u_all = min(s_mot.u_all, s_mot.u_max);
s_mot.u_all = max(s_mot.u_all, 0);
end
-
行12~14
读取角度、角速度控制期望值与各环PID参数。 -
行28~52
使用for循环，运行三轴角度环、角速度环PID。与单轴的串级PID类似。
5.10.6 A2运行与调试
双击进入“A1_main.mlx”文件，点击上方“实时编辑器”中的“运行”按钮即可运行例程。运行结果如图5.10.4所示，可以发现经过5秒后，三轴角度、角速度都接近了控制期望值。
（a）
（b）
图 5.10.4
![World of Warcraft [CLASSIC][80][Grandel] Equipment Recovery](https://i-blog.csdnimg.cn/direct/0b3e83c564f64ad0a441a8246e1c59be.jpeg)
