双臂Matlab仿真建模
文章目录
双臂Matlab仿真建模
写在前面
学习代码都记录在[个人github](https://github.com/xuuyann/RobotLearningCode)上,欢迎关注~
感谢我对象刘博士的大力支持(〃'▽'〃)(主要是源码)~知乎:[OpenRobotSL](https://www.zhihu.com/people/OpenRobotSL)
PUMA560构型双臂
UR构型双臂
如何进行轨迹仿真
Matlab正运动学建模
PUMA560构型
UR构型
最后就可以愉快的画轨迹了~(当然 过程挺艰辛)
写在前面
学习代码都记录在个人github上,欢迎关注~
感谢我对象刘博士的大力支持(〃‘▽’〃)(主要是源码)~知乎:OpenRobotSL

实际上,如果就把双臂中的每个臂当做单个的机械臂进行规划,那么用matlab进行双臂建模就没有太大必要,只需要对每个单臂进行单独规划即可。但是,如果涉及到双臂之间的协同轨迹规划,如上图所示,这时用matlab进行双臂建模仿真就会显得事半功倍。本文先只介绍双臂在matlab中的正运动学建模,后续会补充在matlab中如何进行双臂轨迹规划。

PUMA560构型双臂
熟悉Peter大神工具箱的都知道,该工具箱使用dh连杆建模,建立双臂模型,需要建立腰关节连杆,并将两个单机械臂组合到腰关节上。如下图所示,本文针对puma560构型双臂采用改进DH建模(具体建模步骤见博客机器人学回炉重造(1):正运动学、标准D-H法与改进D-H法的区别与应用(附ABB机械臂运动学建模matlab代码)),此时腰关节连杆坐标系就是基坐标系X0Y0Z0,此时的肩关节坐标系X2Y2Z2就是单臂机器人的基坐标系,然后就从肩关节坐标系开始建立单臂的DH坐标系,也可以认为是单独的腰关节连杆坐标系+单臂的DH坐标系,需要注意的是建立整体DH坐标系时腰关节与肩关节之间需要加-pi/2角度偏置,目的是将puma560构型的机械臂垂下去,下图中的虚线X2即为不加偏置的肩关节坐标系,实线X2即为加了偏置角度后的肩关节坐标系,d=肩宽/2。

在这里插入图片描述

对应的matlab正运动学仿真如下所示:

在这里插入图片描述

腰关节与肩关节之间也可以不加偏置角度,这样的话,两个坐标系之间的转换就可以画成如下所示。

在这里插入图片描述

对应的matlab正运动学仿真如下所示:

在这里插入图片描述

UR构型双臂

本文UR采用标准DH建模,建模过程全网可搜,标准DH坐标系建立如下。

在这里插入图片描述

分析上图,黑色坐标系X0Y0Z0是世界坐标系,也可认为这是腰关节坐标系,红色虚线坐标系X1Y1Z1是肩关节坐标系(也就是UR的基坐标系{0}),此时腰关节与肩关节不存在角度偏置,matlab仿真建模就是如下所示

在这里插入图片描述

亮蓝色实线坐标系X1Y1Z1是加了pi/2偏置角度后的肩关节坐标系,matlab建模仿真如下所示

在这里插入图片描述

如何进行轨迹仿真
进行轨迹仿真的最重要一点就是,如何将世界(全局坐标系)坐标系下(也为腰关节坐标系)的轨迹映射到肩关节坐标系(机械臂的基坐标系)下,弄明白这一点,使用matlab进行双臂轨迹算法验证就简单很多了~

以PUMA560腰关节与肩关节有-pi/2角度偏置的双臂构型为例,如下图所示,



Matlab正运动学建模

PUMA560构型

clear all;
clc;
% 双臂 每个单臂是puma560构型
L(1)=Link([0       -0.25        0        pi/2      0     -pi/2  ],'modified'); % 关节1这里的最后一个量偏置-pi/2设置是为了将puma560构型的臂垂直下去
L(2)=Link([0       0          0        -pi/2      0    0  ],'modified');
L(3)=Link([0       0           0.2        0          0    -pi/2 ],'modified');
L(4)=Link([0       0.248        0           -pi/2      0     ],'modified');
L(5)=Link([0       0           0           -pi/2       0     ],'modified');
L(6)=Link([0       0            0           pi/2      0     ],'modified');
%                  0.262
p560L=SerialLink(L,'name','LEFT');
p560L.tool=[0 -1 0 0;
               1 0 0 0;
               0 0 1 0.262 ;
               0 0 0 1;]; 
           
R(1)=Link([0       0.25        0        pi/2      0     -pi/2   ],'modified');  % 关节1这里的最后一个量偏置-pi/2设置是为了将puma560构型的臂垂直下去
R(2)=Link([0      0          0           -pi/2      0    0  ],'modified');
R(3)=Link([0       0           0.2        0          0     -pi/2],'modified');
R(4)=Link([0       0.248         0           -pi/2      0     ],'modified');
R(5)=Link([0       0           0           -pi/2       0     ],'modified');
R(6)=Link([0       0           0           pi/2      0     ],'modified');
%                  0.262
p560R=SerialLink(R,'name','RIGHT');
p560R.tool=[0 -1 0 0;
               1 0 0 0;
               0 0 1 0.262 ;
               0 0 0 1;]; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   platform

platform=SerialLink([0 0 0 0],'name','platform','modified');%腰部关节
platform.base=[1 0 0 0;
               0 1 0 0;
               0 0 1 0 ;
               0 0 0 1;]; %基座高度
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   R

pR=SerialLink([platform,p560R],'name','R'); % 单独右臂模型,加装底座

view(3)
hold on
grid on
axis([-1.2, 1.2, -1.2, 1.2, -1.2, 1.2])

pR.plot([0 0 0 0 0 0 0]) % 第一个量固定为0,目的是为了模拟腰关节,左臂下同
hold on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   L
pL=SerialLink([platform,p560L],'name','L'); % 单独左臂模型,加装底座

pL.plot([0 0 0 0  0 0 0])
hold on

UR构型

clear all;
clc;
% 双臂 每个单臂是UR构型
figure(2)
dis_shoulder = 0.2;
% theta d ,a , alpha
L(1)=Link([0       0.0891+dis_shoulder    0        pi/2      0     pi/2  ]); % 关节1这里的最后一个量偏置
L(2)=Link([0       0          -0.425      0         0     0]);
L(3)=Link([0       0          -0.392      0         0     0 ]);
L(4)=Link([0       0.1091        0        pi/2      0     -pi/2]);
L(5)=Link([0       0.0946        0       -pi/2      0     0]);
L(6)=Link([0       0.0823        0        0      0     0]);
%                  0.262
urL=SerialLink(L,'name','LEFT');
% urL.tool=[0 -1 0 0;
%                1 0 0 0;
%                0 0 1 0.262 ;
%                0 0 0 1;]; 
           
R(1)=Link([0       -0.0891-dis_shoulder   0        pi/2      0     pi/2  ]); % 关节1这里的最后一个量偏置
R(2)=Link([0       0          -0.425      0         0     0]);
R(3)=Link([0       0          -0.392      0         0     0 ]);
R(4)=Link([0       0.1091        0        pi/2      0     -pi/2]);
R(5)=Link([0       0.0946        0       -pi/2      0     0]);
R(6)=Link([0       0.0823        0        0      0     0]);
%                  0.262
urR=SerialLink(R,'name','RIGHT');
% urR.tool=[0 -1 0 0;
%                1 0 0 0;
%                0 0 1 0.262 ;
%                0 0 0 1;]; 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   platform
offset_ang = 0;
dis_waist = 0;
platform=SerialLink([0 dis_waist 0 pi/2+offset_ang 0 0],'name','platform');%腰部关节
platform.base=[1 0 0 0;
               0 1 0 0;
               0 0 1 0 ;
               0 0 0 1;]; %基座高度
           
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   R
pR=SerialLink([platform,urR],'name','R'); % 单独右臂模型,加装底座

view(3)
hold on
grid on
axis([-1.5, 1.5, -1.5, 1.5, -1.5, 1.5])

pR.plot([0 0 0 0 0 0 0]) % 第一个量固定为0,目的是为了模拟腰关节,左臂下同
hold on

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   L
pL=SerialLink([platform, urL],'name','L'); % 单独左臂模型,加装底座

pL.plot([0 0 0 0 0 0 0])
hold on

最后就可以愉快的画轨迹了~(当然 过程挺艰辛)

在这里插入图片描述

在这里插入图片描述

也可以做双臂协同