一种工业机器人的柔顺加减速控制方法

1.本发明属于工业机器人运动控制技术领域，具体涉及一种工业机器人的柔顺加减速控制方法。


背景技术：

2.随着工业智能化的不断推进，现代工业需要更高精度且更高速的运动控制技术，作为运动控制技术中的重要组成部分，工业机器人的控制技术已经成为了运动控制中的一个重要分支。
3.在工业机器人工作空间中进行轨迹规划时，不仅要对轨迹上的路径进行合理的规划，运动过程的速度也是必须要进行考虑的问题。目前关于工业机器人的加减速控制方法主要包括梯形加减速控制、指数加减速控制、正弦曲线加减速和s型加减速控制。梯形加减速控制方法实现起来简单，而且运行的效率也很高，但是梯形加减速在启动时存在加速度的突变，在启动时会产生柔性冲击，对工业机器人本体造成影响。s型加减速在梯形加减速的基础上加上了启动阶段时的平滑处理，速度和加速度曲线平滑，但是加加速度会在加速度从最大值减少时产生阶跃，高速加工中可能会产生机械的震动。指数加减速和正弦曲线加减速作为上述两种加减速方案的解决方法，虽然在一定程度上做了改进，但是计算复杂，不适合在实际开发中使用。
4.由上述内容可知，不合理的轨迹速度规划不仅会影响工业机器人工作的效率，而且还有可能对机器人本体造成不可逆的冲击损害，为了保证机器人末端能够运行高效且平稳，需要一种计算简单、可靠性高的工业机器人加减速控制方法。


技术实现要素：

5.本发明的目的在于提供一种工业机器人的柔顺加减速控制方法，构造一种基于加速度柔顺变化的加减速模型，对不同轨迹具有自适应调节最大速度功能，计算简单，稳定可靠。解决了传统加减速算法加速度、加加速度不平滑的问题。
6.为实现上述目的，本发明所采取的技术方案为：
7.一种工业机器人的柔顺加减速控制方法，所述工业机器人的柔顺加减速控制方法，包括：
8.s1、对工业机器人使用d-h参数法进行运动学建模，确定工业机器人的运动学正逆解公式；
9.s2、获取运动的起点位置和终点位置，以及工业机器人的起始速度vs、终点速度ve、最大加速度acc、最大减速度dec以及最大运行速度vm；
10.s3、将整个运动段分为变速段和匀速段，其中变速段包括加速段和减速段，拟合加速段和减速段的位移时间函数，包括：
11.s3.1、利用二次多项式的抛物线特性对加速段和减速段的加速度曲线进行拟合，通过积分得出加速度曲线对应的位移时间函数如下：
12.s(t)＝a0 a1t a2t2 a3t3 a4t413.式中，s(t)为t时刻的位移，a0，a1，a2，a3，a4为四次多项式的系数；
14.s3.2、确定加速段的位移时间函数，根据给定条件：起始速度vs、最大加速度acc、最大运行速度vm，以及加速度约束条件a(0)＝a(ta)＝0，a(max)＝acc，s(0)＝0，求出加速段的总时间ta，以及系数如下：
[0015][0016]
进而得到加速段的位移时间函数表达式sa(t)如下：
[0017]
其中t∈[0，ta]
[0018]
式中，a
a0
，a
a1
，a
a2
，a
a3
，a
a4
为加速段的位移时间函数的系数，a(0)为加速段起始时刻的加速度，a(ta)为加速度ta时刻的加速度，a(max)为加速段的最大加速度，s(0)为加速度起始时刻的位移；
[0019]
s3.3、确定减速段的位移时间函数，根据给定条件：减速时速度vm、最大减速度dec、终点速度ve以及加速度约束条件a(0)
′
＝a(td)＝0，a(max)
′
＝-dec，s(0)
′
＝0，求出减速段的总时间td，以及系数如下：
[0020][0021]
进而得到减速段的位移时间函数表达式sd(t)：
[0022]
其中t∈[0，td]
[0023]
式中，a
d0
，a
d1
，a
d2
，a
d3
，a
d4
为减速段的位移时间函数的系数，a(0)
′
为减速段起始时刻的加速度，a(td)为减速度td时刻的加速度，a(max)
′
为减速段的最大加速度，s(0)
′
为减速度起始时刻的位移；
[0024]
s4、根据加速段的总时间ta、减速段的总时间td、加速段的位移时间函数表达式sa(t)和减速段的位移时间函数表达式sd(t)，计算得到加速段的位移长度sa和减速段的位移长度sd；
[0025]
s5、根据加速段的位移长度sa、减速段的位移长度sd以及运动段的总长s
t
判断是否包含匀速段，若不包含匀速段则更新最大运行速度vm；
[0026]
s6、固定插补周期，得到每个插补周期内的位移量，基于位移量利用运动学逆解公式得到工业机器人的控制量，完成的工业机器人的加减速控制。
[0027]
以下还提供了若干可选方式，但并不作为对上述总体方案的额外限定，仅仅是进一步的增补或优选，在没有技术或逻辑矛盾的前提下，各可选方式可单独针对上述总体方案进行组合，还可以是多个可选方式之间进行组合。
[0028]
作为优选，所述运动段的总长s
t
计算如下：
[0029]
在直线轨迹中，使用来计算总位移长度，其中(xs，ys，zs)、(xe，ye，ze)为起点位置和终点位置在基坐标系下的坐标；
[0030]
在圆弧轨迹中，使用极坐标方程来表示圆弧轨迹，即在圆弧坐标系下有：
[0031][0032]
其中x，y为圆弧坐标系下坐标，r为圆弧半径，θ为圆弧的角度，取s
t
为极角的变化，有：
[0033]st
＝θ
s-θe[0034]
式中，θs为起点位置在圆弧坐标系下的极角，θe为终点位置在圆弧坐标系下的极角。
[0035]
作为优选，所述根据加速段的位移长度sa、减速段的位移长度sd以及运动段的总长s
t
判断是否包含匀速段，包括：
[0036]
若s
t
＞(sa sd)，则运动段中包含匀速段，所加匀速段的速度为加速段结束时的速度，即最大运行速度vm，匀速段运行时间为tv＝(s
t-(sa sd))/vm；
[0037]
若s
t
＝(sa sd)，则运动段中不包含匀速段，在最大运行速度vm下经过加速段和减速段后到达目标点，匀速段运行时间为tv＝0；
[0038]
若s
t
＜(sa sd)，则运动段中不包含匀速段，更新最大运行速度vm得到更新后的最大运行速度vm′
使得s
t
≥(sa′
sd′
)，sa′
为最大运行速度vm′
下加速段的位移长度，sd′
为最大运行速度vm′
下减速段的位移长度。
[0039]
作为优选，更新最大运行速度vm得到更新后的最大运行速度vm′
使得s
t
≥(sa′
sd′
)，包括：
[0040]
s5.1、确定速度搜索区间为[vs，vm]，以及确定最后区间的精度δ，使得s
t-(sa′
sd′
)＜δ；
[0041]
s5.2、计算插入的两个黄金分割点的值，使这两个点的速度v1、v2能够满足：
[0042][0043][0044]
s5.3、比较以v1、v2中较大的速度为最大运行速度时的变速段的位移长度s
a2
、s
d2
与运动段的总长s
t
的关系，如果s
t
＜(s
a2
s
d2
)，则跳到步骤s5.4；如果s
t
≥(s
a2
s
d2
)，则跳到步骤s5.5；
[0045]
s5.4、缩小速度搜索空间，将速度搜索空间缩小至[vs，v2]，计算新的黄金分割点v1′
：
[0046]v1
′
＝vs 0.328
·
(v
2-vs)
[0047]
令vm＝v2，v2＝v1，v1＝v1′
，跳到步骤s5.3继续执行；
[0048]
s5.5、计算变速段的位移长度，如果以v2为最大运行速度时s
t-(s
a2
s
d2
)≤δ，则v2即为修正后的最大运行速度vm′
，结束迭代，跳转到步骤s5.6；否则缩小搜索空间，将速度搜索空间缩小至[v1，vm]，计算新的黄金分割点v2′
：
[0049]v2
′
＝v1 0.618
·
(v
m-v1)
[0050]
令vs＝v1，v1＝v2，v2＝v2′
，跳到步骤s5.3继续执行；
[0051]
s5.6、更新最大运行速度修正后的加速段和减速段的位移时间函数表达式，计算最大运行速度修正后的变速段的位移长度sa′
、sd′
，以及修正后的匀速段运行时间tv＝(s
t-(sa′
sd′
))/vm′
。
[0052]
本发明提供的工业机器人柔顺加减速控制方法，可实现工业机器人关节空间与笛卡尔空间运动轨迹的加减速控制，以顺滑的加速度曲线为基准，得出不同运动段的位移曲线，整个运动过程具有良好的平滑性；在保证一定计算精度的基础上，无复杂函数计算，实现方式更简单，具有自适应调节最大运行速度功能，与传统的加减速控制方法相比，简化了运动过程，使得整个系统更加稳定且高效。
附图说明
[0053]
图1为本发明的工业机器人柔顺加减速控制方法的流程图；
[0054]
图2是本发明黄金分割法速度修正流程图；
[0055]
图3是本发明修正最大运行速度后的柔顺加减速控制曲线图；
[0056]
图4是现有技术传统s型加减速控制曲线图；
[0057]
图5是本发明加入匀速段的柔顺加减速控制曲线图。
具体实施方式
[0058]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0059]
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本发明。
[0060]
为了解决现有技术中速度规划的不合理，其中一个实施例中，提供一种工业机器人笛卡尔空间轨迹的柔顺加减速控制方法，满足对机械手末端轨迹的柔顺控制，使其在高速运动中具有良好的稳定性。本实施例中的工业机器人是广泛用于工业领域的多关节机械手或多自由度的机器装置，例如机械臂。
[0061]
本实施例的工业机器人柔顺加减速控制方法，整个运动段主要包括加速段、减速段以及匀速段，如图1所示，具体包括以下步骤：
[0062]
s1、对工业机器人使用d-h参数法进行运动学建模，确定工业机器人的运动学正逆解公式，方便确定初始以及路径点信息。
[0063]
s2、获取运动的起点位置和终点位置，以及工业机器人的起始速度vs、终点速度ve、最大加速度acc、最大减速度dec以及最大运行速度vm。
[0064]
由于实际应用中具有不同的规划空间，因此首先需要先确定运动段所属规划空间(关节空间或笛卡尔空间)，本实施例介绍了一种工业机器人在笛卡尔空间的加减速控制方法。上述所需获取的参数通过手动示教的形式确定。
[0065]
对于关节空间，起点位置和终点位置需要是每个轴的关节角信息，对应的速度和加速度也是关节角速度以及关节角加速度，最大运行速度为系统所能允许的最大关节角速度。
[0066]
对于笛卡尔空间，起点位置和终点位置需要是每个轴的关节角信息，经过运动学正解求出对应的笛卡尔坐标系下的坐标，对应的速度与加速度都是指机器人末端的加速度，速度规划完成后，通过步骤s1确定的运动学模型转换为关节信息。
[0067]
s3、将整个运动段分为变速段和匀速段，其中变速段包括加速段和减速段，拟合加速段和减速段的位移时间函数。
[0068]
s3.1、根据给定信息进行加减速控制，将整个运动段分为变速段和匀速段，其中变速段又包括加速段和减速段，对于变速段，为了获取到连续且平滑的加速度曲线，用二次多项式表现出的抛物线特性来对加速度曲线进行拟合，那么这个加速度曲线对应的速度、位移就可用通过积分的方法得出，积分后的结果为：
[0069][0070]
式中，s(t)为t时刻的位移，为位移时间函数，v(t)为t时刻的速度，为速度时间函数，a0，a1，a2，a3，a4为四次多项式的系数，同时a1，a2，a3，a4为速度时间函数的系数。
[0071]
s3.2、确定加速段的位移时间函数，根据给定条件：起始速度vs、最大加速度acc、最大运行速度vm，以及加速度约束条件a(0)＝a(ta)＝0，a(max)＝acc，s(0)＝0，求出加速段的总时间ta，以及系数如下：
[0072][0073]
进而得到加速段的位移时间函数表达式sa(t)如下：
[0074]
其中t∈[0，ta]
[0075]
式中，a
a0
，a
a1
，a
a2
，a
a3
，a
a4
为加速段的位移时间函数的系数，a(0)为加速段起始时刻的加速度，a(ta)为加速度ta时刻的加速度，a(max)为加速段的最大加速度，s(0)为加速度
起始时刻的位移。
[0076]
s3.3、确定减速段的位移时间函数，根据给定条件：减速时速度vm、最大减速度dec、终点速度ve以及加速度约束条件a(0)
′
＝a(td)＝0，a(max)
′
＝-dec，s(0)
′
＝0，求出减速段的总时间td，以及系数如下：
[0077][0078]
进而得到减速段的位移时间函数表达式sd(t)：
[0079]
其中t∈[0，td]
[0080]
式中，a
d0
，a
d1
，a
d2
，a
d3
，a
d4
为减速段的位移时间函数的系数，a(0)
′
为减速段起始时刻的加速度，a(td)为减速度td时刻的加速度，a(max)
′
为减速段的最大加速度，s(0)
′
为减速度起始时刻的位移。
[0081]
s4、根据加速段的总时间ta、减速段的总时间td、加速段的位移时间函数表达式sa(t)和减速段的位移时间函数表达式sd(t)，计算得到加速段的位移长度sa和减速段的位移长度sd。
[0082]
另外还需要根据不同工作空间来求出运动段的总长s
t
来判断是否需要加入匀速段。在关节空间中，s
t
为每个关节从起始角度到目标角度的角位移，在进行n工业机器人关节空间加减速控制时，需要对每一个关节的运动进行分别控制，需要计算出每一个关节的角位移s
ti
＝θ
si-θ
ei
，其中i＝1，2，...，n，n为关节总数，θ
si
为第i个关节的起点位置的角度，θ
ei
第i个关节的终点位置的角度。
[0083]
在本实施例中，s
t
为机器人末端执行器运动的总位移，需要根据不同的运动轨迹进行计算。笛卡尔空间轨迹中，最常用到的就是直线轨迹和圆弧轨迹：
[0084]
在直线轨迹中，使用来计算总位移长度，其中(xs，ys，zs)、(xe，ye，ze)为起点位置和终点位置在基坐标系下的坐标；
[0085]
在圆弧轨迹中，使用极坐标方程来表示圆弧轨迹，即在圆弧坐标系下有：
[0086][0087]
其中x，y为圆弧坐标系下坐标，r为圆弧半径，θ为圆弧的角度，取s
t
为极角的变化，有：
[0088]st
＝θ
s-θe[0089]
式中，θs为起点位置在圆弧坐标系下的极角，θe为终点位置在圆弧坐标系下的极角。
[0090]
s5、根据加速段的位移长度sa、减速段的位移长度sd以及运动段的总长s
t
判断是否
包含匀速段，若不包含匀速段则更新最大运行速度vm。
[0091]
根据步骤s4计算出的三个位移量sa、sd、s
t
来判断是否需要加入匀速段，判断位移总长s
t
与变速段位移sa sd的关系：
[0092]
取工业机器人以scara机械臂末端轨迹为例，机械臂两个关节臂长分别为30cm、25cm，初始角度为θ1＝78
°
，θ2＝58
°
，目标角度为θ1＝15
°
，θ2＝29
°
，设置最大速度为30cm/s，初始速度为0cm/s，目标点速度为0cm/s，且最大加速度为20cm/s2，最大减速度为30cm/s2，由步骤s4中位移的计算方法得出对于两个笛卡尔坐标点之间直线运动时的位移量，对于末端运行的轨迹使用本发明的柔顺加减速控制方法进行控制。
[0093]
具体的，根据加速段的位移长度sa、减速段的位移长度sd以及运动段的总长s
t
判断是否包含匀速段，包括：
[0094]
若s
t
＞(sa sd)，说明变速段位移并不能满足到达目标点的需求，还需要加入匀速段，则运动段中包含匀速段，所加匀速段的速度为加速段结束时的速度，即最大运行速度vm，匀速段运行时间为tv＝(s
t-(sa sd))/vm。
[0095]
若s
t
＝(sa sd)，则说明变速段结束后恰好能够到达目标点，无需加入匀速段，则运动段中不包含匀速段，在最大运行速度vm下经过加速段和减速段后到达目标点，匀速段运行时间为tv＝0。
[0096]
若s
t
＜(sa sd)，则运动段中不包含匀速段，更新最大运行速度vm得到更新后的最大运行速度vm′
使得s
t
≥(sa′
sd′
)，sa′
为最大运行速度vm′
下加速段的位移长度，sd′
为最大运行速度vm′
下减速段的位移长度。
[0097]
如果s
t
＜(sa sd)，则说明如果加速到当前设定的最大速度，变速段位移会超出目标位置，还需要进行位置修正，运动过程复杂且缓慢，这时候就需要对最大速度进行更改，即修改最大速度的值进而缩短变速段的位移量，使得变速段结束时能够在目标点上或者加入匀速段后在目标点上。如图2所示，本实施例利用黄金分割法来快速搜索出满足s
t
≥(sa′
sd′
)的最大速度vm′
，具体操作步骤如下：
[0098]
s5.1、确定速度搜索区间为[vs，vm]，以及确定最后区间的精度δ，使得s
t-(sa′
sd′
)＜δ；
[0099]
s5.2、计算插入的两个黄金分割点的值，使这两个点的速度v1、v2能够满足：
[0100][0101][0102]
s5.3、比较以v1、v2中较大的速度为最大运行速度时的变速段的位移长度s
a2
、s
d2
与运动段的总长s
t
的关系，如果s
t
＜(s
a2
s
d2
)，则跳到步骤s5.4；如果s
t
≥(s
a2
s
d2
)，则跳到步骤s5.5；
[0103]
s5.4、缩小速度搜索空间，将速度搜索空间缩小至[vs，v2]，计算新的黄金分割点v1′
：
[0104]v1
′
＝vs 0.328
·
(v
2-vs)
[0105]
令vm＝v2，v2＝v1，v1＝v1′
，跳到步骤s5.3继续执行；
[0106]
s5.5、计算变速段的位移长度，如果以v2为最大运行速度时s
t-(s
a2
s
d2
)≤δ，则v2即为修正后的最大运行速度vm′
，结束迭代，跳转到步骤s5.6；否则缩小搜索空间，将速度搜索空间缩小至[v1，vm]，计算新的黄金分割点v2′
：
[0107]v2
′
＝v1 0.618
·
(v
m-v1)
[0108]
令vs＝v1，v1＝v2，v2＝v2′
，跳到步骤s5.3继续执行；
[0109]
s5.6、更新最大运行速度修正后的加速段和减速段的位移时间函数表达式，计算最大运行速度修正后的变速段的位移长度sa′
、sd′
，以及修正后的匀速段运行时间tv＝(s
t-(sa′
sd′
))/vm′
。其中，sa′
为最大运行速度修正后的加速段的位移长度，sd′
为最大运行速度修正后的减速段的位移长度。
[0110]
修正后的柔顺加减速控制方法的运动参数曲线如图3所示，可以看出，相比于不需要修正的曲线，修正后的速度曲线并没有达到设定的最大速度，且并不存在匀速段，使得本发明提出的柔顺加减速控制方法在很短的路径里也适用。
[0111]
s6、固定插补周期，得到每个插补周期内的位移量，基于位移量利用运动学逆解公式得到工业机器人的控制量，完成的工业机器人的加减速控制。
[0112]
根据逐步递增的插补时间对应的运动段，确定每个插补周期的位移量，固定插补周期，更改每次的位移量来达成对速度的控制。计算出位移量之后，根据运动空间的不同，计算出每个轴的速度与位移，由于在关节空间中，是对单个轴来进行速度规划，无需进行进一步的处理。对于笛卡尔空间轨迹还需要通过运动学进行进一步的变换，将直角坐标点通过运动学逆解求出每个关节的值，利用速度雅可比矩阵求出每个关节的速度变化值，到达目标点后，结束流程。
[0113]
需要说明的是，在离散出具体路径下的各个路径信息后可使用常规插补方法进行处理，具体的插补过程本实施例中不再进行赘述。
[0114]
本实施例提出了一种相较于传统s型加减速，具有更加光滑的加速度曲线的加减速控制方法，使用二次方程对加速度曲线进行拟合，进而得出四次多项式形式的位移时间函数。
[0115]
如图4、图5所示，图4是使用传统s型加减速控制方法进行控制的运动参数曲线，图5所示是使用本发明的柔顺加减速控制方法的运动参数曲线，可以看出本发明提出的控制方法的加速度曲线是一个连续变化的光滑曲线，在从加加速状态切换至减加速状态时，并不会出现如图4所示的尖峰，具有更高的光滑性，在进行高速控制的时候具有更高的稳定性。
[0116]
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0117]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明的保护范围应以所附权利要求为准。