CN116117796B - 一种工业机器人姿态轨迹过渡与速度规划方法及系统 - Google Patents

Abstract

本发明属于工业机器人轨迹规划领域，为工业机器人姿态轨迹过渡与速度规划方法及系统，其方法包括：把姿态欧拉角向量转化为单位四元数，计算相邻姿态单位四元数之间的角位移；根据姿态轨迹过渡比例系数和角位移，计算所有过渡姿态处的过渡转角；确定两个相邻姿态之间的剩余非过渡角位移；对剩余非过渡角位移进行速度规划；在过渡姿态处拟合得到转接姿态，对转接姿态进行速度规划；拼接非过渡段和过渡段规划后的姿态四元数，转化为姿态欧拉角向量，得到姿态轨迹插补命令；计算运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束。本发明实现了速度和加速度约束下的姿态转接速度平滑过渡，缩短了姿态频繁变化时的运动时间。

Description

一种工业机器人姿态轨迹过渡与速度规划方法及系统

技术领域

本发明属于工业机器人轨迹规划领域，尤其涉及一种工业机器人笛卡尔姿态轨迹过渡与速度规划方法及系统。

背景技术

目前，随着工业机器人应用场景的多样性和复杂性，工业机器人运动命令中包含的微小姿态轨迹越来越多，导致姿态在工作过程中频繁变化。通常，机器人姿态运动命令是由一系列的离散姿态点组成，在经过每个给定姿态时，线性插补得到的轨迹切线不连续，造成速度和加速度发生突变。为了保持机器人平稳运动，线性插补在姿态过渡处的速度和加速度设置为零，导致机器人的运动时间大幅增加。为了避免不必要的降速，可以通过姿态轨迹平滑过渡处理，使机器人以一定的速度通过每一个给定的姿态。已有的轨迹过渡处理方法大多是针对位置轨迹，对于姿态如何平滑过渡研究相对较少，因此有必要研究姿态频繁变化时如何实现速度规划，以提高机器人运动效率。

于2019年4月16日公开的发明专利CN109623820A提出了一种机器人空间轨迹过渡方法，该方法使用五次贝塞尔曲线和自适应S形速度规划算法实现了直线段轨迹平滑过渡，但是没有涉及姿态过渡。于2020年10月20日公开的发明专利CN111791236A提出了一种工业机器人笛卡尔空间轨迹过渡方法，其是笛卡尔空间不同类型轨迹过渡的方法，针对连续小线段过渡计算量复杂的问题，该专利提出基于抛物线样条的连续微小段轨迹过渡方法，但是采用该方法过渡后的末端轨迹只能满足G1连续。于2019年4月19日公开的发明专利CN109648560A提出的是工业机器人的空间轨迹过渡方法、系统及机器人，其公开了直线运动轨迹和圆弧曲线运动轨的过渡方法，在弓高误差和最大加速度的约束下得到了过渡区域曲线，但是该方法限定了过渡轨迹的类型，适用范围窄。文献“基于5次多项式的机械手姿态平滑规划算法[J].制造业自动化,2013,35(21):3.”公开了一种机械手姿态规划方法，该方法用5次多项式描述角度的插补方程，速度规划后的轨迹可以满足C2连续，但是其在每一个过渡段的首尾速度均为零，使得机器手需要频繁减速为零，影响了工作效率。

从上述现有技术可以看出，机器人姿态的速度规划是限制工作效率的关键，如何考虑最大速度和最大加速度约束，实现速度平滑、加速度连续的姿态速度规划对机器人工作效率提升十分重要。

发明内容

为了克服现有技术所存在的缺陷，本发明提供一种工业机器人姿态轨迹过渡与速度规划方法及系统，实现了速度和加速度约束下的姿态转接速度平滑过渡，缩短了机器人姿态频繁变化时的运动时间。

本发明方法所采取的技术方案是：一种工业机器人姿态轨迹过渡与速度规划方法，包括以下步骤：

输入工业机器人的姿态欧拉角向量；

把姿态欧拉角向量转化为对应的单位四元数；计算所有两个相邻姿态单位四元数之间的角位移；

设置姿态轨迹过渡比例系数；根据姿态轨迹过渡比例系数和角位移，计算除首尾两个姿态之外的所有过渡姿态处的过渡转角；

对每两个相邻姿态，根据过渡转角确定两个相邻姿态之间的剩余非过渡角位移；

根据速度和角速度约束，对剩余非过渡角位移进行速度规划；

根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，根据速度和加速度约束，对转接姿态进行速度规划；

把非过渡段和过渡段规划后的姿态四元数拼接起来，获得插补姿态四元数，并转化为姿态欧拉角向量，得到姿态轨迹插补命令；

根据插补姿态四元数，计算整个运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束。

本发明系统所采取的技术方案是：一种工业机器人姿态轨迹过渡与速度规划系统，包括以下模块：

输入模块，用于输入工业机器人的姿态欧拉角向量；

四元数转化模块，用于把姿态欧拉角向量转化为对应的单位四元数；计算所有两个相邻姿态单位四元数之间的角位移；

过渡转角计算模块，用于设置姿态轨迹过渡比例系数；根据姿态轨迹过渡比例系数和角位移，计算除首尾两个姿态之外的所有过渡姿态处的过渡转角；

非过渡角位移确定模块，用于对每两个相邻姿态，根据过渡转角确定两个相邻姿态之间的剩余非过渡角位移；

速度规划模块，用于根据速度和角速度约束，对剩余非过渡角位移进行速度规划；以及根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，对转接姿态根据速度和加速度约束进行速度规划；

插补模块，用于把非过渡段和过渡段规划后的姿态四元数拼接起来，获得插补姿态四元数，并转化为姿态欧拉角向量，得到姿态轨迹插补命令；

速度计算模块，用于根据插补姿态四元数，计算整个运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束。

从以上技术方案可知，本发明所提供的工业机器人姿态轨迹过渡与速度规划方法及系统，实现了速度和加速度约束下的姿态转接速度平滑过渡，缩短了机器人姿态频繁变化时的运动时间，有效地提供了机器人的工作效率。

附图说明

图1是本发明实施例中工业机器人笛卡尔姿态轨迹过渡与速度规划方法的流程图。

图2是本发明方法实施例中过度后的姿态轨迹图。

图3是本发明方法实施例中速度规划后的合速度和各分速度图。

图4是本发明方法实施例中速度规划后的合加速度和各分加速度图。

图5是本发明方法实施例中速度规划后的合角位移和各分角位移图。

图6是采用本发明方法光顺姿态路径后插补得到的三个姿态欧拉角示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步详细的描述，但本发明的实施方式不限于此。

实施例1

本实施例提供工业机器人笛卡尔姿态轨迹过渡与速度规划方法，如图1-6所示，包括以下步骤：

步骤一：输入工业机器人的离散笛卡尔空间姿态欧拉角向量P_i(α_i,β_i,γ_i)，i＝1,2,3…,N，其中α_i、β_i、γ_i分别是第i个姿态绕x轴、y轴和z轴的转角，N是运动命令中给定的姿态个数(本实施例中可以取N＝3)，设置机器人姿态非过渡段的最大速度和最大加速度，设置机器人过渡段的最大速度和最大加速度。

在本实施例中，可以设置机器人姿态非过渡段起始速度为0rad/s、结束速度为1rad/s、最大速度为5rad/s、最大加速度30rad/s²和最大加加速度600rad/s³，设置过渡段的起始速度为1rad/s、结束速度为1rad/s、最大速度为3rad/s、最大加速度25rad/s²和最大加加速度300rad/s³。

步骤二：把给定的姿态欧拉角向量P_i转化为对应的单位四元数q_i，转化公式如下所示：

计算所有两个相邻姿态单位四元数之间的角位移。在本实施例中，任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移θ_i计算方法如下：

θ_i＝2·acos[q_i(1)q_i+1(1)+q_i(2)q_i+1(2)+q_i(3)q_i+1(3)+q_i(4)q_i+1(4)]；

其中q_i(j)，j＝1,2,3,4，分别表示单位四元数q_i中的第i个元素。

步骤三：设置姿态轨迹过渡比例系数；根据姿态轨迹过渡比例系数和角位移，计算除首尾两个姿态之外的所有过渡姿态处的过渡转角。

本实施例中，将姿态轨迹过渡比例系数ε设置为0.3。对于除首尾两个姿态之外的所有过渡姿态，对比其前两个角位移和后一个角位移的大小，选择较小角位移和过渡比例系数相乘得到过渡转角θ_s,i；所述过渡转角的计算方法如下：

θ_s,i＝min{θ_i-2,θ_i-1,θ_i}·ε

其中θ_i-1是单位四元数q_i和q_i-1之间的角位移，θ_i-2是单位四元数q_i-1和q_i-2之间的角位移。

步骤四：对每两个相邻姿态，根据过渡转角确定两个相邻姿态之间的剩余非过渡角位移。

本实施例中，对所有两个相邻姿态，由两个相邻姿态之间的角位移和过渡转角做差，得到剩余非过渡角位移。

步骤五：根据速度和角速度约束，对剩余非过渡角位移采用S形速度模型直接进行速度规划，具体步骤包括：

根据非过渡段的初始速度、末端速度、最大速度、最大加速度、角位移和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数，非过渡段进入过渡段时的各速度向量和加速度向量；

根据归一化参数对非过渡段采用线性插补，得到插补后的非过渡姿态单位四元数，其插补公式如下：

式中，q_tl是插补后的非过渡姿态单位四元数，Q_sl是非过渡姿态单位四元数的起点，Q_el是非过渡姿态单位四元数的终点，λ是归一化参数，θ_sel是非过渡姿态起点到终点的角位移。

步骤六：根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，根据速度和加速度约束，对转接姿态采用S形速度模型进行速度规划，具体步骤包括：

根据过渡段的初始速度、末端速度、最大速度、最大加速度、角位移和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数u；

根据前一个非过渡段进入过渡段时的各速度向量和加速度向量、过渡段进入后一个非过渡段时的各速度向量和加速度向量，计算得到转接姿态的边界单位四元数，如下式所示：

q₁(1)＝q_1,s(0)；

q_1,s(1)＝q₂(0)；

其中，q₁(1)、和/>分别是第一个非过渡段进入第一个过渡段时的姿态四元数、速度向量和加速度向量，q₂(0)、/>和/>分别是第一个过渡段进入第二个非过渡段时的姿态四元数、速度向量和加速度向量，q_1,s(0)、/>和/>分别是第一个过渡段起始的姿态四元数、速度向量和加速度向量，q_1,s(1)、/>和/>分别是第一个过渡段结束的姿态四元数、速度向量和加速度向量；

采用边界单位四元数得到过渡姿态处的四元数样条曲线，结合归一化参数插补得到转接姿态，如下式所示：

其中，

q₀＝q_1,s(0)

q₃＝q_1,s(1)

f₁(λ)＝u³-3u²+3u

f₂(λ)＝-2u³+3u²

f₃(λ)＝u³

步骤七：把非过渡段和过渡段规划后的姿态四元数拼接起来，获得插补姿态四元数，并转化为姿态欧拉角向量，得到姿态轨迹插补命令；其中转化过程如下：

其中，当-2(q_i(2)q_i(4)-q_i(1)q_i(3))>1时，欧拉角eul的值为1；

步骤八：根据插补姿态四元数，计算整个运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束。其中，速度曲线由下式计算得到：

根据速度曲线，由差分法得到加速度曲线；最后验证是否满足最大速度和加速度约束。

实施例2

与实施例1基于相同的发明构思，本实施例提出的是一种工业机器人姿态轨迹过渡与速度规划系统，包括以下模块：

输入模块，用于输入工业机器人的姿态欧拉角向量；

四元数转化模块，用于把姿态欧拉角向量转化为对应的单位四元数；计算所有两个相邻姿态单位四元数之间的角位移；

过渡转角计算模块，用于设置姿态轨迹过渡比例系数；根据姿态轨迹过渡比例系数和角位移，计算除首尾两个姿态之外的所有过渡姿态处的过渡转角；

非过渡角位移确定模块，用于对每两个相邻姿态，根据过渡转角确定两个相邻姿态之间的剩余非过渡角位移；

速度规划模块，用于根据速度和角速度约束，对剩余非过渡角位移进行速度规划；以及根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，对转接姿态根据速度和加速度约束进行速度规划；

插补模块，用于把非过渡段和过渡段规划后的姿态四元数拼接起来，获得插补姿态四元数，并转化为姿态欧拉角向量，得到姿态轨迹插补命令；

速度计算模块，用于根据插补姿态四元数，计算整个运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束。

其中，所输入的姿态欧拉角向量为P_i(α_i,β_i,γ_i)，i＝1,2,3…,N，其中α_i、β_i、γ_i分别是第i个姿态绕x轴、y轴和z轴的转角，N是运动命令中给定的姿态个数。

把姿态欧拉角向量P_i转化为对应的单位四元数q_i，转化公式为：

任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移θ_i计算方法为：

θ_i＝2·acos[q_i(1)q_i+1(1)+q_i(2)q_i+1(2)+q_i(3)q_i+1(3)+q_i(4)q_i+1(4)]；

其中q_i(j)，j＝1,2,3,4，分别表示单位四元数q_i中的第i个元素。

过渡转角θ_s,i的计算方法为：

θ_s,i＝min{θ_i-2,θ_i-1,θ_i}·ε

其中θ_i-1是单位四元数q_i和q_i-1之间的角位移，θ_i-2是单位四元数q_i-1和q_i-2之间的角位移；θ_i为任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移；ε为姿态轨迹过渡比例系数。

速度规划模块对剩余非过渡角位移进行速度规划，其过程包括：

根据非过渡段的初始速度、末端速度、最大速度、最大加速度、角位移和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数，非过渡段进入过渡段时的各速度向量和加速度向量；

根据归一化参数对非过渡段采用线性插补，得到插补后的非过渡姿态单位四元数，其插补公式如下：

式中，q_tl是插补后的非过渡姿态单位四元数，Q_sl是非过渡姿态单位四元数的起点，Q_el是非过渡姿态单位四元数的终点，λ是归一化参数，θ_sel是非过渡姿态起点到终点的角位移。

根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，对转接姿态根据速度和加速度约束进行速度规划，其过程包括：

根据过渡段的初始速度、末端速度、最大速度、最大加速度、过渡转角和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数；

根据前一个非过渡段进入过渡段时的各速度向量和加速度向量、过渡段进入后一个非过渡段时的各速度向量和加速度向量，计算得到转接姿态的边界单位四元数；

采用边界单位四元数得到过渡姿态处的四元数样条曲线，结合归一化参数插补得到转接姿态。

本实施例的各模块用于实现实施例1对应各步骤，其更详细的实现过程请参见实施例1，本实施例不赘述。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在不脱离本发明的原理和宗旨的情况下在本发明的范围内可对上述实施例进行变化、修改、替换和变形。

Claims (6)

1.一种工业机器人姿态轨迹过渡与速度规划方法，其特征在于，包括以下步骤：

输入工业机器人的姿态欧拉角向量；

把姿态欧拉角向量转化为对应的单位四元数；计算所有两个相邻姿态单位四元数之间的角位移；

设置姿态轨迹过渡比例系数；根据姿态轨迹过渡比例系数和角位移，计算除首尾两个姿态之外的所有过渡姿态处的过渡转角；

对每两个相邻姿态，根据过渡转角确定两个相邻姿态之间的剩余非过渡角位移；所述剩余非过渡角位移，由两个相邻姿态之间的角位移和过渡转角做差得到；

根据速度和角速度约束，对剩余非过渡角位移进行速度规划；

根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，根据速度和加速度约束，对转接姿态进行速度规划；

把非过渡段和过渡段规划后的姿态四元数拼接起来，获得插补姿态四元数，并转化为姿态欧拉角向量，得到姿态轨迹插补命令；

根据插补姿态四元数，计算整个运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束；

所述根据速度和角速度约束，对剩余非过渡角位移进行速度规划，包括步骤：

根据非过渡段的初始速度、末端速度、最大速度、最大加速度、角位移和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数，非过渡段进入过渡段时的各速度向量和加速度向量；

根据归一化参数对非过渡段采用线性插补，得到插补后的非过渡姿态单位四元数，其插补公式如下：

式中，q_tl是插补后的非过渡姿态单位四元数，Q_sl是非过渡姿态单位四元数的起点，Q_el是非过渡姿态单位四元数的终点，λ是归一化参数，θ_sel是非过渡姿态起点到终点的角位移；

所述根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，对转接姿态根据速度和加速度约束进行速度规划，包括步骤：

根据过渡段的初始速度、末端速度、最大速度、最大加速度、过渡转角和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数；

根据前一个非过渡段进入过渡段时的各速度向量和加速度向量、过渡段进入后一个非过渡段时的各速度向量和加速度向量，计算得到转接姿态的边界单位四元数；

采用边界单位四元数得到过渡姿态处的四元数样条曲线，结合归一化参数插补得到转接姿态。

2.如权利要求1所述的方法，其特征在于，所述姿态欧拉角向量为P_i(α_i,β_i,γ_i)，i＝1,2,3…,N，其中α_i、β_i、γ_i分别是第i个姿态绕x轴、y轴和z轴的转角，N是运动命令中给定的姿态个数；

把姿态欧拉角向量P_i转化为对应的单位四元数q_i，转化公式为：

3.如权利要求2所述的方法，其特征在于，任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移θ_i计算方法为：

θ_i＝2·acos[q_i(1)q_i+1(1)+q_i(2)q_i+1(2)+q_i(3)q_i+1(3)+q_i(4)q_i+1(4)]；

其中q_i(j)，j＝1,2,3,4，分别表示单位四元数q_i中的第i个元素。

4.如权利要求1所述的方法，其特征在于，过渡转角θ_s,i的计算方法为：

θ_s,i＝min{θ_i-2,θ_i-1,θ_i}·ε

其中θ_i-1是单位四元数q_i和q_i-1之间的角位移，θ_i-2是单位四元数q_i-1和q_i-2之间的角位移；θ_i为任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移；ε为姿态轨迹过渡比例系数。

5.一种工业机器人姿态轨迹过渡与速度规划系统，其特征在于，包括以下模块：

输入模块，用于输入工业机器人的姿态欧拉角向量；

四元数转化模块，用于把姿态欧拉角向量转化为对应的单位四元数；计算所有两个相邻姿态单位四元数之间的角位移；

过渡转角计算模块，用于设置姿态轨迹过渡比例系数；根据姿态轨迹过渡比例系数和角位移，计算除首尾两个姿态之外的所有过渡姿态处的过渡转角；

非过渡角位移确定模块，用于对每两个相邻姿态，根据过渡转角确定两个相邻姿态之间的剩余非过渡角位移；所述剩余非过渡角位移，由两个相邻姿态之间的角位移和过渡转角做差得到；

速度规划模块，用于根据速度和角速度约束，对剩余非过渡角位移进行速度规划；以及根据过渡转角，在过渡姿态处采用四元数样条曲线拟合得到转接姿态，对转接姿态根据速度和加速度约束进行速度规划；

插补模块，用于把非过渡段和过渡段规划后的姿态四元数拼接起来，获得插补姿态四元数，并转化为姿态欧拉角向量，得到姿态轨迹插补命令；

速度计算模块，用于根据插补姿态四元数，计算整个运动过程的速度和加速度曲线，验证是否满足最大速度和加速度约束；

速度规划模块对剩余非过渡角位移进行速度规划，其过程包括：

根据非过渡段的初始速度、末端速度、最大速度、最大加速度、角位移和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数，非过渡段进入过渡段时的各速度向量和加速度向量；

根据归一化参数对非过渡段采用线性插补，得到插补后的非过渡姿态单位四元数，其插补公式如下：

式中，q_tl是插补后的非过渡姿态单位四元数，Q_sl是非过渡姿态单位四元数的起点，Q_el是非过渡姿态单位四元数的终点，λ是归一化参数，θ_sel是非过渡姿态起点到终点的角位移；

速度规划模块对过渡姿态根据速度和加速度约束进行速度规划，其过程包括：

根据过渡段的初始速度、末端速度、最大速度、最大加速度、角位移和插补周期采用S形速度模型进行速度规划，获取控制速度的归一化参数；

根据前一个非过渡段进入过渡段时的各速度向量和加速度向量、过渡段进入后一个非过渡段时的各速度向量和加速度向量，计算得到转接姿态的边界单位四元数；

采用边界单位四元数得到过渡姿态处的四元数样条曲线，结合归一化参数插补得到转接姿态。

6.如权利要求5所述的系统，其特征在于，所述姿态欧拉角向量为P_i(α_i,β_i,γ_i)，i＝1,2,3…,N，其中α_i、β_i、γ_i分别是第i个姿态绕x轴、y轴和z轴的转角，N是运动命令中给定的姿态个数；

把姿态欧拉角向量P_i转化为对应的单位四元数q_i，转化公式为：

任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移θ_i计算方法为：

θ_i＝2·acos[q_i(1)q_i+1(1)+q_i(2)q_i+1(2)+q_i(3)q_i+1(3)+q_i(4)q_i+1(4)]；

其中q_i(j)，j＝1,2,3,4，分别表示单位四元数q_i中的第i个元素；

过渡转角θ_s,i的计算方法为：

θ_s,i＝min{θ_i-2,θ_i-1,θ_i}·ε

其中θ_i-1是单位四元数q_i和q_i-1之间的角位移，θ_i-2是单位四元数q_i-1和q_i-2之间的角位移；θ_i为任意两个相邻姿态单位四元数q_i和q_i+1之间的角位移；ε为姿态轨迹过渡比例系数。