CN114414231A - 一种动态环境下机械手自主避障规划方法和系统 - Google Patents

Abstract

本发明公开了一种动态环境下机械手自主避障规划方法和系统，包括对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模型；建立无模型深度学习算法模型并进行碰撞训练；根据无模型深度学习算法模型得到机械手自主避障路径，通过设置碰撞负奖励，将避障对象描述为强化学习的对象，即将动态环境下机械手的避障抓取问题描述为寻找使总奖励最大化策略的问题。另外，针对机械手和复杂障碍物两种不规则实体设计了碰撞检测算法，解决了两个不规则实体间的碰撞检测问题。

Description

一种动态环境下机械手自主避障规划方法和系统

技术领域

本发明属于机械手技术领域，具体涉及一种动态环境下机械手自主避障规划方法和系 统。

背景技术

机械手是一种模仿人手手动作的自动操作装置，作为工业机器人的一个分支，其具有 通用性强、运动灵活、易于控制等优点，因而被广泛应用于搬运、焊接、喷涂、切割等领域。通常，机械手的工作场景并不是自由的空间，而是包含较多障碍物的约束空间，因此 在含障碍物约束的空间进行路径规划一直是研究的热点，避障规划技术的好坏直接影响着机械手的控制效果，随着生产需求的扩大和应用场景的复杂化，工作场景中经常会含有一些运动的障碍物，这对工业机械手的作业规划提出了更高的需求，即要求机械手在执行任务的同时不与环境中运动的障碍物发生碰撞。相比传统静态环境下的避障规划，动态环境下机械手的避障抓取主要存在以下几个问题：

(1)动态环境下障碍物的位置实时在改变，如何根据障碍物的运动情况及时调整避障 规划路径，是实现机械手在动态障碍物环境中安全作业的关键。

(2)由于高自由度机械手路径规划问题是一个高维问题，规划空间较大，这使得规划 难度变得异常困难。

(3)大量的在线的碰撞检测成为限制算法规划效率的瓶颈，如何对机械手以及障碍物 进行建模是实现机械手在动态障碍物环境中规划的关键。

发明内容

本发明旨在至少在一定程度上解决相关技术中的技术问题之一：提供一种动态环境下 机械手自主避障规划方法，它包括，其特征在于：

对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模型；

建立无模型深度学习算法模型并进行碰撞训练；

根据无模型深度学习算法模型得到机械手自主避障路径。

根据本发明的一个示例，无模型深度学习算法模型定义为

根据本发明的一个示例，对无模型深度学习算法模型设计优先经验回放机制，以TD误 差ξ作为评价经验值的指标，

根据本发明的一个示例，还包括：对无模型深度学习算法模型设计奖励函数，奖励函 数：

本发明旨在至少在一定程度上解决相关技术中的技术问题之一：一种动态环境下机械 手自主避障规划系统，它包括，其特征在于，

对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模块；

建立无模型深度学习算法模块并进行碰撞训练；

根据无模型深度学习算法模块得到机械手自主避障路径。

根据本发明的一个示例，无模型深度学习算法模块定义为

根据本发明的一个示例，对无模型深度学习算法模块设计优先经验回放机制，以TD误 差ξ作为评价经验值的指标，

根据本发明的一个示例，还包括：对无模型深度学习算法模块设计奖励函数，奖励函 数：

本发明提出了动态环境下机械手的避障规划算法。为了解决高维情况下的避障规划问 题，提出了无模型深度学习算法进行训练避障规划路径。通过设置碰撞负奖励，将避障对 象描述为强化学习的对象，即将动态环境下机械手的避障抓取问题描述为寻找使总奖励最 大化策略的问题。另外，针对机械手和复杂障碍物两种不规则实体设计了碰撞检测算法， 解决了两个不规则实体间的碰撞检测问题。仿真和实验结果表明，由于无模型深度学习算 法中加入了熵项，该算法具有更高的探索性，对于任意起始位置和目标位置该算法都能规 划出一条比现有算法更短、更平滑且实时无碰撞的路线。

附图说明

图1是本发明一种动态环境下机械手自主避障规划方法的自由度机械手实物图和简化 图。

图2是本发明一种动态环境下机械手自主避障规划方法的长方体包络盒简化图。

图3是本发明一种动态环境下机械手自主避障规划方法的无模型深度学习算法模型流 程图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同 或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描 述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

下面参考附图来详细描述根据本发明实施例的一种动态环境下机械手自主避障规划 方法。

一种动态环境下机械手自主避障规划方法，它包括，

对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模型；

建立无模型深度学习算法模型并进行碰撞训练；

根据无模型深度学习算法模型得到机械手自主避障路径。

根据本发明的一个示例，无模型深度学习算法模型定义为

根据本发明的一个示例，对无模型深度学习算法模型设计优先经验回放机制，以TD误 差ξ作为评价经验值的指标，

根据本发明的一个示例，还包括：对无模型深度学习算法模型设计奖励函数，奖励函 数：

本发明提出了动态环境下机械手的避障规划算法。为了解决高维情况下的避障规划问 题，提出了无模型深度学习算法进行训练避障规划路径。通过设置碰撞负奖励，将避障对 象描述为强化学习的对象，即将动态环境下机械手的避障抓取问题描述为寻找使总奖励最 大化策略的问题。另外，针对机械手和复杂障碍物两种不规则实体设计了碰撞检测算法， 解决了两个不规则实体间的碰撞检测问题。仿真和实验结果表明，由于无模型深度学习算 法中加入了熵项，该算法具有更高的探索性，对于任意起始位置和目标位置该算法都能规 划出一条比现有算法更短、更平滑且实时无碰撞的路线。

碰撞检测模型具体的如下：

本文研究对象是七自由度机械手。一般情况下，机械手连杆大多类似为长方体或圆柱 体，使得机械手与障碍物2个不规则实体之间的碰撞检测关系十分复杂。为了便于计算， 本文基于AABB包围盒对障碍物和机械手的空间占位关系进行简化描述。通过将机械手连 杆的径向最大半径叠加到障碍物的包围盒的厚度上，将机械手连杆和障碍物2个实体之间 复杂的空间占位关系转变成了空间线段和空间长方体之间的相对位置关系。虽然会造成机 械手一部分工作空间的浪费，但却极大地简化了机械手和障碍物的空间占位关系，有利于 进行碰撞的判定和检测，提高避障的可靠性。

图1中给出了7自由度空间机器人的实物图和简化图，{j₁,j₂,j₃,j₄,j₅,j₆,j₇}为机械 手的7个关节，其中末端执行器与关j₇节相连接。由于{j₁,j₃,j₅,j₇}4个关节主要是绕连杆的质心轴转动,并不影响机械手的碰撞检测,所以将机械手简化为所示简化图

其中L₁,L₂,L₃是机器人简化图的三个连杆，其中L₁由P₁,P₂限定，L₂由P₂,P₃限定，L₃由P3,P4限定。P4是机械手末端执行器的中心点，r表示机械手的切面半径。

另外，对于空间中的三维障碍物，可能具有不规则的几何形状，空间占位描述复杂， 给碰撞检测带来很大的困难。为此本文采用长方体包围盒对障碍物的空间占位进行简化描 述，这种方法虽然在一定程度上扩大了障碍物空间占位范围，但是能够极大地简化碰撞检 测过程，从而使得机械手避障抓取的效率得到大幅提升。

一般常用的包围盒方式有球形包围盒、方形包围盒、六面体包围盒等，本文的采用的 是方形包围盒对障碍物进行包络，如2图的方形包络盒的简化图

其中(x_min,y_min,z_min)表示长方体包围盒最小的点，(x_max,y_max,z_max)表示最大的点，以此来 定义一个包围体；取半径r，则方形包围盒表示为：

Obstacle{(x_min-r，y_min-r，z_min-r)≤(x，y，z)≤(x_max+r，y_max+r，z_max+r)}

判断三维空间线段和立方体是否相交。由图(1)可知，机械手三个连杆可以简化为空 间线段L₁,L₂,L₃，由公式(1)可知障碍物由AABB包围盒限定。对于机械手碰撞检测问题就可以转换为判断空间中三条线段和空间立方体是否相交的问题。为了叙述方便，我们假设机械手半径已经算入包围盒的厚度中。

Obstacle{(x_min，y_min，z_min)≤(x，y，z)≤(x_max，y_max，z_max)}

对于连杆L₁，连杆L₁与障碍物包围盒中的(无限)平面X＝x_min的交点，可令

P₁+λ_x,near·(P₂-P₁)＝[x_min,0,0]^T

其中P₁，P₂为机械手任意连杆的两个端点。求取系数λ时可只解其x分量部分，即

x₁+λ_x,near·(x₂-x₁)＝x_min

解得

同样的，可求得λ_x,far,λ_y,near,λ_y,far,λ_z,near,λ_z,far。令

λ_near＝max(λ_x,near,λ_y,near,λ_z,near,0),λ_far＝max(λ_x,far,λ_y,far,λ_z,far)

若λ_near，，λ_far则空间连杆L₁与障碍物包围盒碰撞，否则不碰撞。同理可判断连杆L₂，L₃是否和障碍物包围盒碰撞。

建立无模型深度学习算法模型具体的：

如图3所示，动态环境下机械手的自主抓取问题是一个连续性动作问题，即机械手的 动作是各个连续的关节转动角,，无模型深度学习算法模型是一种基于最大化熵值的无模 型深度学习算法。不同于确定性策略(Deterministic Policy)算法，无模型深度学习算法模型是策略随机化的。无模型深度学习算法模型包含一个熵项，使每个动作的输出概率尽可能的分散，以提高agent的探索能力。agent因随机性(更高的熵)而得到更高的奖 励，以使它不要过早收敛到某个次优确定性策略。熵值越大，对环境的探索就越多。因此， 针对最优策略的SAC公式定义为

其中α是熵正则化温度系数，它决定了熵相对于奖励的重要性。SAC的Q值可以用基于 熵值改进的Bellman方程来计算，软Q值函数(或软动作值函数)定义如下：

其中s_t+1从Replay Buffer(D)中采样得到。软状态值函数定义为：

它表示的是在某个状态下预期得到的奖励。此外，SAC中包含策略网络πφ(a_t，s_t)，软状态 值网络V_ψ(s_t)和目标状态值网络

以及两个软Q网络

它们分别由φ，ψ，θ 参数化。为了找到最优策略、软Q值和软状态值，将随机梯度下降方法应用于它们的目标 函数中。软状态值函数通过最小化均方误差来训练：

用类似双Q网络的形式，这有助于避免过高估计不恰当的Q值以提高训练速度。软Q值函 数通过最小化贝尔曼误差来训练：

策略网络通过最小化Kullback-Leibler(KL)散度来更新：

由于奖励稀疏问题，会从经验回放池中随机抽取能够学习的样本少之又少，造成Agent 学习效率低下。而优先经验回放的核心思想是频繁的采样那些具有更高价值的样本，这有 助解决上述问题。由于在大多数强化学习算法中，TD-error被用来更新行为价值函数Q(s， a)的估计。TD误差的值可以作为估计的修正值，并且可以隐含地反映Agent可以从经验 中学到什么程度。绝对TD误差的大小越大，对期望动作值的修正就越积极。在这种情况 下，高TD-error的经验更有可能具有高的价值。此外，TD-error为负的经验是Agent行为恶劣的条件，而这些条件的状态被agent错误地学习，更频繁地取样这些经验有助于agent在相应的状态下逐渐认识到错误行为的后果，避免在这些状态下再次做出错误行为，从而提高整体性能。因此，这些学得不好的经验也被认为具有高的价值。本文选取TD误 差ξ作为评价经验值的指标。ξ的计算如下：

我们的目标就是让TD-error近可能小，如果TD-error比较大，意味着我们当前的Q函数离目标Q函数差距还很大，应该多进行更新，因此用TD-error来衡量经验的价值。 为了防止网络过拟合，通过概率方式进行抽取经验，保证即使是TD-error为0的经验也 能被抽取到。令每个经验的优先值为：

其中ρ_i＝|ξ_t+∈|，其中∈是一个很小的值。

使用优先经验回放还有一个问题是改变了状态的分布，这样势必会引入偏差bias，对 此，我们可以通过引入importance-sample weights来弥补：

其中N是replay buffer的大小，P(i)是采样概率，而β是一个超参数，用来决定有多大 的程度想抵消Prioritized Experience Replay对收敛结果的影响。

动态环境下机械手自主抓取任务是否成功很大程度上依赖于奖赏函数的设计。在每个 时间步，agent都会根据当前的状态S_t，执行的动作转换到下一个状态S_t+1，并从环境中得 到奖励。因此，的设计至关重要。

从环境中得知目标物的当前的位置与夹爪的位置，计算得到两者的距离dis_target：

其中(x_targ，y_targ，z_targ)为目标物的三维坐标，(x_grip，y_grip，z_grip)为夹爪的三维坐标。对于夹爪与目 标物的距离，dis_target越小则给予奖励越大，dis_target越大则给予奖励越小；当dis_target小于 某个阈值δ时，判定夹爪已经到达目标点，并进行抓取，此时给予一个正奖励。当三个连 杆中任意一个连杆与障碍物碰撞时，即判断dis₁,dis₂,dis₃小于某个阈值时，给予一个负 奖励。否则，根据夹爪到目标物的距离以及机械手三个连杆到障碍物的距离计算奖励值。

由上设计可知，奖励函数可以定义为：

其中

其中w_{t arg et}分别为对应的权重。

本发明旨在至少在一定程度上解决相关技术中的技术问题之一：一种动态环境下机械 手自主避障规划系统，它包括，其特征在于，

对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模块；

建立无模型深度学习算法模块并进行碰撞训练；

根据无模型深度学习算法模块得到机械手自主避障路径。

根据本发明的一个示例，无模型深度学习算法模块定义为

根据本发明的一个示例，对无模型深度学习算法模块设计优先经验回放机制，以TD误 差ξ作为评价经验值的指标，

根据本发明的一个示例，还包括：对无模型深度学习算法模块设计奖励函数，奖励函 数：

本发明提出了动态环境下机械手的避障规划系统。为了解决高维情况下的避障规划问 题，提出了无模型深度学习算法模块进行训练避障规划路径。通过设置碰撞负奖励，将避 障对象描述为强化学习的对象，即将动态环境下机械手的避障抓取问题描述为寻找使总奖 励最大化策略的问题。另外，针对机械手和复杂障碍物两种不规则实体设计了碰撞检测算 法，解决了两个不规则实体间的碰撞检测问题。仿真和实验结果表明，由于无模型深度学 习算法中加入了熵项，该算法具有更高的探索性，对于任意起始位置和目标位置该算法都 能规划出一条比现有算法更短、更平滑且实时无碰撞的路线。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、 或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点 包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必 须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

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

对于本领域的技术人员而言，阅读上述说明后，各种变化和修正无疑将显而易见。因 此，所附的权利要求书应看作是涵盖本发明的真实意图和范围的全部变化和修正。在权利 要求书范围内任何和所有等价的范围与内容，都应认为仍属本发明的意图和范围内。

Claims (8)

1.一种动态环境下机械手自主避障规划方法，它包括，其特征在于，

对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模型；

建立无模型深度学习算法模型并进行碰撞训练；

根据无模型深度学习算法模型得到机械手自主避障路径。

2.根据权利要求1所述的一种动态环境下机械手自主避障规划方法，其特征在于，无模型深度学习算法模型定义为

3.根据权利要2所述的一种动态环境下机械手自主避障规划方法，其特征在于，对无模型深度学习算法模型设计优先经验回放机制，以TD误差ξ作为评价经验值的指标，

4.根据权利要求3所述的一种动态环境下机械手自主避障规划方法，其特征在于，还包括：对无模型深度学习算法模型设计奖励函数，奖励函数：

5.一种动态环境下机械手自主避障规划系统，它包括，其特征在于，

对机械手和障碍物进行简化建模，并建立机械手和障碍物的碰撞检测模块；

建立无模型深度学习算法模块并进行碰撞训练；

根据无模型深度学习算法模块得到机械手自主避障路径。

6.根据权利要求5所述的一种动态环境下机械手自主避障规划方法，其特征在于，无模型深度学习算法模块定义为

7.根据权利要求6所述的一种动态环境下机械手自主避障规划系统，其特征在于，对无模型深度学习算法模块设计优先经验回放机制，以TD误差ξ作为评价经验值的指标，

8.根据权利要求7所述的一种动态环境下机械手自主避障规划系统，其特征在于，还包括：对无模型深度学习算法模块设计奖励函数，奖励函数：

Images