CN113618742A - 一种机器人避障方法、装置和机器人 - Google Patents

Abstract

本申请实施例提供一种机器人避障方法、装置和机器人，该方法包括：根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的预测碰撞点与障碍物之间的旋转角度以作为对应关节的防碰撞角度限位；基于关节的防碰撞角度限位和物理角度限位确定该关节的角度约束；根据所述角度约束和关节角速度的自身约束对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，最后利用得到的关节角速度的最优解对机器人进行运动控制。本技术方案提供了一种新的障碍物约束添加方法，可以实现机器人在控制过程中完成避障，同时还尽可能地完成末端速度跟随任务。

Description

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

技术领域

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

背景技术

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

发明内容

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

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

根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，所述旋转角度作为所述对应关节轴所在关节的防碰撞角度限位；

基于所述关节的物理角度限位和所述防碰撞角度限位确定所述关节的角度约束；

根据所述机器人各个关节的所述角度约束和各个关节角速度的自身约束对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到所述关节角速度的最优解；

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

在一些实施例中，所述机器人为平面运动型机器人时，所述根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，包括：

根据障碍物的位置计算所述对应关节轴到所述障碍物的距离；

在锁定所述对应关节轴沿机器人基座方向的所有关节时，以所述对应关节轴为原点、所述距离为半径，计算所述障碍物沿所述对应关节轴连接的连杆运动平面进行虚拟旋转的旋转轨迹；

当所述旋转轨迹与任意一连杆存在交点时，则确定所述交点为预测碰撞点。

在一些实施例中，所述机器人为空间运动型机器人时，所述根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，包括：

根据障碍物的位置计算所述对应关节轴到所述障碍物的垂距；

对所述障碍物在平行于所述对应关节轴的方向上进行预设长度扩张以得到所述障碍物的膨胀区域；

在锁定所述对应关节轴沿机器人基座方向的所有关节时，以所述对应关节轴为原点、所述垂距为半径，计算所述膨胀区域绕所对应关节轴进行虚拟旋转的旋转轨迹；

当所述旋转轨迹与任意一连杆存在交点时，则确定所述交点为预测碰撞点。

在一些实施例中，所述计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，包括：

根据所述对应关节轴到所述障碍物的所述距离和所述预测碰撞点的位置计算所述障碍物顺时针旋转到所述预测碰撞点的第一旋转角度，所述第一旋转角度作为防碰撞角度上限；

根据所述第一旋转角度计算所述障碍物逆时针旋转到所述预测碰撞点的第二旋转角度，所述第二旋转角度作为防碰撞角度下限。

在一些实施例中，所述关节的物理角度限位包括所述关节的物理角度上限和物理角度下限，所述基于所述机器人对应关节的物理角度限位和所述防碰撞角度限位确定所述对应关节的角度约束，包括：

从所述物理角度上限、和当前时刻的关节角度与所述防碰撞角度上限之和中选取出最小值作为所述关节的角度约束上限；

从所述物理角度下限、和当前时刻的关节角度与所述防碰撞角度下限之差中选取出最大值作为所述关节的角度约束下限。

在一些实施例中，当所述障碍物存在多个的情况下，若存在两个或两个以上的预测碰撞点，则计算每个所述预测碰撞点与所述障碍物之间的旋转角度，各组所述旋转角度作为所述对应关节轴所在关节的多个防碰撞角度限位；所述多个防碰撞角度限位用于与对应关节的所述物理角度限位确定所述对应关节的角度约束。

在一些实施例中，所述多个防碰撞角度限位与对应关节的所述物理角度限位按照如下规则来确定所述对应关节的角度约束：

从所述多个防碰撞角度的上限中选取出上限最小值，并从对应关节在当前时刻的关节角度与所述上限最小值之和、以及所述对应关节的物理角度上限中选取出最小值，以作为所述对应关节的角度约束的上限；

从所述多个防碰撞角度的下限中选取出下限最小值，并从对应关节在当前时刻的关节角度与所述下限最小值之差、以及所述对应关节的物理角度下限中选取出最小值，以作为所述对应关节的角度约束的下限。

在一些实施例中，所述避障优化函数以松弛变量的平方为优化指标；所述避障优化函数的表达式如下：

min||w||²

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

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

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

和

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

和

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

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

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

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

碰撞检测模块，用于根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，所述旋转角度作为所述对应关节轴所在关节的防碰撞角度限位；

角度约束确定模块，用于基于所述关节的物理角度限位和所述防碰撞角度限位确定所述关节的角度约束；

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

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

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

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

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

本申请实施例的机器人避障方法通过将障碍物作为机器人的关节位置或角度约束，同时考虑机器人关节的自身限位来确定关节的角度限位，还结合关节角速度的自身约束等来对避障优化问题进行关节角速度求解，以保证得到的解能够完成避障；并且由于该避障优化问题以末端速度为控制目标，在进行避障的同时，还尽可能地完成末端速度跟随的任务。

附图说明

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

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

图2示出了本申请实施例机器人避障方法的平面机器人检测预测碰撞点的流程示意图；

图3a和图3b分别示出了本申请实施例机器人避障方法的平面机器人与一个障碍物和多个障碍物发生碰撞的示意图；

图4示出了本申请实施例机器人避障方法的空间运动型机器人检测预测碰撞点的流程示意图；

图5示出了本申请实施例机器人避障方法的空间机器人与一个障碍物发生碰撞的示意图；

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

具体实施方式

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

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

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

其中，

为优化指标，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，根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的该预测碰撞点与障碍物之间的旋转角度。其中，该旋转角度作为该对应关节轴所在关节的防碰撞角度限位。

其中，上述的预测碰撞点位置是指预测的机器人与障碍物将产生接触的位置。本实施例中，通过将障碍物的位置转变成对关节角度的约束，进而将其作为避障优化函数的约束条件，这样使得求解得到的解能够避免与障碍物发生碰撞。

若按照机器人的运动类型分类，主要分为平面运动型机器人(简称平面机器人)和空间运动型机器人(简称空间机器人)，例如，一些用于平面打磨的机械臂通常为平面机器人；而对于一些能够在三维空间中执行比较复杂操作的车间加工机械臂、双足机器人等通常为空间机器人，当然并不限于此。

以平面机器人为例，在一种实施方式中，示范性地，如图2所示，上述步骤S110中的是否存在预测碰撞点的检测过程，包括：

子步骤S210，根据障碍物的位置计算对应关节轴到障碍物的距离。

通常地，对于位置固定的障碍物，机器人可以提前获得；而对于位置不定的动态障碍物，机器人可以通过与外界设备实时通信或利用感知传感器等实时检测获得，这里并不作限定。可以理解，本实施例的避障方法可以适用于静态障碍物的避障，也适用于动态障碍物的避障，原理相同。

为了将障碍物的位置转换为对应关节的关节约束，本实施例通过计算该障碍物到该机器人的每个关节轴的距离，并判断对应关节轴所在关节是否需要添加相应的障碍物约束。示范性地，可视为已知的障碍物为平面内的一点，对于一关节轴，记为原点，此时利用两点的坐标即计算出该障碍点到当前关节轴原点的距离。

子步骤S220，在锁定该对应关节轴沿机器人基座方向的所有关节时，以该对应关节轴为原点、上述距离为半径，计算障碍物沿该对应关节轴连接的连杆运动平面进行虚拟旋转的旋转轨迹。

本实施例中，对于一个关节轴所在关节的障碍物约束添加，仅需检测该关节后续连杆和障碍物的碰撞情况。例如，如图3a所示，对于待添加障碍物约束的一关节，其对应的关节轴记为原点O，P为障碍物的位置，示范性地，在锁定该对应关节轴沿机器人基座方向的所有关节的情况下，即在基座到该关节之间的所有关节的角度都因定在当前位置时，以O为原点，OP为半径，在连杆运动平面内进行虚拟旋转，得到相应的旋转轨迹。

可以理解，若该虚拟的旋转轨迹与机器人的当前关节到末端之间的任意一连杆存在交点时，即表明在该当前关节轴所在关节在转动时会与障碍物发生碰撞。反之，如果没有一个交点，则表示该关节转动时始终不会碰到障碍物，此时不用对该关节添加障碍物约束。

子步骤S230，当旋转轨迹与任意一连杆存在交点时，则确定该交点为预测碰撞点。

示范性地，若存在一个预测碰撞点，如图3a所示的交点C，则在计算对应连杆上的预测碰撞点C与障碍物P之间的旋转角度时，可根据该关节轴O到障碍物P的距离和预测碰撞点C的位置利用三角形原理计算出障碍物到该预测碰撞点之间的旋转角度。其中，该旋转角度将作为该关节轴O所在关节的防碰撞角度限位。

其中，上述的旋转可包括顺时针旋转和逆时针旋转，在一种实施方式中，将障碍物顺时针旋转到该预测碰撞点的第一旋转角度

作为防碰撞角度限位的上限；以及，根据该第一旋转角度

计算障碍物逆时针旋转到该预测碰撞点的第二旋转角度

以作为该防碰撞角度限位的下限。于是，该关节轴O所在关节的防碰撞角度限位可表示为

可选地，一个障碍物有可能与多个连杆同时相交，此时可针对每个预测碰撞点都计算出对应的旋转角度，以作为添加障碍物约束的当前关节的多个防碰撞角度限位。或者，当障碍物存在多个的情况下，若存在两个或两个以上的预测碰撞点，如图3b所示，则计算每个预测碰撞点与障碍物之间的旋转角度，计算得到的各组旋转角度将作为该对应关节轴所在关节的多个防碰撞角度限位。

在另一种实施方式中，若该机器人为空间运动型机器人，示范性地，如图4所示，上述步骤S110中的是否存在预测碰撞点的检测过程，包括：

子步骤S310，根据障碍物的位置计算对应关节轴到障碍物的垂距。示范性地，具体可参见上述步骤S210，在此不再重复描述。

子步骤S320，对障碍物在平行于该对应关节轴的方向上进行预设长度扩张以得到该障碍物的膨胀区域。

考虑到在三维空间下，障碍物通常具有一定的形态，为了更加准确地反映机器人与具有一定形态的障碍物的碰撞情况，本实施例将对该障碍物进行膨胀处理，示范性地，可在平行于该对应关节轴的方向上按照预设的长度进行膨胀，得到对应的膨胀区域，如图5所示。进而，可利用该障碍物的膨胀区域来进行碰撞检测。可以理解，该预设的长度可通过障碍物的实际形态来确定。

子步骤S330，在锁定该对应关节轴沿机器人基座方向的所有关节时，以该对应关节轴为原点、上述垂距为半径，计算该膨胀区域绕所对应关节轴进行虚拟旋转的旋转轨迹。

子步骤S330与上述步骤S120原理类似，两者区别仅在于子步骤S120是一个障碍点在连杆运动平面内转动，而子步骤S330是一个膨胀区域绕一个关节轴进行旋转，如图5所示。

子步骤S340，当该旋转轨迹与任意一连杆存在交点时，则确定该交点为预测碰撞点。

示范性地，若该旋转轨迹与当前关节轴所在关节到末端之间的任意一连杆存在交点时，如图5所示的交点C，此时交点C即为预测的碰撞点。进而，同样可根据垂距R、预设的长度L等利用三角形原理计算出对应的旋转角度

原理同上述平面运动型机器人的旋转角度的计算方式类似，这里不再重复描述。

可以理解，通过将该障碍物到预测碰撞点之间的旋转角度作为该对应关节轴所在关节的防碰撞角度限位，可以实现将障碍物位置添加为机器人关节的角度约束。可知，该方法不仅适用于平面运动型的机器人，也适用于空间运动型机器人，具有较好的通用性。

步骤S120，基于该关节的物理角度限位和防碰撞角度限位确定该关节的角度约束。

本实施例中，通过将障碍物作为机器人的角度约束，同时考虑机器人的关节自身物理限位来共同确定关节的角度约束，并将其添加到构建的避障优化函数的约束条件中，这样可以保证得到的运动学逆解不会与障碍物发生碰撞，并且能够满足自身的结构限制。可以理解，如果对应关节不存在上述的防碰撞角度限位约束，则该关节的角度约束只需考虑该关节的物理角度限位即可。

关于关节的角度自身约束，由于机器人关节结构的限制而使得一些关节位置或角度不可达，因此会存在关节物理限位。对于机器人的平动关节，由于不存在转动，故关节物理限位主要是指对关节位置的限位；而对于转动关节，关节物理限位主要是指对关节角度的限位。可以理解，本实施例的避障方法可以适用于转动关节类型的机器人，也适用于平动关节类型的机器人，原理类似。

通常地，一个关节的角度物理限位包括物理角度上限和物理角度下限。同样地，该角度约束包括上述的角度约束上限和角度约束下限，对于上述步骤S120，示范性地，包括：

从该物理角度上限、和当前时刻的关节角度与该防碰撞角度上限之和中选取出最小值作为角度约束的上限；以及，从物理角度下限、和当前时刻的关节角度与防碰撞角度下限之差中选取出最大值作为角度约束的下限。

例如，对于存在一个预测碰撞点的情况，则对于当前关节轴所对应的关节i的角度约束，可按照如下方式确定：可从该关节i的物理角度上限

以及关节i在t时刻的关节角度

与防碰撞角度上限

之和这两个值中，选取最小值作为该关节i的角度约束的上限；以及，从关节i的物理角度下限

以及关节i在t时刻的关节角度

与防碰撞角度下限

之差这两个值中，选取最小值作为该关节i的角度约束的下限。示范性地，若用公式表达，则有：

其中，

和

分别为该预测碰撞点对应的防碰撞角度上限和下限；

和

分别为关节i的物理角度的上限和下限；

为关节i在t时刻的关节角度；

和

分别为关节i的角度约束的上限和下限。

在另一种实施方式中，若存在两个或两个以上的预测碰撞点，则计算每个预测碰撞点与障碍物之间的旋转角度，计算得到的各组旋转角度将作为该关节轴所在关节的多个防碰撞角度限位。于是，利用该多个防碰撞角度限位与对应关节的物理角度限位来共同确定对应关节的角度约束。

例如，若某一关节i存在k个预测碰撞点，对应地，存在k组防碰撞角度限位，参照上述的存在一个预测碰撞点的方式来确定，同理，对于第i个关节的角度约束的上限，可先确定k个防碰撞角度的上限中的最小值，然后从该上限的最小值与关节在t时刻的关节角度之和、及关节i的物理角度上限

中选取更小的那个值作为该角度约束的上限。而对于第i个关节的角度约束的下限，可先确定k个防碰撞角度的下限中的最小值，然后从关节在t时刻的关节角度与该下限的最小值之差、及关节i的物理角度下限

中选取更小的那个值作为该角度约束的下限。若通过公式表示，则有：

其中，

和

分别为第j个障碍物对应的防碰撞角度的上限和下限；k为预测碰撞点的数目；

和

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

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

和

分别为第i个关节的角度约束的上限和下限。

步骤S130，根据机器人各个关节的该角度约束和各个关节角速度的自身约束对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到该关节角速度的最优解。

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

以转动关节类型的机器人为例，下面以该机器人的关节角度约束添加为该避障优化函数的约束条件进行说明。假设第i个关节的角度约束上限为

角度约束下限为

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

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

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

应当满足关节角度约束，具体如下：

值得注意的是，这里是以考虑了障碍物在内的关节角度约束，若关节i不存在上述的防碰撞角度限位，即不会与障碍物发生碰撞，则此时公式中的

和

可替换为

和

进一步地，由于机器人的关节角度与关节角速度之间存在转换关系，为了方便计算，这里将其转换为以关节角速度为变量的约束方程。示范性地，将关节角度约束转换为关节角速度约束时，则有：

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

下限为

则第i个关节角速度

应当满足：

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

示范性地，以上述添加的含障碍物限制的关节角度和关节角速度的约束条件为例，可得到如下的关节角速度的合成约束方程：

本实施例中，上述的避障优化函数将引入一松弛变量，并基于该松弛变量得到待优化的指标。例如，该优化指标可为松弛变量的平方等，当然也可以取与松弛变量有关的其他形式，这里不作限定。

可以理解，以松弛变量为优化指标，可以保证即便该避障优化函数在约束条件存在冲突的情况下，依然会求解出该约束方程的接近解，从而保证在优先避障的同时，尽可能地使末端能够完成对应的末端速度跟随任务。

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

示范性地，可利用一些开源的求解器进行二次规划求解，从而得到最优的关节角速度。可以理解，当||w||²＝0时，表示

有解，即在约束条件下存在关节角速度

使得机器人末端速度为

而当||w||²＞0是，

无解，求解出来的关节角速度

是

的最接近解，即是

的解。通过引入松弛量可以保证该避障优化函数不会出现求解出错的情况。

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

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

本实施例的机器人避障方法通过将障碍物作为机器人的关节角度或位置约束，并结合关节角度或位置、关节角速度的自身约束来对构建的避障优化问题进行关节角速度求解，以保证得到的解能够完成避障；同时由于该避障优化问题以末端速度为控制目标，在进行避障的同时，还可尽可能地完成末端速度跟随任务。该避障方法可以适用于一个或多个障碍物、动态或静态的障碍物等多种场景，具有通用性。此外，该方法与机器人的关节类型及运动类型无关，具有普适性，无论是转动型关节还是平动型关节的机器人，或是平面运动型机器人或空间运动型机器人都可以采用该方法进行关节角速度寻优，进而达到避障控制目的。

实施例2

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

碰撞检测模块110，用于根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，所述旋转角度作为所述对应关节轴所在关节的防碰撞角度限位。

角度约束确定模块120，用于基于所述关节的所述防碰撞角度限位和对应关节的物理角度限位确定所述对应关节的角度约束。

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

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

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

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

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

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

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

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

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

Claims (11)

1.一种机器人避障方法，其特征在于，包括：

根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，所述旋转角度作为所述对应关节轴所在关节的防碰撞角度限位；

基于所述关节的物理角度限位和所述防碰撞角度限位确定所述关节的角度约束；

根据所述机器人各个关节的所述角度约束和各个关节角速度的自身约束对以关节角速度为优化变量、末端速度为控制目标的避障优化函数进行求解，得到所述关节角速度的最优解；

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

2.根据权利要求1所述的机器人避障方法，其特征在于，所述机器人为平面运动型机器人时，所述根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，包括：

根据障碍物的位置计算所述对应关节轴到所述障碍物的距离；

在锁定所述对应关节轴沿机器人基座方向的所有关节时，以所述对应关节轴为原点、所述距离为半径，计算所述障碍物沿所述对应关节轴连接的连杆运动平面进行虚拟旋转的旋转轨迹；

当所述旋转轨迹与任意一连杆存在交点时，则确定所述交点为预测碰撞点。

3.根据权利要求1所述的机器人避障方法，其特征在于，所述机器人为空间运动型机器人时，所述根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，包括：

根据障碍物的位置计算所述对应关节轴到所述障碍物的垂距；

对所述障碍物在平行于所述对应关节轴的方向上进行预设长度扩张以得到所述障碍物的膨胀区域；

在锁定所述对应关节轴沿机器人基座方向的所有关节时，以所述对应关节轴为原点、所述垂距为半径，计算所述膨胀区域绕所对应关节轴进行虚拟旋转的旋转轨迹；

当所述旋转轨迹与任意一连杆存在交点时，则确定所述交点为预测碰撞点。

4.根据权利要求2或3所述的机器人避障方法，其特征在于，所述计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，包括：

根据所述对应关节轴到所述障碍物的所述距离和所述预测碰撞点的位置计算所述障碍物顺时针旋转到所述预测碰撞点的第一旋转角度，所述第一旋转角度作为防碰撞角度上限；

根据所述第一旋转角度计算所述障碍物逆时针旋转到所述预测碰撞点的第二旋转角度，所述第二旋转角度作为防碰撞角度下限。

5.根据权利要求4所述的机器人避障方法，其特征在于，所述关节的物理角度限位包括所述关节的物理角度上限和物理角度下限，所述基于所述机器人对应关节的物理角度限位和所述防碰撞角度限位确定所述对应关节的角度约束，包括：

从所述物理角度上限、和当前时刻的关节角度与所述防碰撞角度上限之和中选取出最小值作为所述关节的角度约束上限；

从所述物理角度下限、和当前时刻的关节角度与所述防碰撞角度下限之差中选取出最大值作为所述关节的角度约束下限。

6.根据权利要求1所述的机器人避障方法，其特征在于，当所述障碍物存在多个的情况下，若存在两个或两个以上的预测碰撞点，则计算每个所述预测碰撞点与所述障碍物之间的旋转角度，各组所述旋转角度作为所述对应关节轴所在关节的多个防碰撞角度限位；所述多个防碰撞角度限位用于与对应关节的所述物理角度限位确定所述对应关节的角度约束。

7.根据权利要求6所述的机器人避障方法，其特征在于，所述多个防碰撞角度限位与对应关节的所述物理角度限位按照如下规则来确定所述对应关节的角度约束：

从所述多个防碰撞角度的上限中选取出上限最小值，并从对应关节在当前时刻的关节角度与所述上限最小值之和、以及所述对应关节的物理角度上限中选取出最小值，以作为所述对应关节的角度约束的上限；

从所述多个防碰撞角度的下限中选取出下限最小值，并从对应关节在当前时刻的关节角度与所述下限最小值之差、以及所述对应关节的物理角度下限中选取出最小值，以作为所述对应关节的角度约束的下限。

8.根据权利要求1所述的机器人避障方法，其特征在于，所述避障优化函数以松弛变量的平方为优化指标；

所述避障优化函数的表达式如下：

min||w||²

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

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

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

和

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

和

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

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

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

9.一种机器人避障装置，其特征在于，包括：

碰撞检测模块，用于根据障碍物的位置检测对应关节轴沿机器人末端方向的所有连杆是否存在预测碰撞点，并在存在预测碰撞点时，计算对应连杆上的所述预测碰撞点与所述障碍物之间的旋转角度，所述旋转角度作为所述对应关节轴所在关节的防碰撞角度限位；

角度约束确定模块，用于基于所述关节的物理角度限位和所述防碰撞角度限位确定所述关节的角度约束；

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

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

10.一种机器人，其特征在于，所述机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-8中任一项所述的机器人避障方法。

11.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序被处理器执行时，实施根据权利要求1-8中任一项所述的机器人避障方法。

Images