大家好，又见面了，我是你们的朋友全栈君。
本人利用fmincon做优化计算，其程序如下：
1，主程序
clear all
x0=[0.1,0.3,0.2,0.3,0.1,45,0.214,0.05,0,0.45,0.15,0,0.4,0.12,0,0,0,0,0,0,0,0,0,0,0,0];
A=[ 1 -1 -1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 1 1 -1 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 1 -1 1 -1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; -2 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 -2 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 -2 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0];
b=[0;0;0;0;0;0];
Aeq=[0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 2 3 4 5 6 7 8 9 10; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 6 12 20 30 42 56 72 90];
beq=[0;0;0;0];
[x,fval,exitflag]=fmincon(@myfun,x0,A,b,Aeq,beq);
2，优化程序
function c=myfun(x)
% Parameters of the four-bar linkage(m,rad)
l1=x(1);l2=x(2);l3=x(3);l4=x(4);L=x(5); beta=x(6)/180*pi;
m1x=x(7);r1x=x(8);phi1x=x(9);m2x=x(10);r2x=x(11);phi2x=x(12); m3x=x(13);r3x=x(14);phi3x=x(15);
% orignal linkage
m1o=0.20;r1o=0.05;phi1o=0; m2o=0.5;r2o=0.15; phi2o=0;m3o=0.4;r3o=0.12; phi3o=0;I1o=0.00017;I2o=0.00375;I3o=0.00192;
m1=m1o m1x; m2=m2o m2x; m3=m3o m3x;
r1=sqrt((m1o*r1o)^2 (m1x*r1x)^2-2*m1o*r1o*m1x*r1x*cos(pi-(phi1x-phi1o)))/m1;
r2=sqrt((m2o*r2o)^2 (m2x*r2x)^2-2*m2o*r2o*m2x*r2x*cos(pi-(phi2x-phi2o)))/m2;
r3=sqrt((m3o*r3o)^2 (m3x*r3x)^2-2*m3o*r3o*m3x*r3x*cos(pi-(phi3x-phi3o)))/m3;
phi1=atan((m1o*r1o*sin(phi1o) m1x*r1x*sin(phi1x))/(m1o*r1o*cos(phi1o) m1x*r1x*cos(phi1x)));
phi2=atan((m2o*r2o*sin(phi2o) m2x*r2x*sin(phi2x))/(m2o*r2o*cos(phi2o) m2x*r2x*cos(phi2x)));
phi3=atan((m3o*r3o*sin(phi3o) m3x*r3x*sin(phi3x))/(m3o*r3o*cos(phi3o) m3x*r3x*cos(phi3x)));
v1=r1x^2 r1^2-2*r1x*r1*cos(phi1x-phi1); u1=r1o^2 r1^2-2*r1o*r1*cos(phi1-phi1o);
v2=r2x^2 r2^2-2*r2x*r2*cos(phi2x-phi2); u2=r2o^2 r2^2-2*r2o*r2*cos(phi2-phi2o);
v3=r3x^2 r3^2-2*r3x*r3*cos(phi3x-phi3); u3=r3o^2 r3^2-2*r3o*r3*cos(phi3-phi3o);
I1=I1o m1x*v1^2 m1x*u1^2; I2=I2o m2x*v2^2 m2x*u2^2; I3=I3o m3x*v3^2 m3x*u3^2;
%Parameters of the angle path planning
f=[];g=[];h=[];
%varied input speed(rad,rad/s,rad/s^2)
for t=0:0.01:1;
q1=x(16) x(17)*t x(18)*t^2 x(19)*t^3 x(20)*t^4 x(21)*t^5 x(22)*t^6 x(23)*t^7 x(24)*t^8 x(25)*t^9 x(26)*t^10;
omega1=x(17) 2*x(18)*t 3*x(19)*t^2 4*x(20)*t^3 5*x(21)*t^4 6*x(22)*t^5 7*x(23)*t^6 8*x(24)*t^7 9*x(25)*t^8 10*x(26)*t^9;
alpha1=2*x(18) 6*x(19)*t 12*x(20)*t^2 20*x(21)*t^3 30*x(22)*t^4 42*x(23)*t^5 56*x(24)*t^6 72*x(25)*t^7 90*x(26)*t^8;
% the angle of the coupler and crank(m,rad)
d=sqrt(l4^2 l1^2-2*l4*l1*cos(q1));
theted=atan((-l1.*sin(q1))/(l4-l1*cos(q1)));
q2=acos((l2^2 d.^2-l3^2)/(2*d*l2)) theted;
A=2*l4*l3-2*l1*l3*cos(q1);B=-2*l1*l3*sin(q1);C=l4^2 l1^2 l3^2-l2^2-2*l4*l1*cos(-q1);
q3=2*atan((-B-sqrt(B^2-C^2 A^2))/(C-A));
% the calculation of velocity
h1=-(l1*csc(q2-q3)*sin(q1-q3))/l2;
h2=-(l1*csc(q2-q3)*sin(q1-q2))/l3;
omega2=h1*omega1;
omega3=h2*omega1;
u=l1*cos(q1) L*cos(q2 beta);
v=l1*sin(q1) L*sin(q2 beta);
g=[g;u];
h=[h;v];
v1x=-r1*omega1*sin(q1 phi1);
v1y=r1*omega1*cos(q1 phi1);
v1=sqrt(v1x^2 v1y^2);
v2x=-l1*omega1*sin(q1)-r2*omega2*sin(q2 phi2);
v2y=l1*omega1*cos(q1) r2*omega2*cos(q2 phi2);
v2=sqrt(v2x^2 v2y^2);
v3x=-r3*omega3*sin(q3 phi3);
v3y=r3*omega3*cos(q3 phi3);
v3=sqrt(v3x^2 v3y^2);
% the calculation of acceleration
h3=-((l1*cos(q1-q3) l2*h1^2*cos(q2-q3)-l3*h2^2)*csc(q2-q3))/l2;
h4=((-l1*cos(q1-q2)-l2*h1^2 l3*h2^2*cos(q2-q3))*csc(q2-q3))/l3;
alpha2=h3*omega1^2 h1*alpha1;
alpha3=h4*omega1^2 h2*alpha1;
a1x=-r1*alpha1*sin(q1 phi1)-r1*omega1^2*cos(q1 phi1);
a1y=r1*alpha1*cos(q1 phi1)-r1*omega1^2*sin(q1 phi1);
a1=sqrt(a1x^2 a1y^2);
a2x=-l1*alpha1*sin(q1)-l1*omega1^2*cos(q1)-r2*alpha2*sin(q2 phi2)-r2*omega2^2*cos(q2 phi2);
a2y=l1*alpha1*cos(q1)-l1*omega1^2*sin(q1) r2*alpha2*cos(q2 phi2)-r2*omega2^2*sin(q2 phi2);
a2=sqrt(a2x^2 a2y^2);
a3x=-r3*alpha3*sin(q3 phi3)-r3*omega3^2*cos(q3 phi3);
a3y=r3*alpha3*cos(q3 phi3)-r3*omega3^2*sin(q3 phi3);
a3=sqrt(a3x^2 a3y^2);
% the calculation of each joint’s force
r1x1=l1*cos(q1)-r1*cos(q1 phi1); r1y1=l1*sin(q1)-r1*sin(q1 phi1);
r2x1=l2*cos(q2)-r2*cos(q2 phi2); r2y1=l2*sin(q2)-r2*sin(q2 phi2);
r3x1=l3*cos(q3)-r3*cos(q3 phi3); r3y1=l3*sin(q3)-r3*sin(q3 phi3);
r2x=r2*cos(q2 phi2); r2y=r2*sin(q2 phi2);
Lx=L*cos(beta q2); Ly=L*sin(beta q2);
A=[1,0,1,0,0,0,0,0,0;
0,1,0,1,0,0,0,0,0;
-1,0,0,0,1,0,0,0,0;
0,-1,0,0,0,1,0,0,0;
0,0,0,0,-1,0,1,0,0;
0,0,0,0,0,-1,0,1,0;
-r1y1,r1x1,r1*sin(q1 phi1),-r1*cos(q1 phi1),0,0,0,0,1;
-r2*sin(q2 phi2),r2*cos(q2 phi2),0,0,-r2y1,r2x1,0,0,0;
0,0,0,0,r3y1,-r3x1,r3*sin(q3 phi3),-r3*cos(q3 phi3),0];
B=[m1*a1x;
m1*a1y;
m2*a2x;
m2*a2y;
m3*a3x;
m3*a3y;
I1*alpha1;
I2*alpha2;
I3*alpha3];
X=AB;
f=[f;X’];
end
%把所有的Fsh,MshA，MshD和TIN都加起来
F41x=f(:,3);F41y=f(:,4);F43x=f(:,7);F43y=f(:,8);TIN=f(:,9);
Fsh=sqrt((F41x F43x).^2 (F41y F43y).^2);
MshA=-(F43y*l4 TIN);
MshD=(F41y*l4-TIN);
Fsh=sqrt(sum(Fsh.^2)./101);
%TIN=sqrt(sum(TIN.^2)./101);
MshA=sqrt(sum(MshA.^2)./101);
MshD=sqrt(sum(MshD.^2)./101);
%objective function
c1=sqrt(1/6*(g(14)-0.103)^2 (h(14)-0.220)^2 (g(22)-0.072)^2 (h(22)-0.242)^2 (g(34) 0.004)^2 (h(34)-0.231)^2 (g(51) 0.082)^2 (h(51)-0.149)^2 (g(59) 0.091)^2 (h(59)-0.1)^2 (g(93)-0.036)^2 (h(93)-0.090)^2);
c2=1/3*Fsh 1/3*MshA 1/3*MshD;
c=0.5*c1 0.5*c2;
可是最后出现了：
Warning: Large-scale (trust region) method does not currently solve this type of problem,
using medium-scale (line search) instead.
> In fmincon at 317
In main at 12
Optimization terminated: magnitude of directional derivative in search
direction less than 2*options.TolFun and maximum constraint violation
is less than options.TolCon.
No active inequalities.
望高手们予以解答，不甚感激啊！
[本帖最后由 DHU 于 2011-3-17 21:44 编辑]
发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/134004.html原文链接：https://javaforall.cn
