MATLAB的PUMA560机械臂的正逆解及应用举例.zip

MATLAB关于PUMA 560机械臂的正逆解及应用举例，逆解每一个位姿可有8组逆解，可运用于轨迹规划，在轨迹规划时可进行筛选。正解与逆解配套。

%PUMA560正解

function cartesianVector = foward_Kinematics（joint_variables）

KTable = [ 0      0      -pi/2    joint_variables（11）;
           0      0.432   0       joint_variables（21）;
           0.149  0.02   pi/2    joint_variables（31）;
           0.433  0       -pi/2   joint_variables（41）;
           0      0       pi/2    joint_variables（51）;
           0      0       0       joint_variables（61）;];
       
A = zeros（446）; 

for i = 1:1:6
    A（::i） = [cos（KTable（i4））   -sin（KTable（i4））*cos（KTable（i3））    sin（KTable（i4））*sin（KTable（i3））  KTable（i2）*cos（KTable（i4））;
                sin（KTable（i4））    cos（KTable（i4））*cos（KTable（i3））   -cos（KTable（i4））*sin（KTable（i3））  KTable（i2）*sin（KTable（i4））;
                0                   sin（KTable（i3））                     cos（KTable（i3））                   KTable（i1）;
                0                   0                                    0                                  1;];
            
    for j = 1:1:i
        if j == 1
            T（::i） = A（::1）;            
        end
        
        if j>1
            T（::i） = T（::i）*A（::j）;
        end
    end
end


T6 = zeros（44）;
T6 = T（::6）;    % T6 是 D-H 模型所求的结果

x = T6（14）;
y = T6（24）;
z = T6（34）;



% EulerAngle = zeros（13）;
% if T6（abs（T6（13）） == 0 && abs（T6（23） == 0））
%     if T6（33）>0
%         EulerAngle（12） = 0; % theta = 0
%     else
%         EulerAngle（12） = 180; % theta = 180
%     end
%     EulerAngle（13） = 0;
%     if （（T6（21）==0） && （T6（11）==0））
%         EulerAngle（11） = 0;
%     else
%         EulerAngle（11） = atan2（T6（21）T6（11））;
%     end
% else
%         EulerAngle（11） = atan2（T6（23）T6（13））; 
%     
%         if （cos（EulerAngle（11））*T6（13）+sin（EulerAngle（11））*T6（23））*T6（33） == 0
%             EulerAngle（12） = 0;     
%         else
%             EulerAngle（12） = atan2（（cos（EulerAngle（11））*T6（13）+sin（EulerAngle（11））*T6（23））T6（33））;
%         end
%     
%         if （-sin（EulerAngle（11））*T6（11）+cos（EulerAngle（11））*T6（21））*（ -sin（EulerAngle（11）*T6（12））+cos（EulerAngle（11））*T6（22） ） == 0
%             EulerAngle（13） = 0;
% 
%         else
%             EulerAngle（13） = atan2（  （-sin（EulerAngle（11））*T6（11）+cos（EulerAngle（11））*T6（21））  （ -sin（EulerAngle（11）*T6（12））+cos（EulerAngle（11））*T6（22） ）  ）;
%         
%         end
%     
% end
% 
% phy = EulerAngle（11）;
% theta = EulerAngle（12）;
% psi = EulerAngle（13）;

cartesianVector = [x y z];
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2020-04-27 01:34  PUMA560机械臂的正逆解\
     文件        2575  2020-04-10 13:07  PUMA560机械臂的正逆解\foward_Kinematics.m
     文件        9770  2020-04-11 17:38  PUMA560机械臂的正逆解\inverse_Kinematics.m
     文件         696  2020-04-27 01:32  PUMA560机械臂的正逆解\PUMA560.m
     文件         122  2020-04-27 01:38  PUMA560机械臂的正逆解\说明.txt