CN116061195B - 一种新的工业机械手臂运动轨迹控制方法 - Google Patents

Abstract

本发明公开了一种工业机械臂运动轨迹控制方法，基于约束空间中采样的分段直线路径P₁,P₂,…,P_n，根据机械臂关节速度与加速度大小限制，通过随机迭代求得时间优化的平滑运动轨迹。在前一次优化轨迹上随机选取两个位置，根据所有关节的速度与加速度大小限制，得到机械臂在这两个位置间的最大执行时间T；接着根据执行时间T，计算各关节的最小加速度轨迹；最后采用二分法判断轨迹是否与周围环境产生碰撞，若产生碰撞则丢弃本次优化路径直接进入下一次迭代；否则替换原来的优化路径并进入下一次迭代；直到完成N次迭代。该方法能够在各关节速度和加速度大小限制下得到时间优化的平滑轨迹，且能避免障碍物碰撞。

Description

一种新的工业机械手臂运动轨迹控制方法

技术领域

本发明涉及电机控制算法领域，尤其涉及一种基于快速平滑算法和碰撞检测的工业机械臂运动轨迹控制算法。

背景技术

在计算机、微电子、智能控制等技术推动下，工业机械臂成为车间生产过程自动化的关键设备。在数控车床、锻压机床、铸造机械及仓储设备上，可以协助完成装备传送与其它操作。

然而，现有的工业机械臂轨迹控制算法尽管具备传送速度快的特点，但存在着运动轨迹插补不流畅、计算效率低、难以避免碰撞的缺陷。尤其是对于同时对速度、安全性、流畅性等多方面有较高要求的多自由度机器臂来说，现有算法无法满足。

发明内容

本发明提出一种工业机械臂运动轨迹控制方法，能够在各关节速度和加速度大小限制下得到时间优化的平滑轨迹，且能避免障碍物碰撞。

为了解决上述技术问题，本发明的技术方案为：

一种工业机械臂运动轨迹控制方法，基于约束空间中采样的分段直线路径P₁,P₂,…,P_n，根据机械臂关节速度与加速度大小限制，通过随机迭代求得时间优化的平滑运动轨迹。在前一次优化轨迹上随机选取两个位置，根据所有关节的速度与加速度大小限制，得到机械臂在这两个位置间的最大执行时间T；接着根据执行时间T，计算各关节的最小加速度轨迹；最后采用二分法判断轨迹是否与周围环境产生碰撞，若产生碰撞则丢弃本次优化路径直接进入下一次迭代；否则替换原来的优化路径并进入下一次迭代；直到完成N次迭代。

本发明具有以下的特点和有益效果：

采用上述技术方案，能够在各关节速度和加速度大小限制下得到时间优化的平滑轨迹，且能避免障碍物碰撞，并且能兼顾机械臂移动速度、加速度限制及工作安全性，可有效实现自然平滑的机械臂轨迹控制。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例中机械臂运动轨迹二分法进行碰撞检测示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。

本实施例提供了一种工业机械臂运动轨迹控制方法，能够在各关节速度和加速度大小限制下得到时间优化的平滑轨迹，且能避免障碍物碰撞，其具体步骤包括：

步骤S1：根据约束空间中的拐点确定无碰撞分段直线路径P₁,P₂,…,P_n，设定迭代次数为N；

步骤S2：设置单调插值函数y(t)，把分段直线路径转化为时间优化的转折点轨迹，则分段路径上各线段两端点e,f间轨迹Γ(t)可表示为：

Γ(t)＝e+y(t)(f-e) (1)其两边分别取一阶与二阶导数，得到插补路径上的关节速度与加速度大小分别为：

i表示关节。v_y、a_y分别表示所有关节最大速度限制与最大加速度限制。

步骤S3：根据e,f计算转折点：

若a_yt₁≤v_y (4)

若满足(4)式则转折点在t₁处。否则包含两个转折点。

t₁与t₁+t₂时刻即为转折点。因此，根据(4)、(5)式即可得到时间优化的加速过程。最终根据转折点可以计算每个分段路径上每一点的速度大小。

步骤S4：在路径上随机选取两点α、β，计算运动最慢关节所耗费的时间T：

为两点α,β处的速度大小(第一次迭代由式(4)与(5)得到)，为关节i速度大小限制，为关节i的加速度大小限制。在所有关节中找出最长执行时间T。

其中i表示执行关节。M函数表示为机械手臂关节四类运动模式：M₁,M₂,M₃,M₄。

M₁：由以a_max与-a_max分别为加速度的抛物线轨迹组成；

M₂：由以-a_max与a_max分别为加速度的抛物线轨迹组成；

M₃：由以a_max的加速度的抛物线轨迹、以a_max为速度的直线轨迹、以-a_max为加速度的抛物线轨迹组成；

M₄：由以-a_max的加速度的抛物线轨迹、以-v_max为速度的直线轨迹、以a_max为加速度的抛物线轨迹组成。

对于每种模式有如下公式：

M₁/M₂:

其中，M₁时ω为1，M₂时ω为-1，得到转折时间t，若满足(7)式且v_α+tωa_max≤v_max，则T＝2t+(v_α-v_β)/ωa_max。

M₃∶

其中，t₁为直线轨迹，t₂和t₃分别为第一、二段弧线轨迹，当三段时间计算都不为负时，T＝t₁+t₂+t₃。

对于M₄，将v_max与a_max取为负值进行计算：

M₄∶

其中，t₁为直线轨迹，t₂和t₃分别为第一、二段弧线轨迹，当三段时间计算都不为负时，T＝t₁+t₂+t₃。最后经过比较得到最大执行时间T。

步骤S5：根据T计算各关节的最小加速度运动轨迹。

仍然使用四类运动模式来计算最小加速度：

M₁/M₂:

其中M₁时ω为1，M₂时ω为-1。设b＝ω(2T(v_α+v_β)-4(|α-β|))/T²，则

M₁/M₂:

M₃:

而对于M₄，v_max取为负值。

M₄:

由此获得最小加速度a，根据a得到各关节时间优化轨迹Γⁱ(t)，最终得到优化轨迹Γ(t)。

步骤S6：采用运动轨迹二分法进行碰撞检测以判断优化轨迹是否与约束环境产生碰撞，如产生碰撞丢弃本次优化并跳转步骤S4继续迭代；若没有碰撞，则将原轨迹替换为本次优化轨迹继续跳转步骤S4迭代优化。碰撞检测过程如下：若机械臂约束空间内的最大工作距离为D，机械臂在两个位置α、β处与约束空间距离为d₁和d₂，考虑实际机械臂运动及环境的复杂性，给定空间裕度∈，如果D≤d₁+d₂-2∈，则表明没有碰撞，反之则对轨迹进行二分，重复上述过程，如图1所示。

步骤S7：判断迭代次数是否达到N；如果达到，则返回平滑的时间优化轨迹；否则，跳转步骤S4继续迭代。

上述技术方案中，通过步骤1得到基于约束空间采样的分段直线路径P₁,P₂,…,P_n，步骤2设置单调插值函数，把分段直线路径转化为时间优化的转折点轨迹，步骤3根据所有关节最大速度限制与最大加速度限制，得到时间优化的分段路径加速过程，步骤4在路径上随机选取两点位置信息，计算运动最慢关节所耗费的时间T，步骤5根据T计算各关节的最小加速度运动轨迹，步骤6采用运动轨迹二分法进行碰撞检测以判断优化轨迹是否与约束环境产生碰撞，如产生碰撞丢弃本次优化并跳转步骤4继续迭代；若没有碰撞，则将原轨迹替换为本次优化轨迹继续跳转步骤4迭代优化。本发明能兼顾机械臂移动速度、加速度限制及工作安全性，可有效实现自然平滑的机械臂轨迹控制。

以上结合附图对本发明的实施方式作了详细说明，但本发明不限于所描述的实施方式。对于本领域的技术人员而言，在不脱离本发明原理和精神的情况下，对这些实施方式包括部件进行多种变化、修改、替换和变型，仍落入本发明的保护范围内。

Claims (6)

1.一种工业机械臂运动轨迹控制方法，其特征在于，具体步骤包括：

步骤S1、根据约束空间中的拐点确定无碰撞分段直线路径P₁，P₂，...，P_n，设定迭代次数为N；

步骤S2、设置单调插值函数y(t)，把分段直线路径转化为时间优化的转折点轨迹，则分段路径上各线段两端点e，f间轨迹Γ(t)可表示为：

Γ(t)＝e+y(t)(f-e) (1)

其两边分别取一阶与二阶导数，得到插补路径上的关节速度与加速度大小分别为：

i表示关节；v_y、a_y分别表示所有关节最大速度限制与最大加速度限制；

步骤S3、根据e，f计算转折点：

若a_yt₁≤v_y (4)

若满足(4)式则转折点在t₁处；否则包含两个转折点；

t₁与t₁+t₂时刻即为转折点；因此，根据(4)、(5)式即可得到时间优化的加速过程；最终根据转折点可以计算每个分段路径上每一点的速度大小；

步骤S4、在路径上随机选取两点α、β，计算运动最慢关节所耗费的时间T；

步骤S5、根据T计算各关节的最小加速度运动轨迹；

步骤S6、采用运动轨迹二分法进行碰撞检测以判断优化轨迹是否与约束环境产生碰撞，如产生碰撞丢弃本次优化并跳转步骤S4继续迭代；若没有碰撞，则将原轨迹替换为本次优化轨迹继续跳转步骤S4迭代优化；

步骤S7：判断迭代次数是否达到N；如果达到，则返回平滑的时间优化轨迹；否则，跳转步骤S4继续迭代。

2.根据权利要求1所述的一种工业机械臂运动轨迹控制方法，其特征在于，所述步骤S4中，运动最慢关节所耗费的时间T的计算方法如下：

为两点α，β处的速度大小，为关节i速度大小限制，为关节i的加速度大小限制；在所有关节中找出最长执行时间T；

其中i表示执行关节；M函数表示为机械手臂关节的四类运动模式：M₁，M₂，M₃，M₄。

3.根据权利要求2所述的一种工业机械臂运动轨迹控制方法，其特征在于，所述机械手臂关节的四类运动模式分别为：

M₁：由以a_max与-a_max分别为加速度的抛物线轨迹组成；

M₂：由以-a_max与a_max分别为加速度的抛物线轨迹组成；

M₃：由以a_max的加速度的抛物线轨迹、以v_max为速度的直线轨迹、以-a_max为加速度的抛物线轨迹组成；

M₄：由以-a_max的加速度的抛物线轨迹、以-v_max为速度的直线轨迹、以a_max为加速度的抛物线轨迹组成。

4.根据权利要求3所述的一种工业机械臂运动轨迹控制方法，其特征在于，所述机械手臂关节的四类运动模式的表达式如下：

其中，M₁时ω为1，M₂时ω为-1，得到转折时间t，若满足(7)式且v_α+tωa_max≤v_max，则T＝2t+(v_α-v_β)/ωa_max；

M₃：

其中，t₁为直线轨迹，t₂和t₃分别为第一、二段弧线轨迹，当三段时间计算都不为负时，T＝t₁+t₂+t₃；

对于M₄，将v_max与a_max取为负值进行计算：

M₄：

其中，t₁为直线轨迹，t₂和t₃分别为第一、二段弧线轨迹，当三段时间计算都不为负时，T＝t₁+t₂+t₃；最后经过比较得到最长执行时间T。

5.根据权利要求4所述的一种工业机械臂运动轨迹控制方法，其特征在于，所述步骤S5的具体方法：

仍然使用四类运动模式来计算最小加速度：

M₁/M₂：

其中M₁时ω为1，M₂时ω为-1；设b＝ω(2T(V_α+v_β)-4(|α-β|))/T²，则

M₁/M₂：

M₃：

而对于M₄，v_max取为负值；

M₄：

由此获得最小加速度a，根据a得到各关节时间优化轨迹Γⁱ(t)，最终得到优化轨迹Γ(t)。

6.根据权利要求5所述的一种工业机械臂运动轨迹控制方法，其特征在于，所述步骤S6中，碰撞检测过程如下：若机械臂约束空间内的最大工作距离为D，机械臂在两个位置α、β处与约束空间距离为d₁和d₂，考虑实际机械臂运动及环境的复杂性，给定空间裕度∈，如果D≤d₁+d₂-2∈，则表明没有碰撞，反之则对轨迹进行二分，重复上述过程。