基于CoppeliaSim和KUKA youBot的移动机械臂抓取仿真(四):KUKA youBot机械臂轨迹规划

用钢铁意志，成就不平凡人生。

上期我们学习了用代码控制KUKA youBot移动底盘运动https://blog.csdn.net/m0_71721954/article/details/131432663

这期我们学习KUKA youBo机械臂轨迹规划，可以参考【V-REP自学笔记（六）】基于V-REP逆运动学模块的机械臂轨迹规划 - 知乎

完整附件见最后链接

4.1 机器臂正运动学方程的建立

KUKA youBot机械臂可以看作由6个转动关节串联的连杆组成，每个关节由独立的电机驱动。这种机械臂实际上采用了半闭环的控制结构，即系统只能精确控制关节伺服电机的位置，而无法直接控制机器人末端执行器的位姿。关节位置和机器人末端执行器位姿之间的关系是通过运动学建模来确定。

为了进行运动学建模，我们采用了改进的DH方法（MDH）。D-H方法是一种用于描述机器人运动学的通用方法，它由Denavit和Hartenberg在1955年提出。该方法的核心思想是在机器人每个连杆上建立一个坐标系，并使用4x4的齐次变换矩阵描述相邻两个连杆之间的空间关系。通过逐步进行变换矩阵的乘法运算，可以将各个连杆的坐标系转换到同一个参考坐标系下，进而得到移动机械臂末端相对于基坐标系下的位姿。这个过程中，需要确定每个坐标系的原点和坐标轴方向，以及各个连杆之间的相对位置关系和旋转关系。这些参数可以通过机器人的实际结构和运动学特性来确定。D-H方法可以应用于不同类型的机器人，包括工业机器人、移动机器人、人型机器人等。它为机器人的运动学建模提供了一种简单而有效的方式，使机器人的设计和控制更加方便快捷[12,13]。

从机器人固定基座开始对各个连杆进行编号，第一根可动的连杆为连杆1，依次往后排序。建模步骤如下

(1)确定各关节的z轴：根据图4.1，每个连杆i-1的一端有一个关节，关节轴线i-1位于连杆i-1靠近基座的一端。同样，关节轴线i位于连杆i靠近基座的一端。我们选择沿关节i-1的轴线方向设置坐标轴，并将沿关节i的轴线方向设置为z轴。在确定z轴的正方向时，通常会选择相互平行的关节的z轴取为相同的正方向。

(2)通过z轴确定各关节的x轴：在每对相邻关节轴线之间存在一条公垂线，我们沿着这条公垂线设置坐标轴，并指向轴的方向。公垂线与z轴的交点被定义为原点。

(3)确定y轴方向：在关节i-1确定了轴、轴及原点后，我们可以使用右手法则来确定y轴的方向。

 

 

 

 

 

4.2 使用CoppeliaSim自带的逆运动学模块

CoppeliaSim自带的逆运动学（IK）模块不仅可以支持官方提供的模型，也支持用户自己设计的机器人模型，所以用户在设计好自己的机器人模型以后可以导入到V-REP中，定义好机器人模型连杆和关节的约束关系，构建好机器人模型的“树结构”，然后就可以使用CoppeliaSim自带的逆运动学模块了。在CoppeliaSim官方提供的模型当中，许多需要使用IK模块的机器人模型的关节已经被设定为IK模式了，例如youBot机器人，因此我们可以直接使用。但是对于一个新设计的机器人模型来说，CoppeliaSim逆运动学模块的使用过程大致可以分为以下6个步骤：

(1)将所有参与逆运动学计算的关节设定为IK模式；

(2)添加target dummy、tip dummy；

(3)添加Path，并让target dummy跟随Path运动；

(4)Link target dummy 和 tip dummy；

(5)添加IK group和IK element；

(6)添加script，开始仿真。

4.2.1将所有参与逆运动学计算的关节设定为IK模式

如下图所示，依次选择youBot机器人机械臂的joint0 - joint3，点击左边的“放大镜”，将Joint设置为Inverse Kinematics Mode。

此外，关节可选的控制模式还有Passive Mode、Torque/force Mode等，这里youBot机械臂的加持机构就采用的Torque/force Mode，直接给关节驱动力进行扭矩控制，而非关节的位置控制。

 

4.2.2 添加target dummy、tip dummy和Path

第2和第3个步骤是添加target dummy、tip dummy、Path和控制dummy随Path运动，添加完成以后可以改一个自己喜欢的名字。target dummy用于跟随Path运动，作为机械臂末端机构的“领航者”，而tip dummy作为机械臂末端执行机构的“传感点”，用于反馈机械臂末端的位置和姿态，同时如果将target dummy与tip dummy绑定并且构建逆运动学模型，则tip dummy可以带动机械臂跟随 target dummy运动。

这里我们使用Circle型的Path，添加Path以后按照前面介绍的过程调整Path为一个三维的封闭路径，如下图所示。Target dummy与Path绑定之前已经讲过了，而tip dummy需要绑定到youBot机械臂的某一个连杆上，同样的，直接使用鼠标拉拽到对应的树形结构下即可，可以是youBotArmJoint3，也可以是youBotArmJoint4。然后按照前面介绍的设置Dummy位置的办法将taget dummy和 tip dummy的初始位置设定机械臂的夹持机构中间位置，作为路径的起点。

   

 

4.2.3 将Target dummy 和 tip dummy链接起来

这里介绍一下如何将target dummy 和 tip dummy绑定到对应的机构以及如何Link彼此，也非常的简单，点击左边的放大镜图标，在邮编选择linked dummy到youBot_positionTip，选择完成以后，在target dummy和tip dummy之间会有一个红色的箭头链接彼此，出现了这个箭头就说明设定成功了，如下图所示：

 

到了这一步，我们已经可以让target dummy随着Path运动了

4.2.4 添加IK group和IK element

前面我们将机械臂的各个关节都设定为了IK模式，这里我们需要将他们组合起来共同运动，这里需要用到CoppeliaSim的Calculation Modules，也就是左边的f(x)按钮，如题下图所示：

点击f(x)，在弹出的窗口中选择Kinematics，在下面添加新的IK Group，可以自定义名字。然后点击新建的IK_Group设定Calc.method，这里可选的有两个，一个是Pseudo inverse，一个是DLS，这里任意选一个都可以。点击Edit IK Elements，添加IK Element with tip，一个IK Group可以添加多个IK Element，将下面的Base设定为youBot表示以youBot为参考基准，此外，下面的Constraints里面的X、Y、Z表示空间位置的三维坐标信息，Alpha-beta 和 Gamma表示三维姿态信息，由于youBot的机械臂自由度不够，这里我们不选姿态信息，仅仅设置跟随位置信息即可。

 

设定完成IK Group以后，我们还需要做最后一步的工作，给机器人添加script。

4.2.5 给youBot添加Script

前面的教程里面我们已经详细的介绍了如何使用Lua代码来控制机器人，这里不再赘述，对于本文的目标来说，我们只需要做一个工作：调用sim.setJointTargetVelocity()函数控制机器人的四个轮子不让其随意运动即可，即将其速度设置为零，最后的效果如下图所示：

 

 

4.3 matlab仿真

%kuka youBot

clear;

clc;

close all;

d1=0.147;

a1=0.033;

a2=0.155;

a3=0.135;

d5=0.113;

tool = 0.105;



% Joint angle limit.

qlim1=[-169,169]*pi/180;

qlim2=[-65,90]*pi/180;

qlim3=[-151,146]*pi/180;

qlim4=[-102.5,102.5]*pi/180;

qlim5=[-167.5,167.5]*pi/180;

qlim6=[-167.5,167.5]*pi/180;

 theta=[-pi/3 pi/5 0 pi/3 pi/6 0]

 L(1) = Link('d', d1, 'a', a1, 'alpha', pi/2, 'qlim',qlim1);

 L(2) = Link('d', 0, 'a', a2, 'alpha', 0, 'qlim', qlim2, 'offset', 1.57);

 L(3) = Link('d', 0, 'a', a3, 'alpha', 0,'qlim',qlim3);

 L(4) = Link('d', 0, 'a', 0, 'alpha', -pi/2,'qlim',qlim4, 'offset', -1.57);

 L(5) = Link('d', d5, 'a', 0, 'alpha', 0,'qlim',qlim5);

L(6) = Link('d', 0.01, 'a', 0, 'alpha', 0,'qlim',qlim6);

 bot=SerialLink(L,'name','KUKA youBot'),

 bot.display

 bot.teach

 bot.fkine(theta)

 bot.ikine( bot.fkine(theta))

该段代码使用Robotics Toolbox for MATLAB/Octave创建了一个KUKA youBot机器人模型，并进行了正逆运动学的计算。

首先，定义了机器人的DH参数和关节角度限制。这些参数包括机械臂的长度、偏移量、关节角度范围等。

然后，通过SerialLink函数创建了一个名为bot的机器人对象。SerialLink函数将连接所有的连杆和关节，并使用提供的DH参数和关节角度限制初始化机器人模型。

接下来，使用bot.display函数显示了机器人的DH参数和关节角度限制。这可以帮助您检查和确认机器人模型的设置是否正确。

使用bot.teach函数可以在CoppeliaSim中交互地控制机器人。它会打开一个图形用户界面，其中包含机器人模型和控制器。您可以点击关节控制器手柄来改变机器人的关节角度。

bot.fkine(theta)计算给定关节角度theta下的正运动学，返回末端执行器的位姿矩阵。这个位姿矩阵描述了机器人末端执行器在基坐标系中的位置和姿态信息。

bot.ikine(bot.fkine(theta))则根据末端执行器的位姿矩阵反解得到关节角度。这个操作可以帮助您计算出使机器人达到特定位姿的关节角度。

请注意，以上代码假设您已经安装了Robotics Toolbox for MATLAB/Octave，并且正确配置了CoppeliaSim与MATLAB/Octave之间的连接。如果您在其他环境中使用该代码或者使用其他机器人库，请相应地进行调整和适配。特别注意，matlab下求逆解是六自由度，但是KUKA youBot机械臂是五自由度，我们在求解是设置为最后一个杆长0.01m即可。

链接：https://pan.baidu.com/s/1HC1n65bu3pDlZEIBIxdr9w?pwd=j2ff 
提取码：j2ff