PUMA560机器人工具箱运动控制A:路径规划-运动学

时间:2022-07-25
本文章向大家介绍PUMA560机器人工具箱运动控制A:路径规划-运动学，主要内容包括其使用实例、应用技巧、基本知识点总结和需要注意事项，具有一定的参考价值，需要的朋友可以参考一下。

以下主要给出PUMA560构型机械臂的运动控制仿真。

1. 机器人定义

在机器人工具箱matlab robot toolbox 中给出机器人定义：

clear;
clc;
%建立机器人模型
% theta d a alpha offset
SL1=Link([0 0 0.150 -pi/2 0 ],'standard'); 
SL2=Link([0 0 0.700 0 0 ],'standard');
SL3=Link([0 0 0.109 -pi/2 0 ],'standard');
SL4=Link([0 0.600 0 pi/2  0 ],'standard');
SL5=Link([0 0 0 -pi/2 0 ],'standard');
SL6=Link([0 0.065 0 0 0 ],'standard');
robot=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','robot');
figure(1),teach(starobot);

得到其三维视图如下所示：

Robot model

2. 模型验证——正向运动学/逆向运动学验证

A 假设机械臂关节角度是q=[0, -pi/2, pi/3, pi/3, pi/2, pi/5],采用robot1.fkine(q)得到机械臂末端的齐次变换矩阵为

T= [-0.0363 -0.9007 -0.4330 0.5163； -0.2939 -0.4045 0.8660 0.0563； -0.9551 0.1587 -0.2500 0.2186； 0 0 0 1.0000]

采用逆向运动学计算得到机械臂的关节角度是qr=[0， -pi/2 ，pi/3 ，pi/3 ，pi/2， pi/5],从而正向运动学和反向运动学得到有效验证

B假设机械臂关节角度是q= [0， -pi/3， pi/6， 0， 0， 0],采用robot1.fkine(q)得到机械臂末端的齐次变换矩阵为

T=[ 0.8660 0.0000 0.5000 0.9269; -0.0000 -1.0000 0.0000 0.0000; 0.5000 -0.0000 -0.8660 0.0848; 0 0 0 1.0000]

采用逆向运动学计算得到机械臂的关节角度是qr= [0， -pi/3， pi/6， 0， 0， 0],从而正向运动学和反向运动学得到有效验证

3 基于样条曲线的关节空间轨迹规划（笛卡尔点到点的控制）

假设空间机械臂的经过的末端位置对应的T矩阵分别是

tt1 =[ 0.5000 0.0000 0.8660 1.1304; -0.0000 -1.0000 0.0000 0.0000; 0.8660 -0.0000 -0.5000 0.3681; 0 0 0 1.0000];

tt2 =[ 0.1782 -0.5762 0.7976 0.9353; -0.7615 -0.5942 -0.2592 -0.3039; 0.6233 -0.5612 -0.5446 0.3950; 0 0 0 1.0000]

tt3 =[ -0.0062 -0.9938 0.1106 0.6058; -0.9938 -0.0062 -0.1106 -0.6058; 0.1106 -0.1106 -0.9877 -0.1448; 0 0 0 1.0000]

tt4 =[ -0.2706 -0.5938 -0.7578 0.0963; -0.7813 0.5954 -0.1875 -0.2643; 0.5625 0.5413 -0.6250 -0.0850; 0 0 0 1.0000]

由此得到机器人轨迹

机械臂关节轨迹
机械臂关节速度
机械臂笛卡尔空间轨迹

如上图所示，机械臂末端经过了预设的四个点，且在此过程中，关节空间的轨迹为经过样条曲线规划的轨迹。

4 基于样条曲线的笛卡尔空间轨迹规划（笛卡尔连续路径规划）

4.1 直线运动

假设机械臂从初始位置A到达B点

A点的变换矩阵为

[0.5000 0.0000 0.8660 1.1304; -0.0000 -1.0000 0.0000 0.0000; 0.8660 -0.0000 -0.5000 0.3681; 0 0 0 1.0000]

B点的变换矩阵为

[0.8367 -0.5476 -0.0134 0.5304; -0.5475 -0.8368 0.0083 -0.3500; -0.0158 0.0003 -0.9999 -0.0319; 0 0 0 1.0000]

在此过程中，机械臂末端从[1.1304 0 0.3681]到[0.5304 -0.35 -0.3681]

直线运动
关节轨迹
笛卡尔轨迹
clear;
clc;
%建立机器人模型
%       theta    d           a        alpha     offset
SL1=Link([0       0          0.150    -pi/2        0     ],'standard'); 
SL2=Link([0       0          0.700     0           0     ],'standard');
SL3=Link([0       0          0.109    -pi/2        0     ],'standard');
SL4=Link([0       0.600       0        pi/2        0     ],'standard');
SL5=Link([0       0           0       -pi/2        0     ],'standard');
SL6=Link([0       0.065       0        0           0     ],'standard');
robot1=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','robot');
% figure(1),teach(robot1);
%% verification of kinematics
qq1=[0 -pi/3 0 0  0  0]
tt1=robot1.fkine(qq1)
q1r=180/pi*robot1.ikine(tt1,qq1,[1 1 1 1 1 1])
robot1.plot(qq1)
n=100;
[xd,dxd,ddxd]=jtraj(0,-0.6,n);
[yd,dyd,ddyd]=jtraj(0,-0.35,n);
[zd,dzd,ddzd]=jtraj(0,-0.4,n);
tt1xyz=tt1;
q1_save=qq1;
for i=1:n
    tt1xyz(1,4)=tt1(1,4)+xd(i);
    tt1xyz(2,4)=tt1(2,4)+yd(i);
    tt1xyz(3,4)=tt1(3,4)+zd(i);
    q1_save=robot1.ikine(tt1xyz,q1_save,[1 1 1 0 0 0])
    q1r_save(i,1)=q1_save(1);
    q1r_save(i,2)=q1_save(2);
    q1r_save(i,3)=q1_save(3);
    q1r_save(i,4)=q1_save(4);
    q1r_save(i,5)=q1_save(5);
    q1r_save(i,6)=q1_save(6);
 end
%
robot1.fkine(q1r_save(1,1:6))
robot1.fkine(q1r_save(end,1:6))
pause(2)
for i=1:5:n
    i=n
    robot1.plot(q1r_save(i,:));
    hold on;plot3(tt1(1,4)+xd,tt1(2,4)+yd,tt1(3,4)+zd,'r:','LineWidth',2);
    pause(0.3);
    grid on
end
figure(111)
t1=linspace(0,10,n);grid on
plot(t1,q1r_save(:,1)*180/pi,'r:','LineWidth',2);hold on
plot(t1,q1r_save(:,2)*180/pi,'b:','LineWidth',2);
plot(t1,q1r_save(:,3)*180/pi,'k:','LineWidth',2);
plot(t1,q1r_save(:,4)*180/pi,'r-.','LineWidth',2);
plot(t1,q1r_save(:,5)*180/pi,'b','LineWidth',2);
plot(t1,q1r_save(:,6)*180/pi,'k-.','LineWidth',2);
grid on
legend('q1','q2','q3','q4','q5','q6')
xlabel('time(s)')
ylabel('关节角度（deg）')

figure(12)
plot3(tt1(1,4)+xd,tt1(2,4)+yd,tt1(3,4)+zd,'b','LineWidth',2);
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on

figure(13)
plot(t1,dxd,'r','LineWidth',2);hold on
plot(t1,dyd,'b-.','LineWidth',2);
plot(t1,dzd,'k:','LineWidth',2);grid on
xlabel('time(s)')
ylabel('末端速度（m/s）')

4.2 圆弧运动

假设机械臂从初始位置A到达B点

A点的变换矩阵为

[0.3606 0.4045 0.8404 1.1304; 0.1595 -0.9145 0.3717 0.5000; 0.9190 0.0000 -0.3943 0.3681; 0 0 0 1.0000]

B点的变换矩阵为

[0.4244 -0.4198 0.8023 1.0598; -0.1895 -0.9076 -0.3746 -0.4950; 0.8854 0.0069 -0.4648 0.3681; 0 0 0 1.0000]

在此过程中，机械臂末端从[1.1304 0 0.3681]到[1.0598 -0.4950 0.3681]

圆弧运动
关节角度
clear;
clc;
%建立机器人模型
%       theta    d           a        alpha     offset
SL1=Link([0       0          0.150    -pi/2        0     ],'standard'); 
SL2=Link([0       0          0.700     0           0     ],'standard');
SL3=Link([0       0          0.109    -pi/2        0     ],'standard');
SL4=Link([0       0.600       0        pi/2        0     ],'standard');
SL5=Link([0       0           0       -pi/2        0     ],'standard');
SL6=Link([0       0.065       0        0           0     ],'standard');
robot1=SerialLink([SL1 SL2 SL3 SL4 SL5 SL6],'name','robot');
% figure(1),teach(robot1);
%% verification of kinematics
qq1=[0 -pi/3 0 0  0  0]
tt1=robot1.fkine(qq1)
q1r=180/pi*robot1.ikine(tt1,qq1,[1 1 1 1 1 1])
robot1.plot(qq1)
n=100;
tx=linspace(0,10,n);
xd=-0.5*sin(0.3*tx);
zd=0.5*cos(0.3*tx);

tt1xyz=tt1;
q1_save=qq1;
for i=1:n
    tt1xyz(1,4)=tt1(1,4)+xd(i);
    tt1xyz(2,4)=tt1(2,4)+zd(i);
%     tt1xyz(3,4)=tt1(3,4)+zd(i);
    q1_save=robot1.ikine(tt1xyz,q1_save,[1 1 1 0 0 0])
    q1r_save(i,1)=q1_save(1);
    q1r_save(i,2)=q1_save(2);
    q1r_save(i,3)=q1_save(3);
    q1r_save(i,4)=q1_save(4);
    q1r_save(i,5)=q1_save(5);
    q1r_save(i,6)=q1_save(6);
 end
robot1.fkine(q1r_save(1,1:6))
robot1.fkine(q1r_save(end,1:6))
pause(2)
for i=1:5:n
    i=1
    robot1.plot(q1r_save(i,:));
    hold on;plot3(tt1(1,4)+xd,tt1(2,4)+zd,tt1(3,4)+0*zd,'r:','LineWidth',2);
    pause(0.3);
    grid on
end
figure(111)
t1=linspace(0,10,n);grid on
plot(t1,q1r_save(:,1)*180/pi,'r:','LineWidth',2);hold on
plot(t1,q1r_save(:,2)*180/pi,'b:','LineWidth',2);
plot(t1,q1r_save(:,3)*180/pi,'k:','LineWidth',2);
plot(t1,q1r_save(:,4)*180/pi,'r-.','LineWidth',2);
plot(t1,q1r_save(:,5)*180/pi,'b','LineWidth',2);
plot(t1,q1r_save(:,6)*180/pi,'k-.','LineWidth',2);
grid on
legend('q1','q2','q3','q4','q5','q6')
xlabel('time(s)')
ylabel('关节角度（deg）')

figure(12)
plot3(tt1(1,4)+xd,tt1(2,4)+yd,tt1(3,4)+0*zd,'b','LineWidth',2);
xlabel('X')
ylabel('Y')
zlabel('Z')
grid on
% figure(13)
% plot(t1,dxd,'r','LineWidth',2);hold on
% plot(t1,dyd,'b-.','LineWidth',2);
% plot(t1,dzd,'k:','LineWidth',2);grid on
% xlabel('time(s)')
% ylabel('末端速度（m/s）')

关键词 ：机器人运动学 机器人动力学 路径规划 机器人工具箱 matlab代码 圆弧 直线

随机文章
本站知识点必读
最近更新