模型预测控制—符号公式

1. fossen船舶模型。

syms u v r m11 m22 m33 d11 d22 d33 tau delta w1 w2 w3
M=[m11 0 0;0 m22 0;0 0 m33]
C=[0 0 -m22*v;0 0 m11*u;m22*v -m11*u 0]
D=[d11 0 0;0 d22 0;0 0 d33]
V=[u v r]'
Tau=[tau 0 delta]'
W=[w1 w2 w3]'
MV_d=-C*V-D*V+Tau+W
V_d=inv(M)*MV_d
u_dot=(tau+w1-d11*u+m22*v*r)/m11;
v_dot=(w2-d22*v-m11*u*r)/m22;
r_dot=(delta+w3-d33*r+(m11-m22)*u*v)/m33;

2. 对上述方程使用一阶次泰勒展开，并进行欧拉方法离散化处理，得到极径方向代价函数，单点跟踪。

syms T u v r pu pv pr m11 m22 m33 d11 d22 d33 taou taov taor Wu Wv Wr Tr Q R
puk=(1-d11*T/m11)*pu+m22*T/m11*r*pv+m22*T/m11*v*pr+T*T/m11*(taou+Wu);
pvk=(1-d22*T/m22)*pv-m11*T/m22*r*pu-m11*T/m22*u*pr+T*T/m22*(taov+Wv);
prk=(1-d33*T/m33)*pr+(m11-m22)*T/m33*v*pu+(m11-m22)*T/m33*u*pv+T*T/m33*(taor+Wr);

yk=puk*sin(prk)+pvk*cos(prk);
xk=puk*cos(prk)-pvk*sin(prk);
%rourou=xk*xk+yk*yk;
rourou=puk*puk+pvk*pvk;
collect(rourou,taou)
J1=(rourou-Tr*Tr)*Q*(rourou-Tr*Tr);
J2=rourou*R*rourou;
J=J1+J2;
collect(J1+J2,taou)
% ((Q*T^8)/m11^4 + (R*T^8)/m11^4)*taou^4 + ((Q*T^6*((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)*4)/m11^3 + (R*T^6*((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)*4)/m11^3)*taou^3 + (R*((T^4*((pv*((T*d22)/m22 - 1) - (T^2*(Wv + taov))/m22 + (T*m11*pu*r)/m22 + (T*m11*pr*u)/m22)^2 + ((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2)*2)/m11^2 + (T^4*((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2*4)/m11^2) + Q*((T^4*((pv*((T*d22)/m22 - 1) - (T^2*(Wv + taov))/m22 + (T*m11*pu*r)/m22 + (T*m11*pr*u)/m22)^2 + ((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2 - Tr^2)*2)/m11^2 + (T^4*((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2*4)/m11^2))*taou^2 + ((R*T^2*((pv*((T*d22)/m22 - 1) - (T^2*(Wv + taov))/m22 + (T*m11*pu*r)/m22 + (T*m11*pr*u)/m22)^2 + ((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2)*((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)*4)/m11 + (Q*T^2*((pv*((T*d22)/m22 - 1) - (T^2*(Wv + taov))/m22 + (T*m11*pu*r)/m22 + (T*m11*pr*u)/m22)^2 + ((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2 - Tr^2)*((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)*4)/m11)*taou + R*((pv*((T*d22)/m22 - 1) - (T^2*(Wv + taov))/m22 + (T*m11*pu*r)/m22 + (T*m11*pr*u)/m22)^2 + ((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2)^2 + Q*((pv*((T*d22)/m22 - 1) - (T^2*(Wv + taov))/m22 + (T*m11*pu*r)/m22 + (T*m11*pr*u)/m22)^2 + ((T^2*Wu)/m11 - pu*((T*d11)/m11 - 1) + (T*m22*pv*r)/m11 + (T*m22*pr*v)/m11)^2 - Tr^2)^2

3. 极径方向代价函数另一种表达式，单点跟踪。

    a1=(1-d11*T/m11);
    a2=2*m22*T*T/m11*v*r+T*T/m11*Wu;
    a3=T*T/m11;
    b1=(1-d22*T/m22);
    b2=-2*m11*T*T/m22*u*r+T*T/m22*Wv;
    b3=T*T/m22;
    c1=(1-d33*T/m33);
    c2=2*(m11-m22)*T*T/m33*u*v+T*T/m33*Wr;
    c3=T*T/m33;

syms a1 a2 a3 b1 b2 b3 c1 c2 c3 taou taor Wu Wv Wr u v r pu pv pr T Q R Tr

puk=a1*pu+a2+a3*taou;
pvk=b1*pv+b2;
prk=c1*pr+c2+c3*taor;

rourou=puk*puk+pvk*pvk;
collect(rourou,taou)
J1=(rourou-Tr*Tr)*Q*(rourou-Tr*Tr);
J2=rourou*R*rourou;
collect(J1+J2,taou)
a=(Q*a3^4 + R*a3^4);
b=(4*Q*a3^3*(a2 + a1*pu) + 4*R*a3^3*(a2 + a1*pu));
c=(R*(2*a3^2*((a2 + a1*pu)^2 + (b2 + b1*pv)^2) + 4*a3^2*(a2 + a1*pu)^2) + Q*(2*a3^2*((a2 + a1*pu)^2 + (b2 + b1*pv)^2 - Tr^2) + 4*a3^2*(a2 + a1*pu)^2));
d=(4*R*a3*((a2 + a1*pu)^2 + (b2 + b1*pv)^2)*(a2 + a1*pu) + 4*Q*a3*(a2 + a1*pu)*((a2 + a1*pu)^2 + (b2 + b1*pv)^2 - Tr^2));
e=R*((a2 + a1*pu)^2 + (b2 + b1*pv)^2)^2 + Q*((a2 + a1*pu)^2 + (b2 + b1*pv)^2 - Tr^2)^2;
J=a*taou^4 + b*taou^3 + c*taou^2 + d*taou + e;

4.  预测方程表达式，每组预测值由P个点组成。

syms a1 a2 a3 b1 b2 b3 c1 c2 c3 taou taor pu pv pr kk
p=5
Q=100
R=100
% prediction function
Ap1=[];
Ap2=[];
Bp1=[];
Bp2=[];
Cp1=[];
Cp2=[];
for i=1:p
    Ap1=[Ap1;a1^i];
    Ap2=[Ap2;(1-a1^i)/(1-a1)];
    Bp1=[Bp1;b1^i];
    Bp2=[Bp2;(1-b1^i)/(1-b1)];
    Cp1=[Cp1;c1^i];
    Cp2=[Cp2;(1-c1^i)/(1-c1)];
end
Ap3=Ap2;
Bp3=Bp2;
Cp3=Cp2;
faip=Cp1*pr+Cp2*c2+Cp3*c3*taor;
pup=Ap1*pu+Ap2*a2+Ap3*a3*taou;
pvp=Bp1*pv+Bp2*b2;
rrp=pup.*pup+pvp.*pvp
roup=sqrt(pup.*pup+pvp.*pvp);
% prediction function

5.  极径和极角指数函数代价函数，单点跟踪。

syms a1 a2 a3 b1 b2 taou puk pvk Q R Tr1
rr1 = (b2 + b1*pvk)^2 + (a2 + a1*puk + a3*taou)^2;
Jrr1=Q*exp(rr1-Tr1)+R*taou*taou
collect(rr1-Tr1,taou)
Jrr2=Q*exp(Tr1-rr1)+R*taou*taou
collect(Tr1-rr1,taou)
dJrr1=diff(Jrr1,taou)
dJrr2=diff(Jrr2,taou)

syms c1 c2 c3 taor pr Q Tf1
prk=c1*pr+c2+c3*taor;
J1=Q*exp((Tf1 - c2 - c1*pr)/c3-taor);

6. 极径方向代价函数，P点一组进行跟踪。

syms a1 a2 a3 b1 b2 taou puk pvk Ap1 Ap2 Bp1 Bp2 Trrp
pup=Ap1*puk+Ap2*a2+Ap2*a3*taou;
pvp=Bp1*pvk+Bp2*b2;
rrp=pup'*pup+pvp'*pvp;
collect(Trrp-rrp,taou);
rrp=Ap2'*Ap2*a3*a3*taou*taou+((Ap1'*puk+Ap2'*a2)*Ap2*a3+Ap2'*a3*(Ap1*puk+Ap2*a2))*taou+(Ap1'*puk+Ap2'*a2)*(Ap1*puk+Ap2*a2)+(Bp1'*pvk+Bp2'*b2)*(Bp1*pvk+Bp2*b2);
Trrp-rrp
AA=-Ap2'*Ap2*a3*a3;
BB=-((Ap1'*puk+Ap2'*a2)*Ap2*a3+Ap2'*a3*(Ap1*puk+Ap2*a2));
CC=Trrp-(Ap1'*puk+Ap2'*a2)*(Ap1*puk+Ap2*a2)-(Bp1'*pvk+Bp2'*b2)*(Bp1*pvk+Bp2*b2);
AA*taou^2 + BB*taou + CC

AA=-Ap2'*Ap2*a3*a3;
BB=-((Ap1'*puk+Ap2'*a2)*Ap2*a3+Ap2'*a3*(Ap1*puk+Ap2*a2));
CC=Trr-(Ap1'*puk+Ap2'*a2)*(Ap1*puk+Ap2*a2)-(Bp1'*pvk+Bp2'*b2)*(Bp1*pvk+Bp2*b2); 
fun=@(taou) Q*exp(AA*taou^2 + BB*taou + CC)+R*taou^2;

Trrp_rrp=AA*taou^2 + BB*taou + CC;
Delta=BB^2-4*AA*CC;
Mm=(4*AA*CC-BB^2)/(4*AA);

7. 极角方向代价函数，P点一组进行跟踪。

% cost function of faip
syms  Cp1 Cp2 Cp3 pr c2 c3 taor Wr Q3 R3 Tf
faip=Cp1*pr+Cp2*c2+Cp3*c3*taor;
J2=R3*c3*c3*taor*taor;
J1=(faip-Tf)'*Q3*(faip-Tf)
J3=J1+J2;
J4=(Cp3'*Q3*Cp3+R3)*c3*c3*taor*taor+2*(Cp1*pr+Cp2*c2-Tf)'*Q3*Cp3*c3*taor+(Cp1*pr+Cp2*c2-Tf)'*Q3*(Cp1*pr+Cp2*c2-Tf);
collect(J3,taor)
collect(J4,taor)

%优化目标函数min{1/2*x′Hx+f′x+D}
[tao1,fval,exitflag,output,lambda]=quadprog(H,f',A,b,[],[],lb,ub);
taok(:,k+1) = tao1;

欢迎交流指正。