概念
考虑初始系统的微分方程是非线性,其标准形式为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)
只有很少的一部分系统具有已知的解析解。
设有一名义参考轨迹(xN)\left(\mathbf{x}_{N})\right.(xN),其轨迹为
xN(t)=xN(t0)+∫0tf(τ,xN,uN)dτy(t)=h(t,xN,uN)(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)=xN(t)+δx(t)u(t)=uN(t)+δu(t)y(t)=yN(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∣xN,uN,B(t)=∂f∂u∣xN,uNH(t)=∂h∂x∣xN,uN,D(t)=∂h∂u∣xN,uN(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=[f11f12f13001f310f33]F=\left[\begin{array}{ccc}
f_{11} & f_{12} & f_{13} \\
0 & 0 & 1 \\
f_{31} & 0 & f_{33}
\end{array}\right]F=⎣⎡f110f31f1200f131f33⎦⎤
其中
f11=−2x1x3−0.09x3−0.88+0.94x1+11.55x12f12=−0.04x2f13=1−x12−0.09x1f31=−4.208−0.94x1−10.692x12f33=−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(t0)=[25∘00]T\mathbf{x}(t_0)=\left[\begin{array}{lll}25^{\circ} & 0 & 0\end{array}\right]^{T}x(t0)=[25∘00]T,xN(t0)=[24∘00]T\mathbf{x}_N(t_0)=\left[\begin{array}{lll}24^{\circ} & 0 & 0\end{array}\right]^{T}xN(t0)=[24∘00]T,x(t0)=[25∘00]T\mathbf{x}(t_0)=\left[\begin{array}{lll}25^{\circ} & 0 & 0\end{array}\right]^{T}x(t0)=[25∘00]T,δx(t0)=[1∘00]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);
参考文献
动态系统最优估计