%机械手臂的控制PD+PD前馈控制+PD修正前馈控制
%机械手臂的控制PD+PD前馈控制+PD修正前馈控制
function a=PD_control_feedforward()
clc
clear all
close all
global F
global Select
global kp1 kv1 kp2 kv2
%%%%%%%%%%%%%%%%%%%%initial vallue and set the parameter of system %%%%%%%%
F=2; %tack the two locus
Select=3; %select the thrid method as control law
%------------------------PD参数的调整是核心--------------------------%
kp1=3000;kv1=230; %PD parameters
kp2=2000;kv2=210;
%-------------------------------------------------------------%
ts=0.001; Tmax=5;Timeset=; %仿真时间和步长
X0=; %初始状态
para=[];
=ode45(@fun,Timeset,X0,[],para); %'fun' 代表文件名。@fun代表函数名
switch F
case 1
r1=pi/6;r2=pi/6;
dr1=0;dr2=0;
ddr1=0;ddr2=0;
case 2
r1=sin(3*t);r2=cos(3*t);
dr1=3*cos(3*t);dr2=-3*sin(3*t);
ddr1=-3*3*sin(3*t);ddr2=-3*3*cos(3*t);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%结果输出%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure(1)
plot(t,r1,'b',t,y(:,1),'r');grid on %关节角1
legend('angle_one');
%figure(2)
%plot(t,dr1,'b',t,y(:,2),'r');grid on
%legend('dangle one');
figure(3)
plot(t,r2,'b',t,y(:,3),'r');grid on %关节角2
legend('angle two');
%figure(4)
%plot(t,dr2,'b',t,y(:,4),'r');grid on
%legend('dangle two');
end
function dx=fun(t,x,flag,para)
global F
global Select
global kp1 kv1 kp2 kv2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%system model%%%%%%%%%%%%%%%%%%%%%%%%%%%
%be control model 模型准备M*ddq+C*dq+G=Tao
x1=x(1);x3=x(3);x2=x(2);x4=x(4);
D11=2.462;D22=0.362;D12=0.147;D21=0.147;
M=;
C=[-x4*sin(x3),-(x2+x4)*sin(x3);x2*sin(x3),0];
G=;
%tracking locus
switch F
case 1
r1=pi/6;r2=pi/6;
dr1=0;dr2=0;
ddr1=0;ddr2=0;
case 2
r1=sin(3*t);r2=cos(3*t);
dr1=3*cos(3*t);dr2=-3*sin(3*t);
ddr1=-3*3*sin(3*t);ddr2=-3*3*cos(3*t);
end
%性能指标
dr=;
ddr=;
e1=x(1)-r1;de1=x(2)-dr1;
e2=x(3)-r2;de2=x(4)-dr2;
%%%%%%%%%%%%%%%%%%%%%%%%计算输出力矩%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch Select
case 1 %PD控制
u1=-kp1*e1-kv1*de1;
u2=-kp2*e2-kv2*de2;
case 2 %PD+前馈控制
u1=-kp1*e1-kv1*de1+M(1,:)*ddr+C(1,:)*dr+G(1);
u2=-kp2*e2-kv2*de2+M(2,:)*ddr+C(2,:)*dr+G(2);
case 3 %PD控制+修正的前馈控制
x1=r1;x2=dr1;x3=r2;x4=dr2;
Md=;
Cd=[-x4*sin(x3),-(x2+x4)*sin(x3);x2*sin(x3),0];
Gd=;
u1=-kp1*e1-kv1*de1+Md(1,:)*ddr+Cd(1,:)*dr+Gd(1);
u2=-kp2*e2-kv2*de2+Md(2,:)*ddr+Cd(2,:)*dr+Gd(2);
end
tol=; %每步仿真的计算控制力矩输出
%%%%%%%%%%%%%%%%%%%%%求解加速度导数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Q=inv(M)*(tol-C*)-G; %根据动力学模型建立的方程
dx(1)=x(2); %一阶微分方程组结果
dx(2)=Q(1);
dx(3)=x(4);
dx(4)=Q(2);
dx=dx.'; %结果输出必须为列向量
end
页:
[1]