串联操作臂的运动学方程和力/位置控制程序 （Force Position control of robot）

串联操作臂的运动学方程和力/位置控制程序。

%  A prorgam to find the torques of three joints for movement of
%  end-effector from source location to goal location
%  PID controller is used to control the torqe calculation and movement
%  cubic polynomial velocity principle is applied for trajectory Planning
%  By : Pranaya Ranjan Behera Electronics and Instrumentation Engg.
%  
%--------------------------------------------------------------------------

clear all;
clc
g=9.8 ;          % in mitre/sec^2
m2= 12;             % m2 and m3 are in Kg; mass m1=0;
m3= 8;
l= .68;           % length in mitre  length1=0; length2=length3=l;
kp= 1;
kv= 5;

ls=4*l*l; 
count=0;
while（count==0）
disp（‘Insert the source location of end-effector（in centi-miter）‘）
xs1= input（‘Xs=‘）;
ys1= input（‘Ys=‘）;
zs1= input（‘Zs=‘）;  
xs=xs1/100; ys=ys1/100;  zs=zs1/100;  % converting into miter
% check singularity
ds2=（xs^2）+（ys^2）+（zs^2）;
if （ls    disp（‘*** *** *** That is a Singular Point *** *** ***‘）; 
else
    count=1;
end   
end

while（count==1）
disp（‘Insert the destination（goal） location of end-effector（in centi-miter）‘）
xg1= input（‘Xg=‘）;
yg1= input（‘Yg=‘）;
zg1= input（‘Zg=‘）;    
xg=xg1/100; yg=yg1/100;  zg=zg1/100; % converting into miter
% check singularity
dg2=（xg^2）+（yg^2）+（zg^2）;
if （ls    disp（‘*** *** *** That is a Singular Point *** *** ***‘）;
else
    count=0;
end
end
     
while（count==0）     
t= input（‘Insert time of movement（in seconds）=‘）;
if （t<.999）    % time of travel should be minimun 1sec.
    disp（‘*** *** *** Time should be at least 1 second *** *** ***‘）;
else
    count=1;
end
end


%  1 unit = 50ms i.e.  each 50ms torques will be caculated

qs1=atan2（ysxs）

qs3=acos（（（（（xs* cos（qs1））+ （ys* sin（qs1）））^2） + （zs^2）-（2*l*l） ）/ （2*l*l））

qs2=asin（（（zs*（1+cos（qs3））^2）-（（（zs*（1+cos（qs3））^2）^2）- 2*（1+cos（qs3））*（（zs^2）-（l^2）*（sin（qs3））^2））^0.5）/（2*l*（1+cos（qs3））））

%----------------------------------------------------------------

qg1=atan2（ygxg）

qg3=acos（（（（（xg* cos（qg1））+ （yg* sin（qg1）））^2） + （zg^2）-（2*l*l） ）/ （2*l*l））

qg2=asin（（（zg*（1+cos（qg3））^2）-（（（zg*（1+cos（qg3））^2）^2）- 2*（1+cos（qg3））*（（zg^2）-（l^2）*（sin（qg3））^2））^0.5）/（2*l*（1+cos（qg3））））


%-----------------------------------------------------------------
q1=q_cubic_fn（qs1 qg1 t）;
q2=q_cubic_fn（qs2 qg2 t）;
q3=q_cubic_fn（qs3 qg3 t）;

qd11=qd_cubic_fn（qs1 qg1 t）;
qd22=qd_cubic_fn（qs2 qg2 t）;
qd33=qd_cubic_fn（qs3 qg3 t）;

qdd1=qdd_cubic_fn（qs1 qg1 t）;
qdd2=qdd_cubic_fn（qs2 qg2 t）;
qdd3=qdd_cubic_fn（qs3 qg3 t）;



 for i=1:20*t
     qd1=qd11（i）;
     qd2=qd22（i）;
     qd3=qd33（i）;
     qs1= qd1^2;
     qs2= qd2^2;
     qs3= qd3^2;           %  square of qd ;
     
     c1= cos（q1（i））;
     s1= sin（q1（i））;
     c2= cos（q2（i））;
     s2= sin（q2（i））;
     c3= cos（q3（i））;
     s3= sin（q3（i））;
     s223=sin（q3（i）+ q2（i）+q2（i））;
     c12= cos（q1（i）+ q2（i））;
     s12= sin（q1（i）+ q2（i））;
     c23= cos（q3（i）+ q2（i））;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       5485  2010-04-16 18:30  Force Position control of a manipulator\main_program.m

     文件       1331  2014-03-19 14:54  Force Position control of a manipulator\Matlab_Program_1.m

     文件        582  2010-03-31 19:54  Force Position control of a manipulator\qdd_cubic_fn.m

     文件        589  2010-03-31 19:55  Force Position control of a manipulator\qd_cubic_fn.m

     文件       1494  2010-04-16 21:16  Force Position control of a manipulator\qd_trap_fn.m

     文件        578  2010-04-01 19:58  Force Position control of a manipulator\q_cubic_fn.m

     文件    1495267  2010-04-16 18:29  Force Position control of a manipulator\report.docx

    ..A..H.       162  2014-03-19 14:43  Force Position control of a manipulator\~$report.docx

     目录          0  2014-08-06 00:00  Force Position control of a manipulator

----------- ---------  ---------- -----  ----

              1505488                    9