首先检讨一下,该主题本应该发到四轴板块,但考虑到这边人气较多,所以。。。。
我的故事是这样的:刚刚第一个月拿到工资就到淘宝上买了无刷电机、电调、高倍率电池等等做四轴飞行器,现在快递还没到我用matlab做了一个四轴自平衡的控制仿真,用了PID算法(模糊逻辑也写了一个,对倾角自平衡控制也不错),然后出现了我自己技术能力有限的问题,只好来请教这边的神们。
我在以下代码中做了一对浆的自平衡,需要平衡的对象是 1 倾角 2 垂直速度 3 水平速度
控制量自己设的两个 F_basic(t) 和 F_rot(t), 其中左浆升力大小=F_basic(t)+F_rot(t) , 右浆升力= F_basic(t) - F_rot(t)
在我的模拟中,倾角 和 垂直速度 都可以在较短的时间内达到要求值(即平衡状态 0), 但是由于有两个控制量和三个输出量,水平速度一直无法控制为零。
上代码 (scilab写的, 要在matlab中跑只需将 注释 // 改为%)
clear;
//basic para
L=0.5;
m=0.8;
J=2;
g=10;
delt_T=0.01;
N=400;
//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) = ( 2*F_basic(1)*cos(angle(1)) - m*g )/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) = ( 2*F_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;
error_angle(i) = angle_standard - angle(i);
sum_error_angle(i) = sum_error_angle(i-1) + error_angle(i)* delt_T;
vert_acc(i) = ( 2*F_basic(i)*cos(angle(i)) - m*g )/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) = ( 2*F_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);
command_rot(i)= P_rot*error_angle(i) + D_rot*( error_angle(i) - error_angle(i-1) )/delt_T + I_rot*sum_error_angle(i);
command_basic(i) = I_basic* sum_error_vert_vit(i) +P_basic*error_vert_vit(i) + D_basic*(error_vert_vit(i)-error_vert_vit(i-1))/delt_T ;
end
subplot(311);
plot (1:N, angle);
subplot(312);
plot (1:N, vert_vit);
subplot(313);
plot (1:N, hori_vit);
能否给出建议如何解决 ? 如解决我很快就可以将控制算法写成arduino的c语言,卡尔曼滤波都是现成的
|