MATLAB - 线性二次型最优控制（LQR）矢量推力飞机控制_线性二次型最优控制matlab仿真

线性二次型最优控制（LQR）示例 —— 矢量推力飞机模型

---

---

一、绪论

考虑矢量推力飞机模型，飞机可以通过调整向下的推力，使用位于机翼上的机动推进器完成起飞。

---

二、运动学及动力学建模

下图为矢量推力飞机的简化模型图，我们只关注飞机在竖直平面内的运动。我们将主推进器和机动推进器产生的力分解为作用在飞机下方距离 $r$ 处的一对力 $F_1$ 和 $F_2$。令 $(x,y,\theta )$ 表示飞机质心的位置和方向，$m$ 表示飞机的质量，$J$ 表示转动惯量，$g$ 表示重力加速度，$c$ 表示阻尼系数。则飞机的动力学方程可表示为：
m x ¨ = F 1 cos ⁡ θ − F 2 sin ⁡ θ − c x ˙ , m y ¨ = F 1 sin ⁡ θ + F 2 cos ⁡ θ − m g − c y ˙ , J θ ¨ = r F 1 .

$$\begin{aligned} &m\"{x}=F_1\cos\theta-F_2\sin\theta-c\.{x}, \\ &m\"{y}=F_1\sin\theta+F_2\cos\theta-mg-c\.{y}, \\ &J\"{\theta}=rF_1. \end{aligned}$$ ​mx¨=F1​cosθ−F2​sinθ−cx˙,my¨​=F1​sinθ+F2​cosθ−mg−cy˙​,Jθ¨=rF1​.​

重新定义输入使得原点为系统零输入时的平衡点，令 $u_1=F_1$，$u_2=F_2-mg$，此时系统的方程为
$$\begin{array}{rl}& m\ddot{x}=-mg\sin \theta -c\dot{x}+u_1\cos \theta -u_2\sin \theta ,\\ & m\ddot{y}=mg(\cos \theta -1)-c\dot{y}+u_1\sin \theta +u_2\cos \theta ,\\ & J\ddot{\theta }=r{u}_1.\end{array}$$
矢量推力飞机的运动可用上述三个耦合的二阶微分方程来描述。

---

三、非线性系统的线性化

3.1 雅可比线性化

考虑单输入单输出的非线性系统：
d x d t = f ( x , u ) , x ∈ R n , u ∈ R , y = h ( x , u ) , y ∈ R

$$\begin{aligned} \dfrac{dx}{dt}&=f(x,u),\quad x\in\R^n,u\in\R,\\ y&=h(x,u),\quad y\in\R \end{aligned}$$ dtdx​y​=f(x,u),x∈Rn,u∈R,=h(x,u),y∈R​
系统平衡点为 $x=x_e,u=u_e$，假设 $x_e=0,u_e=0$，假定在系统的平衡点附近输入和状态的变化量很小，即 $x-x_e,u-u_e$ 很小。因此与(低阶)线性项相比，该平衡点周围的非线性扰动可以忽略。这和我们做小角度近似时用到的论证是差不多的，当θ接近0时，用θ代替sin θ，用1代替cos θ。
定义一组新的状态变量 $z=x-x_e$，输入变量 $v=u-u_e$，输出变量 $w=y-h(x_e,u_e)$。在平衡点附近时，这些变量都接近于零，所以这些变量中的非线性项可以被认为是相应向量的泰勒级数展开式中的高阶项。

对于式（3）所描述的非线性系统，雅可比矩阵线性化为：
$$\frac{dz}{dt}=Az+Bv,w=Cz+Dv$$
式中，
$$A={\frac{\partial f}{\partial x}|}_{(x_e,u_e)},B={\frac{\partial f}{\partial u}|}_{(x_e,u_e)},C={\frac{\partial h}{\partial x}|}_{(x_e,u_e)},D={\frac{\partial h}{\partial u}|}_{(x_e,u_e)}.$$
如果线性化是渐近稳定的，那么平衡点 $x_e$ 对于非线性系统是局部渐近稳定的。

---

3.2 矢量推力飞机系统线性化

将矢量推力飞机的动力学方程写为状态空间形式为：
d z d t = ( z 4 z 5 z 6 − c m z 4 − g − c m z 5 0 ) + ( 0 0 0 F 1 m cos ⁡ θ − F 2 m sin ⁡ θ F 1 m sin ⁡ θ + F 2 m cos ⁡ θ r J F 1 ) \dfrac{dz}{dt}=

$$\begin{pmatrix} z_4 \\ z_5 \\ z_6 \\ -\frac{c}{m}z_4 \\ -g-\frac{c}{m}z_5 \\ 0 \end{pmatrix}$$ + $$\begin{pmatrix} 0 \\ 0 \\ 0 \\ \frac{F_1}{m}\cos\theta-\frac{F_2}{m}\sin\theta \\ \frac{F_1}{m}\sin\theta+\frac{F_2}{m}\cos\theta \\ \frac{r}{J}F_1 \end{pmatrix}$$ dtdz​= ​z4​z5​z6​−mc​z4​−g−mc​z5​0​ ​+ ​000mF1​​cosθ−mF2​​sinθmF1​​sinθ+mF2​​cosθJr​F1​​ ​
系统的参数 $m=4\text{kg},J=0.0475\text{kg}\cdot {\text{m}}^2,r=0.25\text{m},g=9.8\text{m}/{\text{s}}^2,c=0.05\text{N}\cdot \text{s}/\text{m}$。系统的平衡点为 $F_1=0,F_2=mg,z_e=(x_e,y_e,0,0,0,0)$，则平衡点附近系统线性化之后的系数矩阵为：
$$\begin{array}{rl}A& =\left(\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& -g& -c/m& 0& 0\\ 0& 0& 0& 0& -c/m& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right),\\ B& =\left(\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 1/m& 0\\ 0& 1/m\\ r/J& 0\end{array}\right),\\ C& =\left(\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ 1/m& 0\\ 0& 1/m\\ r/J& 0\end{array}\right),\\ D& =\left(\begin{array}{cc}0& 0\\ 0& 0\end{array}\right)\end{array}$$
令 $\xi =z-z_e,v=F-F_e$，线性化的方程为：
$$\frac{d\xi }{dt}=A\xi +Bv,y=C\xi$$
为了计算线性二次型调节器，定义代价函数为：
$$J={\int }_0^{\infty }({\xi }^T{Q}_{\xi }\xi +v^T{Q}_{v}v)dt,$$
式中，$\xi =z-z_e,v=F-F_e$ 表示在平衡点 $(z_e,F_e)$ 附件的局部坐标。
用对角矩阵表示状态和输入代价：
$$\begin{array}{rl}{Q}_{\xi }& =\left(\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right),\\ Q_v& =\left(\begin{array}{cc}\rho & 0\\ 0& \rho \end{array}\right).\end{array}$$
则有控制律 $v=-K\xi$，推导出原始变量的控制律为 $F=v+F_e=-K(z-z_e)+F_e$。

四、MATLAB 仿真

4.1 系统参数

文件名为 pvtol_params.m

% System parameters (系统参数)
m = 4;				% mass of aircraft 飞机质量
J = 0.0475;			% inertia around pitch axis 俯仰轴转动惯量
r = 0.25;			% distance to center of force 力距离中心的距离
g = 9.8;			% gravitational constant 引力常数
c = 0.05;	 		% damping factor (estimated) 阻尼系数
1
2
3
4
5
6

4.2 线性化函数

文件名为 pvtol_linearize.m

function [A, B, C, D] = pvtol_linearize(xd, ud)

pvtol_params;				% load system parameters

A = [  0     0     0     1     0     0;
       0     0     0     0     1     0;
       0     0     0     0     0     1;
       0, 0, (-ud(1)*sin(xd(3)) - ud(2)*cos(xd(3)))/m, -c/m, 0, 0;
       0, 0, (ud(1)*cos(xd(3)) - ud(2)*sin(xd(3)))/m, 0, -c/m, 0;
       0     0     0     0     0     0 ];

B = [0 0; 0 0; 0 0;
  cos(xd(3))/m, -sin(xd(3))/m;
  sin(xd(3))/m,  cos(xd(3))/m;
  r/J, 0 ];

C = [1 0 0 0 0 0; 0 1 0 0 0 0];
D = [0 0; 0 0];
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

4.3 主函数

文件名为 pvtol_lqr.m

% pvtol_lqr.m - LQR design for vectored thrust aircraft
% RMM, 14 Jan 03

aminit;
amuse('pvtol');

%%
%% System dynamics
%%
%% These are the dynamics for the PVTOL system, written in state space
%% form.
%%

pvtol_params;			% System parameters

% System matrices (entire plant: 2 input, 2 output) (状态空间动力学)
xe = [0 0 0 0 0 0]; 
ue = [0 m*g];
[A, B, C, D] = pvtol_linearize(xe, ue);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

%%
%% Construct inputs and outputs corresponding to steps in xy position
%%
%% The vectors xd and yd correspond to the states that are the desired
%% equilibrium states for the system.  The matrices Cx and Cy are the
%% corresponding outputs.
%% (xd and yd 对应于期望的系统平衡状态向量，Cx and Cy 为对应的输出矩阵)
%% The way these vectors are used is to compute the closed loop system
%% dynamics as
%%
%%	xdot = Ax + B u		=>	xdot = (A-BK)x + K xd
%%         u = -K(x - xd)		   y = Cx
%%
%% The closed loop dynamics can be simulated using the "step" command,
%% with K*xd as the input vector (assumes that the "input" is unit size,
%% so that xd corresponds to the desired steady state.
%% 闭环动力学可以使用“step”命令模拟，以K*xd作为输入向量(假设“输入”是单位大小，因此xd对应于所需的稳态。

xd = [1; 0; 0; 0; 0; 0];  Cx = [1 0 0 0 0 0];
yd = [0; 1; 0; 0; 0; 0];  Cy = [0 1 0 0 0 0];
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

LQR 控制器设计

%%
%% LQR design
%%

% Start with a diagonal weighting
Qx1 = diag([1, 1, 1, 1, 1, 1]);
Qu1a = diag([1, 1]);
K1a = lqr(A, B, Qx1, Qu1a);

% Close the loop: xdot = Ax + B K (x-xd)
H1a = ss(A-B*K1a, B*K1a*[xd, yd], [Cx; Cy], 0);
[Y, T] = step(H1a, 10);

figure(1); clf;
subplot(221);
plot(T, Y(:,1, 1), '-', T, Y(:,2, 2), '--', ...
    'Linewidth', AM_data_linewidth); hold on;
plot([0 10], [1 1], 'k-', 'Linewidth', AM_ref_linewidth); hold on;
amaxis([0, 10, -0.1, 1.4]);
xlabel('time');   ylabel('position');

lgh = legend({'x', 'y'}, 'Location', 'southeast');
amprint('pvtol-lqrstep1.eps');
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

观察不同的输出权重的影响，即 $Q_x$ 的影响；

% Look at different input weightings
figure(2);
Qu1a = diag([1, 1]); K1a = lqr(A, B, Qx1, Qu1a);
H1ax = ss(A-B*K1a,B(:,1)*K1a(1,:)*xd,Cx,0);

Qu1b = 40^2*diag([1, 1]); K1b = lqr(A, B, Qx1, Qu1b);
H1bx = ss(A-B*K1b,B(:,1)*K1b(1,:)*xd,Cx,0);

Qu1c = 200^2*diag([1, 1]); K1c = lqr(A, B, Qx1, Qu1c);
H1cx = ss(A-B*K1c,B(:,1)*K1c(1,:)*xd,Cx,0);
subplot(222);
step(H1ax, 10);
[Y1, T1] = step(H1ax, 10);
subplot(223);
step(H1bx, 10);
[Y2, T2] = step(H1bx, 10);
subplot(224);
step(H1cx, 10);
[Y3, T3] = step(H1cx, 10);
 clf; 
plot(T1, Y1, 'b-',  'Linewidth', AM_data_linewidth); hold on;
plot(T2, Y2, 'b-', 'Linewidth', AM_data_linewidth); hold on;
plot(T3, Y3, 'b-', 'Linewidth', AM_data_linewidth); hold on;
plot([0 10], [1 1], 'k-', 'Linewidth', AM_ref_linewidth); hold on;

amaxis([0, 10, -0.1, 1.4]);
xlabel('time');   ylabel('position');

arcarrow([1.3 0.8], [5 0.45], -6);
text(5.3, 0.4, 'rho');

amprint('pvtol-lqrstep2.eps');

% Output weighting - change Qx to use outputs
Qx2 = [Cx; Cy]' * [Cx; Cy];
Qu2 = 0.1 * diag([1, 1]);
K2 = lqr(A, B, Qx2, Qu2);

H2x = ss(A-B*K2,B(:,1)*K2(1,:)*xd,Cx,0);
H2y = ss(A-B*K2,B(:,2)*K2(2,:)*yd,Cy,0);

figure(3); step(H2x, H2y, 10);
legend('x', 'y');
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

由实际物理需求确定权重矩阵；

%%
%% Physically motivated weighting
%%
%% Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle
%% less than 5 degrees in making the adjustments.  Penalize side forces
%% due to loss in efficiency.
%% x 方向误差 1cm，y 方向误差 10cm，角度误差在 5°

Qx3 = diag([100, 10, 2*pi/5, 0, 0, 0]);
Qu3 = 0.1 * diag([1, 10]);
K3 = lqr(A, B, Qx3, Qu3);

H3x = ss(A-B*K3,B(:,1)*K3(1,:)*xd,Cx,0);
H3y = ss(A-B*K3,B(:,2)*K3(2,:)*yd,Cy,0);
figure(4);  clf; subplot(221);
step(H3x, H3y, 10);
legend('x', 'y');
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

4.4 利用内环/外环设计进行横向控制

为了控制矢量推力飞机的横向动态，我们采用了 "内/外 "环路设计方法。本示例摘自第 11 章–频域设计。
我们首先用方框图来表示动态过程

其中，$H_{\theta u_1}=\frac{r}{J{s}^2},H_{x{u}_1}=\frac{J{s}^2-mgr}{J{s}^2(m{s}^2+cs)}.$

控制器的结构是将过程动力学和控制器分成两个部分：一个是由滚动动力学 $P_i$ 和控制 $C_i$ 组成的内环，另一个是由横向位置动力学 $P_o$ 和控制器 $C_o$ 组成的外环。

闭合的内环动力学 $H_i$ 利用矢量推力控制飞机的滚转角，而外环控制器 $C_o$ 则命令滚转角来调节横向位置。作为整个系统的性能指标，我们希望侧向位置的稳态误差为零、带宽约为 1 rad/s、相位裕度为 45°。

4.4.1 内环设计

对于内环，我们选择的设计规范是为外环提供精确、快速的滚动控制。内环动力学特性如下
$$P_i=H_{\theta u_1}=\frac{r}{J{s}^2}.$$

我们选择所需的带宽为 10 rad/s（外环的 10 倍），低频误差不超过 5%。为了达到我们的性能指标，我们希望在 10 rad/s 的频率下增益至少为 10，这就要求增益分频频率较高。从下图（左）的环路形状可以看出，为了达到理想的性能，我们不能简单地提高增益，因为这样相位裕度会非常低。相反，我们必须在所需的交叉频率上增加相位。

---

4.3 进一步讨论

在本节中，我们将简要讨论最优控制中的一些相关主题，并在适当的地方参考更详细的论述。
在适当的地方，我们会参考更详细的论述。

最优控制问题的另一种表述方法是利用 “最优原则”。
“最优原则”（大致）是说，如果我们给定了一个最优政策，那么在执行该政策的过程中，任何一点的最优政策
也一定是最优的。在最优轨迹生成的背景下，我们可以
在最优轨迹生成的背景下，我们可以将其解释为：如果我们从最优轨迹上的任何状态出发，解决最优控制问题
沿最优轨迹求解最优控制问题，我们将得到最优轨迹的剩余部分
(怎么可能不是这样呢？）

这句话对轨迹生成的意义在于，我们可以从最优控制问题的最终时间出发
从最优控制问题的最终时间开始计算成本。
计算成本，直至到达初始时间。为此，我们定义了
从给定状态 x 出发的 "时间成本 "为

$J(x,t)={\int }_t^{T}L(x(\tau ),u(\tau ))d\tau +V(x(T)).$

给定一个状态 x(t)，我们可以看到，T 时刻的成本为 J(T, x) = V (x(T))
而其他时间的成本包括从时间 t 到时间 T 的成本积分加上
终端成本。

可以证明，轨迹 x(-)、u(-) 最佳的必要条件是满足以下条件
最优的必要条件是满足汉密尔顿-雅各比-贝尔曼方程（HJB 方程）：

$\frac{\partial J^{\ast }}{\partial t}(x,t)=-H(x,u^{\ast },\frac{\partial J^{\ast }T}{\partial x}(x,t)),J(x,T)=V(x),$

其中，H 是哈密顿函数，u
∗
是最优输入，而 V : R
n → R
是终端成本。与最大原则一样，我们选择 u
∗
以
最小化哈密顿：

---