【V-REP自学笔记（五）】YouBot底盘运动学与路径规划

导语

在这一系列的V-REP自学笔记中，我们定了一个小目标，完成一个Demo。使用官方提供的KUKA公司的YouBot机器人模型来实验机器人的感知和控制过程，控制机器人从A点抓取物品，然后移动到B点将物品放置在B点的工作台上，这其中涉及到V-REP环境中的机器人感知和控制过程。没有看过前期学习笔记的读者，可以在文末找到往期文章地址。

在上一期的自学笔记中，我们讲述了通过键盘来控制YouBot机器人的前后左右移动。在本期笔记中，我们将学习YouBot机器人底盘的运动学模型，并使用V-REP中的Dummy和Path来进行路径规划，控制YouBot机器人按照规划的路径进行运动。话不多说，先放一个最终效果的演示视频：

1. YouBot底盘的运动学模型

在上一期的文章中我们详细的介绍了麦克纳姆轮的运动方式和原理，这里不再详细介绍，有疑问的读者可以上一期的笔记【V-REP自学笔记（四）】键盘控制YouBot机器人。如下图所示，注意观察，YouBot机器人的底盘是由2个A型麦克纳姆轮和2个B型麦克纳姆轮组成的。

除去上面的机械臂，我们单独来看底盘，则我们可以将底盘的结构简化为下面这个图：

麦克纳姆轮不止提供前后方向的运动，还提供左右方向的运动，其中，A轮向前运动时会同时提供一个侧向向左的运动速度，A轮向后运动时会同时提供一个侧向向右的运动速度。B轮侧向的运动方向与A轮刚好相反。

从机器人学的角度来说，给定4个轮子的运动速度，求YouBot机器人整体的运动状态为正运动学求解。而大多数情况下我们是给定机器人的空间运动轨迹来反求四个轮子在各个时刻的运动速度，称为“逆运动学求解”。因此，本文将针对逆运动学求解进行分析和计算。此处，我们将底盘的运动状态合并到中心点处，将A、B、C、D四个轮子分别编号为1、2、3、4，则我们有下图所示的关系图：

给定世界坐标系X-O-Y如图所示，其中Y的正方向表示YouBot机器人向正前方运动，X的正方向表示YouBot机器人向正右方运动。所有与X、Y正方向同向的量的符号都为正，反之则为负，所有的角度旋转方向的定义遵循右手定则。则底盘的中心点运动情况可以分解为中心点沿X方向运动的速度，沿Y方向运动的速度和绕中心点旋转的角速度：

其中，

以编号为1的轮子的运动情况为例，求解中心点的运动状态与四个轮子的运动状态之间的关系。对于该轮子来说，我们能控制它向前后反向旋转，同时，由于麦克麦克纳姆轮的特性，它在前后运动时也会同时向侧向移动，在向前运动的时候也同时向左运动，向后运动的时候也同时向右运动，而且侧向运动的速度与前后运动的速度有一定的比例关系，这里我们假设轮子向前运动，速度为 v_1，则其向前运动的速度和侧向运动的速度合成为一个速度k*v_1，由于中心点的运动状态与四个轮子的运动状态为线性关系，因此我们有：

其中 i 表示轮子的编号：

将公式(3)变化一下形式我们有：

再次变换可以得到：

对于YouBot机器人的麦克纳姆轮来说，其侧向角度为45度，因此在X-O-Y构成的坐标系中，我们有如下关系：

然后可以推导得到：

假设轮子的运动速度表示为w_i，则：

最后，我们可以得到四个轮子的转动速度与中心点的运动状态的关系为：

这样，当已知YouBot机器人底盘的基本尺寸a和b的大小以及4个轮子的半径R时，就可以通过中心点的运动状态（公式（1））来计算各个轮子的转动速度了。

2. Lua代码实现底盘运动学模型

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

function chassisInverseKinematics(vx, vy, omega, wheel_R, a, b)
    omega_1 = (vy - vx + (a+b)*omega)/wheel_R
    omega_2 = (vy + vx - (a+b)*omega)/wheel_R
    omega_3 = (vy - vx - (a+b)*omega)/wheel_R
    omega_4 = (vy + vx + (a+b)*omega)/wheel_R
    
    -- set the right direction for each wheel
    v_wheel_1 = -omega_1
    v_wheel_2 = -omega_2
    v_wheel_3 = -omega_3
    v_wheel_4 = -omega_4
end

代码比较简单，且符号表示与公式（9）一致，因此这里不再对代码进行详细解释，需要特别说明的就是在V-REP中，YouBot的4个轮子的正方向与我们定义的正方向刚好相反，因此在计算完成以后4个轮子的速度方向均要反向。到这里，我们已经完成了YouBot机器人的底盘逆运动学模型建模，只需要指定中心点位置的运动状态（见公式（1）），即可求出各个轮子的运动状态。

来看一个简单的例子，通过给定中心点的目标运动位置和姿态来控制YouBot机器人，这里我们使用了一个简单的PD控制器来控制YouBot机器人的位置和姿态（详细代码见文末的github仓库），控制YouBot机器人分别到达地面上位置为（0,0）、(0,1)、(1,0)、（1,1）的位置，然后在原地做正负90度的姿态变换，Lua代码如下：

simu_time = sim.getSimulationTime()
    
if simu_time < 10 then
    target_pos = {0, 1, 0}
elseif simu_time < 20 then
    target_pos = {0, 0, 0}
elseif simu_time < 30 then
    target_pos = {1, 0, 0}
elseif simu_time < 40 then
    target_pos = {0, 0, 0}
elseif simu_time < 50 then
    target_pos = {1, 1, 0}
elseif simu_time < 60 then
    target_pos = {0, 0, 0}
elseif simu_time < 70 then
    target_pos = {0, 0, math.pi/2}
elseif simu_time < 80 then
    target_pos = {0, 0, 0}
elseif simu_time < 90 then
    target_pos = {0, 0, -math.pi/2}
elseif simu_time < 100 then
    target_pos = {0, 0, 0}
end

完整的代码请参考文末的github仓库，完成后的运行效果：

前面用了一个简单的小例子来验证了YouBot机器人的底盘逆运动学模型，但在V-REP中实际使用的时候，YouBot机器人的运动路径不会这么简单直接，可能是一条完全不规则的运动轨迹，因此我们需要一种更灵活的方式来定义路径。在V-REP中，通常使用Path来定义机器人的运动“路径”。下面，我们将用一个路径规划的小例子来说明如何使用Path，并使用YouBot机器人的逆运动学模型来控制机器人沿着自定义的路径运动。

3. 基于Dummy和Path的路径规划

在这个例子中，我们使用了2个YouBot机器人，分别用于沿着两个自定义的路径（Path）运动，在文末可以找到本文的完整工程文件和Lua代码的github仓库，欢迎下载使用：

第一个Path使用的是一个标准的圆形，YouBot机器人沿着这个圆形的path做逆时针运动，路径为A-B-C-D-A:

第二个Path如图所示，我们给定一条由2个相切的圆构成的一个“8”字形的绕圈路径。要求机器人从第一个圆周路径的A点出发，经过第一个圆周上的B点，在通过两圆相切的C点时切换到另一个圆周的路径上，通过D点、E点、F点，运动到C点时再切换回到原来的圆周路径上，经过G点，最后回到A点，整个过程的路径表示为A-B-C-D-E-F-C-G-A：

这里我们会用到V-REP中的路径“Path”和带姿态信息的虚拟点“Dummy”。

简单介绍一下V-REP中的Path。在V-REP中，Path是Scence的一种“实体”，与其他model一样，用户可以使用Path来自定义各种运动路径，简单来说，从Path上我们可以给定某一个时刻机器人运动到某一点的位置以及其姿态，定义机器人在整个路径中每一时刻的运动过程，V-REP中默认的Path是通过给定一些关键的控制点，并在这些控制点中间采用贝塞尔曲线插值来得到光滑路径的。对Path进一步的详细介绍可以参考V-REP的官网教程。

Dummy是一种带有姿态信息的虚拟“质点”，我们可以忽略它的体积，也可以通过设定它的属性来赋予它被视觉传感器等传感器“看到”的能力，在以后的笔记中，我们会多次使用Dummy，这里不详述。在V-REP中，Dummy常用来与Path相结合，Dummy可以沿着Path向前运动，实时获取当前时刻在Path上的位置和姿态，作为路径上的一个“领航者”。

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

当然，Dummy也可以绑定到其他模型，例如绑定到机械臂的末端上作为一个“引导点”，引导机械臂的末端进行空间运动。

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

function sysCall_threadmain()
    dummy_guider_handle = sim.getObjectHandle('DummyGuider')
    path_handle = sim.getObjectHandle('Path')
    
    print('start follow path')
    sim.followPath(dummy_guider_handle, path_handle, 3, 0, 0.1, 1)
end

在仿真运行时，这个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运动，使用方法如下：

dummy_guider_position = sim.getObjectPosition(dummy_guider_handle, -1)
dummy_guider_orientation = sim.getObjectOrientation(dummy_guider_handle, -1)

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

-- Path 1: one circle
vx = 0
vy = dummy_guider_velocity
omega = dummy_guider_velocity / circle_path_R

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

到此为止，YouBot机器人底盘的运动学模型以及控制机器人跟随一个自定义的路径运动就已经介绍完了，其中定义路径的Path可以由用户根据需要进行自定义，本项目涉及到的代码都将发布在Github上，欢迎大家Star和下载使用。

项目地址：https://github.com/chauby/V-REP-YouBot-Demo.git

往期回顾

作者知识有限，有不正确的地方还望读者能够指出来，大家共同学习和进步，不胜感激！