matlab坐标系转换_双臂Matlab仿真建模：正运动学

r囧r小猫 2022-10-25 14:07 240阅读 0赞

![efc141fe9144c64f84227ad1ae4a7e75.png][]

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

**PUMA560构型双臂**

![097ce722288c5f31449be78314b465cf.gif][]

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

![be039d4954add5758a171b4b78b62410.png][]

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

![5538ca714b97c9a04d56bdc45b0339d6.png][]

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

![c6218b5a42a6ac3556a93f65e813ff32.png][]

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

![feba2516945bb565a073b2a4a0ce3445.png][]

**UR构型双臂**

![097ce722288c5f31449be78314b465cf.gif][]

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

![65c186795413284465e5a0f4ac2f1316.png][] ![3d7b0f49319c528d675f7e4b0becdde8.png][]

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

![86df6eed25e3d3f7de5b3267247633dc.png][]

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

![27f8be01afe3be5fd830f9a3470956cf.png][]

**如何进行轨迹仿真**

![097ce722288c5f31449be78314b465cf.gif][]

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

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

![c6218b5a42a6ac3556a93f65e813ff32.png][]

世界(全局，也为腰关节)坐标系下的位姿表示为![c3eb908159f7ec7aba2be5f41a26f84b.png][]，这就是我们算法验证时所给定的轨迹位姿，都是相对于世界坐标系的；现在的问题就是，将轨迹位姿映射到肩关节(机械臂基坐标系)下，也就是求![a7bddedc42d9f2e11d35c73b37e023a2.png][]。这个就很简单了，即![bc62614e917d829e6eab3d8b9d278870.png][]。![74b7e207f3247eb29ff0002410d6879a.png][]即为单臂基坐标系相对于腰关节坐标系的姿态变换矩阵，根据腰关节和肩关节之间的坐标变换，可以分析得出，腰关节坐标系先绕X1轴(自己的)旋转pi/2，再绕动轴Z1旋转-pi/2，最后再沿着动轴Z1移动d长度，故![8fbc6cf1576e4c93ec3097ef6098ee6e.png][]。按照上述步骤即可将全局坐标系下的位姿映射到单臂局部坐标系下了。

同理，UR也可如此。

**Matlab正运动学建模**

![097ce722288c5f31449be78314b465cf.gif][]

**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.262p560L=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.262p560R=SerialLink(R,'name','RIGHT');p560R.tool=[0 -1 0 0;               1 0 0 0;               0 0 1 0.262 ;               0 0 0 1;]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   platformplatform=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;]; %基座高度%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   RpR=SerialLink([platform,p560R],'name','R'); % 单独右臂模型，加装底座view(3)hold ongrid onaxis([-1.2, 1.2, -1.2, 1.2, -1.2, 1.2])pR.plot([0 0 0 0 0 0 0]) % 第一个量固定为0，目的是为了模拟腰关节，左臂下同hold on%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   LpL=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 , alphaL(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.262urL=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.262urR=SerialLink(R,'name','RIGHT');% urR.tool=[0 -1 0 0;%                1 0 0 0;%                0 0 1 0.262 ;%                0 0 0 1;]; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   platformoffset_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;]; %基座高度           %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   RpR=SerialLink([platform,urR],'name','R'); % 单独右臂模型，加装底座view(3)hold ongrid onaxis([-1.5, 1.5, -1.5, 1.5, -1.5, 1.5])pR.plot([0 0 0 0 0 0 0]) % 第一个量固定为0，目的是为了模拟腰关节，左臂下同hold on%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   LpL=SerialLink([platform, urL],'name','L'); % 单独左臂模型，加装底座pL.plot([0 0 0 0 0 0 0])hold on

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

![5a106ea90ddce0d1dee7a390b1967800.gif][] ![f26a88965b63730331f34342b8372a1a.gif][]

也可以做双臂协同

![fe267ee8c7088cbfba79c0b8afad9414.gif][]

(本文经授权转载自CSDN，作者OpenRobotSL，未经授权禁止二次转载与摘编。)

![0ea646a938de0bf4a74ffc6e1b464091.gif][]

**公开课《自动驾驶入门：从建模到跟随》** 将带你入门自动驾驶，以车辆建模与路径跟踪为目标，从创建开发环境开始，一步一步完成自动驾驶中纯跟踪算法(pure persuit)的应用课程。

![64ff1b820e3b278faa0a21344de56a1f.png][]

![421f6717f2bb599961df0a2cae3ddfab.gif][] 点击“

[efc141fe9144c64f84227ad1ae4a7e75.png]: /images/20221024/50db8a850ac5496a9d509f270f5cafaf.png
[097ce722288c5f31449be78314b465cf.gif]: /images/20221024/c5fa3fab11de4e01b60c644df2a53720.png
[be039d4954add5758a171b4b78b62410.png]: /images/20221024/9322737442a548e0b6b058e17615cb30.png
[5538ca714b97c9a04d56bdc45b0339d6.png]: /images/20221024/901e913565a044f087de236055048cf2.png
[c6218b5a42a6ac3556a93f65e813ff32.png]: /images/20221024/9a20c4d79cef436d8c76d10e929b3225.png
[feba2516945bb565a073b2a4a0ce3445.png]: /images/20221024/72323f7e37144b158e0034dffad202f1.png
[65c186795413284465e5a0f4ac2f1316.png]: https://img-blog.csdnimg.cn/img_convert/65c186795413284465e5a0f4ac2f1316.png
[3d7b0f49319c528d675f7e4b0becdde8.png]: /images/20221024/1fc34bed85b447fea16c51f417288b06.png
[86df6eed25e3d3f7de5b3267247633dc.png]: /images/20221024/5512e7f7293246e9b1f6cb1d7638d1ed.png
[27f8be01afe3be5fd830f9a3470956cf.png]: /images/20221024/0996d106482742348d3f4f1e3558bd5b.png
[c3eb908159f7ec7aba2be5f41a26f84b.png]: https://img-blog.csdnimg.cn/img_convert/c3eb908159f7ec7aba2be5f41a26f84b.png
[a7bddedc42d9f2e11d35c73b37e023a2.png]: https://img-blog.csdnimg.cn/img_convert/a7bddedc42d9f2e11d35c73b37e023a2.png
[bc62614e917d829e6eab3d8b9d278870.png]: https://img-blog.csdnimg.cn/img_convert/bc62614e917d829e6eab3d8b9d278870.png
[74b7e207f3247eb29ff0002410d6879a.png]: https://img-blog.csdnimg.cn/img_convert/74b7e207f3247eb29ff0002410d6879a.png
[8fbc6cf1576e4c93ec3097ef6098ee6e.png]: /images/20221024/1835eb48b07549ea9edf19e42934688b.png
[5a106ea90ddce0d1dee7a390b1967800.gif]: https://img-blog.csdnimg.cn/img_convert/5a106ea90ddce0d1dee7a390b1967800.gif
[f26a88965b63730331f34342b8372a1a.gif]: /images/20221024/d1b6dc84c3414adca8c4a51c295181fa.png
[fe267ee8c7088cbfba79c0b8afad9414.gif]: /images/20221024/ffcc4034dba14d058f6bc181adfd9f0b.png
[0ea646a938de0bf4a74ffc6e1b464091.gif]: /images/20221024/e40d5b2d9b684638a43bf3ead6015fa9.png
[64ff1b820e3b278faa0a21344de56a1f.png]: /images/20221024/c79b0bfe3d8c488497f3f481a25b65af.png
[421f6717f2bb599961df0a2cae3ddfab.gif]: /images/20221024/68820a041928460b89505576c27df3d8.png