CN113618739A

CN113618739A - 一种机器人动态避障方法、装置和机器人

Info

Publication number: CN113618739A
Application number: CN202110975114.8A
Authority: CN
Inventors: 刘益彰; 罗璇; 陈金亮; 张志豪; 熊友军
Original assignee: Ubtech Robotics Corp
Current assignee: Ubtech Robotics Corp
Priority date: 2021-08-24
Filing date: 2021-08-24
Publication date: 2021-11-09
Anticipated expiration: 2041-08-24
Also published as: CN113618739B; WO2023024279A1

Abstract

本申请实施例提供一种机器人动态避障方法、装置和机器人，该机器人动态避障方法包括：检测机器人当前与障碍物之间的最短距离，以确定机器人与障碍物各自的预测碰撞点位置；根据机器人与障碍物各自的预测碰撞点位置确定当前机器人避免碰撞的速度约束方程；基于所述速度约束方程、机器人的关节位置和关节角速度的自身约束条件对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到关节角速度的最优解；利用所述最优解对所述机器人进行运动控制。本申请的技术方案可以使得机器人在完成动态避障的同时，尽可能地执行末端在笛卡尔空间的速度任务，还具有普适性。

Description

一种机器人动态避障方法、装置和机器人

技术领域

本申请涉及机器人控制技术领域，尤其涉及一种机器人动态避障方法、装置和机器人。

背景技术

随着机器人应用范围的扩展，机器人所处的工作环境也是多变的，在机器人的作业空间范围内，经常会出现各类静态或者动态的障碍物，如果不做处理，则很可能出现机器人与障碍物相撞，导致机器人本体或者障碍物受到伤害。虽然基于视觉设备动态监测机器人与环境的距离，能够一定程度上对碰撞进行预知，但很难实时根据动态的相对位姿对轨迹进行重新规划，而且一般的避障算法很难保证在躲避障碍物的同时保证任务的完成。

发明内容

本申请实施例提供一种机器人动态避障方法、装置和机器人，该机器人动态避障方法通过将障碍物作为机器人的运动位置约束，并结合关节位置约束、关节角速度约束来共同对避障优化问题进行求解，以保证得到的解在完成动态避障的同时，尽可能地完成末端速度任务。

本申请的实施例提供一种机器人动态避障方法，包括：

检测机器人当前与障碍物之间的最短距离，以确定所述机器人与所述障碍物各自的预测碰撞点位置；

根据所述机器人与所述障碍物各自的所述预测碰撞点位置确定当前所述机器人避免碰撞的速度约束方程；

基于所述速度约束方程、所述机器人的关节位置和关节角速度的自身约束条件对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到所述关节角速度的最优解；

利用所述关节角速度的所述最优解对所述机器人进行运动控制。

在一些实施例中，所述根据所述机器人与所述障碍物各自的所述预测碰撞点位置确定当前所述机器人避免碰撞的速度约束方程，包括：

根据所述机器人和所述障碍物各自的所述预测碰撞点位置得到碰撞方向向量；

基于所述机器人上的预测碰撞点位置到机器人基座之间的各个关节角速度计算所述机器人上的预测碰撞点位置对应的关节末端速度；

根据所述关节末端速度在所述碰撞方向向量上的投影、所述机器人上的预测碰撞点位置和所述障碍物上的预测碰撞点位置构建得到所述机器人避免碰撞的速度约束方程。

在一些实施例中，所述机器人避免碰撞的速度约束方程的表达式为：

其中，

其中，

为t时刻的所述机器人上的预测碰撞点位置；x_p为所述障碍物的预测碰撞点位置；

表示所述关节末端速度在所述碰撞方向向量

上的投影；||cp||为所述机器人与所述障碍物之间的最短距离；J_c为所述机器人上的预测碰撞点到机器人基座的雅克比矩阵；

为所述机器人上的预测碰撞点到机器人基座之间的第i个关节角速度；T为机器人的控制指令周期。

在一些实施例中，所述基于所述速度约束方程、所述机器人的关节位置和关节角速度的自身约束条件对所述避障优化函数进行求解，之前还包括：

构建满足所述关节位置的自身约束条件的关节位置约束方程，并将所述关节位置约束方程转换为对应的关节角速度约束方程；

根据所述关节角速度约束方程和所述关节角速度的自身约束条件构建得到所述关节角速度的合成约束方程，所述合成约束方程用于和所述速度约束方程共同对所述避障优化函数进行求解。

在一些实施例中，所述机器人的关节位置约束方程为：

其中，

和

分别为所述机器人的第i个关节角度的上限和下限；

为t时刻的第i个关节角度；

为第i个关节的关节角速度；T为机器人的控制指令周期。

在上述实施例中，所述关节角速度约束方程为：

在一些实施例中，所述避障优化函数的表达式如下：

其中，w为松弛变量，J为所述机器人的速度雅可比矩阵，

为所述机器人所有关节的关节角速度向量；

为所述机器人的关节末端速度向量；

和

分别为第i个关节角速度的上限和下限；

和

分别为第i个关节角度的上限和下限；

为t时刻的第i个关节角度；

为第i个关节的关节角速度。

在一些实施例中，当所述障碍物存在多个时，所述速度约束方程的数量与所述障碍物的数量相同；

每个所述速度约束方程通过对应的障碍物与所述机器人各自的所述预测碰撞点位置确定，所有所述速度约束方程用于与所述机器人的关节位置和关节角速度的自身约束条件共同对所述避障优化函数进行求解。

本申请的实施例还提供一种机器人动态避障装置，包括：

碰撞检测模块，用于检测机器人当前与障碍物之间的最短距离，以确定所述机器人与所述障碍物各自的预测碰撞点位置；

约束确定模块，用于根据所述机器人与所述障碍物各自的所述预测碰撞点位置确定当前所述机器人避免碰撞的速度约束方程；

优化求解模块，用于基于所述速度约束方程、所述机器人的关节位置和关节角速度的自身约束条件对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到所述关节角速度的最优解；

运动控制模块，用于利用所述关节角速度的所述最优解对所述机器人进行运动控制。

本申请的实施例还提供一种机器人，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的机器人动态避障方法。

本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序被处理器执行时，实施上述的机器人动态避障方法。

本申请的实施例具有如下有益效果：

本申请实施例的机器人动态避障方法通过将障碍物作为机器人的运动位置约束，并结合关节位置、关节角速度的自身约束来对构建的避障优化问题进行关节角速度求解，以保证得到的解能够完成动态避障；并且该避障优化问题以末端速度为控制目标，这样在进行避障的同时，还尽可能地完成末端速度跟随任务。此外，由于可以对单个障碍物进行对应的约束添加，该方法可以适用用于一个或多个障碍物、以及静态障碍物等多种场景，因此具有普适性。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本申请实施例机器人动态避障方法的第一流程示意图；

图2示出了本申请实施例机器人动态避障方法的第二流程示意图；

图3示出了本申请实施例机器人动态避障方法的一种应用示意图；

图4示出了本申请实施例机器人动态避障装置的结构示意图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

二次规划问题主要是在满足等式及不等式约束条件下，从多个解中选取最优解的过程。示范性地，二次规划问题的主要形式如下：

其中，

为优化指标，H为海森矩阵；x为n维优化变量；f为行向量；A_eq·x＝b_eq为等式约束，A_eq为mxn(m≤n)维矩阵，b_eq为m行列向量；Ax≤b为不等式约束，A为n列矩阵，b为列向量。

对于机器人来说，常常通过添加冗余关节来增大机器人的运动范围和灵活度等。现有的对冗余关节的处理一般非常复杂且计算时间长，在实际使用中存在一定的限制。对此，考虑到机器人自身结构的特性，其往往在不同运用场合下会存在关节角度、关节角速度、关节力矩等多种限制，为求解得到能够用于执行相应任务且满足对应多种限制的最优运动学逆解，本申请实施例将利用二次规划问题来进行动态避障优化求解。

本申请实施例提出的机器人动态避障方法通过构建一个以关节角速度为优化变量、末端速度为控制目标的避障优化函数，并将障碍物作为机器人的运动位置约束，然后与机器人的关节位置和关节角速度的自身约束等共同作为该避障优化函数的约束条件，以用于对避障优化函数进行最优求解，这样得到的解将使得机器人在完成动态避障的同时，还尽可能地完成末端速度跟随任务，可以很好地解决现有方案中存在的问题。

下面结合具体的实施例来进行说明。

实施例1

请参照图1，本实施例提出一种机器人动态避障方法，可用于在存在障碍物等多种场景下进行避障控制。示范性地，该机器人动态避障方法包括：

步骤S110，检测机器人当前与障碍物之间的最短距离，以确定机器人与障碍物各自的预测碰撞点位置。

其中，上述的预测碰撞点位置是指预测的机器人与障碍物将产生接触的接触点的位置。示范性地，可通过一些开源的碰撞检测器(如FCL碰撞检测等)进行距离检测或碰撞检测，从而获得机器人与障碍物之间的最短距离，以及机器人与该障碍物之间预测发生碰撞的碰撞点信息。例如，该碰撞点信息可包括但不限于包括，机器人上的预测碰撞点的位置和障碍物上的预测碰撞点的位置。

步骤S120，根据机器人与障碍物各自的预测碰撞点位置确定当前机器人避免碰撞的速度约束方程。

本实施例中，通过将障碍物作为机器人的运动位置约束，并将其添加到构建的避障优化函数的约束条件中，这样可以保证得到的运动学逆解能够不与障碍物发生碰撞。示范性地，可基于机器人与障碍物两者各自的碰撞点位置以及机器人与障碍物之间的最短距离，将机器人不与障碍物发生碰撞的运动位置约束转化为机器人的末端速度约束。

在一种实施方式中，如图2所示，上述步骤S120包括：

步骤S210，根据机器人和障碍物各自的预测碰撞点位置得到碰撞方向向量。

例如，如图3所示，假设机器人上的预测碰撞点记为c，障碍物上的预测碰撞点记为p，若碰撞点c的位置记为x_c，碰撞点p的位置记为x_p，则两个碰撞点之间的向量

可表示为：

步骤S220，基于机器人上的预测碰撞点位置到机器人基座之间的各个关节角速度计算机器人上的预测碰撞点位置对应的关节末端速度。

通常地，机器人设有基座坐标系，当机器人的某一点与外界的障碍物发生碰撞时，基于机器人的链乘法则，其对应的关节末端的运动状态可以根据该碰撞点位置到基座之间的所有关节的运动状态计算得到。

在一种实施方式中，示范性地，对于机器人的预测碰撞点位置对应的关节末端速度，则可利用碰撞点位置x_c到机器人基座O之间的所有关节的关节角速度计算得到。具体地，该关节末端速度的计算公式为：

其中，J_c为碰撞点c到机器人基座O的雅克比矩阵；

为碰撞点位置x_c到机器人基座之间的各个关节角速度。

步骤S230，根据所述关节末端速度在所述碰撞方向向量上的投影、机器人上的预测碰撞点位置和障碍物上的预测碰撞点位置构建得到机器人避免碰撞的速度约束方程。

为了避免机器人与障碍物在上述的两个预测碰撞点位置发生碰撞，本实施例从运动位置约束层面构建出机器人不与障碍物发生碰撞时的速度约束条件。

示范性地，该机器人避免碰撞的速度约束方程的表达式为：

其中，

其中，

为t时刻的机器人上的预测碰撞点位置；x_p为障碍物的预测碰撞点位置；

表示该关节末端速度在碰撞方向向量

上的投影；||cp||为机器人与障碍物之间的最短距离；J_c为机器人上的预测碰撞点到机器人基座的雅克比矩阵；

为机器人上的预测碰撞点到机器人基座之间的第i个关节角速度；T为机器人的控制指令周期。

步骤S130，基于所述速度约束方程、机器人的关节位置和关节角速度的自身约束条件对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到所述关节角速度的最优解。

为了保证机器人的末端在多个约束条件下能够优先执行笛卡尔空间中的任务，本实施例将预先构建出一个以关节角速度为控制变量，以末端速度为控制目标的避障优化函数，其中，该避障优化函数将添加包含与障碍物有关的相应约束条件，当然还可添加其他的约束，例如，可包括但不限于包括机器人的关节角度或关节位置的自身约束条件(即关节限位)，关节角速度的自身约束条件等，具体可根据实际需求来添加。

由于机器人关节结构的限制而使得一些关节位置或角度不可达，因此会存在关节限位。可以理解，对于机器人的平动关节，由于不存在转动，故该关节限位主要是指对关节位置的限位；而对于转动关节，该关节限位主要是指对关节角度的限位。

以转动关节类型的机器人为例，下面对该机器人的关节角度约束的添加进行说明。假设第i个关节的关节角度的上限为

下限为

机器人的控制指令周期为T，

为t时刻的第i个关节角度，

为第i个关节的关节角速度，则t+1时刻的关节角度

应当满足关节位置的自身约束条件，具体如下：

由于机器人的关节角度与关节角速度之间存在转换关系，为了方便计算，这里将其转换为以关节角速度为变量的约束方程。

进一步地，将其转换为关节角度约束方程时，则有：

对于各个关节角速度的自身约束条件，示范性地，若第i个关节角速度的上限为

下限为

则第i个关节角速度

应当满足：

由于同一时刻的各个关节角速度需要同时满足多种限制，为了方便避障优化函数的优化求解，这里将对多个约束条件进行约束合成，以便与其他的约束方程共同作为上述避障优化函数的不同约束条件。

示范性地，以上述添加的关节角度和关节角速度各自的自身约束条件为例，可得到如下的关节角速度的合成约束方程：

本实施例中，上述的避障优化函数将引入一松弛变量，并基于该松弛变量得到待优化的指标。例如，该优化指标可为松弛变量的平方等，当然也可以取与松弛变量有关的其他形式，这里不作限定。可以理解，以松弛变量为优化指标，可以保证即便该避障优化函数在约束条件存在冲突的情况下，依然会求解出该约束方程的接近解，从而保证在优先避障的同时，尽可能地使末端能够完成对应的末端速度跟随任务。

在一种实施方式中，若以将松弛变量的平方作为优化指标，并以上述添加的关于障碍物相关的速度约束以及关节角速度的合成约束为例，此时该避障优化函数的表达式如下：

作为一种可选的实施方式，对于多个障碍物的场景，由于每个障碍物均可以作为机器人的运动位置约束，因此，机器人与每个障碍物的最短距离都可以转换为对应的关节速度约束。应当理解，无论障碍物是动态的还是静止的，都可以利用上述方法将其转换为对应的速度约束方程。

对于存在多个障碍物的情景，其与仅有一个障碍物的不同之处在于，当障碍物存在多个时，上述的速度约束方程的数量与障碍物的数量相同，即每个障碍物都对应有一个速度约束方程。

其中，每个速度约束方程将通过对应的障碍物与机器人各自的预测碰撞点位置来确定，具体可参见上述步骤S110～S120。并且，所有的速度约束方程将用于与机器人的关节位置和关节角速度的自身约束条件来共同对上述的避障优化函数进行最优求解。

示范性地，假设存在j个障碍物，此时的避障优化函数的形式如下：

对于上述步骤S130，示范性地，在得知机器人在对应控制指令周期的速度约束方程，以及结合机器人的关节位置和关节角速度的自身约束条件可确定避障优化函数在对应控制指令周期的具体约束表达式。进而，通过对该避障优化函数进行关节角速度的最优求解，从而可得到下一控制指令周期的最优解，而该最优解既能保证躲避障碍物而不发生碰撞，又能尽量使得末端速度为期望的速度

即实现执行末端速度跟随任务。

步骤S140，利用所述关节角速度的最优解对机器人进行运动控制。

示范性地，在求解得到一个最优的关节角速度后，可以利用该关节角速度进行积分处理，得到最优的关节速度或关节位置。进而，将计算到的关节角度或关节位置作为控制指令并发送到对应的关节电机，使得机器人能够实现避障运动。与此同时，机器人末端也能够执行相应的速度跟随任务。

本实施例的机器人动态避障方法通过将障碍物作为机器人的运动位置约束，将机器人与障碍物之间的最短距离转化为机器人的速度约束，并结合关节位置、关节角速度的自身约束来对构建的避障优化问题进行关节角速度求解，以保证得到的解能够完成动态避障；并且该避障优化问题以末端速度为控制目标，这样在进行避障的同时，还可尽可能地完成末端速度跟随任务。并且该动态避障方法可以适用于一个或多个障碍物、动态或静态的障碍物等多种场景，具有通用性。

实施例2

请参照图4，基于上述实施例1的方法，本实施例提出一种机器人动态避障装置100，示范性地，该机器人动态避障装置100包括：

碰撞检测模块110，用于检测机器人当前与障碍物之间的最短距离，以确定所述机器人与所述障碍物各自的预测碰撞点位置。

约束确定模块120，用于根据所述机器人与所述障碍物各自的所述预测碰撞点位置确定当前所述机器人避免碰撞的速度约束方程。

优化求解模块130，用于基于所述速度约束方程、所述机器人的关节位置和关节角速度的自身约束条件对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到所述关节角速度的最优解。

运动控制模块140，用于利用所述关节角速度的所述最优解对所述机器人进行运动控制。

可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1中的可选项同样适用于本实施例，故在此不再重复描述。

本申请还提供了一种机器人，例如，该机器人可为具有多自由度的机器人。示范性地，该机器人包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行所述计算机程序，从而使移动终端执行上述的机器人动态避障方法或者上述机器人动态避障装置中的各个模块的功能。

本申请还提供了一种可读存储介质，用于储存上述机器人中使用的所述计算机程序。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。