基于无人机自身模型的模型预测控制-有约束情况
1. 模型建立
无人机运动学模型：
{
x
˙
=
v
x
v
x
˙
=
u
x
y
=
v
y
v
y
˙
=
u
y
\left\{ \begin{aligned} \dot x & = v_x \qquad \dot{v_x}=u_x\\ y & = v_y \qquad \dot{v_y}=u_y \\ \end{aligned} \right.
{x˙y=vxvx˙=ux=vyvy˙=uy
领航者模型：
{
x
˙
r
=
v
x
r
v
x
r
˙
=
u
x
r
y
r
=
v
y
r
v
y
r
˙
=
u
y
r
\left\{ \begin{aligned} \dot x_r & = v_{x_r} \qquad \dot{v_{x_r}}=u_{x_r}\\ y_r & = v_{y_r} \qquad \dot{v_{y_r}}=u_{y_r} \\ \end{aligned} \right.
{x˙ryr=vxrvxr˙=uxr=vyrvyr˙=uyr
其中
n
x
：
状
态
变
量
量
个
数
，
n
u
:
控
制
变
量
个
数
，
n
m
:
输
出
变
量
个
数
n_x：状态变量量个数，n_u:控制变量个数，n_m:输出变量个数
nx：状态变量量个数，nu:控制变量个数，nm:输出变量个数，我们得到如下状态空间：
[
x
˙
v
˙
x
y
˙
v
˙
y
]
=
[
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
]
[
x
v
x
y
v
y
]
+
[
0
0
1
0
0
0
0
1
]
[
u
x
u
y
]
\begin{bmatrix} \dot{x}\\ \dot v_x \\ \dot y\\ \dot v_y \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ \end{bmatrix} \begin{bmatrix} x\\ v_x \\ y\\ v_y \end{bmatrix}+ \begin{bmatrix} 0 & 0\\ 1 & 0 \\ 0 & 0\\ 0 & 1\\ \end{bmatrix} \begin{bmatrix} u_x \\ u_y\\ \end{bmatrix}
⎣⎢⎢⎡x˙v˙xy˙v˙y⎦⎥⎥⎤=⎣⎢⎢⎡0000100000000010⎦⎥⎥⎤⎣⎢⎢⎡xvxyvy⎦⎥⎥⎤+⎣⎢⎢⎡01000001⎦⎥⎥⎤[uxuy]
[
x
y
]
=
[
1
0
0
0
0
0
1
0
]
[
x
v
x
y
v
y
]
\begin{bmatrix} x\\ y \end{bmatrix} = \begin{bmatrix} 1&0&0&0\\ 0&0&1&0\\ \end{bmatrix} \begin{bmatrix} x\\ v_x\\ y\\ v_y \end{bmatrix}
[xy]=[10000100]⎣⎢⎢⎡xvxyvy⎦⎥⎥⎤
其中
A
=
[
0
1
0
0
0
0
0
0
0
0
0
1
0
0
0
0
]
B
=
[
0
0
1
0
0
0
0
1
]
C
=
[
1
0
0
0
0
0
1
0
]
x
(
k
)
=
[
x
v
x
y
v
y
]
u
(
k
)
=
[
u
x
u
y
]
A = \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & 0 & 0 & 0\\ 0 & 0 & 0 & 1\\ 0 & 0 & 0 & 0\\ \end{bmatrix} \quad B = \begin{bmatrix} 0 & 0\\ 1 & 0 \\ 0 & 0\\ 0 & 1\\ \end{bmatrix} \quad C=\begin{bmatrix} 1&0&0&0\\ 0&0&1&0\\ \end{bmatrix}\quad x(k)=\begin{bmatrix} x\\ v_x \\ y\\ v_y \end{bmatrix} \quad u(k)=\begin{bmatrix} u_x \\ u_y\\ \end{bmatrix} \quad
A=⎣⎢⎢⎡0000100000000010⎦⎥⎥⎤B=⎣⎢⎢⎡01000001⎦⎥⎥⎤C=[10000100]x(k)=⎣⎢⎢⎡xvxyvy⎦⎥⎥⎤u(k)=[uxuy]
将上述模型离散化，我们得到：
A
k
=
A
∗
Δ
t
+
I
B
k
=
B
∗
Δ
t
\begin{aligned} &A_k=A*\Delta t+I\\ &B_k=B*\Delta t \end{aligned}
Ak=A∗Δt+IBk=B∗Δt
即我们得到系统方程：
x
(
k
+
1
)
=
A
k
∗
x
(
k
)
+
B
k
u
(
k
)
y
(
k
+
1
)
=
C
x
(
k
+
1
)
=
C
A
k
∗
x
(
k
)
+
C
B
k
u
(
k
)
x
(
k
+
1
)
∈
n
x
×
1
A
k
∈
n
x
×
n
x
B
k
∈
n
x
×
n
u
\begin{aligned} &x(k+1)=A_k*x(k)+B_ku(k)\\ &y(k+1) = Cx(k+1)=CA_k*x(k)+CB_ku(k)\\ &x(k+1)\in{n_{x}\times1}\quad A_k\in{n_{x}\times n_{x}}\quad B_k\in{n_{x}\times n_{u}} \end{aligned}
x(k+1)=Ak∗x(k)+Bku(k)y(k+1)=Cx(k+1)=CAk∗x(k)+CBku(k)x(k+1)∈nx×1Ak∈nx×nxBk∈nx×nu
递推公式推导：
{
x
(
k
i
+
1
∣
k
i
)
=
A
k
x
(
k
i
)
+
B
k
u
(
k
i
)
x
(
k
i
+
2
∣
k
i
)
=
A
k
2
x
(
k
i
)
+
A
k
B
k
u
(
k
i
)
+
B
k
u
(
k
i
+
1
)
x
(
k
i
+
3
∣
k
i
)
=
A
k
3
x
(
k
i
)
+
A
k
2
B
k
u
(
k
i
)
+
A
k
B
k
u
(
k
i
+
1
)
+
B
k
u
(
k
i
+
2
)
⋮
x
(
k
i
+
N
p
∣
k
i
)
=
A
k
N
p
x
(
k
i
)
+
A
k
N
p
−
1
B
k
u
(
k
i
)
+
A
k
N
p
−
2
B
k
u
(
k
i
+
1
)
+
⋯
+
A
k
N
p
−
N
c
B
k
u
(
k
i
+
N
c
−
1
)
\left\{ \begin{aligned} &x(k_i+1|k_i)=A_kx(k_i)+B_ku(k_i)\\ &x(k_i+2|k_i)=A_k^{2}x(k_i)+A_kB_ku(k_i)+B_ku(k_i+1)\\ &x(k_i+3|k_i)=A_k^{3}x(k_i)+A_k^{2}B_ku(k_i)+A_kB_ku(k_i+1)+B_ku(k_i+2)\\ &\qquad\vdots\\ &x(k_i+N_p|k_i)=A_k^{N_p}x(k_i)+A_k^{N_p-1}B_ku(k_i)+A_k^{N_p-2}B_ku(k_i+1)+\cdots +A_k^{N_p-N_c}B_ku(k_i+N_c-1)\\ \end{aligned} \right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧x(ki+1∣ki)=Akx(ki)+Bku(ki)x(ki+2∣ki)=Ak2x(ki)+AkBku(ki)+Bku(ki+1)x(ki+3∣ki)=Ak3x(ki)+Ak2Bku(ki)+AkBku(ki+1)+Bku(ki+2)⋮x(ki+Np∣ki)=AkNpx(ki)+AkNp−1Bku(ki)+AkNp−2Bku(ki+1)+⋯+AkNp−NcBku(ki+Nc−1)
{
y
(
k
i
+
1
∣
k
i
)
=
C
A
k
x
(
k
i
)
+
C
B
k
u
(
k
i
)
y
(
k
i
+
2
∣
k
i
)
=
C
A
k
2
x
(
k
i
)
+
C
A
k
B
k
u
(
k
i
)
+
C
B
k
u
(
k
i
+
1
)
y
(
k
i
+
3
∣
k
i
)
=
C
A
k
3
x
(
k
i
)
+
C
A
k
2
B
k
u
(
k
i
)
+
C
A
k
B
k
u
(
k
i
+
1
)
+
C
B
k
u
(
k
i
+
2
)
⋮
y
(
k
i
+
N
p
∣
k
i
)
=
C
A
k
N
p
x
(
k
i
)
+
C
A
k
N
p
−
1
B
k
u
(
k
i
)
+
C
A
k
N
p
−
2
B
k
u
(
k
i
+
1
)
+
⋯
+
C
A
k
N
p
−
N
c
B
k
u
(
k
i
+
N
c
−
1
)
\left\{ \begin{aligned} &y(k_i+1|k_i)=CA_kx(k_i)+CB_ku(k_i)\\ &y(k_i+2|k_i)=CA_k^{2}x(k_i)+CA_kB_ku(k_i)+CB_ku(k_i+1)\\ &y(k_i+3|k_i)=CA_k^{3}x(k_i)+CA_k^{2}B_ku(k_i)+CA_kB_ku(k_i+1)+CB_ku(k_i+2)\\ &\qquad\vdots\\ &y(k_i+N_p|k_i)=CA_k^{N_p}x(k_i)+CA_k^{N_p-1}B_ku(k_i)+CA_k^{N_p-2}B_ku(k_i+1)+\cdots +CA_k^{N_p-N_c}B_ku(k_i+N_c-1)\\ \end{aligned} \right.
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎧y(ki+1∣ki)=CAkx(ki)+CBku(ki)y(ki+2∣ki)=CAk2x(ki)+CAkBku(ki)+CBku(ki+1)y(ki+3∣ki)=CAk3x(ki)+CAk2Bku(ki)+CAkBku(ki+1)+CBku(ki+2)⋮y(ki+Np∣ki)=CAkNpx(ki)+CAkNp−1Bku(ki)+CAkNp−2Bku(ki+1)+⋯+CAkNp−NcBku(ki+Nc−1)
令
Y
=
[
y
(
k
i
+
1
∣
k
i
)
y
(
k
i
+
2
∣
k
i
)
y
(
k
i
+
3
∣
k
i
)
⋯
y
(
k
i
+
N
p
∣
k
i
)
]
(
N
p
∗
n
m
)
×
1
T
U
=
[
u
(
k
i
)
u
(
k
i
+
1
)
⋯
u
(
k
i
+
N
u
)
]
(
N
c
∗
n
u
)
×
1
T
\begin{aligned} &Y=[y(k_i+1|k_i) \quad y(k_i+2|k_i) \quad y(k_i+3|k_i)\cdots y(k_i+N_p|k_i)]^{T}_{(Np *n_m)\times 1}\\ &U = [u(k_i)\quad u(k_{i}+1)\cdots u(k_{i}+N_u)]^{T}_{(N_c*n_u)\times 1}\\ \end{aligned}
Y=[y(ki+1∣ki)y(ki+2∣ki)y(ki+3∣ki)⋯y(ki+Np∣ki)](Np∗nm)×1TU=[u(ki)u(ki+1)⋯u(ki+Nu)](Nc∗nu)×1T
F = [ C A k C A k 2 ⋯ C A k N p ] ( N p ∗ n m ) × n x T Φ = [ C B k 0 0 ⋯ 0 C A k B k C B k 0 ⋯ 0 ⋮ ⋮ C A k N p B k C A k N p − 1 B k C A k N p − 2 B k ⋯ C A k N p − N c B k ] ( N p ∗ n m ) × ( n u ∗ N c ) \begin{aligned} &F=[CA_k \quad CA^{2}_k\quad\cdots CA^{N_p}_k]^{T}_{(N_p*n_m)\times n_x} \quad \\ &\Phi = \begin{bmatrix} CB_k & 0 & 0 &\cdots &0\\ CA_kB_k &CB_k&0&\cdots &0\\ \vdots &\quad&\quad&\quad & \vdots\\ CA^{N_p}_kB_k&CA^{N_p-1}_kB_k &CA^{N_p-2}_kB_k &\cdots&CA^{N_p-N_c}_kB_k \\ \end{bmatrix}_{(Np*n_m)\times(n_u*N_c)} \end{aligned} F=[CAkCAk2⋯CAkNp](Np∗nm)×nxTΦ=⎣⎢⎢⎢⎡CBkCAkBk⋮CAkNpBk0CBkCAkNp−1Bk00CAkNp−2Bk⋯⋯⋯00⋮CAkNp−NcBk⎦⎥⎥⎥⎤(Np∗nm)×(nu∗Nc)
即我们得到：
Y
=
F
x
(
k
i
)
+
Φ
U
Y=Fx(k_i)+\Phi U
Y=Fx(ki)+ΦU
性能指标：
J
=
(
R
s
−
Y
)
T
(
R
s
−
Y
)
+
U
T
R
U
=
(
R
s
−
F
x
(
k
i
)
−
Φ
U
)
T
(
R
s
−
F
x
(
k
i
)
−
Φ
U
)
+
U
T
R
U
=
(
R
s
−
F
x
(
k
i
)
)
T
(
R
s
−
F
x
(
k
i
)
)
−
2
U
T
Φ
T
(
R
s
−
F
x
(
k
i
)
)
+
U
T
(
Φ
T
Φ
+
R
)
U
\begin{aligned} J&=(R_s-Y)^{T}(R_s-Y)+U^{T}RU\\ &=(R_s-Fx(k_i)-\Phi U)^{T}(R_s-Fx(k_i)-\Phi U)+U^{T}RU\\ &=(R_s-Fx(k_i))^{T}(R_s-Fx(k_i))-2U^{T}\Phi^{T} (R_s-Fx(k_i))+U^{T}(\Phi^{T}\Phi+R)U \end{aligned}
J=(Rs−Y)T(Rs−Y)+UTRU=(Rs−Fx(ki)−ΦU)T(Rs−Fx(ki)−ΦU)+UTRU=(Rs−Fx(ki))T(Rs−Fx(ki))−2UTΦT(Rs−Fx(ki))+UT(ΦTΦ+R)U
二次规划标准形式：
m
i
n
x
J
=
1
2
x
T
H
x
+
f
T
x
s
.
t
.
A
x
≤
b
A
e
q
=
b
e
q
l
b
≤
x
≤
u
b
\underset{x}{min} \quad J=\frac{1}{2}x^{T}Hx+f^{T}x\\ s.t.\quad Ax\leq b\\ \qquad \quad A_{eq}=beq\\ \qquad \quad lb\leq x\leq ub
xminJ=21xTHx+fTxs.t.Ax≤bAeq=beqlb≤x≤ub
根据二次规划标准式子，我们得到我们的性能指标标准式如下所示：
H
=
2
(
Φ
T
Φ
+
R
)
f
=
−
2
Φ
T
(
R
s
−
F
x
(
k
i
)
)
\begin{aligned} &H=2(\Phi^{T}\Phi+R)\\ &f=-2\Phi^{T}(R_s-Fx(k_i)) \end{aligned}
H=2(ΦTΦ+R)f=−2ΦT(Rs−Fx(ki))
则可以写成如下形式：
J
=
1
2
U
T
H
U
+
f
T
U
s
.
t
.
A
U
≤
b
A
e
q
U
≤
b
e
q
l
b
≤
U
≤
u
b
J= \frac{1}{2}U^{T}HU+f^{T}U\\ s.t. AU\leq b\\ \qquad A_{eq}U\leq beq\\ \qquad lb\leq U\leq ub
J=21UTHU+fTUs.t.AU≤bAeqU≤beqlb≤U≤ub
直接将其带入二次规划工具箱中求解U即可。
我们可以直接使用matlab的二次规划工具箱，也可以直接使用希尔雷德恩二次规划算法求解。本文仿真使用希尔德二次规划算法对以上问题进行求解。
首先对于不等式约束，我们采用对偶法将原式子变换为一下形式：
max
λ
≥
0
min
x
[
1
2
x
T
E
x
+
x
T
F
+
λ
T
(
M
x
≤
λ
)
]
\max_{\lambda \geq 0}\min_{x}[\frac{1}{2}x^{T}Ex+x^{T}F+\lambda^{T}(Mx\leq \lambda)]
λ≥0maxxmin[21xTEx+xTF+λT(Mx≤λ)]
对
x
x
x求导我们得到:
x
=
−
E
−
1
(
F
+
M
T
λ
)
x=-E^{-1}(F+M^{T}\lambda)
x=−E−1(F+MTλ)
将
x
x
x带入上式中：
max
λ
≥
0
(
−
1
2
λ
T
H
λ
−
λ
T
K
−
1
2
F
T
E
−
1
F
)
\max_{\lambda\geq 0}(-\frac{1}{2}\lambda^{T}H\lambda -\lambda^{T}K-\frac{1}{2}F^{T}E^{-1}F)
λ≥0max(−21λTHλ−λTK−21FTE−1F)
H
=
M
E
−
1
M
T
K
=
γ
+
M
E
−
1
F
H=M E^{-1}M^{T}\\ K=\gamma +ME^{-1}F
H=ME−1MTK=γ+ME−1F
因此上式等价于：
min
λ
≥
0
(
1
2
λ
T
H
λ
+
λ
T
K
+
1
2
λ
T
E
−
1
λ
)
\min_{\lambda \geq 0}(\frac{1}{2}\lambda^{T}H\lambda +\lambda^{T}K+\frac{1}{2}\lambda^{T}E^{-1}\lambda)
λ≥0min(21λTHλ+λTK+21λTE−1λ)
所以
x
x
x的解为：
x
=
−
E
−
1
F
−
E
−
1
M
a
c
t
T
λ
a
c
t
x=-E^{-1}F-E^{-1}M^{T}_{act}\lambda_{act}
x=−E−1F−E−1MactTλact
希尔德雷恩二次规划算法：
λ
i
m
+
1
=
m
a
x
(
0
,
ω
i
m
+
1
)
ω
i
m
+
1
=
−
1
h
i
i
[
k
i
+
∑
j
=
i
+
1
n
h
i
j
λ
j
m
+
1
+
∑
j
=
i
+
1
n
h
i
j
λ
j
m
]
\lambda_{i}^{m+1}=max(0,\omega_{i}^{m+1})\\ \omega_{i}^{m+1}=-\frac{1}{h_{ii}}[k_i+\sum_{j=i+1}^{n}h_{ij}\lambda_{j}^{m+1}+\sum_{j=i+1}^{n}h_{ij}\lambda_{j}^{m}]
λim+1=max(0,ωim+1)ωim+1=−hii1[ki+j=i+1∑nhijλjm+1+j=i+1∑nhijλjm]
3. matlab 仿真代码
%================无人机模型预测控制-基与自身模型的模型预测附加约束================%
clear all;clc;close all;
%% 无人机参数设定--采用运动学模型进行轨迹跟踪
x0 = 10; y0 = 5;
vx0 = 0; vy0 = 0;
x(1) = x0; y(1) = y0;vx(1) = vx0;vy(1) = vy0;
%% 领航者参数设定
inter = 0.05; % 采样周期
time = 60; % 总时长
R = 2;
omega = 2;
t = 0:inter:time;
%% 八字形
for i = 1:1:length(t)
if (mod(floor(omega*t(i)/(2*pi)),2) == 0)
Xr(i) = R*cos(omega*t(i))-R;
Yr(i) = R*sin(omega*t(i));
Vxr(i) = -R*sin(omega*t(i))*omega;
Vyr(i) = R*cos(omega*t(i))*omega;
Uxr(i) = -R*cos(omega*t(i))*omega^2;
Uyr(i) = -R*sin(omega*t(i))*omega^2;
else
Xr(i) = -R*cos(omega*t(i))+R;
Yr(i) = R*sin(omega*t(i));
Vxr(i) = R*sin(omega*t(i))*omega;
Vyr(i) = R*cos(omega*t(i))*omega;
Uxr(i) = R*cos(omega*t(i))*omega^2;
Uyr(i) = -R*sin(omega*t(i))*omega^2;
end
end
%% 直线
% Xr = (5*t)';
% Yr = 3*ones(length(t),1);
% Vxr = 2*ones(length(t),1);
% Vyr = 2*zeros(length(t),1);
% Uxr = zeros(length(t),1);
% Uyr = zeros(length(t),1);
%% 圆形
% Xr = -R*cos(t);
% Yr = R*sin(t);
% Vxr = R*sin(t);
% Vyr = R*cos(t);
% Uxr = R*cos(t);
% Uyr = -R*sin(t);
%%
% EX(:,1) = [x0 - Xr(1);vx0 - Vxr(1);y0 - Yr(1);vy0 - Vyr(1)];
X(:,1) = [x0;vx0;y0;vy0];
%% 领航者轨迹
% figure
% grid minor
% l1 = [];
% axis([-7 7 -7 7]);
% axis equal
% for i = 2:1:length(t)
% hold on
% plot([Xr(i) Xr(i-1)],[Yr(i) Yr(i-1)],'b');
% hold on
% delete(l1);
% l1 = plot(Xr(i),Yr(i),'r.','MarkerSize',20);
% pause(0.1);
%
% end
%% 模型预测控制参数设定
Np = 15; % 预测步长
Nc = 5; % 控制步长
A = [0 1 0 0;0 0 0 0;0 0 0 1;0 0 0 0]; B = [0 0;1 0;0 0;0 1];
C = diag([1 0 1 0]);
lena = size(A);
lenb = size(B);
R = 0.0002*eye(Nc*lenb(2));
Ak = A*inter + eye(lena(1));
Bk = B*inter;
F = cell(Np,1);
PHI = cell(Np,Nc);
for i = 1:1:Np % 计算预测方程矩阵
F{i,1} = C*Ak^i;
end
F = cell2mat(F);
for i = 1:1:Np
for j = 1:1:Nc
if (j<=i)
PHI{i,j} = C*Ak^(i-j)*Bk;
else
PHI{i,j} = zeros(lena(1),lenb(2));
end
end
end
PHI = cell2mat(PHI);
k1 =2;k2 =2;
XX = [];
M = [];
for k = 1:1:Nc*4
if (mod(k,2)==0)
mm = -eye(Nc*lenb(2));
M = [M;mm(k/2,:)];
else
mm = eye(Nc*lenb(2));
M = [M;mm(round(k/2),:)];
end
end
r = 12*ones(Nc*4,1); % 限幅值
%% 迭代计算
k = 1;
for i = 1:1:length(t)-1
for j = i:1:(Np+i-1)
if j >= length(Xr)
j = length(Xr);
end
XX = [XX;[Xr(j);0;Yr(j);0]];
end
E = 2*(PHI'*PHI + R);
P = -2*PHI'*(XX- F*X(:,i));
% U = inv(PHI'*PHI + R)*PHI'*(XX- F*X(:,i));
U = quad(E,P,M,r);
XX = [];
u = U(1:2,1);
% u = u +10*rand(2,1); % 噪声干扰；
% u = min(max(u,-30),30); % 限幅
UU(:,i) = u;
X(:,i+1) = Ak*X(:,i) + Bk*u;
err =[X(:,i+1) - [Xr(i+1);Vxr(i+1);Yr(i+1);Vyr(i+1)]] ;
end
x = (X(1,:))';
vx = (X(2,:))';
y = (X(3,:))';
vy = (X(4,:))';
% VV = vecnorm([Vxr;Vyr]);
% VX = vecnorm([vx;vy]);
% plot(t,VV,'r')
% hold on
% plot(t,VX(1:length(t)),'b')
figure
thetr = atan2(Yr,Xr);
thet = atan2(y,x);
plot(t,thetr(1:length(t)),'r');
hold on
plot(t,thet(1:length(t)),'k');
legend('Leader','follower1')
figure
plot(t(1:length(UU)),UU(1,:),'r');
hold on
plot(t(1:length(UU)),UU(2,:),'k');
legend('ux','uy')
l1 = [];
l2 = [];
pic_num = 1;
figure
grid minor
axis([-10 10 -5 5])
axis equal
Tag1 = animatedline('Color','r');
for i = 1:1:length(Xr)-1
hold on
delete(l1);
delete(l2);
plot([x(i) x(i+1)],[y(i) y(i+1)],'b');
hold on
plot([Xr(i) Xr(i+1)],[Yr(i) Yr(i+1)],'r');
hold on
l1 = plot(x(i+1),y(i+1),'b.','MarkerSize',20);
hold on
l2 = plot(Xr(i+1),Yr(i+1),'r.','MarkerSize',20);
pause(0.1);
% addpoints(Tag1,t(i),x(i));
% drawnow;
% F=getframe(gcf);
% I=frame2im(F);
% [I,map]=rgb2ind(I,256);
% if pic_num == 1
% imwrite(I,map,'test.gif','gif', 'Loopcount',inf,'DelayTime',0.2);
% else
% imwrite(I,map,'test.gif','gif','WriteMode','append','DelayTime',0.2);
% end
% pic_num = pic_num + 1;
F = getframe(gcf);
I = frame2im(F);
[I,map] = rgb2ind(I,256);
if pic_num == 1
imwrite(I,map,'test.gif','gif','Loopcount',inf,'DelayTime',0.2);
else
imwrite(I,map,'test.gif','gif','WriteMode','append','DelayTime',0.2);
end
pic_num = pic_num + 1;
end
function eta = quad(E,F,M,r)
[n1,m1] = size(M); % n1 = 约束变量个数，m1
eta = -E\F;
kk = 0;
for i = 1:1:n1
if (M(i,:)*eta>r(i))
kk = kk + 1;
else
kk = kk + 0;
end
end
if (kk == 0)
return;
end
H = M*(E\M');
K = M*(E\F)+r;
[n,m] = size(K);
lambda = zeros(n,m);
error = 10;
km = 1500; % 最大迭代次数
% for i = 1:1:km
while(1)
lambda_last = lambda;
for j = 1:1:n
w = H(j,:)*lambda - H(j,j)*lambda(j,1);
w = w + K(j,1);
w = -w/H(j,j);
lambda(j,1) = max(0,w);
end
error = (lambda - lambda_last)'*(lambda - lambda_last);
if (error <= 10e-8)
break;
end
end
eta = -E\F - E\M'*lambda;
end