概念
考虑初始系统的微分方程是非线性,其标准形式为 x ˙ = f ( t , x , u ) y = h ( t , x , u ) (1) \begin{array}{l} \dot{\mathbf{x}}=\mathbf{f}(t, \mathbf{x}, \mathbf{u}) \\ \mathbf{y}=\mathbf{h}(t, \mathbf{x}, \mathbf{u}) \end{array}\tag{1} x˙=f(t,x,u)y=h(t,x,u)(1)
只有很少的一部分系统具有已知的解析解。
设有一名义参考轨迹
(
x
N
)
\left(\mathbf{x}_{N})\right.
(xN),其轨迹为
x
N
(
t
)
=
x
N
(
t
0
)
+
∫
0
t
f
(
τ
,
x
N
,
u
N
)
d
τ
y
(
t
)
=
h
(
t
,
x
N
,
u
N
)
(2)
\begin{array}{c} \mathbf{x}_{N}(t)=\mathbf{x}_{N}\left(t_{0}\right)+\int_{0}^{t} \mathbf{f}\left(\tau, \mathbf{x}_{N}, \mathbf{u}_{N}\right) d \tau \\ \mathbf{y}(t)=\mathbf{h}\left(t, \mathbf{x}_{N}, \mathbf{u}_{N}\right) \end{array}\tag{2}
xN(t)=xN(t0)+∫0tf(τ,xN,uN)dτy(t)=h(t,xN,uN)(2)
此时,假定真实轨迹是名义轨迹加上一个扰动
x
(
t
)
=
x
N
(
t
)
+
δ
x
(
t
)
u
(
t
)
=
u
N
(
t
)
+
δ
u
(
t
)
y
(
t
)
=
y
N
(
t
)
+
δ
y
(
t
)
(3)
\begin{array}{l} \mathbf{x}(t)=\mathbf{x}_{N}(t)+\delta \mathbf{x}(t) \\ \mathbf{u}(t)=\mathbf{u}_{N}(t)+\delta \mathbf{u}(t) \\ \mathbf{y}(t)=\mathbf{y}_{N}(t)+\delta \mathbf{y}(t) \end{array}\tag{3}
x(t)=xN(t)+δx(t)u(t)=uN(t)+δu(t)y(t)=yN(t)+δy(t)(3)
其中 δ x ( t ) \delta \mathbf{x}(t) δx(t), δ u ( t ) \delta \mathbf{u}(t) δu(t)和 δ y ( t ) \delta \mathbf{y}(t) δy(t)分别是状态,输入和输出扰动。对 f ( t , x , u ) \mathbf{f}(t, \mathbf{x}, \mathbf{u}) f(t,x,u)和 h ( t , x , u ) \mathbf{h}(t, \mathbf{x}, \mathbf{u}) h(t,x,u)进行一阶泰勒展开,得 δ x ˙ ( t ) = F ( t ) δ x ( t ) + B ( t ) δ u ( t ) δ y ( t ) = H ( t ) δ x ( t ) + D ( t ) δ u ( t ) (4) \begin{array}{l} \boldsymbol{\delta} \dot{\mathbf{x}}(t)=F(t) \boldsymbol{\delta} \mathbf{x}(t)+B(t) \boldsymbol{\delta} \mathbf{u}(t) \\ \boldsymbol{\delta} \mathbf{y}(t)=H(t) \boldsymbol{\delta} \mathbf{x}(t)+D(t) \boldsymbol{\delta} \mathbf{u}(t) \end{array}\tag{4} δx˙(t)=F(t)δx(t)+B(t)δu(t)δy(t)=H(t)δx(t)+D(t)δu(t)(4)
其中 F ( t ) = ∂ f ∂ x ∣ x N , u N , B ( t ) = ∂ f ∂ u ∣ x N , u N H ( t ) = ∂ h ∂ x ∣ x N , u N , D ( t ) = ∂ h ∂ u ∣ x N , u N (5) \begin{array}{ll} F(t)=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{x}}\right|_{\mathbf{x}_{N}, \mathbf{u}_{N}}, & B(t)=\left.\frac{\partial \mathbf{f}}{\partial \mathbf{u}}\right|_{\mathbf{x}_{N}, \mathbf{u}_{N}} \\ H(t)=\left.\frac{\partial \mathbf{h}}{\partial \mathbf{x}}\right|_{\mathbf{x}_{N}, \mathbf{u}_{N}}, & D(t)=\left.\frac{\partial \mathbf{h}}{\partial \mathbf{u}}\right|_{\mathbf{x}_{N}, \mathbf{u}_{N}} \end{array}\tag{5} F(t)=∂x∂f∣∣xN,uN,H(t)=∂x∂h∣∣xN,uN,B(t)=∂u∂f∣∣xN,uND(t)=∂u∂h∣∣xN,uN(5)
对式(4)进行积分,并加回(3)式,即可在式(2)所示的名义轨迹足够小的领域内接近真实轨迹。如果名义估计距离真实轨迹的偏差并不是很小时,就会出现误差。
例子
有这样一个非线性系统 α ˙ = θ ˙ − α 2 θ ˙ − 0.09 α θ ˙ − 0.88 α + 0.47 α 2 + 3.85 α 3 − 0.02 θ 2 θ ¨ = − 0.396 θ ˙ − 4.208 α − 0.47 α 2 − 3.564 α 3 \begin{array}{c} \dot{\alpha}=\dot{\theta}-\alpha^{2} \dot{\theta}-0.09 \alpha \dot{\theta}-0.88 \alpha+0.47 \alpha^{2}+3.85 \alpha^{3}-0.02 \theta^{2} \\ \ddot{\theta}=-0.396 \dot{\theta}-4.208 \alpha-0.47 \alpha^{2}-3.564 \alpha^{3} \end{array} α˙=θ˙−α2θ˙−0.09αθ˙−0.88α+0.47α2+3.85α3−0.02θ2θ¨=−0.396θ˙−4.208α−0.47α2−3.564α3
状态选为
x
=
[
α
θ
θ
˙
]
T
\mathbf{x}=\left[\begin{array}{lll}\alpha & \theta & \dot{\theta}\end{array}\right]^{T}
x=[αθθ˙]T。线性化的状态矩阵为
F
=
[
f
11
f
12
f
13
0
0
1
f
31
0
f
33
]
F=\left[\begin{array}{ccc} f_{11} & f_{12} & f_{13} \\ 0 & 0 & 1 \\ f_{31} & 0 & f_{33} \end{array}\right]
F=⎣⎡f110f31f1200f131f33⎦⎤
其中
f
11
=
−
2
x
1
x
3
−
0.09
x
3
−
0.88
+
0.94
x
1
+
11.55
x
1
2
f
12
=
−
0.04
x
2
f
13
=
1
−
x
1
2
−
0.09
x
1
f
31
=
−
4.208
−
0.94
x
1
−
10.692
x
1
2
f
33
=
−
0.396
\begin{array}{c} f_{11}=-2 x_{1} x_{3}-0.09 x_{3}-0.88+0.94 x_{1}+11.55 x_{1}^{2} \\ f_{12}=-0.04 x_{2} \\ f_{13}=1-x_{1}^{2}-0.09 x_{1} \\ f_{31}=-4.208-0.94 x_{1}-10.692 x_{1}^{2} \\ f_{33}=-0.396 \end{array}
f11=−2x1x3−0.09x3−0.88+0.94x1+11.55x12f12=−0.04x2f13=1−x12−0.09x1f31=−4.208−0.94x1−10.692x12f33=−0.396
仿真参数为
x
(
t
0
)
=
[
2
5
∘
0
0
]
T
\mathbf{x}(t_0)=\left[\begin{array}{lll}25^{\circ} & 0 & 0\end{array}\right]^{T}
x(t0)=[25∘00]T,
x
N
(
t
0
)
=
[
2
4
∘
0
0
]
T
\mathbf{x}_N(t_0)=\left[\begin{array}{lll}24^{\circ} & 0 & 0\end{array}\right]^{T}
xN(t0)=[24∘00]T,
x
(
t
0
)
=
[
2
5
∘
0
0
]
T
\mathbf{x}(t_0)=\left[\begin{array}{lll}25^{\circ} & 0 & 0\end{array}\right]^{T}
x(t0)=[25∘00]T,
δ
x
(
t
0
)
=
[
1
∘
0
0
]
T
\delta \mathbf{x}(t_0)=\left[\begin{array}{lll}1^{\circ} & 0 & 0\end{array}\right]^{T}
δx(t0)=[1∘00]T。
仿真结果如下
程序如下
% This example shows a linear perturbation technique to
% study the behavior of a highly maneuverable aircraft
% which exhibits nonlinear behavior. This program provides
% plots of the state perturbation trajectories and final
% state trajectories.
% Optimal Estimation of Dynamic Systems (2nd ed.) by Crassidis and Junkins
% Example A.3
% Written by John L. Crassidis 9/03
% Other Required Routines: f8_fun.m
% Initialize Variables
m=501;dt=.01;
t=[0:dt:m*dt-dt]';
x0=[25*pi/180;0;0];
x=zeros(m,3);x(1,:)=x0';%实际状态
xn=zeros(m,3);xn(1,:)=[x0(1)-1*pi/180;0;0]';%名义状态
dx=zeros(m,3);dx(1,:)=[1*pi/180;0;0]';%扰动
% True and Nominal Values
c=[1;1;0.09;0.88;0.47;3.85;0.01;.396;4.208;0.47;3.564];
cn=c*1;
% Main Loop for Integration
for i=1:m-1,
f1=dt*f8_fun(x(i,:),c);
f2=dt*f8_fun(x(i,:)+0.5*f1',c);
f3=dt*f8_fun(x(i,:)+0.5*f2',c);
f4=dt*f8_fun(x(i,:)+f3',c);
x(i+1,:)=x(i,:)+1/6*(f1'+2*f2'+2*f3'+f4');
f1=dt*f8_fun(xn(i,:),cn);
f2=dt*f8_fun(xn(i,:)+0.5*f1',cn);
f3=dt*f8_fun(xn(i,:)+0.5*f2',cn);
f4=dt*f8_fun(xn(i,:)+f3',cn);
xn(i+1,:)=xn(i,:)+1/6*(f1'+2*f2'+2*f3'+f4');
x1=xn(i,1);x2=xn(i,2);x3=xn(i,3);
a11=-2*c(2)*x1*x3-c(3)*x3-c(4)+2*c(5)*x1+3*c(6)*x1*x1;
a12=-2*c(7)*x2;
a13=c(1)-c(2)*x1^2-c(3)*x1;
a21=0;a22=0;a23=1;
a31=-c(9)-2*c(10)*x1-3*c(11)*x1^2;
a32=0;
a33=-c(8);
a=[a11 a12 a13;a21 a22 a23;a31 a32 a33];
phi=c2d(a,zeros(3,1),dt);
dx(i+1,:)=(phi*dx(i,:)')';
end
%Plot Results
plot(t,dx(:,1)*180/pi,t,dx(:,2)*180/pi,'--',t,dx(:,3)*180/pi,'-.');
set(gca,'Fontsize',12);
ylabel('State Perturbations (Deg)')
xlabel('Time (Sec)');
ax=legend('{{\it \delta}{\it x}_1}','{{\it \delta}{\it x}_2}','{{\it \delta}{\it x}_3}');
leg=findobj(ax,'type','text');
set(leg,'FontUnits','points','fontsize',12);
grid
disp(' Press any key to continue')
pause
plot(t,x(:,1)*180/pi,t,x(:,2)*180/pi,'--',t,x(:,3)*180/pi,'-.');
set(gca,'Fontsize',12);
ylabel('States (Deg)')
xlabel('Time (Sec)');
ax=legend('{{\it x}_1}','{{\it x}_2}','{{\it x}_3}');
leg=findobj(ax,'type','text');
set(leg,'FontUnits','points','fontsize',12);
grid
所用到的非线性函数为
function f=f8_fun(x,c)
% Written by John L. Crassidis 9/03
% Main Function Routine for f8 Aircraft
% Coefficients
c1=c(1);c2=c(2);c3=c(3);c4=c(4);c5=c(5);c6=c(6);c7=c(7);
c8=c(8);c9=c(9);c10=c(10);c11=c(11);
% Function
f=zeros(3,1);
f(1)=c1*x(3)-c2*x(1)^2*x(3)-c3*x(1)*x(3)-c4*x(1)+c5*x(1)^2+c6*x(1)^3-c7*x(2)^2;
f(2)=x(3);
f(3)=-c8*x(3)-c9*x(1)-c10*x(1)^2-c11*x(1)^3;
心得,评注
disp(' Press any key to continue')
表示显示‘’中的话phi=c2d(a,zeros(3,1),dt);
是指以a为状态矩阵,零矩阵为输入矩阵,dt为时间间隔的离散时间状态转移矩阵。ax=legend('{{\it x}_1}','{{\it x}_2}','{{\it x}_3}');
中\it表示斜体set(gca,'Fontsize',12);
ax=legend('{{\it x}_1}','{{\it x}_2}','{{\it x}_3}'); leg=findobj(ax,'type','text'); set(leg,'FontUnits','points','fontsize',12);
参考文献
动态系统最优估计