CN113370210A

CN113370210A - 一种机器人主动避撞***及方法

Info

Publication number: CN113370210A
Application number: CN202110698331.7A
Authority: CN
Inventors: 黎冠
Original assignee: North China Institute of Science and Technology
Current assignee: North China Institute of Science and Technology
Priority date: 2021-06-23
Filing date: 2021-06-23
Publication date: 2021-09-10

Abstract

本申请实施例公开了一种机器人主动避撞***及方法，优化人体与机械臂之间的距离的计算方法；在每个机器人环节上建立了一个控制屏障函数和***优化方法，使得机器人***是对安全距离非常敏感：当***远离最小安全距离时，***允许精确跟踪预设的控制输入，而当连杆和障碍物之间的距离接近最小碰撞距离时，它将修改机械臂的运行轨迹，以确保人机共享空间的人的安全。最大限度地减小了期望加速度输入与指令加速度输入之间的差异，使机器人在检测到发生碰撞前，主动躲避人体，同时保持对期望轨迹的跟踪。

Description

一种机器人主动避撞***及方法

技术领域

本申请实施例涉及人机协作技术领域，具体涉及一种机器人主动避撞***及方法。

背景技术

人机协作单元中，人类操作员和机器人共享一个工作空间，***设计的首要条件是必须满足人类的安全，同时还应避免对***和环境造成损害。这将对机器人***的关节、工作空间、速度、力或扭矩产生限制，并需要一个控制方案，以确保在任务执行期间遵守限制。

当前，最广泛使用的控制方案是基于优化的模型预测控制和参考调节器强制执行输入、输出和状态约束。然而，***动力学中的大量约束或非线性可能会影响求解的实时性。不变性控制方法通过将***控制的不变性保持在状态空间的约束，容许子集内来实施输出和状态约束，但是在实际***上的实现时，由于开关控制规则的原因而导致边界处的抖振。通过创建虚拟斥力的方法，比如也可以设计势函数和虚拟夹具强化输出和状态约束。

然而，这两种方法在确定排斥控制作用时都忽略了固有的***动力学，对于高阶动力学、高加速度或惯性，这些动力学对约束的逼近有着实质性的影响，无论是势场还是虚拟夹具都不能保证一直遵守约束。

发明内容

为此，本申请实施例提供一种机器人主动避撞***及方法，提出了一种基于安全控制屏障函数的控制结构，设计主动避撞约束执行控制器并串联在现有的控制回路中，在人机之间添加一个虚拟的安全屏障，在保障人机协作安全的同时，改变机械臂运行轨迹，从而提高人机协作单元的生产效率。

为了实现上述目的，本申请实施例提供如下技术方案：

根据本申请实施例的第一方面，提供了一种机器人主动避撞***，所述***包括：

数据获取模块，用于通过人体传感器获取人体骨骼坐标数据；

机械臂离线路径规划模块，用于在ROS测试***环境中进行机械臂离线路径规划，将规划的结果传送至机械臂控制器，以使得机械臂按照规划的路径运行；

虚拟人体模型构造模块，用于按照关节分布将人体虚拟成一个个胶囊连接体，并对应人体传感器获取人体骨骼坐标数据；并将机械臂的关节虚拟成胶囊的形式；

数据滤波融合模块，用于根据数据滤波融合算法得到一组人体骨骼的时间序列，将所述时间序列传送至ROS测试***；

人机距离计算模块，用于所述ROS测试***计算骨骼数据中与机械臂每个胶囊中点以及机械臂末端执行器的距离，并确定出最短的距离；

优化模块，用于通过碰撞测试程序启动控制屏障函数的避撞算法，并利用凸优化设计的代码产生器进行控制***优化，将优化结果发送至ROS路径规划器，以生成目标轨迹路径；

主动避撞模块，用于机械臂基于目标轨迹路径进行运动，以完成人体主动避让。

可选地，所述主动避撞模块具体用于：

当检测到机器人主动避撞***远离最小安全距离时，允许精确跟踪预设的控制输入；利用安全屏障函数，将避撞约束添加到机器人控制回路中，当人机距离小于最小安全距离时，将修改机械臂的运行轨迹，生成新的路径规划，以完成人体主动避让。

可选地，所述控制屏障函数的避撞算法包括一个对数项来形成屏障和一个依赖于

的可调项，通过调整参数a_E和b_E，来调整***和约束的距离；

给定加速度输入，则机器人每个连杆的容许控制值集定义按照如下公式：

将机器人第i个连杆的每个约束重写为：

式中，h_i(x_i)和

是约束函数及其一次导数。

可选地，所述利用凸优化设计的代码产生器进行控制***优化，包括：

有线性约束的二次优化问题允许计算将用作控制输入的关节加速度；优化问题的目标是最小化指令控制输入

和期望的末端执行器笛卡尔加速度

之间的差异；

优化过程按照如下公式定义：

其中J_rob为串联机器人从底座到末端执行器的全部关节运动的雅可比矩阵，Δt为采样周期；为了获得可行的运动，在约束条件中加入关节加速度和速度边界α和β。

可选地，所述控制屏障函数按照如下步骤进行定义：

在定义的集合C∈Rⁿ上，令一个连续可微的h:Rⁿ→R；一个局部Lipschitz连续函数B(x):C→R是一个安全屏障函数，若它的Lie导数L_fB(x)和L_gB(x)满足局部Lipschitz连续，且存在K类函数α₁，α₂和γ＞0，则对于所有的x∈C：

既然B(x)→∞，当h(x)→0且

保证当***状态接近约束时，控制屏障函数停止增长；

计算与控制值u的关系：

K_B(x)为容许控制值集，由于控制屏障函数是在一组容许状态C中定义，因此保证集合C正向不变。

根据本申请实施例的第二方面，提供了一种机器人主动避撞方法，所述方法包括：

通过人体传感器获取人体骨骼坐标数据；

在ROS测试***环境中进行机械臂离线路径规划，将规划的结果传送至机械臂控制器，以使得机械臂按照规划的路径运行；

按照关节分布将人体虚拟成一个个胶囊连接体，并对应人体传感器获取人体骨骼坐标数据；并将机械臂的关节虚拟成胶囊的形式；

据数据滤波融合算法得到一组人体骨骼的时间序列，将所述时间序列传送至ROS测试***；

ROS***与人体传感器通信，以获取人体骨骼坐标数据；

根据数据滤波融合算法得到一组人体骨骼的时间序列，将所述时间序列传送至ROS测试***；

所述ROS测试***计算骨骼数据中与机械臂每个胶囊中点以及机械臂末端执行器的距离，并确定出最短的距离；

通过碰撞测试程序启动控制屏障函数的避撞算法，并利用凸优化设计的代码产生器进行控制***优化，将优化结果发送至ROS路径规划器，以生成目标轨迹路径；

机械臂基于目标轨迹路径进行运动，以完成对人体的主动避让。

可选地，所述机械臂基于目标轨迹路径进行运动，以完成人体主动避让，包括：

以固定的末端执行器的一个执行运动来检测人体接近机器人时的行为；

离线规划机器人轨迹，计算笛卡尔空间中末端执行器的位置和速度；

利用PD控制器，得到目标加速度方式。

可选地，所述目标加速度方式按照如下公式计算：

其中，K_P∈R^3×3和K_D∈R^3×3为正定对角矩阵，x_ref和

为末端执行器的参考姿态和速度，x和

为机器人的当前位置和速度；所述目标加速度作为***优化问题所需的加速度反馈给安全优化单元。

根据本申请实施例的第三方面，提供了一种设备，所述设备包括：数据采集装置、处理器和存储器；所述数据采集装置用于采集数据；所述存储器用于存储一个或多个程序指令；所述处理器，用于执行一个或多个程序指令，用以执行第二方面任一项所述的方法。

根据本申请实施例的第四方面，提供了一种计算机可读存储介质，所述计算机存储介质中包含一个或多个程序指令，所述一个或多个程序指令用于执行如第二方面任一项所述的方法。

综上所述，本申请实施例提供了一种机器人主动避撞***及方法，通过数据获取模块，用于通过ROS测试***获取人体骨骼坐标数据；机械臂离线路径规划模块，用于在ROS测试***环境中进行机械臂离线路径规划，将规划的结果传送至机械臂控制器，以使得机械臂按照规划的路径运行；虚拟人体模型构造模块，用于按照关节分布将人体虚拟成一个个胶囊连接体，并对应人体传感器获取人体骨骼坐标数据；并将机械臂的关节虚拟成胶囊的形式；数据滤波融合模块，用于根据数据滤波融合算法得到一组人体骨骼的时间序列，将所述时间序列传送至ROS测试***；人机距离计算模块，用于所述ROS测试***计算骨骼数据中与机械臂每个胶囊中点以及机械臂末端执行器的距离，并确定出最短的距离；优化模块，用于通过碰撞测试程序启动控制屏障函数的避撞算法，并利用凸优化设计的代码产生器进行控制***优化，将优化结果发送至ROS路径规划器，以生成目标轨迹路径；主动避撞模块，用于机械臂基于目标轨迹路径进行运动，以完成人体主动避让。提出了一种基于安全控制屏障函数的控制结构，设计主动避撞约束执行控制器并串联在现有的控制回路中，在人机之间添加一个虚拟的安全屏障，在保障人机协作。

附图说明

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

本说明书所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明可实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容能涵盖的范围内。

图1为本申请实施例提供的一种机器人主动避撞***框图；

图2为本申请实施例提供的控制***的结构示意图；

图3为本申请实施例提供的机械手的连续连杆子集示例；

图4为本申请实施例提供的人体胶囊模型示意图；

图5为本申请实施例提供的人机距离计算示意图；

图6为本申请实施例提供的人机协作场景示意图；

图7为本申请实施例提供的机械臂主动避让图；

图8为本申请实施例提供的机器人主动避撞方法流程示意图。

具体实施方式

以下由特定的具体实施例说明本发明的实施方式，熟悉此技术的人士可由本说明书所揭露的内容轻易地了解本发明的其他优点及功效，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1示出了本申请实施例提供的一种机器人主动避撞***框图，所述***包括：

数据获取模块101，用于通过ROS测试***获取人体骨骼坐标数据；

机械臂离线路径规划模块102，用于在ROS测试***环境中进行机械臂离线路径规划，将规划的结果传送至机械臂控制器，以使得机械臂按照规划的路径运行；

虚拟人体模型构造模块103，用于按照关节分布将人体虚拟成一个个胶囊连接体，并对应人体传感器获取人体骨骼坐标数据；并将机械臂的关节虚拟成胶囊的形式；

数据滤波融合模块104，用于根据数据滤波融合算法得到一组人体骨骼的时间序列，将所述时间序列传送至ROS测试***；

人机距离计算模块105，用于所述ROS测试***计算骨骼数据中与机械臂每个胶囊中点以及机械臂末端执行器的距离，并确定出最短的距离；

优化模块106，用于通过碰撞测试程序启动控制屏障函数的避撞算法，并利用凸优化设计的代码产生器进行控制***优化，将优化结果发送至ROS路径规划器，以生成目标轨迹路径；

主动避撞模块107，用于机械臂基于目标轨迹路径进行运动，以完成人体主动避让。

在一种可能的实施方式中，所述主动避撞模块具体用于：

本申请实施例提出一种基于控制屏障函数的机器人主动避撞方法：通过胶囊法简化机械臂和人体模型，优化人体与机械臂之间的距离的计算方法；为了保证协作过程中机器人与人体无碰撞运行，在每个机器人环节上建立了一个控制屏障函数和***优化方法，使得机器人***是对安全距离非常敏感：当***远离最小安全距离时，***允许精确跟踪预设的控制输入，而当连杆和障碍物之间的距离接近最小碰撞距离时，它将修改机械臂的运行轨迹，以确保人机共享空间的人的安全。该算法最大限度地减小了期望加速度输入与指令加速度输入之间的差异，使机器人在检测到发生碰撞前，主动躲避人体，同时保持对期望轨迹的跟踪。

在本申请实施例提供的方法中，首先考虑一个机器人(或多台机器人的集合)，它的动态性能可由一个仿射非线性控制***的形式描述，如式(1)：

其中，x∈Rⁿ是***的状态变量，表示机器人的状态或者多个机器人的复合状态，x(0)＝x₀、

是***控制输入，U是***允许的控制值的集合；f是对不受控状态的函数描述，G是对应于机器人的驱动方式，G＝[g₁,…,g_m]。f:Rⁿ→Rⁿ和

是局部Lipschitz连续向量场。***输出y_out(x)由输出向量h_out:Rⁿ→R^q给出。

用机器人末端执行器的笛卡尔位置x∈R³来表示，假设机器人是最终安装在基座上的机械臂，其一般模型如式(2)：

式中：

是关节位置矢量，

分别代表关节位置的速度矢量和加速度矢量。

是关节扭矩矢量

是对称的正定惯性矩阵，

是离心项和科里奥利项矩阵，

是粘性摩擦矩阵模型

g_i(q_i)是重力项矢量，

是机械手雅可比矩阵，h_i是机器人末端执行器和环境之间的相互作用力矢量，令

作为第i个机器人的关节参考位置(速度、加速度)。

下面作出以下假设：

当前所有的商业机器人***都配备有一个内部运动控制回路，以保证每个参考关节的轨迹都能够被跟踪，如式(3)：

其数学描述为式(4)：

这里仅仅讨论单个机械臂与一个操作员进行人机协作，上式简化为式(5)：

如图2所示的控制结构，机器人***是由标称控制和约束控制构成的组合控制。设计这样的控制结构的优点在于可以将安全约束添加到预先存在的控制回路中，设计标称控制律来实现期望的性能和Lyapunov稳定跟踪误差，如式(6)所示：

e＝w-y_out (6)

为了确保***保持在安全区域内，控制输入应将***状态保持在安全集合内。在应用程序中，在遵循输入、状态或输出约束的同时，需要执行任务，基于安全屏障函数(Control Barrier Functions，CBFs)实现这一目标。控制屏障函数是非负函数，对于接近安全约束的状态，它增长到无穷大，而远离安全约束的状态则变得更小。

第一方面，控制屏障函数的定义。

首先给定一个连续可微的平滑函数

当h(x)＞0时，该安全屏障函数将***状态限制在安全区域内，当h(x)＜0时，表示违反了无碰撞轨迹的约束条件，这意味着必须根据障碍物的形状和测量数据设计约束函数，那么h(x)＝0可以看成是完全包围障碍物的外壳。由一组容许状态集合C定义安全区域及其边界，如公式(7)所示：

***的状态始终保持在可容许状态集合中***只能保持在容许状态的集合内。由于容许状态集合C仅包含约束容许状态x，因此需要设计一个控制算法，使***在施加约束以后的所有时间都保持在C内，即集合在未来所有时间都保持前向不变性。

令u∈U为***(1)的控制值，设x₀＝x(0)为初始状态，如果x(0)∈C，那么对于所有的t≥0，x(t)∈C，则集合C称为被控正不变量。

对于控制屏障函数是能够满足这样的控制方法。假设存在一个函数B(x)∈C满足控制屏障函数的条件，它必须满足以下性质：

(1)SBF-p1：一个有效的控制屏障函数是集合C上一个非负的函数。

(2)SBF-p2：当状态C在容许集内接近约束时，屏障将逐渐增大。

(3)SBF-p3：控制屏障函数随增长率而增长(γ＞0)。

符合上述性质的B(x)的行为本质上类似于K类函数的逆，下面给出K类函数的定义。

如果连续函数α:[0,∞)→[0,∞)是严格递增且α(0)＝0，则它属于K类函数。K类函数α具有以下性质，它们是满足控制屏障函数的B(x)的期望性质：

到此，将先前的性质与形式(1)的一般***的动力学联系起来，可以得到控制屏障函数的定义。

控制屏障函数：在(7)(8)中定义的集合C∈Rⁿ上，令一个连续可微的h:Rⁿ→R。一个局部Lipschitz连续函数B(x):C→R是一个安全屏障函数，如果它的Lie导数L_fB(x)和L_gB(x)满足局部Lipschitz连续，且存在K类函数α₁，α₂和γ＞0，则对于所有的x∈C：

既然B(x)→∞，当h(x)→0且

式(10)保证当***状态接近约束时，控制屏障函数停止增长。一旦定义了一个合适的控制屏障函数，就得到了一个与控制值u的关系(比如前提L_gB(x)≠0是在可接受集C内)属于容许控制值集K_B(x)，如式(11)：

将一个控制输入u∈K_B(x)应用到如式(5-13)描述的***中，由于控制屏障函数是在一组容许状态C中定义，因此***能够保证集合C是正向不变。

下面把机器人控制输入的计算定义为一个优化问题，目的是最小化标称控制(即潜在不安全)和最优控制之间的差异范数，后者受控制屏障函数约束(即它必须属于给定的K_B(x))。

机器人控制***必须在线求解一个优化问题，以生成最接近标称轨迹的无碰撞轨迹。如果所有的机械臂连杆都保持在一个安全的集合中，那么整个机器人将满足无碰撞条件。因此，需要计算每个机器人的连杆与每个障碍物可能发生的所有碰撞。为此，需要为人机协作***的每个成员定义一个模型(即一个用于人类操作员，一个用于机械臂)。这些模型允许从数学上表示***的任何实体，定义其行为(动态和动态)、空间形状和实体之间的关系。为了增加***的实时性，选择使用虚拟胶囊仓的方法对机器人连杆和人体骨架进行建模，将它们封装在最合适和最简单的形状中，从而可以方便地计算人和机械臂的距离。给定两个笛卡尔点，胶囊是由两个以该点为中心的半球体和一个纵轴连接两个点的圆柱体组成的虚拟物体。下面分别给出机械和人体的胶囊模型。

第二方面，机器人简化模型。

对于机器人***(地面、水下或者空中)，这种控制是标准的仿射控制结构。进行基于约束的控制设计的一种方法是选择控制输入u，以确保机器人能够一直停留在某个集合中

中。用x∈R³来表示机器人末端执行器的笛卡尔位置，这里考虑机器人的轨迹是根据期望的加速度

来规划的，以避免非平稳运动(即不连续的速度)。将***状态定义为

因此，***动力学模型由式(1)给出，其输入u为期望的末端执行器质心加速度

如式(12)：

为了简化计算机器人各环节与操作者之间的距离，这里将机器人本体分解为连续关节的子集，这样就可以明确地考虑到机器人各环节之间的运动耦合。特别是根据机械手的串联连接，考虑到与机器人基座连接的第一个环节，得到第一个子集，考虑到第一和第二个环节，得到第二个子集，依次可以得到所有的子集。考虑具有自由度(DOF)的串联机械手，定义m个连杆和m个关节，关节位置矢量q＝(q₁,...,q_m)唯一指定机器人配置。如图3所示，定义从基部到末端执行器的m个子集，并且每个子集可以与向量Θ_i＝(q₁,...,q_i)∈Rⁱ i＝1,...,m通过雅可比矩阵J_p,i(Θ_i)相关联。

令x_e,i∈R³表示每个子集最后一个连杆上末端点，x_c,i∈R³表示每个子集最后一个链接上的中点，则相应的笛卡尔加速度可以定义如式(13)：

其中，J_p,i∈R^3×i是每个雅可比子模型的线速度部分，这里每个子模型

的状态都受到与式(12)结构相同的动力学的影响。

第三方面，人类模型。

关于人类操作者的模型，一个简单的解决方案是将整个身体围成一个单一的单元(例如，球体或圆柱体)。然而，这个解决方案对于一个正确的人体模型来说过于保守和简单。一个不那么保守和精确的人体表现包括定义每个身体部位周围的障碍物。这里结合Kinect传感器的人体骨骼数据输出，选择了12个类胶囊模型(如图4所示)来体现人体部分，每个胶囊由两个人体关节和一个半径的位置来定义，同时考虑到实际情况，人的头部和手部采用球来。关节位置是由定位***获得的，而半径是在考虑一般人体的情况下预先定义的。选择了12个胶囊，因为这是对人体精确表示和在线计算它们的计算负荷之间的权衡。包含手和脚的胶囊有一个半径，允许甚至包括手指。计算时，针对人机协作场景，并没有选取全部的骨骼点进行计算，选择了手、手腕、小臂、肩膀、颈部、头部、腰部和膝盖位置的骨骼点，进行实时计算到机械臂的距离，同时本文还提出，在人体小臂、大臂和头颈之间，颈腰之间分别增加一个中点坐标参与与机械臂的计算。

第四方面，人机距离计算。

一旦将人体和机器人身体定义为胶囊的组成部分，就可以计算它们之间的距离。然而，对每一对机器人胶囊和人体胶囊进行这种在线操作需要很多时间，威胁着人类的安全。为了进一步简化计算负载，为每个机器人连杆识别最接近的人机胶囊，即距离链接本身最小距离的人体胶囊(图5)。

因此，首先计算出人体骨架和机器人连杆中间点之间的所有距离，如式(14)：

其中x_c,i∈R³是第i个连杆的中点，

是人体骨架j上最接近x_c,i和r_e,i的点。r_c,i是机器人胶囊的半径，r_h,j是人体胶囊模型的半径，可以根据不同的机械臂位置和人体关节位置设置为不同的两组常数。

由于此评估是在每个时刻执行的，因此人体上的最近点会根据人体运动动态变化。

值得注意的是，机器人连杆中点和末点的选择是一种设计选择。理论发现对于所考虑的机器人连杆的纵轴上的任何其它不动点都成立。实际上，距离是根据公式(14)计算的，其中机器人胶囊的半径是沿着胶囊本身的常数。因此，必须根据沿机器人连杆选择的点建立运动链。

然后，在优化问题中，只需要针对机器人的每一个关节，关注一个人体胶囊。为了便于记法，对应于最近的人类胶囊，在下面将直接使用术语x_h来指代点x_hj。

第五方面，基于安全屏障函数的机械臂避撞算法。

优化问题中的约束条件(保证无碰撞轨迹)是根据控制屏障函数定义的，可以把人类看作一个没有控制输入的移动障碍物，它的骨架被限制在胶囊中。一旦确定了人类身体上最近的胶囊，就可以为机器人链接的每个子集在状态

上定义一个约束函数h_i(x_i)，如式(15)所示：

h_i(x_i)＝d_ij-d_PSD≥0,i＝1,...,m (15)

式中，d_PSD是保证操作员安全的最小安全距离。

考虑(1)，(12)中机器人的动力学模型，其中u_i是控制输入，第i个连杆中点和末端的期望加速度。

在具有广义动力学方程(13)的力矩控制机器人***中，位置约束相对于输入u_i的相对阶数为2，即

依赖于输入。这意味着仅基于约束h_i(x_i)的函数(其相对于输入u_i的相对阶数为2)不是有效的候选函数。然而，控制输入u_i的依赖性出现在约束函数h_i(x_i)的时间导数中。对于这种情况，引入如式(16)的函数：

式中，h_i(x_i)和

是约束函数及其一次导数，这是一个可容许的控制屏障函数；

必须满足：属性SBF-p1、SBF-p2和SBF-p3，它的时间导数必须明确地依赖于输入。换句话说，控制屏障函数和输入u之间的相对度必须是1。所提出的控制屏障函数包括一个对数项来形成屏障和一个依赖于

的可调项，通过适当地调整参数a_E和b_E，来调整***和约束的距离。

给定加速度输入，则机器人每个连杆的容许控制值集(11)定义如式(17)、(18)和(19)：

不仅考虑末端效应器的加速度，同时考虑每个连杆中点的加速度。因此，得到每个连杆的屏障函数，其中控制输入u_i是(13)中期望的笛卡尔加速度

因此，可以将机器人第i个连杆(17)的每个约束重写为式(20)：

由于不可能任意设置关节速度和加速度的值，因此这里选择关节加速度向量

作为与安全约束所允许的最优控制输入，同时将当前关节速度作为***优化控制的参数。

第六方面，***优化算法。

具有线性约束的二次优化问题允许计算将用作控制输入的关节加速度。优化问题的目标是最小化指令控制输入

和期望的末端执行器笛卡尔加速度

之间的差异。同时，通过安全屏障函数保证人机协作过程的安全。

假设雅可比矩阵包含在R和S中，如式(21)和(22)所示：

S_i＝a_BiJ_pi,S_i∈R^1×i (22)

优化问题定义如下：

满足式(23)：

其中J_rob为串联机器人从底座到末端执行器的全部关节运动的雅可比矩阵，Δt为采样周期。为了获得可行的运动，在约束条件中加入关节加速度和速度边界α和β。

机器人***是对安全距离非常敏感：当***远离最小安全距离时，***允许精确跟踪预设的控制输入，而当连杆和障碍物之间的距离接近最小碰撞距离时，它将修改机械臂的运行轨迹。

下面介绍本申请实施例具体实施时的用于人机协作的机器人主动避撞方法实施场景。

为了验证基于控制屏障函数的机器人主动避撞算法的有效性，设计了一个人机协作单元场景，包括一台UR10机械臂和一个人类操作员，如图6所示。结合Kinect传感器输出的人体空间位置的结果，利用上述机器人主动避障理论，实现进行验证。

如图6所示，以固定的末端执行器的一个执行运动来检测人体接近机器人时的行为。首先离线规划机器人轨迹，计算笛卡尔空间中末端执行器的位置和速度。然后利用PD控制器，得到所需的加速度方式为式(24)：

其中K_P∈R^3×3和K_D∈R^3×3为正定对角矩阵，x_ref和

为末端执行器的参考姿态和速度，x和

为机器人的当前位置和速度。该加速度作为***优化问题所需的加速度反馈给安全优化单元。

在测试过程中，机械臂在保证人身安全的前提下，将尽可能的遵循预期规划的轨迹。根据加速度和速度，机器人的期望轨迹是使用离线模拟器进行规划。人类操作员的运动感知由Kinect传感器进行监测。在机械臂在预定的轨迹运行过程中，当人类操作员接近机械臂时，***实时计算出人类与末端执行器距离，并实时保障人机距离大于最小安全距离，当距离小于等于最小安全距离时，机器人主动避让，保障人身安全。

(1)首先打开ROS软件，测试ROS***能收到人体骨骼坐标数据。

(2)在ROS环境中进行机械臂离线路径规划，传送给机械臂控制器，机械臂按照规划的路径运行。

(3)移动人体手臂，经过Kinect传感器得到人体骨骼坐标数据，经过数据滤波融合算法得到一组人体骨骼的时间序列；将时间序列传送至ROS软件，数据刷新频率为25Hz。

(4)ROS软件计算骨骼数据中与机械臂每个胶囊中点以及机械臂末端执行器的距离，返回人机最短距离，启动碰撞检测程序，启动控制屏障函数的避撞算法，并利用凸优化设计的快速代码产生器进行控制***优化。将优化结果传送至ROS路径规划器，生成新的轨迹路径。

(5)机械臂按照新的轨迹路径运动，完成人体主动避让。

为了简化实验，实验中仅仅将Kinect检测到人手的数据传送给ROS***。启动人机协作测试程序后，机械臂开始运动，随着人体手臂的运动，***将实时的计算机械臂和操作员之间的距离，随着操作员右手逐步靠近机械臂，当检测到人机最短距离小于安全值时，机器人将偏离标称轨迹，主动躲避人手，并生成新的运行轨迹，继续执行任务。当人类操作员右手与机械臂末端的距离小于设定的安全距离0.1m时，机械臂将启动主动避撞算法，主要表现在Z轴的明显的向上移动，以保持与人手的距离大于最小安全距离。

上述方法是在三维欧氏空间中评估人-机器人的安全协作。利用安全屏障函数，将避撞约束添加到机器人控制回路中，以实现当人机距离小于最小安全距离时，机械臂主动避让，避免因人体操作员的频繁靠近而频繁停机。基于优化的算法在人-机器人共享环境中进行了测试，在该环境中，当人类在附近工作时，机器人必须遵循一个标称轨迹。结果表明，当人机距离超过设定值时，该机器人偏离了标称轨迹，避免了与操作者的碰撞，保证了操作者在执行任务时的安全，从而兼顾安全性和生产率。

可以看出，本申请实施例提出一种基于控制屏障函数的机器人主动避撞方法：通过胶囊法简化机械臂和人体模型，优化人体与机械臂之间的距离的计算方法；为了保证协作过程中机器人与人体无碰撞运行，在每个机器人环节上建立了一个控制屏障函数和***优化方法，使得机器人***是对安全距离非常敏感：当***远离最小安全距离时，***允许精确跟踪预设的控制输入，而当连杆和障碍物之间的距离接近最小碰撞距离时，它将修改机械臂的运行轨迹，以确保人机共享空间的人的安全。该算法最大限度地减小了期望加速度输入与指令加速度输入之间的差异，使机器人在检测到发生碰撞前，主动躲避人体，同时保持对期望轨迹的跟踪。

基于相同的技术构思，本申请实施例还提供了一种机器人主动避撞方法，如图8所示，所述方法包括：

步骤801：通过人体传感器获取人体骨骼坐标数据；

步骤802：在ROS测试***环境中进行机械臂离线路径规划，将规划的结果传送至机械臂控制器，以使得机械臂按照规划的路径运行；

步骤803：按照关节分布将人体虚拟成一个个胶囊连接体，并对应人体传感器获取人体骨骼坐标数据；并将机械臂的关节虚拟成胶囊的形式；

步骤804：根据数据滤波融合算法得到一组人体骨骼的时间序列，将所述时间序列传送至ROS测试***；

步骤805：所述ROS测试***计算骨骼数据中与机械臂每个胶囊中点以及机械臂末端执行器的距离，并确定出最短的距离；

步骤806：通过碰撞测试程序启动控制屏障函数的避撞算法，并利用凸优化设计的代码产生器进行控制***优化，将优化结果发送至ROS路径规划器，以生成目标轨迹路径；

步骤807：机械臂基于目标轨迹路径进行运动，以完成人体主动避让。

在一种可能的实施方式中，所述机械臂基于目标轨迹路径进行运动，以完成人体主动避让，包括：以固定的末端执行器的一个执行运动来检测人体接近机器人时的行为；离线规划机器人轨迹，计算笛卡尔空间中末端执行器的位置和速度；利用PD控制器，得到目标加速度方式。

基于相同的技术构思，本申请实施例还提供了一种设备，所述设备包括：数据采集装置、处理器和存储器；所述数据采集装置用于采集数据；所述存储器用于存储一个或多个程序指令；所述处理器，用于执行一个或多个程序指令，用以执行所述的方法。

基于相同的技术构思，本申请实施例还提供了一种计算机可读存储介质，所述计算机存储介质中包含一个或多个程序指令，所述一个或多个程序指令用于执行所述的方法。

本说明书中上述方法的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。相关之处参见方法实施例的部分说明即可。

需要说明的是，尽管在附图中以特定顺序描述了本发明方法的操作，但这并非要求或者暗示必须按照该特定顺序来执行这些操作，或是必须执行全部所示的操作才能实现期望的结果。附加地或备选地，可以省略某些步骤，将多个步骤合并为一个步骤执行，和/或将一个步骤分解为多个步骤执行。

虽然本申请提供了如实施例或流程图的方法操作步骤，但基于常规或者无创造性的手段可以包括更多或者更少的操作步骤。实施例中列举的步骤顺序仅仅为众多步骤执行顺序中的一种方式，不代表唯一的执行顺序。在实际中的装置或客户端产品执行时，可以按照实施例或者附图所示的方法顺序执行或者并行执行(例如并行处理器或者多线程处理的环境，甚至为分布式数据处理环境)。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、产品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、产品或者设备所固有的要素。在没有更多限制的情况下，并不排除在包括所述要素的过程、方法、产品或者设备中还存在另外的相同或等同要素。

上述实施例阐明的单元、装置或模块等，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。为了描述的方便，描述以上装置时以功能分为各种模块分别描述。当然，在实施本申请时可以把各模块的功能在同一个或多个软件和/或硬件中实现，也可以将实现同一功能的模块由多个子模块或子单元的组合实现等。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内部包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

本申请可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构、类等等。也可以在分布式计算环境中实践本申请，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。

通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，移动终端，服务器，或者网络设备等)执行本申请各个实施例或者实施例的某些部分所述的方法。

本说明书中的各个实施例采用递进的方式描述，各个实施例之间相同或相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。本申请可用于众多通用或专用的计算机***环境或配置中。例如：个人计算机、服务器计算机、手持设备或便携式设备、平板型设备、多处理器***、基于微处理器的***、置顶盒、可编程的电子设备、网络PC、小型计算机、大型计算机、包括以上任何***或设备的分布式计算环境等等。

以上所述的具体实施例，对本申请的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本申请的具体实施例而已，并不用于限定本申请的保护范围，凡在本申请的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。