用于工业机器人的末端执行器的旋转运动规划方法
【技术领域】
[0001] 本发明设及工业机器人技术领域，特别设及一种用于工业机器人的末端执行器的 旋转运动规划方法。
【背景技术】
[0002] 工业机器人在使用过程中通常要安装面向特定应用的工具，即末端执行器。工具 的运动可用刚体运动来描述，包括工具中屯、点的平动和工具的转动。因为工业机器人的控 制发生各轴上，工具的旋转运动与工具中屯、点的平动一样会映射到轴空间，所W旋转运动 也需要平滑地进行。现有的工业机器人末端执行器旋转运动规划方法主要有：
[0003] (1) Ξ轴同时进行姿态插补
[0004] 将机器人末端姿态表示为欧拉角的形式，即相当于绕Ξ个轴从单位姿态做旋转运 动。通过对初末姿态的Ξ个轴的欧拉角进行插值，得到末端旋转运动的轨迹。运种方法将姿 态分解为Ξ个独立变量进行插补，在两个旋转运动平滑衔接时易于处理。但是欧拉角表示 姿态存在奇异性，而且运种方法插补出来的旋转运动绕Ξ个轴旋转，不直观。
[000引（2巧由角法插补
[0006] 将机器人末端姿态的变化等效于绕等效转轴转动某个角度。运种插补方法形成的 旋转运动的转轴不变，将旋转运动规划转化为对某个角度的单自由度规划问题从而得W解 决。但在两个旋转运动平滑衔接时，机器人末端的旋转角速度方向会发生突变，造成很大加 速度，使机器人发生抖动。
【发明内容】
[0007] 本发明的目的旨在至少解决所述技术缺陷之一。
[0008] 为此，本发明的目的在于提出一种用于工业机器人的末端执行器的旋转运动规划 方法，使用四元数来描述机器人末端的姿态，避免了欧拉角奇异性的问题，将旋转运动规划 转化为初末四元数的插补问题，旋转过程绕单轴运动更加直观。
[0009] 为了实现上述目的，本发明的实施例提供一种用于工业机器人的末端执行器的旋 转运动规划方法，包括如下步骤：
[0010] 步骤S1，输入末端执行器的旋转运动的初末姿态，并将所述初末姿态变换为四元 数表示；
[0011] 步骤S2，判断所述末端执行器是否处于旋转运动转弯区；
[0012] 步骤S3,如果所述末端执行器未处于转弯区内，则采用四元数球面线性插补方法 对所述初末姿态进行插补，计算初末姿态的四元数表示计算转轴向量，根据所述转轴向量 和所述末端执行器绕等效转轴的转角计算所述末端执行器旋转运动的角速度和角加速度；
[0013] 步骤S4,如果所述末端执行器处于转弯区内，则采用四元数球面线性插补方法连 续两次对转弯区的两条旋转轨迹进行插补，计算末端执行器的姿态数据，计算所述姿态数 据的一阶导数，根据所述姿态数据的一阶导数计算所述末端执行器旋转运动的角速度;计 算所述姿态数据的二阶导数，根据所述姿态数据的二阶导数计算所述末端执行器旋转运动 的角加速度。
[0014]进一步，在所述步骤S1中，所述初末姿态变换的四元数q表示为：
[0019] 其中，trace(R)为所述末端执行器旋转运动的旋转矩阵的迹。
[0020] 进一步，在所述步骤S3中，所述根据采用四元数球面线性插补方法对所述初末姿 态进行插补，包括如下：
[0021 ]首先，根据初末姿态qo，qi，计算两个姿态间的夹角，
[0022] 0〇 = arccos(qoqi)；
[0023] 采用所述四元数球面线性插补算法对所述初末姿态进行插补，得到t时刻姿态四 元数为：
[0024]
[0025] 其中，0(t)为t时刻末端执行器绕等效转轴的转角。
[0026] 进一步，在所述步骤S3中，所述计算初末姿态的四元数表示计算转轴向量为：
[0027] r = log(qo-iqi)/arccos(qoqi)。
[0028] 进一步，在所述步骤S3中，所述计算所述末端执行器旋转运动的角速度为：
[0029]
[0030] 其中，^(0为末端执行器绕等效转轴的转角0(t)的一阶导数；
[0031 ]所述计算所述末端执行器旋转运动的角加速度为：
[0032]技=r Q(j)
[003引其中，为末端执行器绕等效转轴的转角e(t)的二阶导数。
[0034] 进一步，采用四元数球面线性插补方法连续两次对转弯区的两条旋转轨迹进行插 补，包括如下步骤：
[0035] 设转弯区旋转轨迹的参数为h，当h变化时，两条旋转轨迹的四元数为和 二者都采用四元数球面线性插补方法的一次插值而得到，然后再对两者采用四元数 球面线性插补方法的二次插值，并将第Ξ个参数改为P化)的控制函数；
[0038] 进一步，在所述步骤S4中，计算末端执行器的姿态数据为：
[0039] 其中
[0040] 根据本发明实施例的用于工业机器人的末端执行器的旋转运动规划方法，使用四 元数来描述机器人末端的姿态，避免了欧拉角奇异性的问题，将旋转运动规划转化为初末 四元数的插补问题，旋转过程绕单轴运动更加直观。在机器人末端从上一条轨迹到下一条 轨迹的转弯区内利用两次四元数球面线性插补来实现旋转运动的平滑衔接，使用旋转运动 四元数的一二阶导数计算输出全旋转运动过程的姿态、角速度和角加速度算法，可用于工 业机器人动力学前馈控制和操作空间控制。
[0041] 本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0042] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得 明显和容易理解，其中：
[0043] 图1为根据本发明一个实施例的用于工业机器人的末端执行器的旋转运动规划方 法的流程图；
[0044] 图2为根据本发明另一个实施例的用于工业机器人的末端执行器的旋转运动规划 方法的流程图；
[004引图3为根据本发明实施例的末端执行器的姿态轨迹示意图。
【具体实施方式】
[0046] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终 相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附 图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。
[0047] 如图1所示，本发明实施例的用于工业机器人的末端执行器的旋转运动规划方法， 包括如下步骤：
[0048] 步骤S1，输入末端执行器的旋转运动的初末姿态，并将初末姿态变换为四元数表 /J、- 〇
[0049] 初末姿态变换的四元数q表示为：
[0050] q = yj{/race(R)) / 2
[0051] qx=(I?32-R23)/4qw;
[0052] qy=(Ri3-R3i)/4qw；
[005引 qz= (R2i-Ri2)/4qw;
[0054] 其中，trace(R)为末端执行器旋转运动的旋转矩阵R的迹。
[0055] 步骤S2,判断末端执行器是否处于旋转运动转弯区。
[0056] 步骤S3,如果末端执行器未处于转弯区内，则采用四元数球面线性插补方法对初 末姿态进行插补，计算初末姿态的四元数表示计算转轴向量，根据转轴向量和末端执行器 绕等效转轴的转角计算末端执行器旋转运动的角速度和角加速度。
[0057]根据采用四元数球面线性插补方法对初末姿态进行插补，包括如下：
[005引首先，根据初末姿态qo，qi，计算两个姿态间的夹角，
[0059] 0〇 = arccos(qoqi) ； (1)
[0060] 采用四元数球面线性插补算法对初末姿态进行插补，得到t时刻姿态四元数为：
[0061]
[0062] 其中，0(t)为t时刻末端执行器绕等效转轴的转角。由初末角度和角速度都为零的 边界条件可调用一般S形速度规划方法来进行规划。
[0063] 插补完成后，四元数球面线性插值中转轴方向不变，计算初末姿态的四元数表示 计算转轴向量为：
[0064] r = log(q〇-iqi)/a;rccos(qoqi); (3)
[0065] 其中，log为初末姿态四元数差值的对数。
[0066] 根据转轴向量和末端执行器绕等效转轴的转角计算末端执行器旋转运动的角速 度和角加速度。
[0067] 具体地，计算末端执行器旋转运动的角速度为：
[006 引
[0069] 其中，为末端执行器绕等效转轴的转角0(t)的一阶导数。
[