【V-REP自学笔记（八）】控制youBot抓取和移动物体

导语

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

在前面的几期自学笔记中，我们分别从V-REP软件使用、Lua语言编程、Matlab/Python的远程接口控制等角度来阐述了如何使用V-REP进行机器人仿真环境的搭建和代码编写过程。在本期的笔记中，我们将实现本系列自学笔记的小目标，通过编写代码来控制youBot机器人从A点的工作台上抓取物体，并移动到B点将物体放置在B点的工作台上。此外，我们还测试了在多个位置之间移动的路径规划，而非单一A、B点的路径规划。话不多说，先放一个最终的效果演示视频：

本期笔记约6000字，如果要全部看懂需要一定的时间，建议先关注+收藏。本文配套的代码采用Python语言编写，由于前面几期笔记已经介绍了有关V-REP平台和Python语言构建控制算法的基本过程，这里不再对其中的基础进行介绍，如果有问题，可以参考我之前写的自学笔记。本文所涉及到的所有工程文件和算法代码已经开源在github上：https://github.com/chauby/V-REP-YouBot-Demo

另外，本文的参考书籍是《Modern Robotics: Mechainism, Planning and Control》，中文版本为《现代机器人学：机构、规划与控制》，英文版的书籍可在本公众号后台回复【MR】获取下载链接，但是对于初次接触机器人学的读者（尤其是没有接触过旋量、李群李代数等的读者）来说，我强烈建议大家购买中文版的书籍：

1. 问题的整体分析

在开始之前，我们可以先构思一下，如果想要控制youBot机器人进行物体的抓取和移动任务，我们需要做一些什么工作？流程是什么？看下图，先在脑袋里构思1分钟 ......

那么，你想到了什么呢？我们试着来看看以下问题：

A点和B点的位置怎么确定？是实现规定好的呢还是实际通过传感器检测得到？
A点的工作台形状是什么样的？高度和宽度有多少？youBot的机械臂能够得着吗？
实际待抓取的物体放置在工作台的什么位置呢？如何去感知这个物体的位置？
A点工作台上待抓取的物体形状呢？放置的姿态呢？youBot的机械手尺寸够大吗？该用什么姿势抓取物体才能保证不掉落？
A点工作台上待抓取的物体重量有多少？youBot的机械臂是否有足够的力量来移动它？
待抓取的物体是什么材料的呢？表面纹理是什么？如果机械手用力过大是否会破坏待抓取的物体？
如果规划youBot的路径让它从当前位置移动到A点，以及从A点移动到B点？
从A点移动到B点只有一条路径吗？如果不止一条，如何选择呢？
根据规划的路径移动的时候如果撞到了工作台怎么办？如何避免碰撞？

dots

由于实际控制机器人完成这个任务时所面临的问题太多，这里不再赘述，但是希望大家明白，在控制一个实际的机器人去完成类似的任务时我们面临的问题往往要比我们事先知道的多得多。但是，在V-REP等机器人的仿真环境中，有许多现实世界要面临的问题都可以被忽略或者采用比较简单的方式进行约束和实现，因此，在本期笔记中，我们不会解决上述所有的问题，只专注在机器人的运动规划上，而对其中的环境感知问题、机械动力学问题与控制问题进行简化。例如，在仿真环境中，我们可以通过接口函数直接得到A点位置、B点位置以及待抓取物体的位置和姿态。当然，如果读者想要尝试使用传感器去感知也是可以的，这里我们不做探讨。

2. 算法的构思过程

假设我们要抓取的物体是一个正方体，我们叫它Cube，再假设在整个移动过程中，我们的机械手有一个相对于youBot机器人的“待命”位置，称为standoff位置，所有的操作过程都是基于这个standoff位置来开展的。则我们可以将任务分段简化为以下8个过程：

移动youBot，从当前位置到目标位置A点，并将机械手移动到standoff位置；
打开机械手，控制机械臂将机械手移动到Cube处；
闭合机械手，抓取Cube；
控制机械臂将机械手抬起回到standoff位置；
移动youBot到B点的Cube目标位置上方，并将机械手至于standoff位置；
控制机械臂下降到Cube的目标位置；
打开机械手，放置Cube；
控制机械臂抬起机械手，回到standoff位置；

我们考虑上述过程，规划好每一步当中机器人各个关节的运动过程，然后将所有的步骤串联起来，就完成了整个Pick and Place的过程。在这八个步骤中，我们需要用到以下几个部分的内容：

youBot的运动学解析；
一个运动规划仿真器（Simulator）
参考轨迹的生成
前馈/反馈控制器

2.1 youBot的运动学分析

这部分内容其实在之前的笔记中我们已经完成了，这里不再重复，如果不记得了，可以参考：（放置VREP学习笔记四的链接）。这里我们对V-REP中youBot机器人模型的运动学参数做一个说明：

V-REP中youBot机器人的模型示意图如上图所示，底盘轮子编号和机械臂尺寸参数如图所示，其夹持机构（机械手）的尺寸参数为：

其中：

d_{1, min} = 2 cm, d_{1, max} = 7 cm, d_2 = 3.5 cm, d_3 = 4.3 cm

。

对于youBot的底盘来说，前后轮之间的距离是

0.47 m

，左右轮之间的距离是

0.3 m

，轮子的半径为

0.0475 m

。参考这些参数我们可以合理的设置A、B两点工作台的高度和宽度，Cube的尺寸以及规划的路径参数等，保证我们的想法能够实现。比如，在本工程中，所用的正方体Cube的尺寸为：

其他的参数详见源代码。

2.2 运动规划仿真器（Simulator）

回忆一下，如果要控制youBot进行移动和机械臂抓取操作，我们一共需要控制多少机构？4个轮子+5个机械臂关节+1个机械手。在V-REP中，这个机械手只有一个关节，有2种状态（open和close），由于机械手的打开和关闭需要时间，因此为了确保机械手能够正确打开和关闭，设定好对应的机械手状态以后要在该位置保持0.625秒以上，一般直接设置为1秒。

这里我们引入一个来自《Modern Robotics》书中的概念：Configuration。Configuration来自于机器人Motion Planning里面的一个空间概念，即所谓的构型空间（Configuration Space, C-Space，中文译名也有配置空间，构位空间，C空间等），是一个与机器人构型相关的空间，其本身的理论非常丰富，本文不做深入介绍，可以参考相关书籍。简单理解，Conciguration就是指的一些特殊的欧式群（例如向量），用来表征机器人的空间状态。例如本文中，youBot底盘在二维平面内运动，因此其空间状态我们可以用

(x, y, theta)^T

来表示，这个在我们之前的运动学分析笔记部分已经做了介绍，只是没有突出Configuration这个概念。又如，一个六轴的机械臂，我们可以用一个六维向量来表示其Configuration：

(theta_1, theta_2, theta_3, theta_4, theta_5, theta_6)^T

，即所谓的关节空间。而所谓空间就是一组向量的集合+特定运算，对于机器人来说，这个特定的运算过程就是指的在各个Configuration之间的变换。

那么所谓的Simulator就是设计一个函数，使得其可以在输入机器人的各个Configuration和所需的运动时间以后能够随着时间输出对应的机器人运动状态作为仿真模型的参考输入。这个函数我们取名NextState()，其输入输出分别为：

输入：

一个表征youBot机器人当前Configuration的具有12个元素的向量（底盘空间位姿的3个变量，机械臂关节的5个变量，轮子关节的4个变量）；
一个表征运动状态的具有9个元素的向量（机械臂关节运动速度

dot{theta}

的5个变量，4个轮子的运动速度）；

最小时间间隔

Delta_t

；

一个表征机械臂和轮子最大运动速度的正实数，用来限制机械臂和轮子的最大运动速度（为什么要做这个，因为实际情况下为了防止出现意外事故，运动关节的最高速必须要被限制）。

输出：

一个表征下一时刻youBot机器人Configuration的具有12个元素的向量。

函数NextState()采用的计算方式是最简单的一阶线性方法：

theta_{t+1} = theta_t + dot{theta}_t cdot Delta_t

，详细的实现过程见源代码。

有了NextState()这个函数以后，我们只需要给定初始的机器人的Configuration，以及在各个时刻点的Configuration，将所有的Configuration输入到函数，其输出就是我们要的随时间变化的机器人空间状态。

2.3 参考轨迹生成器（TrajectoryGenerator）

首先，为了描述方便，我们在这里做一些符号定义。

如上图所示：

定义世界坐标系为

{s}

;

定义youBot模型底盘中心位置所在的局部坐标系为

{b}

;

定义机械臂在youBot底盘上的基座的局部坐标系为

{0}

;

定义youBot机械臂末端的机械手所在的局部坐标系为

{e}

；

定义Cube所在的局部坐标系为

{c}

；（图中未画出）

参考轨迹生成的核心内容是通过获取每一个局部坐标系在全局世界坐标系中的位置和姿态，并使用四阶方阵

来表示，然后通过设计插值算法来实现各个局部坐标系之间的转移过程，最后使用运动规划器和控制器来完成机器人的动作规划和控制。这里需要注意的是，所谓的“参考轨迹”并不是通俗意义上的那种在三维空间中使用

(x, y, z)

等来表示的空间位置点的集合，这里的参考轨迹是

的集合，除了空间位置，可能还包括关节角度，速度等信息。

为了生成参考轨迹，我们定义一个轨迹生成器函数TrajectoryGenerator()，用于生成末端执行机构（机械手）的空间运动轨迹，即局部坐标系

{e}

的空间变换轨迹。前面我们已经将整个过程分为了8个阶段，因此，对于每个阶段来说，我们的处理过程一样，从每一段初始的Configuration到结束的Configuration（下一段初始的Configuration）。因此，这里函数TrajectoryGenerator()的输入输出分别为：

输入：

机械手的初始Configuration：

T_{se_initial}

Cube的初始位置的Configuration：

T_{sc_initial}

Cube的目标位置的Configuration：

T_{sc_final}

在机械手抓取Cube时表征两者之间的相对位置的Configuration：

T_{ce_grasp}

在抓取前和抓取后，机械手相对于Cube的Standoff位置上的Configuration：

T_{ce_standoff}

。

举个例子，假设在初始时刻机械手在世界坐标系

{s}

中位置为

p=(p_x, p_y, p_z)^T

, 其姿态为：

R=left[begin{array}{ccc} r_{11} & r_{12} & r_{13}\ r_{21} & r_{22} & r_{23}\ r_{31} & r_{32} & r_{33}\ end{array}right]

则

{e}

在世界坐标系

{s}

中的Configuration可以表示为

T_{se}

：

T_{s e}=left[begin{array}{cccc} r_{11} & r_{12} & r_{13} & p_{x} \ r_{21} & r_{22} & r_{23} & p_{y} \ r_{31} & r_{32} & r_{33} & p_{z} \ 0 & 0 & 0 & 1 end{array}right]

这部分可能不好理解，可以理解为youBot的机械手默认不是直接到达Cube所在的位置，而是到达Cube上方的一个待命位置（Standoff），所有的抓取都是基于这个位置出发，然后回到这个位置。在抓取Cube完成，且机械手回到standoff位置以后，youBot再移动底盘来完成整体移动。

输出：

一个具有

个Configuration的，能覆盖所有8个阶段的参考轨迹。其中，参考轨迹上的每一个时刻用一个

T_{se}

来表示机械手在那一刻的Configuration，外加一个0/1 state用来表示机械手的open/close状态。

在我们实际实现的过程中，TrajectoryGenerator()函数的输出（参考轨迹）可以表示为一个具有13个元素的向量，我们定义其顺序和对应的内容如下：

其中，

x、 y、phi 分别表示World_X_Joint、World_Y_Joint、World_Th_Joint的坐标和偏转角度；
J1，

dots

， J5 分别表示youBot的机械臂上的5个关节角度；

w1，

dots

， w4分别表示front left, front right, rail right, rail left四个轮子的位置（旋转角度），如下图所示；

gripper state即表示gripper（机械手）的状态，0表示打开，1表示闭合。

注意：x、y 的单位是米，phi、J1~J5， w1~w4等角度的单位是弧度。控制Gripper的打开和闭合过程至少需要0.625秒，所以为了保证Gripper能正常完成Open/Close的动作，在编写程序控制Gripper从打开到闭合（或从闭合到打开），确保要在该位置保持0.625秒以上，这里可以直接设置为1秒。youBot底盘上四个轮子的编号顺序如图所示：

分别对应w1、w2、w3、w4四个变量作为轮子旋转的角度。

只是生成了参考轨迹还不行，如何合理的提取随时间变化的参考轨迹，并转换为youBot各个关节轨迹的控制指令，使其能够“平滑地”跟随参考轨迹运动是一个难点。所以我们需要设计一个前馈/反馈控制器。

2.4 前馈/反馈控制器（FeedbackControl）

有了随时间变化的参考轨迹以后，我们就可以设计机器人的控制器，控制机器人的各个关节来跟随预定的关节角度参考曲线实现控制。这部分的内容在《Modern Robotics》的第11.6章节和13.5章节部分有更为详细的介绍，如果不是很理解这部分的内容，可以参考书籍的内容，会更详细。

还是从这个图出发，机械手的局部坐标系

{e}

在全局坐标系

{s}

中可以表示为

X(q,theta)

：

X(q, theta)=T_{s e}(q, theta)=T_{s b}(q) T_{b 0} T_{0 e}(theta) in S E(3)

其中，

theta in mathbb{R}^n

表示具有

个关节的机械臂的关节角度，也称之为机械臂的Configuration，这里

n=5

。

T_{0e}(theta)

表示机械臂的正运动学计算，

T_{b0}

表示坐标系

{0}

相对于坐标系

{b}

的位置偏移，

q=(phi,x,y)

表示底盘的Configuration，于是我们有：

T_{s b}(q)=left[begin{array}{cccc} cos phi & -sin phi & 0 & x \ sin phi & cos phi & 0 & y \ 0 & 0 & 1 & z \ 0 & 0 & 0 & 1 end{array}right]

其中，

表示坐标系

{b}

的原点离地面的高度，其实就是底盘的高度，是一个固定值。

假设

X(t)

是一个随时间变化的函数，表示机械手的空间轨迹，则

[mathcal{V}(t)]=X^{-1}(t) dot{X}(t)

表示机械手在坐标系

{e}

中的旋量（twist）。旋量是数学几何与物理中的一个方法，在机器人学理论中可以用来构建机器人运动学模型（在某些情况下优于DH方法）和对应的控制理论，这部分内容本身比较多，够写好多篇笔记了，这里按下不表，除了《Modern Robotics》一书，感兴趣的读者还可以参考Peter Corke老爷子的经典书籍。

假设轮子的速度为

u in mathbb{R}^m

，其中

表示轮子的数量，这里为4。在构建我们的控制器之前，我们需要使用到运动学模型的雅克比矩阵

J_e(theta) in mathbb{R}^{6 times (m+n)}

，并使其满足：

mathcal{V}_{e}=J_{e}(theta)left[begin{array}{c} u \ dot{theta} end{array}right]=left[J_{mathrm{base}}(theta) J_{operatorname{arm}}(theta)right]left[begin{array}{c} u \ dot{theta} end{array}right]

其中，

J_{base}(theta) in mathbb{R}^{6 times m}

和

J_{arm}(theta) in mathbb{R^{6 times n}}

分别表示底盘和机械臂的雅克比矩阵。这两个雅克比矩阵的构建方法在《Modern Robtics》的第6.2章节有详细的介绍，请参考原书内容。在我们的源代码中，我们直接使用了这个雅克比矩阵进行计算。

这里我们要构建一个函数FeedbackControl()用于叠加前馈和反馈控制算法，其计算公式如下：

mathcal{V}(t)=left[operatorname{Ad}_{X^{-1} X_{d}}right] mathcal{V}_{d}(t)+K_{p} X_{mathrm{err}}(t)+K_{i} int_{0}^{t} X_{mathrm{err}}(mathrm{t}) d mathrm{t}

其中，

K_p

和

K_i

是控制器的参数。函数的输入和输出分别为：

输入：

当前机时刻械手的实际Configuration

（或者写作

T_{se}

）

当前机时刻械手的参考Configuration

X_d

（或者写作

T_{se}_d

），

是desired的意思

从参考轨迹中取得的下一个时刻(一个

Delta_t

以后)机械手的参考Configuration

X_{d_next}

（或者写作

T_{se}_{d_next}

）

PI控制的参数

K_p

和

K_i

时间

Delta_t

输出：

在坐标系

{e}

中表示的机械手的旋量

mathcal{V}

当前时刻机械手的Configuration可以表示为

X(q, theta)

，

和

theta

分别表示底盘和机械臂的Configuration，这两个Configuration可以从我们前面设计的Simulator获取，其中具体元素的值在V-REP中可以直接通过函数接口获取得到，在现实情况下也可以通过高精度的传感器获取。

X_{err}

表示

X_d

与

的偏差，计算方式为

[X_{err}]=log(X^{-1}_d)

，此外，

[mathcal{V}_d]=(frac{1}{Delta_t}) log (X^{-1}_d X_{d_next})

，

[A d_{X^{-1}X_d}]

表示一个变换，用来将旋量

mathcal{V}_d

从参考坐标系

X_d

中转换到当前的实际坐标系

中。最后，我们需要将FeedbackControl()函数的输出

mathcal{V}

转换为轮子的角速度和机械臂各个关节的运动角速度：

left[begin{array}{c} u \ dot{theta} end{array}right]=J_{e}^{dagger}(theta) mathcal{V}

其中，

J^{dagger}_e(theta)

表示雅克比矩阵

J_e(theta)

的逆。

这部分内容应该是本期笔记中最难的部分，由于篇幅有限，很大一部分内容我都直接略过去了，没有讲细节，如果想要搞明白其中的细节，请参考《Modern Robotics》原书内容，所以还是强烈建议读者买中文版的书籍，结合英文版的看，更容易理解。

3. 算法实现

算法实现我们使用Python来完成，并通过远程通信接口将计算结果发送给V-REP中的youBot模型，完成仿真过程，因此我们会用到前期系列笔记中的部分代码，例如youBot的运动学模型代码，Python远程控制V-REP中模型的代码等，所有的代码都已经开源在github上，我们分别实现了前面所说的3个函数：

在实际编码的过程中，我们还会用到一些前期笔记中未使用到的接口函数，这里做一些简要说明。

3.1 获取Cube的位置信息

首先，我们通过手动放置和调整工作台让它处于一个合适的位置，可以自己随意设定，然后在上面放置我们的物体Cube。本文附带的源代码中有相关的V-REP工程文件，其中内置的工作台和Cube可以自己移动和编辑。在V-REP中仿真时，我们可以通过simxGetObjectPosition( )这个Python的函数接口来获得Cube在V-REP中的空间位置，因此我们可以直接获取得到Cube的初始位置以及需要放置的目标位置，Python代码如下：

_, cube_1_position = vrep_sim.simxGetObjectPosition(clientID, cube_1_handle, -1, vrep_sim.simx_opmode_blocking)
_, cube_2_position = vrep_sim.simxGetObjectPosition(clientID, cube_2_handle, -1, vrep_sim.simx_opmode_blocking)
_, cube_goal_position = vrep_sim.simxGetObjectPosition(clientID, cube_goal_handle, -1, vrep_sim.simx_opmode_blocking)

同样地，youBot机器人本身的位置和姿态也可以通过该函数接口直接获取，详见源代码。

本文开放的源代码已经在Python3.6的平台上测试通过，在实际使用时记得先将V-REP对应的仿真环境运行起来，再运行main.py。

3.2 机器人运动学模型计算和雅克比矩阵

算法的实现过程使用了《Modern Robotics》一书中的部分基础性的源码，主要是一些矩阵运算、机器人运动学计算的一些函数，这些代码都是开源的，地址在这里。当然，这个开源的代码除了Python版本，还有Matlab和C++版本，能够覆盖大部分读者的使用需求了。获取《Modern Robotics》书籍的英文版，可在公众号“博士的沙漏”后台回复【MR】。

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