二关节机械臂计算力矩跟踪控制

二关节机械臂计算力矩跟踪控制，包括阶跃信号和正弦信号，程序能完好运行，并画出跟踪效果

clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Model of three-link robot （“Inverse Control of a two-link manipulator“ 2001）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
l   = [0.20.3]; %length of link i （each colum corresponds to one robot）
lc  = [0.10.15]; %distances from previous joint to CM of link i （each colum corresponds to one robot）
m   = [11.5]; %mass of inertia of link i
I   = [0.0130.045]; %moment of inertia of link i
%constant part of parameters
Theta1 = m（1）*lc（1）^2+m（2）*（l（1）^2+lc（2）^2）+I（1）+I（2）; % ;
Theta2 = m（2）*l（1）*lc（2）;                              % ;
Theta3 = m（2）*lc（2）^2+I（2）;                            % ;
Theta4 = m（1）*lc（1）+m（2）*l（1）;
Theta5 = m（2）*l（2）;
Theta  = [Theta1; Theta2; Theta3; Theta4; Theta5];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%paremeters of the graph with 4 leaders and 6 followers
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%control paremeter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
k_p=3;
k_v=2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%initial data of six two-link robots
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%qd=[0.3;0.1];                        %%the reference position
%vd0=[0.1;0.1]*0;                     %%initial value of the velocity estimtor
q0=[0;0]; %intitial positions of the followers    
% for i=1:6
% dq0（:i） = [0.2*i-0.5; -0.1*i+0.5]; %intitial velocities of the followers
% end
dq0=[0 ; 0];
%s0=zeros（36）;
%zd=0;
g0=9.8;                               %%g is the gravitational acceleration

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-11-15 10:02  机械臂计算力矩控制\
     目录           0  2018-11-15 10:02  机械臂计算力矩控制\Tracking Control\
     文件       20480  2017-12-08 17:45  机械臂计算力矩控制\Tracking Control\Thumbs.db
     文件        1899  2017-12-27 10:24  机械臂计算力矩控制\Tracking Control\initialcase2.m
     文件         178  2010-02-16 08:32  机械臂计算力矩控制\Tracking Control\invM.m
     文件         819  2017-12-29 00:20  机械臂计算力矩控制\Tracking Control\plotcase1.m
     文件         880  2017-12-29 00:11  机械臂计算力矩控制\Tracking Control\plotcase2.m
     文件         723  2017-12-27 10:28  机械臂计算力矩控制\Tracking Control\plotcase3.m
     文件       18229  2017-12-27 10:28  机械臂计算力矩控制\Tracking Control\trackingcontrol.slx
     目录           0  2018-11-15 10:02  机械臂计算力矩控制\Tracking Control Sin（x）\
     文件       20480  2017-12-08 17:45  机械臂计算力矩控制\Tracking Control Sin（x）\Thumbs.db
     文件        1899  2017-12-27 10:30  机械臂计算力矩控制\Tracking Control Sin（x）\initialcase2.m
     文件         178  2010-02-16 08:32  机械臂计算力矩控制\Tracking Control Sin（x）\invM.m
     文件         819  2017-12-29 00:32  机械臂计算力矩控制\Tracking Control Sin（x）\plotcase1.m
     文件         880  2017-12-08 18:10  机械臂计算力矩控制\Tracking Control Sin（x）\plotcase2.m
     文件       18464  2017-12-27 10:39  机械臂计算力矩控制\Tracking Control Sin（x）\trackingcontrolsin.slx