首页 > 其他分享> > 机器人轨迹生成--基于《MODERN ROBOTICS》

机器人轨迹生成--基于《MODERN ROBOTICS》

2020-01-30 15:09:41 作者：互联网

机器人轨迹的定义是：机器人位置随时间变化的规范称为轨迹。
有些情况下，机器人的轨迹完全由任务决定，例如，末端执行器需要跟踪一个已知运动的物体；另一些情况下，任务只是简单的要求机器人在一个给定的时间内从一个位置运动到另一个位置。
轨迹应该是关于时间平滑的函数，而且要满足关节速度、加速度、力矩的要求。
这里主要考虑三种情况的轨迹生成：在关节和任务空间点到点的直线轨迹、在给定时间通过点的轨迹、考虑关节驱动限制的时，通过给定路径的最短时间轨迹。

一、定义

路径：机器人完成的配置序列的纯几何描述。路径θ(s)，将标量路径参数s（假设路径开始时s为零，结束时s为一）映射到机器人配置空间Θ中的一个点，即θ : [0, 1] -> Θ。当s从0变到1，机器人即沿着路径从起点运动到终点。

时间尺度：上个定义中的s就是时间尺度，是实际时间的函数，即s(t)。当这段轨迹使用的实际时间为T，则t∈[0, T]，s : [0, T] -> [0, 1]。

轨迹：轨迹（trajectory）由路径（path）和时间尺度（time scaling）组成，即θ(s(t))或θ(t)。

二、点到点的轨迹

2.1 直线路径
1.关节空间
在这里插入图片描述
2.用最小坐标集表示的笛卡尔空间
最小坐标集的意思是，在笛卡尔空间用三个变量表示位置，用三个变量表示姿态的方法。例如表示姿态的方法：固定角坐标系、欧拉角、RPY角等都属于最小坐标集表示姿态的方法。
在这里插入图片描述
3.齐次变换矩阵表示的笛卡尔空间

其中X表示4x4齐次变换矩阵。
将位置和姿态解耦表示为：

其中，p是表示位置的向量，R是表示姿态的3x3旋转矩阵。

位置和姿态解耦之前和之后对比，可以看到姿态都是连续变化，但解耦之前位置不是直线而是螺旋轨迹。

笛卡尔空间插补需要注意两个问题：
1.当路径通过奇异点附近时，关节速度会变得非常大，以至于不可达；
2.由于机器人在任务空间可达的范围不一定是凸面，所以有可能起始点和终止点都在可达范围，但是中间插补点却超出了范围，如下图：
在这里插入图片描述
图中表示的是一个两关节连杆，上图是关节空间直线插补，在各关节都是直线运动，但是在任务空间却不是直线。下图表示的是笛卡尔空间直线插补，任务空间是直线，但在关节空间不是直线，而且起始点和终止点都在运动范围内，但插补出的中间直线有一段不在运动范围内。

2.2 直线路径的时间尺度
1.三次多项式
在这里插入图片描述
如图所示为三次多项式时间尺度的

三次曲线的最常用形式如下：

在点到点的轨迹中，需要满足的条件为：

于是得到三次多项式的系数如下：

代入上述点到点直线路径公式中得到：

由三次曲线的图像可以看出，最大关节速度在t=T/2处得到，所以：
在这里插入图片描述
同样，最大关节加速度和减速度分别在t=0和t=T时得到：

由此可以达到一个适当的运动时间T，以满足关节速度或加速度的限制。
同样的方法，可以得到五次多项式的曲线如下：

五次多项式增加了两个变量，需要增加两个限制条件：
在这里插入图片描述
同理可以得到梯形曲线：

以及S形曲线：

三、通过点的轨迹

假设关节轨迹需要通过k个点，通过第一个点的时间为T1=0，通过最后一个点的时间为Tk=T。对于每一个通过点 i∈{1,…,k}，其期望的位置为：
在这里插入图片描述
期望的速度为：

轨迹共有k-1段，第 j∈{1,…,k-1}段的持续时间为：

在第j段的关节轨迹用三次多项式表示为：

其中

第j段的轨迹需要满足如下四个条件：

由此可以求解出系数：

上图表示了两种通过四个点的曲线轨迹，通过点的路径形状取决于通过的点的指定速度大小和方向。选择合适的时间和速度，可以使轨迹更加平滑。

四、时间最优的时间尺度

4.1问题描述
这一节的目的是，找到一个时间尺度函数s(t)，使得在满足机器人各关节力矩限制的情况下，以最短时间沿给定的轨迹运动。
机器人的一般动力学模型如下：
在这里插入图片描述
机器人第i个关节力矩限制如下：

可见，第i个关节的最大最小力矩与关节的位置和速度都有关，这一点从动力学方程可以看出。
将上述动力学方程做如下改写：

得到

于是由动力学方程可以得到如下更紧凑的形式：
在这里插入图片描述
同样将关节力矩限制改写得到：

代入得：

令最小最大的s加速度分别为：

则

定义：

则第i个关节驱动器的力矩限制可以写成：

则时间最优的时间尺度问题可以改写成如下：
给定一个θ(s)，求一个两次可微的单调递增函数s(t):[0,T] -> [0,1]，满足：
在这里插入图片描述
以及沿着路径运动的总时间最小，且要满足上式的关节力矩限制。

4.2
在这里插入图片描述

如上图所示，上述的时间最优时间尺度问题，就是要在该坐标系中找到一条从(0,0)到(1,0)的曲线，由于s(t)必须单调递增，所以：

所以该时间尺度曲线必须处于第一象限。
根据限制条件：

可以在该平面内画出限制条件的边界，如下图黑色实线。
当：
在这里插入图片描述
属于不可达区域，如下图灰色区域。
而在可达区域的每个点都能找到一个满足限制条件的锥形范围，时间尺度曲线的切线必须处于该锥形范围才能满足关节限制条件。
在边界上，只有一个方向可选，即边界曲线的切线。
在这里插入图片描述
如上图中右图就不能满足关节限制条件，因为时间尺度曲线（半圆曲线）上的一点切线方向不处于锥形范围内。
由上述分析可知，如果让关节一直以最大或最小加速度运行，那么运行的时间肯定是最短的，即让时间尺度曲线的切线沿着锥形的上边界或下边界。
在这里插入图片描述
如上图a所示，时间最优化和非时间最优化的时间尺度曲线对比。最优化时，曲线上每一点的切线都是锥形的上边界或下边界，而非最优时，曲线上每一点的切线都处于锥形中间。对于时间最优化问题最重要的是找到从最大加速度转换成最小减速度的临界点s*。如右图所示，当曲线遇到边界时，会出现不止一个临界点的情况，所以寻找时间最优的时间尺度曲线问题变成了寻找临界点的问题。
4.3时间尺度算法
时间尺度算法按如下步骤进行：
step 1. 初始化空的临界点集合：
在这里插入图片描述
初始化临界点计数：i=0，且：

step 2. 从(1,0)开始，令

反向推倒曲线，直到

（表示碰到边界曲线）或s=0为止，这条曲线叫做F，如下图step 2所示。
step 3. 令

从

开始前向推倒曲线，直到

（表示碰到边界曲线）。这条曲线叫做Ai，如果Ai与F相交，则递增i，且
在这里插入图片描述
并且将si加入到临界点集合S中。si就是最大加速度到最大减速度转换的临界点。那么问题就已经解决了，集合S就是所有临界点的集合。
如果Ai与F没有交点，而是与边界曲线相交，如下图step 3所示，那么交点设为
在这里插入图片描述
进行下一步。
step 4. 如图step 4所示，在

以二分法的方式搜索一个点

以

从起始点

前向推倒曲线，直到与边界曲线相切，令切点为

step 5. 令

从

开始反向推倒曲线，直到与Ai曲线相交，如图step 5所示。将i递增，将这段曲线命名为Ai，将交点坐标记录到集合S中，作为第i个临界点。
step 6. 将i递增，同时
在这里插入图片描述
将交点坐标记录到集合S中，如下图step 6所示。然后重复step 3.