0070] 计算末端执行器旋转运动的角加速度为：
[0071]
[0072] 其中，^1为末端执行器绕等效转轴的转角0(t)的二阶导数。
[0073] 步骤S4,如果末端执行器处于转弯区内，则采用四元数球面线性插补方法连续两 次对转弯区的两条旋转轨迹进行插补，计算末端执行器的姿态数据，计算姿态数据的一阶 导数，根据姿态数据的一阶导数计算末端执行器旋转运动的角速度;计算姿态数据的二阶 导数，根据姿态数据的二阶导数计算末端执行器旋转运动的角加速度。
[0074] 采用四元数球面线性插补方法连续两次对转弯区的两条旋转轨迹进行插补，包括 如下步骤：
[0075] 旋转运动的轨迹由相连两条旋转轨迹确定，设转弯区旋转轨迹的参数为h，其范围 是[0，1 ]，当h变化时，两条旋转轨迹的四元数为：和iW，二者都采用四元数球面线 性插补方法的一次插值而得到，然后再对两者采用四元数球面线性插补方法的二次插值， 并将第Ξ个参数改为P化)的控制函数。在运种条件下可使得平滑相连的两个旋转运动平滑 过渡。
[0076]
[0077] 其中，p化)满足
[0078] 在转弯时，转轴平滑过渡，因此需要通过计算姿态随时间的变化率来计算角速度。 转弯区时任意时刻的姿态都可写成W下形式：
[0079] 计算末端执行器的姿态数据为
[0080] 其中
[0081] 然后，计算姿态数据的一阶导数，根据姿态数据的一阶导数计算末端执行器旋转 运动的角速度。
[0089]计算姿态数据的二阶导数，根据姿态数据的二阶导数计算末端执行器旋转运动的 角加速度。
[0099] 图2为根据本发明另一个实施例的用于工业机器人的末端执行器的旋转运动规划 方法的流程图。
[0100] 步骤S201，输入旋转运动初末姿态。
[0101] 步骤S202，将初末姿态变换为四元数表示。
[0102] 初末姿态变换的四元数q表示为：
[0107] 步骤S203,判断是否处于旋转运动转弯区。
[0108] 步骤S204,通用四元数球面线性插补子程序和通用一维速率规划子程序，调用初 末slerp和速率规划子程序进行插补。
[0109] 步骤S205,计算初末四元数差值得对数，得到转轴矢量。
[0110] r = log(q〇-iqi)/a;rccos(qoqi)
[0111] 步骤S206，速度方向和速率组合为速度，加速度方向和加速率组合为加速度。
[0112] 步骤S207,两次调用初末sle巧对相连旋转运动姿态进行插补。
[0113] 步骤S208,计算姿态四元数的一阶导数。
[0114] 步骤S209，计算姿态四元数的二阶导数。
[0115] 步骤S210,根据姿态的一二阶导数直接求得旋转运动的角速度和角加速度。
[0116] 末端执行器旋转运动的角速度如下
[0117] 末端执行器旋转运动的角加速度如下：
[0121] 图3为根据本发明实施例的末端执行器的姿态轨迹示意图。
[0122] 步骤S211，输出旋转运动的姿态、角速度和角加速度。
[0123] 根据本发明实施例的用于工业机器人的末端执行器的旋转运动规划方法，使用四 元数来描述机器人末端的姿态，避免了欧拉角奇异性的问题，将旋转运动规划转化为初末 四元数的插补问题，旋转过程绕单轴运动更加直观。在机器人末端从上一条轨迹到下一条 轨迹的转弯区内利用两次四元数球面线性插补来实现旋转运动的平滑衔接，使用旋转运动 四元数的一二阶导数计算输出全旋转运动过程的姿态、角速度和角加速度算法，可用于工 业机器人动力学前馈控制和操作空间控制。
[0124] 在本说明书的描述中，参考术语"一个实施例"、"一些实施例"、"示例"、"具体示 例"、或"一些示例"等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特 点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不 一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可w在任何 的一个或多个实施例或示例中W合适的方式结合。
[0125]尽管上面已经示出和描述了本发明的实施例，可W理解的是，上述实施例是示例 性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨 的情况下在本发明的范围内可W对上述实施例进行变化、修改、替换和变型。本发明的范围 由所附权利要求极其等同限定。
【主权项】
1. 一种用于工业机器人的末端执行器的旋转运动规划方法，其特征在于，包括如下步 骤： 步骤S1，输入末端执行器的旋转运动的初末姿态，并将所述初末姿态变换为四元数表 示； 步骤S2,判断所述末端执行器是否处于旋转运动转弯区； 步骤S3，如果所述末端执行器未处于转弯区内，则采用四元数球面线性插补方法对所 述初末姿态进行插补，计算初末姿态的四元数表示计算转轴向量，根据所述转轴向量和所 述末端执行器绕等效转轴的转角计算所述末端执行器旋转运动的角速度和角加速度； 步骤S4,如果所述末端执行器处于转弯区内，则采用四元数球面线性插补方法连续两 次对转弯区的两条旋转轨迹进行插补，计算末端执行器的姿态数据，计算所述姿态数据的 一阶导数，根据所述姿态数据的一阶导数计算所述末端执行器旋转运动的角速度;计算所 述姿态数据的二阶导数，根据所述姿态数据的二阶导数计算所述末端执行器旋转运动的角 加速度。2. 如权利要求1所述的用于工业机器人的末端执行器的旋转运动规划方法，其特征在 于，在所述步骤S1中，所述初末姿态变换的四元数q表示为：qx= (R32-R23)/4qw； Qy= (Rl3-R3l)/4qw； 化=(R2i_Ri2)/4qw; 其中，trace(R)为所述末端执行器旋转运动的旋转矩阵的迹。3. 如权利要求1所述的用于工业机器人的末端执行器的旋转运动规划方法，其特征在 于，在所述步骤S3中，所述根据采用四元数球面线性插补方法对所述初末姿态进行插补，包 括如下： 首先，根据初末姿态qo，qi，计算两个姿态间的夹角， 白o = arc cos(qoqi)； 采用所述四元数球面线性插补算法对所述初末姿态进行插补，得到t时刻姿态四元数 为：其中，e(t)为t时刻末端执行器绕等效转轴的转角。4. 如权利要求3所述的用于工业机器人的末端执行器的旋转运动规划方法，其特征在 于，在所述步骤S3中，所述计算初末姿态的四元数表示计算转轴向量为： r = log(q〇-iqi)/arccos(qoqi)。5. 如权利要求4所述的用于工业机器人的末端执行器的旋转运动规划方法，其特征在 于，在所述步骤S3中，所述计算所述末端执行器旋转运动的角速度为：其中，为末端执行器绕等效转轴的转角e(t)的一阶导数； 所述计算所述末端执行器旋转运动的角加速度为：其中，为末端执行器绕等效转轴的转角e(t)的二阶导数。6. 如权利要求1所述的用于工业机器人的末端执行器的旋转运动规划方法，其特征在 于，在所述步骤S4中，采用四元数球面线性插补方法连续两次对转弯区的两条旋转轨迹进 行插补，包括如下步骤： 设转弯区旋转轨迹的参数为h，当h变化时，两条旋转轨迹的四元数为和 二者都采用四元数球面线性插补方法的一次插值而得到，然后再对两者采用四元数球面线 性插补方法的二次插值，并将第Ξ个参数改为P化)的控制函数；其中，P化)满足7. 如权利要求6所述的用于工业机器人的末端执行器的旋转运动规划方法，其特征在 于，在所述步骤S4中，计算末端执行器的姿态数据为：= 其中，
【专利摘要】本发明提出了一种用于工业机器人的末端执行器的旋转运动规划方法，包括：输入末端执行器的旋转运动的初末姿态并变换为四元数表示；判断末端执行器是否处于旋转运动转弯区；如果未处于转弯区内，则采用四元数球面线性插补方法对初末姿态进行插补，计算初末姿态的四元数表示计算转轴向量，根据转轴向量和末端执行器绕等效转轴的转角计算末端执行器旋转运动的角速度和角加速度；如处于转弯区内，则采用四元数球面线性插补方法连续两次对转弯区的两条旋转轨迹进行插补，计算角速度和角加速度。本发明使用四元数来描述机器人末端的姿态，避免了欧拉角奇异性的问题，将旋转运动规划转化为初末四元数的插补问题，旋转过程绕单轴运动更加直观。
【IPC分类】B25J9/16
【公开号】CN105563482
【申请号】CN201510852696
【发明人】韩建欢, 曹华
【申请人】珞石（北京）科技有限公司
【公开日】2016年5月11日
【申请日】2015年12月1日