文章目录
- 5.11 飞行控制——定点飞行
- 5.11.1 加入三轴位置的飞行硬件系统 FLY(s)
- 5.11.2 数学模型——三轴位置系统
- （1）x、y轴位置系统的微分方程
- （2）z轴位置系统的微分方程
- （3）三轴位置系统的状态空间方程
- 5.11.2 A1软件设计
- 5.11.3 A1运行与调试
- 5.11.4 三轴位置串级PID控制器
- 5.11.5 A2软件设计
- 5.11.6 A2运行与调试
- 5.11.7 期望误差限伏
- 5.11.8 A3软件设计
- 5.11.9 A3运行与调试
总目录：http://t.csdnimg.cn/YDe8m
5.11 飞行控制——定点飞行
实现定点飞行基本上也就对无人机所有的运动状态进行地了控制，定高飞行相当于半定点飞行。
5.11.1 加入三轴位置的飞行硬件系统 FLY(s)
四旋翼硬件系统可以大体分为三轴角度系统、三轴位置系统、电机系统。根据状态空间方程的性质，我们自然地想到角度、位置系统可以列写为一个状态空间方程，该状态空间方程具有12个状态变量与输出，分别为三轴角速度、角度、线速度、位置；具有4个输入，分别为三轴力矩、电机合力。
然而实际建立时会发生困难，在位置系统部分，有些项与角度、电机合力同时相关，系统将是非定常的。非线性系统可以在某点近似线性化，但限于本人水平，非定常系统的状态空间方程表示，作者一直没找到方法，闻道在先的同志如果了解的话，欢迎联系与指正。
对于上述非定常系统表达的问题，作者想到了这样一种方法：利用之前求解得到的电机系统，将飞行硬件系统 FLY(s)划分为电机系统、角度系统、位置系统，这样三个子系统，其运行原理如图5.11.1所示：
图 5.11.1 FLY(s)飞行硬件系统
1）飞行硬件系统 FLY(s)，其总输入为 u 1 ∼ u 4 u_1 \sim u_4 u1∼u4 四个电机功率，这也是控制器的输出。
2）电机系统的输入分为两部分，一部分是四个电机的功率
u
1
∼
u
4
u_1 \sim u_4
u1∼u4，一部分则是四旋翼当前的角度状态
A
x
∼
A
z
A_x \sim A_z
Ax∼Az 。
其输出也分为两部分，其一为三轴力矩
M
x
∼
M
z
M_x \sim M_z
Mx∼Mz，其二为东北天坐标系下各轴的分力
F
x
∼
F
z
F_x \sim F_z
Fx∼Fz（可由北东地坐标系下分力进行转换），其中第二部分的求解综合使用了
u
1
∼
u
4
u_1\sim u_4
u1∼u4、
A
x
∼
A
z
A_x \sim Az
Ax∼Az，涉及到之前所讲的旋转矩阵。
3）在末端，角度系统的输入为三轴力矩
M
x
∼
M
y
M_x \sim M_y
Mx∼My，输出为三轴角度、角速度
A
x
∼
A
z
A_x \sim A_z
Ax∼Az、
W
x
∼
W
z
W_x\sim W_z
Wx∼Wz 。其中角度
A
x
∼
A
z
A_x \sim A_z
Ax∼Az 又被电机系统M(s) 所使用。
4）在末端，位置系统的输入有两部分，其一为东北天坐标系下各轴的分力
F
x
∼
F
z
F_x\sim F_z
Fx∼Fz， 其二为重力加速度g，至于为什么有个g，之后会具体讲解。位置系统输出为：三轴位置、线速度，即
P
x
∼
P
z
、
W
x
∼
W
z
P_x\sim P_z、W_x\sim W_z
Px∼Pz、Wx∼Wz，注意，其方向都是取自东北天坐标系的。
5.11.2 数学模型——三轴位置系统
（1）x、y轴位置系统的微分方程
对于四旋翼在东北天坐标系x轴上位置系统的微分方程，按照5.4.1 小节的步骤进行建立：
1）确定系统输入、输出
结合图5.11.1 与控制目的，设置x轴位置系统的输入为
F
x
F_x
Fx，即电机合力在x轴上的分力。由于优先控制的状态为四旋翼在x轴上的位置
P
x
P_x
Px， 设置输出为
P
x
P_x
Px。
2）细分系统环节
在x轴位置系统内，作用有电机合力分力
F
x
F_x
Fx、空气阻力
F
D
x
F_{Dx}
FDx、惯性力
F
m
x
F_{mx}
Fmx，三者之合为零：
F
x
+
F
D
x
+
F
m
x
=
0
(
5.11.1
)
F_x + F_{Dx}+F_{mx} = 0 \qquad (5.11.1)
Fx+FDx+Fmx=0(5.11.1)
3）各一节微分方程
可知
F
x
F_x
Fx 为输入，
F
D
x
、
F
m
x
F_{Dx}、F_{mx}
FDx、Fmx有：
F
D
x
=
−
K
4
P
x
˙
(
5.11.2.
a
)
F
m
x
=
−
m
P
x
¨
(
5.11.2.
b
)
\begin{aligned} &F_{Dx} = -K_4\dot{P_x} \qquad &(5.11.2.a) \\ &F_{mx} = -m\ddot{P_x} \qquad &(5.11.2.b) \end{aligned}
FDx=−K4Px˙Fmx=−mPx¨(5.11.2.a)(5.11.2.b)
K
4
K_4
K4——四旋翼在x方向移动时所受空气阻力的移阻系数。
m
m
m——四旋翼无人机的质量。
式中的负号表示两个力的方向与
F
x
F_x
Fx的方向相反。对于空气阻力
F
D
x
F_{Dx}
FDx，此处已经线性化，原理与5.9 小节相同。
4）各环节合并并整理
式（5.11.1）与式（5.11.2）联立，即得：
m
P
x
¨
+
K
4
P
x
˙
=
F
x
(
5.11.3.
a
)
m\ddot{P_x} + K_4 \dot{P_x} = F_x \qquad (5.11.3.a)
mPx¨+K4Px˙=Fx(5.11.3.a)
y轴位置系统的微分方程与x轴类似，即：
m
P
y
¨
+
K
5
P
y
˙
=
F
y
(
5.11.3.
b
)
m\ddot{P_y} + K_5 \dot{P_y} = F_y \qquad (5.11.3.b)
mPy¨+K5Py˙=Fy(5.11.3.b)
（2）z轴位置系统的微分方程
与x、y轴不同的是，z轴方向上还作用有四旋翼的重力
F
g
F_g
Fg（
=
m
g
=mg
=mg），方向与电机分力
F
z
F_z
Fz的方向相反，则其微分方程可写为：
m
P
z
¨
+
K
6
P
z
˙
=
F
z
−
m
g
(
5.11.3.
c
)
m \ddot{P_z} + K_6 \dot{P_z} = F_z - mg \qquad (5.11.3.c)
mPz¨+K6Pz˙=Fz−mg(5.11.3.c)
（3）三轴位置系统的状态空间方程
1）设计输入量、状态变量、输出量
式（5.11.3）三式的左侧都为输入项，很明显的，输入有
F
x
∼
F
z
F_x\sim F_z
Fx∼Fz三个分力，可作为
u
1
∼
u
3
u_1 \sim u_3
u1∼u3。但式（5.11.3.c）有些特殊，其输入有两项，除分力
F
z
F_z
Fz外，还
m
g
mg
mg项。为了满足状态空间方程的标准形式，这里设计输入
u
4
=
g
u_4 = g
u4=g。
设计输入：
u
1
(
t
)
=
F
x
(
5.11.4.
a
)
u
2
(
t
)
=
F
y
(
5.11.4.
b
)
u
3
(
t
)
=
F
z
(
5.11.4.
c
)
u
4
(
t
)
=
g
(
5.11.4.
d
)
\begin{aligned} &u_1(t) = F_x \qquad & (5.11.4.a) \\ &u_2(t) = F_y \qquad & (5.11.4.b) \\ &u_3(t) = F_z \qquad & (5.11.4.c) \\ &u_4(t) = g \qquad & (5.11.4.d) \end{aligned}
u1(t)=Fxu2(t)=Fyu3(t)=Fzu4(t)=g(5.11.4.a)(5.11.4.b)(5.11.4.c)(5.11.4.d)
设计状态变量：
x
1
(
t
)
=
P
x
(
5.11.5.
a
)
x
2
(
t
)
=
V
x
(
5.11.5.
b
)
x
3
(
t
)
=
P
y
(
5.11.5.
c
)
x
4
(
t
)
=
V
y
(
5.11.5.
d
)
x
5
(
t
)
=
P
z
(
5.11.5.
e
)
x
6
(
t
)
=
V
z
(
5.11.5.
f
)
\begin{aligned} &x_1(t) = P_x \qquad &(5.11.5.a) \\ &x_2(t) = V_x \qquad &(5.11.5.b) \\ &x_3(t) = P_y \qquad &(5.11.5.c) \\ &x_4(t) = V_y \qquad &(5.11.5.d) \\ &x_5(t) = P_z \qquad &(5.11.5.e) \\ &x_6(t) = V_z \qquad &(5.11.5.f) \\ \end{aligned}
x1(t)=Pxx2(t)=Vxx3(t)=Pyx4(t)=Vyx5(t)=Pzx6(t)=Vz(5.11.5.a)(5.11.5.b)(5.11.5.c)(5.11.5.d)(5.11.5.e)(5.11.5.f)
设计输出：
y
1
(
t
)
=
x
1
(
t
)
(
5.11.6.
a
)
y
2
(
t
)
=
x
2
(
t
)
(
5.11.6.
b
)
y
3
(
t
)
=
x
3
(
t
)
(
5.11.6.
c
)
y
4
(
t
)
=
x
4
(
t
)
(
5.11.6.
d
)
y
5
(
t
)
=
x
5
(
t
)
(
5.11.6.
e
)
y
6
(
t
)
=
x
6
(
t
)
(
5.11.6.
f
)
\begin{aligned} &y_1(t) = x_1(t) \qquad &(5.11.6.a) \\ &y_2(t) = x_2(t) \qquad &(5.11.6.b) \\ &y_3(t) = x_3(t) \qquad &(5.11.6.c) \\ &y_4(t) = x_4(t) \qquad &(5.11.6.d) \\ &y_5(t) = x_5(t) \qquad &(5.11.6.e) \\ &y_6(t) = x_6(t) \qquad &(5.11.6.f) \\ \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.11.6.a)(5.11.6.b)(5.11.6.c)(5.11.6.d)(5.11.6.e)(5.11.6.f)
2）列写一队微分方程
式（5.11.3）可拆解成一系列一阶微分方程，即：
P
x
˙
=
V
x
(
5.11.7.
a
)
V
x
˙
=
−
K
4
m
V
x
+
1
m
F
x
(
5.11.7.
b
)
P
y
˙
=
V
y
(
5.11.7.
c
)
V
y
˙
=
−
K
5
m
V
y
+
1
m
F
y
(
5.11.7.
d
)
P
z
˙
=
V
z
(
5.11.7.
e
)
V
z
˙
=
−
K
6
m
V
z
+
1
m
F
z
−
g
(
5.11.7.
f
)
\begin{aligned} &\dot{P_x} = V_x \qquad &(5.11.7.a) \\ &\dot{V_x} = -\frac{K_4}{m}V_x + \frac{\mathrm{1}}{m} F_x \qquad &(5.11.7.b) \\ &\dot{P_y} = V_y \qquad &(5.11.7.c) \\ &\dot{V_y} = -\frac{K_5}{m}V_y + \frac{\mathrm{1}}{m} F_y \qquad &(5.11.7.d) \\ &\dot{P_z} = V_z \qquad &(5.11.7.e) \\ &\dot{V_z} = -\frac{K_6}{m}V_z + \frac{\mathrm{1}}{m} F_z - g \qquad &(5.11.7.f) \\ \end{aligned}
Px˙=VxVx˙=−mK4Vx+m1FxPy˙=VyVy˙=−mK5Vy+m1FyPz˙=VzVz˙=−mK6Vz+m1Fz−g(5.11.7.a)(5.11.7.b)(5.11.7.c)(5.11.7.d)(5.11.7.e)(5.11.7.f)
3）整理并转化为矩阵形式
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
4
m
0
1
0
−
K
5
m
0
1
0
−
K
6
m
]
[
x
1
(
t
)
x
2
(
t
)
x
3
(
t
)
x
4
(
t
)
x
5
(
t
)
x
6
(
t
)
]
+
[
0
1
m
0
1
m
0
0
1
m
−
1
]
[
u
1
(
t
)
u
2
(
t
)
u
3
(
t
)
u
4
(
t
)
]
(
5.11.8.
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
×
4
[
u
1
(
t
)
u
2
(
t
)
u
3
(
t
)
u
4
(
t
)
]
(
5.11.8.
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_4}{m} \\ & &0 &1 && \\ & &0 &-\frac{K_5}{m} \\ &&& &0 &1 \\ &&& &0 &-\frac{K_6}{m} \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}}{m} & & \\ &0 & \\ &\frac{\mathrm{1}}{m} & \\ & &0 &0\\ & &\frac{\mathrm{1}}{m} &-\mathrm{1}\\ \end{bmatrix} \begin{bmatrix} u_1(t) \\ u_2(t) \\ u_3(t) \\ u_4(t) \end{bmatrix}&(5.11.8.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 4} \begin{bmatrix} u_1(t) \\ u_2(t) \\ u_3(t) \\ u_4(t) \end{bmatrix} &(5.11.8.b) \end{aligned}
dtd
x1(t)x2(t)x3(t)x4(t)x5(t)x6(t)
=
001−mK4001−mK5001−mK6
x1(t)x2(t)x3(t)x4(t)x5(t)x6(t)
+
0m10m10m10−1
u1(t)u2(t)u3(t)u4(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×4
u1(t)u2(t)u3(t)u4(t)
(5.11.8.a)(5.11.8.b)
式中，矩阵中空缺的部分为0； I 6 × 6 \boldsymbol{I}_{6\times 6} I6×6为 6 × 6 6\times6 6×6单位矩阵； O 6 × 4 \boldsymbol{O}_{6\times 4} O6×4为 6 × 4 6\times 4 6×4零矩阵。
5）提取矩阵
A = [ 0 1 0 − K 4 m 0 1 0 − K 5 m 0 1 0 − K 6 m ] ( 5.11.9. a ) B = [ 0 1 m 0 1 m 0 0 1 m − 1 ] ( 5.11.9. b ) C = I 6 × 6 ( 5.11.9. c ) D = O 6 × 4 ( 5.11.9. d ) u ( t ) = [ u 1 ( t ) u 2 ( t ) u 3 ( t ) u 4 ( t ) ] T ( 5.11.9. e ) x ( t ) = [ x 1 ( t ) x 2 ( t ) ⋯ x 6 ( t ) ] T ( 5.11.9. f ) d x ( t ) d t = A x ( t ) + B u ( t ) ( 5.11.9. g ) y ( t ) = C x ( t ) + D u ( t ) ( 5.11.9. h ) \begin{aligned} &\boldsymbol{A} = \begin{bmatrix} 0 &1 \\ 0 &-\frac{K_4}{m} \\ & &0 &1 && \\ & &0 &-\frac{K_5}{m} \\ &&& &0 &1 \\ &&& &0 &-\frac{K_6}{m} \end{bmatrix} &(5.11.9.a) \\ & \boldsymbol{B} = \begin{bmatrix} 0 & & \\ \frac{\mathrm{1}}{m} & & \\ &0 & \\ &\frac{\mathrm{1}}{m} & \\ & &0 &0\\ & &\frac{\mathrm{1}}{m} &-\mathrm{1}\\ \end{bmatrix} &(5.11.9.b) \\ & \boldsymbol{C} = \boldsymbol{I}_{6\times 6} & (5.11.9.c) \\ & \boldsymbol{D} = \boldsymbol{O}_{6\times 4} & (5.11.9.d) \\ &\boldsymbol{u}(t) = \begin{bmatrix} u_1(t) &u_2(t) &u_3(t) & u_4(t) \end{bmatrix}^{\mathrm{T}} & (5.11.9.e) \\ &\boldsymbol{x}(t) = \begin{bmatrix} x_1(t) & x_2(t) &\cdots &x_6(t) \end{bmatrix}^{\mathrm{T}} & (5.11.9.f) \\ &\frac{\mathrm{d}\boldsymbol{x}(t)}{\mathrm{d}t} = \boldsymbol{A}\boldsymbol{x}(t) + \boldsymbol{B}\boldsymbol{u}(t) & (5.11.9.g) \\ & \boldsymbol{y}(t) = \boldsymbol{C}\boldsymbol{x}(t) + \boldsymbol{D}\boldsymbol{u}(t) &(5.11.9.h) \end{aligned} A= 001−mK4001−mK5001−mK6 B= 0m10m10m10−1 C=I6×6D=O6×4u(t)=[u1(t)u2(t)u3(t)u4(t)]Tx(t)=[x1(t)x2(t)⋯x6(t)]Tdtdx(t)=Ax(t)+Bu(t)y(t)=Cx(t)+Du(t)(5.11.9.a)(5.11.9.b)(5.11.9.c)(5.11.9.d)(5.11.9.e)(5.11.9.f)(5.11.9.g)(5.11.9.h)
5.11.2 A1软件设计
本小节例程位于“2、飞控例程\Matlab\（4）飞行控制——定点飞行\A1_四旋翼位置系统”中。
例程功能为建立四旋翼三轴位置系统状态空间方程形式的数学模型，并仿真其对初始状态的响应。
例程中的文件如图5.11.2所示，主要增加了“M_Position.m”文件，修改了“A1_main.mlx”、“P_plot_all.m”、“输入_系统参数.xlsx”等文件。
图5.11.2
M_Position.m文件
该文件分为初始化（行8~56）、具体运行（行59~65）两个部分。
%% 四旋翼位置系统 动态模型
% 注意，位置系统及相关PID使用的坐标为东北天坐标系，
%,x、y坐标在零初始时与机体坐标反向，机头与x轴重合
%%
if init == 0 %未初始化
% 矩阵中元素的默认顺序： Px、Vx、Py、Vy、Pz、Vz
%四旋翼位置系统 系统参数
s_P.m = data_i(27, 4); % 重量
s_P.g = 9.8; % 重力加速度
s_P.K = data_i(28, 4:6); % 位阻系数
%过程噪声方差、数学期望期望
s_P.Q_d = [0 0 0 0 0 0]; %数学期望期望为0
s_P.Q_c = data_i(29, 4:9);
% 定义矩阵
s_P.S_n = 6; %状态变量数
s_P.S_m = 6; %输出变量数
s_P.S_p = 4; %输入变量数
s_P.u = zeros(s_P.S_p, 1); %位置系统输入
s_P.y = zeros(s_P.S_m, 1); %位置系数输出
s_P.x_last = zeros(s_P.S_n, 1); %上次状态变量
s_P.x = zeros(s_P.S_n, 1); %当前状态变量
s_P.x_last(:, 1) = data_i(30:35, 4); %初始状态 位置、速度
s_P.A = zeros(s_P.S_n, s_P.S_n);
s_P.B = zeros(s_P.S_n, s_P.S_p);
s_P.C = eye(s_P.S_n); %生成一个 Sp_n 阶的单位矩阵
s_P.D = zeros(s_P.S_m, s_P.S_p);
s_P.A(1:2, 1:2) = [0, 1; 0, -s_P.K(1)/s_P.m];
s_P.A(3:4, 3:4) = [0, 1; 0, -s_P.K(2)/s_P.m];
s_P.A(5:6, 5:6) = [0, 1; 0, -s_P.K(3)/s_P.m];
s_P.B(2, 1) = 1/s_P.m;
s_P.B(4, 2) = 1/s_P.m;
s_P.B(6, 3) = 1/s_P.m;
s_P.B(6, 4) = -1;
% 获取离散化状态空间方程
sys_d = c2d(ss(s_P.A, s_P.B, s_P.C, s_P.D),Ts);
% 提取离散系统A矩阵
s_P.A = sys_d.a;
% 提取离散系统B矩阵
s_P.B = sys_d.b;
% 提取离散系统C矩阵
s_P.C = sys_d.c;
% 提取离散系统D矩阵
s_P.D = sys_d.d;
else %计算系统响应
% 动态系统响应
w_noise = normrnd(s_P.Q_d, s_P.Q_c)'; %过程噪声
s_P.x = s_P.A*s_P.x_last + s_P.B*s_P.u; %求当前状态
s_P.x = s_P.x + w_noise; %添加过程噪声，式（6.3.1.a）
s_P.y = s_P.C*s_P.x + s_P.D*s_P.u; %求输出，y有不同,式（2.2.6）
s_P.x_last = s_P.x;
end
- 行10~35
建立“s_P”位置系统参数结构体变量，并将Excel数据赋值给位置系统相关参数。 - 行37~44
构建状态空间方程的特征矩阵 A 、 B \boldsymbol{A}、\boldsymbol{B} A、B，对应式（5.11.9.a）、（5.11.9.b） - 行47~56
建立状态空间方程及对其离散化，最终获得四个离散状态空间方程的特征矩阵。 - 行60~65
三轴位置系统的具体运行部分，与三轴角度系统相似。
A1_main.mlx 文件
主要添加了三轴位置系统的运行部分，及对其输出数据的保存。
......
init = 1; %已初始化完毕
for i = 1:(Num)
% 串级PID控制
C_Cascade_PID;
% 动态系统响应
M_Motor; %电机
s_A.u = s_mot.M_xyz; %赋值角度系统输入
M_Angle; %四旋翼角度系统
s_P.u(1:3, 1) = s_mot.F_xyz; %赋值位置系统输入
s_P.u(4, 1) = s_P.g;
M_Position; %四旋翼位置系统
%保存电机模型输入输出数据
s_History.u_all(i, :) = s_mot.u_all';
s_History.F_xyz(i, :) = s_mot.F_xyz';
s_History.M_xyz(i, :) = s_mot.M_xyz';
%保存角度系统输出数据
s_History.ya(i, :) = s_A.y.*180/pi; %角度，单位（deg）
%保存位置系统输出数据
s_History.yp(i, :) = s_P.y; %位置，单位（m）
......
- 行14~16
赋值位置系统的输入并运行三轴位置系统。 - 行27
保存位置系统的输出到s_History 结构体的yp 元素。
P_plot_all.m 文件
主要添加了位置系统相关曲线图，包括三轴分力曲线图、三轴位置曲线图、三轴线速度曲线图。
%% 绘图文件
......
%% 位置系统
% 输入三轴力
figure(3);
subplot(3, 1, 1);
plot(t, s_History.F_xyz(:, 1), 'r');
hold on;
plot(t, s_History.F_xyz(:, 2), 'g');
hold on;
plot(t, s_History.F_xyz(:, 3), 'b');
hold on;
legend('F_x', 'F_y', 'F_z');
xlabel('时间(s)', 'Color', 'k', "FontSize", 15);
ylabel('力（N）', 'Color', 'k', "FontSize", 15);
title('四旋翼 位置系统')
grid on;
hold off;
% 三轴位置（东北天坐标系，机头x轴, x、y坐标在零初始时与机体坐标反向）
subplot(3, 1, 2);
plot(t, s_History.yp(:, 1), 'r');
hold on;
plot(t, s_History.yp(:, 3), 'g');
hold on;
plot(t, s_History.yp(:, 5), 'b');
hold on;
legend('P_x', 'P_y', 'P_z');
xlabel('时间(s)', 'Color', 'k', "FontSize", 15);
ylabel('位置（m）', 'Color', 'k', "FontSize", 15);
grid on;
hold off;
% 三轴速度
subplot(3, 1, 3);
plot(t, s_History.yp(:, 2), 'r');
hold on;
plot(t, s_History.yp(:, 4), 'g');
hold on;
plot(t, s_History.yp(:, 6), 'b');
hold on;
legend('V_x', 'V_y', 'V_z');
xlabel('时间(s)', 'Color', 'k', "FontSize", 15);
ylabel('速度（m/s）', 'Color', 'k', "FontSize", 15);
grid on;
hold off;
5.11.3 A1运行与调试
双击进入“A1_main.mlx”文件，点击上方“实时编辑器”中的“运行”按钮即可运行例程。运行结果如图5.11.3所示，其中（c）图为位置系统相关曲线图，由于未使用控制器进行控制，位置等参数呈发散状态。
（a）
（b）
（c）
图 5.11.3
5.11.4 三轴位置串级PID控制器
与角度系统的串级PID控制器类似，此处不再赘述。
5.11.5 A2软件设计
本小节例程位于“2、飞控例程\Matlab\（4）飞行控制——定点飞行\A2_位置系统串级PID”中。
例程功能为：使用4组（共12环）的串级PID对四旋翼三轴的位置、速度、角度、角速度进行控制。
例程主要修改了“C_Cascade_PID.m”、“输入_系统参数.xlsx”等文件。
C_Cascade_PID.m 文件
%% 四旋翼 串级PID 控制器
% 输入：
% s_A.y 角度系统的输出
%
% 输出
% s_mot.u_all 四个电机的功率
%%
if init == 0 %未初始化
% 角度系统PID相关，变量默认顺序，Ax、Wx、Ay、Wy、Az、Wz
s_pid.d = data_i(20, 4:9); %串级PID期望，三轴角度、角速度，单位：deg、deg/s
s_pid.A_max = data_i(43, 4); %x、y角度环的期望最大值
s_pid.d = s_pid.d *pi/180; %转化单位为弧度
s_pid.A_max = s_pid.A_max *pi/180;
s_pid.K_pid = data_i(14:19, 4:6); % 角度系统相关PID参数（*6）
s_pid.e = zeros(6, 1);
s_pid.e_last = zeros(6, 1);
s_pid.Ei = zeros(6, 1);
s_pid.y = zeros(6, 1);
% 位置系统PID相关，变量默认顺序，Px、Vx、Py、Vy、Pz、Vz
s_pid.p_d = data_i(42, 4:9); % 位置、速度PID 期望
s_pid.Kp_pid = data_i(36:41, 4:6); % 位置系统相关PID参数（*6）
s_pid.p_e = zeros(6, 1);
s_pid.p_e_last = zeros(6, 1);
s_pid.p_Ei = zeros(6, 1);
s_pid.p_y = zeros(6, 1);
%PID临时期望存储
s_pid.short_d = 0;
% 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;
kp = 1;
%%%%%%%%%%%%%%%%%%%%%% 位置系统PID
for PID_index = 1:3
% 位置环
s_pid.short_d = s_pid.p_d(kp);
s_pid.p_e(kp) = s_pid.short_d - s_P.y(kp, 1); %角速度 期望与测量误差
[s_pid.p_y(kp), s_pid.p_Ei(kp)] = C_PID(Ts, s_pid.Kp_pid(kp, :), s_pid.p_e(kp), s_pid.p_e_last(kp), s_pid.p_Ei(kp));
s_pid.p_e_last(kp) = s_pid.p_e(kp); %保存本次误差
kp = kp+1;
%速度环
s_pid.short_d = s_pid.p_d(kp) + s_pid.p_y(kp-1);
s_pid.p_e(kp) = s_pid.short_d - s_P.y(kp, 1); %角速度 期望与测量误差
[s_pid.p_y(kp), s_pid.p_Ei(kp)] = C_PID(Ts, s_pid.Kp_pid(kp, :), s_pid.p_e(kp), s_pid.p_e_last(kp), s_pid.p_Ei(kp));
s_pid.p_e_last(kp) = s_pid.p_e(kp); %保存本次误差
kp = kp+1;
end
%%%%%%%%%%%%%%%%%%%%%% 角度系统PID
for PID_index = 1:3
% 角速度环
if PID_index ~= 3 %不为z轴时，进行角度期望限制
if (PID_index == 1) % 为x轴角度控制
s_pid.short_d = s_pid.d(k) - s_pid.p_y(4); % x轴角度PID 期望（叠加y轴速度环输出）
elseif (PID_index == 2) % 为y轴角度控制
s_pid.short_d = s_pid.d(k) - s_pid.p_y(2); % y轴角度PID 期望（叠加x轴速度环输出）
end
s_pid.short_d = min(s_pid.short_d, s_pid.A_max);
s_pid.short_d = max(s_pid.short_d, -s_pid.A_max);
else
s_pid.short_d = s_pid.d(k); % z轴角度环PID 期望
end
s_pid.e(k) = s_pid.short_d - 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;
% 角速度环
s_pid.short_d = s_pid.d(k) + s_pid.y(k-1);
s_pid.e(k) = s_pid.short_d - 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
% PID输出转化为各电机功率
s_pid.PID_out(1:3, 1) = [s_pid.y(2) s_pid.y(4) s_pid.y(6)]'; % x、y、z三轴角速度环输出
s_pid.PID_out(4, 1) = s_P.m*s_P.g*s_mot.u_max/(4*s_mot.F_max) + s_pid.p_y(6); % 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
- 行26~33
将Excel中位置系统相关参数赋值给相关变量。 - 行49~26
添加三轴位置环、线速度环PID
5.11.6 A2运行与调试
双击进入“A1_main.mlx”文件，点击上方“实时编辑器”中的“运行”按钮即可运行例程。运行结果如图5.11.4所示，其中（c）图为位置系统相关曲线图，经过一定时间，各轴线速度与位置都达到期望。
（a）
（b）
（c）
图 5.11.4
5.11.7 期望误差限伏
对于四旋翼控制器来说，有些期望值的大小是不合理的，如x轴的角度期望，用于定点飞行时，其期望应在 − 3 0 ∘ ∼ 3 0 ∘ -30^{\circ} \sim 30^{\circ} −30∘∼30∘间浮动，若期望为270 ∘ ^{\circ} ∘等，是不合理的。由于外环PID的输出将成为内环PID期望的一部分，期望幅度过大的情况在串级PID中是常见的，故对期望要采取限幅。
而误差是PID的直接输入，PID控制器的参数常只对一定范围内的输入控制效果较好。例如，若PID能很好对初始误差 e 0 = 1000 e_0 = 1000 e0=1000的系统进行控制，那么其对 e 0 = 1 e_0=1 e0=1系统控制就会是迟顿的。故也要对串级PID各环的误差进行限幅。
5.11.8 A3软件设计
本小节例程位于“2、飞控例程\Matlab\（4）飞行控制——定点飞行\A3_期望_误差限幅”中。
例程功能为：加入期望与误差限幅，并进行串PID控制。
例程中的文件如图5.11.5所示，主要增加了“F_Limiter.m”文件，修改了“输入_系统参数.xlsx”、“C_Cascade_PID.m”等文件。
图5.11.5
F_Limiter.m
