UR3机器人轨迹规划之笛卡尔空间轨迹规划

UR3机器人系列文章是我以及同学做的大作业，在此分享出来。本文轨迹规划部分是我同学所做。

笛卡尔空间规划是在笛卡尔空间中规划机器人末端执行器的运行轨迹。其在实际中使用非常广泛。由于笛卡尔空间的轨迹规划是在三维空间中进行的，因此其轨迹也是在三维空间中表示的。同时，可以证明在笛卡尔空间中光滑的轨迹，只要其没有穿越奇异点，其在关节空间中也是光滑的，因此我们不需要在关节空间重新进行轨迹规划。笛卡尔空间规划可以实现机器人末端执行器按照直线，圆，甚至一般曲线运动。以下将分别介绍在笛卡尔空间中的三种轨迹的实现方法。

1. 直线轨迹的实现

直线轨迹适用于点对点之间的规划。不适用于含有中间点的情况下的轨迹规划。其具体算法为可描述如下。不妨令起点为，终点为，一个插补周期为，速度随时间的变化曲线为。

1. 计算起点和终点的距离；
2. 检查是否满足；
3. 将在轴上分解为；
4. 计算每个插补点的坐标；
5. 将这些点通过逆运动学求逆得到对应的关节值，然后输入到关节驱动器中即可使机器人按照该直线运动。

大部分情况下，我们对机器人在直线运动时的速度没有太多的限制，只要在指定时间内运动到目标点即可。因此不妨假设机器人匀速运动，则其算法可以描述为：

1. 计算起点和终点的距离；
2. 计算总插补时间；
3. 计算总的插补点数；
4. 计算插补增量；
5. 计算直线插补轨迹上各个点的坐标；
6. 将这些点通过逆运动学求逆得到对应的关节值，然后输入到关节驱动器中即可使机器人按照该直线运动。

根据上述描述的算法，我们可以写出直线轨迹规划的程序。不妨假设机器人从运行到，插入点数为50个。运行程序，将得到的结果可视化，如下图所示：

从图中我们可以看出，程序在起始点和终止点之间插入了50个点，从而形成一条直线型的轨迹。但是当运动轨迹中含有多个中间点时，采用直线规划将会导致机器人运动轨迹不平滑。此时就需要使用曲线规划。

2. 圆轨迹的实现

我们知道在空间中，三个点就可以确定一个圆，因此除非恰好需要的轨迹是圆，否则采用圆来进行轨迹规划只适用于只有三个路径点的情况。首先我们需要介绍根据空间中三个点求圆的方程的基本原理。根据空间圆的基本方程和三点共面的要求，我们可以得到如下方程组：

但该算法存在缺陷，当且时，结果分母中会出现0。此时无法得到结果，并且该种方法也不方便进行插值。因此比较好的一种方法是将三维空间问题转换为平面问题然后再求解。此方法的另外一种好处是方便进行插补。在平面上对轨迹进行插补之后通过转换矩阵即可转移到三维空间中。

其具体实现如下所示。假设三个路径点为，，，其形成的平面为平面，原坐标系为，新坐标系为，期望的运动时间为， 轨迹的插补周期为。

1. 由平面方程计算平面的法矢向量的方向是为：
   

2. 以平面的法矢向量方向作为新坐标系的轴方向，轴方向余弦为：
   

3. 以方向作为新坐标系的轴方向，轴的方向余弦为：
   

4. 以轴的方向余弦与轴的方向余弦的叉乘作为新坐标系的轴的方向余弦，轴余弦为：
   

5. 求出新坐标系在坐标系下的变换矩阵：
   

6. 求出点在新坐标系下对应的坐标值，其中：
   

7. 规定插补角。我们不妨约定在坐标系中平面内逆时针方向为其插补方向，即由到再到的圆弧始终为逆时针圆弧，其大小满足：
   

根据以上原理我们可以写出笛卡尔空间圆形轨迹规划的代码。我们不妨以机器人末端执行器在桌面上以圆形轨迹运动为例来验证该轨迹规划的效果。假设平面上的三个点分别为（下图右方图像）和（下图左方图像）运行程序，将得到的路径点用matplotlib进行可视化如下：

从图中可以看出，程序根据给出的三个路径点，规划出了一条依次经过的圆形轨迹。但是当存在多个路径点时，用圆规划就只能三个点三个点地规划，这将很难保证轨迹的连续性，且在计算过程中也很麻烦，因此人们提出了用一般曲线来规划轨迹。

3. 一般曲线轨迹的实现

空间一般曲线的规划往往非常复杂，涉及到空间曲线表达式的求解。而我没有找到用于空间曲线规划的python库。因此只能自己研究一种新的算法。我的整体思路是把三维空间转换到二维空间来求解，于是自然就想到了对三个变量两两进行拟合，求得的数取平均作为拟合曲线的点，其整体思路可以描述如下：

1. 用python自带的二维拟合函数分别对进行拟合和插值得到；
2. 计算，从而得到拟合和插值后的曲线

根据以上算法我们可以写出相应程序。不妨假设机器人末端执行器从点开始运动，依次经过、、。运行程序，得到结果图如下所示：

从图中我们可以看出在空间中该轨迹规划方法拟合效果很差，空间曲线几乎没有经过我们给定的任何一个路径点。将拆开来看，分别观察、和的拟合效果，其结果可以用下图表示：

从图中我们可以看出，三坐标轴规划后的点没有一个是通过我们原来给定的路径点的。因此该方法效果很差。仔细观察拟合效果我们可以发现，其实两两规划时，python第三方库SciPy的效果特别好。因此我们不妨可以先规划，得到对应的规划点，然后再规划，得到对应的拟合函数。此时我们将规划出来的带入到规划出来的函数中去求出最终的值，从而完成轨迹的完整规划。其算法可以描述如下：

1. 将进行拟合和插值得到（此处、均为数组）
2. 将进行拟合得到相应的函数表达式
3. 将代入中得到
4. 即为我们需要规划出来的轨迹

根据如上算法我们可以写出相应程序。不妨仍以机器人末端执行器从点开始运动，依次经过、、为例。运行程序，得到效果图如下所示：

从图中我们可以看出，规划出来的轨迹以平滑的曲线准确地经过了空间中我们设置的所有路径点，效果较为理想。我们再依次检验、和的拟合效果，其效果如下所示：

从上图中我们可以看出，、和的拟合效果都非常理想，平滑曲线准确经过了我们设定的各个路径点。因此这种算法是可靠的。

但是，我们又会发现一个问题，那就是一般的函数拟合都要求自变量必须是单调递增的。而我们在设置轨迹时往往并不能保证中至少存在一个变量单调递增。因此我们需要对算法进行相应的改进。这时我们自然想到了自然界中一个存在于任何场景的保证永远单调递增的实物-时间。因此改进后的算法可以描述为：

1. 获取输入的时间列表，如果没有的话自动补充时间
2. 分别拟合、、，得到函数、、
3. 将时间按照步长为进行插值，得到新的列表
4. 依次将带入、、得到对应的、、
5. 点集即为规划出来的轨迹上的路径点

根据以上算法我们可以写出相应程序，考虑到有时候我们并不需要机器人末端执行器准确经过我们给定的路径点，因此该函数除了接收给定的路径点外，还可以通过参数accurate指定曲线是否准确经过每个路径点，accurate越大曲线越光滑，但越可能不经过给定的路径点。与此同时，考虑到有时候我们希望机器人循环某一个动作，这时候就需要考虑返回起始点时的曲线和开始时的出发曲线之间的光滑性问题。程序也提供了该功能，可以通过loop参数指定该轨迹是否要求循环，loop=1表示轨迹需要循环执行。不妨假设机器人末端执行器从开始，依次经过、、、后又回到起始点，并且要求机器人准确经过各点，同时在终点和起点之间要光滑连接（即要循环执行这段指令)。运行程序，得到如下结果：

图中可以看出，轨迹光滑，且准确经过了我们设置的每个路径点，同时在连接起点和终点时曲线光滑，效果较为理想。

源码请前往MyShare/UR3 at master · TyroGzl/MyShare (github.com)

如无法访问，也可关注公众号【G的科研生活】，回复【UR3】