二关节机械手PD控制simulink仿真

 simulink主程序

 

 其中的PDcontrol函数表示的是PD控制器，PD控制器共六个输入，分别是关节1、关节2的理想关节角，实际关节角，关节角速度；共两个输出，为关节1、关节2的驱动力矩。PD控制器的控制律为：

 PD控制器子程序：——PDcontrol

%% 控制器程序
function[sys,x0,str,ts]=spacemodle(t,x,u,flag)

switch flag,
case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
case 3,
    sys=mdlOutputs(t,x,u);
case {2,4,9}
    sys=[];
otherwise
    error(['Unhandled flag = ',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 2;%输出变量为驱动力矩，t1，t2
sizes.NumInputs      = 6;%输入变量为qd1，qd2，q1，q1的导数，q2，q2的导数
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1;
sys = simsizes(sizes);
x0  = [];
str = [];
ts  = [0,0];

function sys=mdlOutputs(t,x,u)  %描述控制器输出
R1=u(1);dr1=0;%u1和u2为两个关节的理想位置qd1和qd2
R2=u(2);dr2=0;

x(1)=u(3);%关节1的实际位置
x(2)=u(4);%关节1的速度
x(3)=u(5);%关节2的位置
x(4)=u(6);%关节2的速度

e1=R1-x(1);
e2=R2-x(2);
e=[e1;
    e2];

de1=dr1-x(2);
de2=dr2-x(4);
de=[de1;
    de2];

kp=[30,0;
    0,30];
kd=[30,0;
    0,30];

tol=kp*e+kd*de;%输出两个关节的驱动力矩

sys(1)=tol(1);
sys(2)=tol(2);

二自由度机械臂动力学模型：

 机械臂模型子程序：——s_function

%% 被控对象——二自由度机械臂动力学模型
function[sys,x0,str,ts]=s_function(t,x,u,flag)

switch flag,
case 0,
    [sys,x0,str,ts]=mdlInitializeSizes;
case 1,
     sys=mdlDerivatives(t,x,u);
case 3,
    sys=mdlOutputs(t,x,u);
case {2,4,9}
    sys=[];
otherwise
    error(['Unhandled flag = ',num2str(flag)]);
end

%% mdlInitializeSize初始化
function [sys,x0,str,ts]=mdlInitializeSizes
global p g   %定义全局变量
sizes = simsizes;
sizes.NumContStates  = 4;%连续状态变量个数为4个
sizes.NumDiscStates  = 0;
sizes.NumOutputs     = 4;%输出变量为q1，q1的导数，q2，q2的导数
sizes.NumInputs      = 2;%输入变量为驱动力矩t1，t2
sizes.DirFeedthrough = 0;
sizes.NumSampleTimes = 0;
sys = simsizes(sizes);
x0  = [0 0 0 0];
str = [];
ts  = [];

p=[2.9 0.76 0.87 3.04 0.87];
g=9.8;

%微分方程描述，用该函数了描述被控对象和自适应律
function sys=mdlDerivatives(t,x,u)
global p g
%使用全局变量
q=[x(1);x(3)];
dq=[x(2);x(4)];
DO=[p(1)+p(2)+2*p(3)*cos(q(2)),p(2)+p(3)*cos(q(2));
   p(2)+p(3)*cos(q(2)),p(2) ];
%惯性矩阵

CO=[-p(3)*dq(2)*sin(q(2)) ,-p(3)*(dq(1)+dq(2))*sin(q(2));
    p(3)*dq(1)*sin(q(2)) ,0];
%科氏力与向心力矩阵

tol=u(1:2);
%输入两个关节的驱动力矩
%tol=u(1:2);
% dq=[x(2);x(4)];
%状态变量q导数

s=inv(DO)*(tol-CO*dq);%由机械臂的动力学方程，得到二阶导数
sys(1)=x(2);
sys(2)=s(1);
sys(3)=x(4);
sys(4)=s(2);

function sys=mdlOutputs(t,x,u)
sys(1)=x(1);
sys(2)=x(2);
sys(3)=x(3);
sys(4)=x(4);

绘图子程序

close all;
figure(1);
subplot(211);
plot(t,x1(:,1),'r',t,x1(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 1');
subplot(212);
plot(t,x2(:,1),'r',t,x2(:,2),'b');
xlabel('time(s)');ylabel('position tracking of link 2');

figure(2);
subplot(211);
plot(t,tol(:,1),'r')
xlabel('time(s)');ylabel('tol1');
subplot(212);
plot(t,tol(:,2),'r')
xlabel('time(s)');ylabel('tol2');