PID+模糊逻辑控制四轴悬停的思路（老子终于调通了）

camion 于 2013-5-15 15:14 编辑



原帖在http://www.geek-workshop.com/thread-4556-1-2.html



对问题经过了反复的尝试，先写了一个纯粹模糊逻辑的算法， 不光各种 if elseif 老长，而且对三个指标任然控制不好，直到想出来了这个： （如要在matlab中运行，需要改注释//为%, 将elseif 改为 elsif， 将自定义的PorportionDet函数移到主代码外重新定义）



clear;

//basic para

L=0.5;

m=0.8;

J=2;

g=10;

delt_T=0.01;

N=900;



function result = PorportionDet(limA, limB, outA, outB, x)

a = (x - limB) / (limA-limB);

result = aoutA + (1-a)outB;

endfunction



//consigne

ver_vit_standard = 0;

hori_vit_standard = 0;

angle_standard =0;

//registres

F_basic = zeros(1,N);

F_rot = zeros(1,N);

command_basic= zeros(1,N);

command_rot = zeros(1,N);



angle_acc = zeros(1,N);

angle_vit = zeros(1,N);

angle = zeros(1,N);



hori_acc = zeros(1,N);

hori_vit = zeros(1,N);

hori = zeros(1,N);



vert_acc = zeros(1,N);

vert_vit = zeros(1,N);

vert = zeros(1,N);



error_angle = zeros(1,N);

error_vert_vit = zeros(1,N);



sum_error_angle = zeros(1,N);

sum_error_vert_vit = zeros(1,N);



error_hori_vit = zeros(1,N);

sum_error_hori_vit = zeros(1,N);

//PID paras

P_basic=2;

I_basic=3.5;

D_basic=0.2;



P_rot=16;

I_rot=0.02;

D_rot=13;



// time 1 sample

F_rot(1)= 4.9;

F_basic(1) =0.1;



angle(1) =0.1;

angle_vit(1) = 0.2;

angle_acc(1) = F_rot(1)L/J;

vert_vit(1) =1.2;

vert_acc(1) = ( 2F_basic(1)cos(angle(1)) - mg )/m ;

error_angle(1) = angle_standard - angle(1);

error_vert_vit(1) = ver_vit_standard- vert_vit(1);

sum_error_angle(1) = error_angle(1)delt_T;

sum_error_vert_vit(1) = error_vert_vit(1)delt_T;

hori_acc(1) = -( 2F_basic(1)sin(angle(1)) )/m ;

hori_vit(1)=0;



//recurrence

for i=2:N

F_rot(i)= command_rot(i-1);

F_basic(i)= command_basic(i-1);



angle_acc(i) = F_rot(i)L/J;

angle_vit(i) = angle_vit(i-1) + angle_acc(i-1)delt_T;

angle(i) = angle(i-1) + angle_vit(i-1)delt_T;



vert_acc(i) = ( 2F_basic(i)cos(angle(i)) - mg )/m ;

vert_vit(i) = vert_vit(i-1) + vert_acc(i-1)* delt_T;

vert(i) = vert(i-1) + vert_vit(i-1)delt_T;

error_vert_vit(i) = ver_vit_standard - vert_vit(i);

sum_error_vert_vit(i) = sum_error_vert_vit(i-1) + error_vert_vit(i)delt_T;



hori_acc(i) =-( 2F_basic(i)sin(angle(i)) )/m ;

hori_vit(i) = hori_vit(i-1) + hori_acc(i-1)delt_T;

hori(i) = hori(i-1) + hori_vit(i-1)delt_T;

error_hori_vit(i) = hori_vit_standard - hori_vit(i);



if ( hori_vit(i)> 3 )

angle_standard = 0.3;

error_angle(i) = angle_standard - angle(i);

sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;

command_rot(i)= P_roterror_angle(i) + D_rot( error_angle(i) - error_angle(i-1) )/delt_T + I_rotsum_error_angle(i);

command_basic(i) = I_basic sum_error_vert_vit(i) +P_basicerror_vert_vit(i) + D_basic(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;

elseif ( hori_vit(i)> 0.1 &hori_vit(i)< 3 )

angle_standard = PorportionDet(0.1, 3, 0.1, 0.3, angle(i) );

error_angle(i) = angle_standard - angle(i);

sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;

command_rot(i)= P_roterror_angle(i) + D_rot( error_angle(i) - error_angle(i-1) )/delt_T + I_rotsum_error_angle(i);

command_basic(i) = I_basic sum_error_vert_vit(i) +P_basicerror_vert_vit(i) + D_basic(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;

elseif ( hori_vit(i)> -0.1 &hori_vit(i)< 0.1 )

//angle_standard = PorportionDet(-0.1, 0.1, -0.1, 0.1, angle(i) );

angle_standard = 0;

error_angle(i) = angle_standard - angle(i);

sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;

command_rot(i)= P_roterror_angle(i) + D_rot( error_angle(i) - error_angle(i-1) )/delt_T + I_rotsum_error_angle(i);

command_basic(i) = I_basic sum_error_vert_vit(i) +P_basicerror_vert_vit(i) + D_basic(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;

elseif ( hori_vit(i)> -3 &hori_vit(i)< -0.1 )

angle_standard = PorportionDet(-3, -0.1, -0.3, -0.1, angle(i) );

error_angle(i) = angle_standard - angle(i);

sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;

command_rot(i)= P_roterror_angle(i) + D_rot( error_angle(i) - error_angle(i-1) )/delt_T + I_rotsum_error_angle(i);

command_basic(i) = I_basic sum_error_vert_vit(i) +P_basicerror_vert_vit(i) + D_basic(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;

elseif ( hori_vit(i)< -3 )

angle_standard = -0.3;

error_angle(i) = angle_standard - angle(i);

sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;

command_rot(i)= P_roterror_angle(i) + D_rot( error_angle(i) - error_angle(i-1) )/delt_T + I_rotsum_error_angle(i);

command_basic(i) = I_basic sum_error_vert_vit(i) +P_basicerror_vert_vit(i) + D_basic(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;

end



end



subplot(311);

plot (1:N, angle);

subplot(312);

plot (1:N, vert_vit);

subplot(313);

plot (1:N, hori_vit);



终于可以放心装飞机了

via - 极客工坊