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

上期我们学习了CoppeliaSim安装和操作界面介绍https://mp.csdn.net/mp_blog/creation/editor/131431311

这期我们学习用代码控制机器人运动，可以参考https://zhuanlan.zhihu.com/p/129000694

学习用代码控制机器人运动，就必须掌握KUKA youBot移动底盘运动学，KUKA youBot移动底盘采用麦克纳姆轮结构，这种结构可以实现全向移动和旋转的能力，使机器人在狭小空间中更加灵活和可靠。为了实现youBot底盘的精确控制，我们需要建立其运动学模型，并解决逆运动学问题。本章着重研究youBot底盘的逆运动学问题，推导出底盘各轮的速度与底盘姿态之间的关系，并进行运动仿真分析。

麦克纳姆轮是一种非常特殊的轮子设计，它能够实现全向移动。我们以机器人的正前方为x轴正方向，左侧为y轴方向，正上方为z轴方向，右手定则确定旋转方向

​

 图3.1 KUKA  youBot中XYZ方向

麦克纳姆轮全向底盘的前进模式是指底盘沿水平x轴方向移动的运动模式。在这个模式下，底盘上的麦克纳姆轮通过调整各个轮子的速度和方向来实现运动。

在侧视图中，主轮与底盘平行，它的运动速度与底盘的前进方向一致。在俯视图中，小轮毂与地面接触，它的运动速度与主轮的速度有关。我们来解释小轮毂与主轮的速度关系。小轮毂是通过齿轮机构与主轮相连的。当主轮向前滚动时，小轮毂会向外滚动，与地面接触的部分会产生向后的速度分量。这个向后的速度分量会产生一个反向的摩擦力，使底盘能够前进。麦克纳姆轮全向底盘的前进模式实际上是通过调整各个轮子的速度和方向，使得底盘能够沿水平x轴方向前进。在这个模式下，主轮的速度确定了底盘的整体运动方向和速度，而小轮毂与主轮的速度关系则保证了底盘的稳定性和可控性。

​

 图3.2 麦克纳姆轮全向底盘单个轮子的运动分析

上图中的变量定义描述：

单个轮子的旋转角速度,为轮子围绕自身轴线旋转的速度箭头方向为旋转方向；麦克纳姆轮主轮半径，是一个固定的值，决定了主轮的大小; 麦克纳姆轮主轮的边缘线速度,主轮边缘上的点的线速度，它与主轮的旋转速度和方向相关。箭头的方向表示形成的速度方向; 指轮子与地面接触的点产生的速度，该速度与小轮毂的轴方向平行。箭头的方向表示轮子产生的速度方向。

从上图中可以得出关系式:

​

麦克纳姆轮全向底盘沿水平x轴方向前前进时的运动模型下图所示。

 图3.3    麦克纳姆轮全向底盘沿水平x轴方向前进时的运动模型

上图中的变量描述,

整个底盘复合作用在单个轮子上形成的最终速度,其箭头方向为所形成的速度方向。当箭头朝前时，表示此处产生的速度让车体向前移动;当箭头草期后时,表示此处产生的速度让车体向后移动;轮子在地面接触点产生的速度,该速度与 小轮毂轮轴方向平行,其箭头方向为轮子产生的速度方向;从轮子旋转轴向的外侧面向轮子时所看到的轮子转速，下标数字表示轮子的编号。箭头方向为该视角下看到的轮子旋转方向。

从上图中可以得出关系式:

 联立前面单个轮子的关系式:

 可以推出关系:

根据前进时运动模型分析图中的速度方向，可以得出四个轮子的转速关系:

麦克纳姆轮全向底盘在水平y轴方向上的移动模式又分为左平移和右平移，这两种模式只是速度方向不同,本质是一样的,这里仅以左平移(仰视底盘视角看是右平移)模式为例进行分析。麦克纳姆轮全向底盘沿水平y轴方向左平移时的运动模型如图3.5所示。

图3.4   麦克纳姆轮全向底盘沿水平y轴方向左平移时的运动模型 

上图中的变量描述：整个底盘复合作用在单个轮子上形成的最终速度，其箭头方向为所形成的速度方向。因为是仰视底盘的视图，所以当箭头朝左时，表示此处产生的速度让车体向右移动;当箭头朝右时，表示此处产生的速度让车体向左移动；轮子在地面接触点产生的速度，该速度与小轮毂轮轴方向平行，其箭头方向为轮子产生的速度方向 ；从轮子旋转轴向的外侧面向轮子时所看到的轮子转速，下标数字表示轮子的编号，箭头方向为该视角下看到的轮子旋转方向。

从上图中可以得出关系式:

麦克纳姆轮全向底盘的旋转模式又分为顺时针和逆时针,这两种模式只是速度方向不同,本质是一样的,这里仅以机器人逆时针旋转专为例进行分析。当机器人逆时针旋转时，麦克纳姆轮的速度矢量方向如图3.6所示。前右轮和后左轮的速度矢量方向与机器人正方向成45度夹角，前左轮和后右轮的速度矢量方向与机器人正方向成135度夹角。这种运动模型使得机器人可以在旋转时保持固定的位置，并且能够在任何方向上移动。

 图3.5 麦克纳姆轮全向底盘旋转时运动模型

图3.5中的变量描述：为四个轮子在地面接触点产生的线速度,其箭头方向为轮子产生的速度方向，与小轮毂的轴向平行; 为车体旋转的实际线速度，由小轮毂线速度 v和小轮毂自由滚动方向的速度分量合成产生，其速度方向和主轮触地点与车体中心连线“c”垂直;为从轮子旋转轴向的外侧面向轮子时所看到的轮子转速，下标数字表示轮子的编号。箭头方向为该视角下看到的轮子旋转方向;a麦克纳姆轮的触地点到车体中心的横向距离,为定值; b为麦克纳姆轮的触地点到车体中心的纵向距离,为定值;c为麦克纳姆轮的触地点到车体中心的连线，也是车体旋转的角速度半径;α为麦克纳姆轮的触地点到车体中心连线与水平方向的夹角,也是车体旋转线速度与竖直方向的夹角，这两个夹角与同一个角形成余角，所以它们相等; θ为车体旋转的实际线速度和小轮毂在触地点产生的线速度之间的夹角;为机器人自身旋转的角速度。

前面提到过，麦克纳姆轮全向底盘所有的运动状态都可以看成上述三种运动模式的复合状态,所以最后四个轮子的转速的关系式为上述运动模式的速度和。

上式中的变量描述：从轮子旋转轴向的外侧面向轮子时所看到的轮子转速，下标数字表示轮子的编号;车体沿水平轴方向移动的速度值,车体往前行进时该值为正，车体向后行进时该值为负; 车体沿水平y轴方向移动的速度值。 车体往左侧平移时该值为正，车体向右侧平移时该值为负; 麦克纳姆轮的外圆半径，为定值;机器人自身旋转的角速度;a麦克纳姆轮的触地点到车体中心的横向距离,为定值;b麦克纳姆轮的触地点到车体中心的纵向距离,为定值。

有了运动学计算公式，在代码里面就很好实现了，由公式（3.16），可以将youBot机器人的底盘逆运动学计算过程用下面的Lua代码进行函数实现:

完整运动仿真和代码见附件

对于KUKA youBot移动小车的路径规划，可以使用CoppeliaSim中提供的Path实体来定义机器人在整个路径中每一时刻的运动过程。具体而言，可以在场景中添加一个Path对象，并将其与KUKA youBot移动小车绑定。然后，通过在Path对象上设置关键控制点的位置和姿态信息，以及相邻控制点之间的插值参数，即可生成一条光滑的运动路径。最后，可以通过CoppeliaSim提供的API或脚本语言来控制KUKA youBot移动小车按照预先设定的路径进行运动。

需要注意的是，在使用Dummy进行路径规划时，应根据实际需求对其属性进行设置，以保证其在场景中的可视性和传感器感知能力。此外，还应考虑到机器人的运动速度和加速度限制，以避免Dummy在移动过程中出现撞墙或失控等情况。

在CoppeliaSim中，Dummy是一种带有姿态信息的虚拟“质点”，它具有轻量化、高效化的特点，可以用来进行仿真和路径规划等任务。通过将Dummy与Path结合使用，我们可以实现机器人沿着预先定义的路径进行运动，并实时获取当前时刻在Path上的位置和姿态信息。

图3.6  CoppeliaSim下的Dummy与Path

具体而言，当我们在CoppeliaSim场景中创建一个路径对象（Path）并设置好其关键控制点后，可以创建一个Dummy对象并将其放置在路径的起始点。然后，通过编写Lua脚本或使用CoppeliaSim提供的API，可以控制Dummy沿着Path向前移动，并获取当前时刻在Path上的位置和姿态信息。这些信息可以被其他控制模块或设备使用，用于执行更复杂的任务或操作。

下面，我们将用一个路径规划的小例子来说明如何使用Path，并使用youBot机器人的逆运动学模型来控制机器人沿着自定义的路径运动。第一个Path使用的是一个标准的圆形，youBot机器人沿着这个圆形的path做逆时针运动。第二个Path是由2个相切的圆构成的一个“8”字形的绕圈路径，如下图所示。

图3.7  圆形路径轨迹

图3.8  “8”字形的绕圈路径轨迹

在CoppeliaSim中，使用鼠标右键->Add->...来快速新建Dummy和Path。如果想要Dummy跟随某一条Path运动，首先要将这个Dummy“绑定”到对应的Path下面，绑定过程非常简单。只需要使用鼠标选中并将Dummy拉到Path的子目录下就行了。

Path在创建以后，用户也可以通过添加自定义的控制点和设定它的属性来定义不同形状的路径，包括在三维空间中的运动。在构建完Dummy和Path以后，需要为Dummy创建一个进程式的Child script来获取Dummy的运动信息，Lua代码如下：

在仿真运行时，这个Child script会启动一个新的进程。其中，sim.followPath指定Dummy跟随哪一个Path进行运动，参数中的数字3表示同时获取Path的位置和姿态信息（1表示只获取位置，2表示只获取姿态），0.1表示Dummy会以0.1m/s的速度前进，加速度为1，读者可以在文末找到本文的完整代码，可以自行改改参数试一试。

在youBot的Child script中调用sim.getObjectPosition和sim.getObjectOrientation两个函数来实时获取Dummy的位置和姿态，从而控制机器人跟随Dummy运动，即跟随规划好的路径Path运动，使用方法如下

其中，参数中的-1表示获取的是“绝对信息”，即在世界坐标系下的位置和姿态信息，当我们获取到了Dummy的位置和姿态以后，以Dummy的运动位置和姿态作YouBot机器人中心点运动的“领航者”，通过获取Dummy的速度来得到youBot机器人的中心点目标运动速度。在本文的Circle型Path中，中心点的运动状态可以计算为：

此时，需要注意的是，我们建立的youBot运动学模型是基于youBot机器人自身的局部坐标系建立的，而非全局世界坐标系，所以Dummy的运动状态要转换为youBot机器人的局部坐标系中的运动状态才能使用，在这里，youBot随Circle型路径运动的线速度为0.1m/s，在youBot机器人的局部坐标系中，相当于vy = 0.1m/s，而vx = 0。