CN112706158B - 基于视觉和惯导定位的工业人机交互系统及方法 - Google Patents

Abstract

本发明涉及基于视觉和惯导定位的工业人机交互系统及方法，包括：至少1个机械手，用于抓握以及加工目标工件；至少1个机械臂，机械臂的底部为基座，机械臂末端安装所述机械手；至少4个工业摄像机，用于获取操作人员工作区域的手臂、手指关节及机械手的彩色图像；至少4个摄像机，用于对空间机械臂机械手的位置进行立体定位，至少1组体感穿戴设备，穿戴于操作人员手臂位置，用于获取移动方向的位移、速度、加速度信息；至少1台计算机，用于获取信息并在操作人员工作区域的小视场内，计算机通过获取穿戴设备信息以及摄像机图像信息，判断操作人员是否与机械臂及机械手发生碰撞。

Description

基于视觉和惯导定位的工业人机交互系统及方法

技术领域

本发明涉及一种基于视觉和惯导定位的工业人机交互系统及方法，属于人 机交互和工业测量领域。

背景技术

当前工业现场有很多人机交互保证人机安全的方法，包括有通过手持控制 器保证人在安全范围内、通过护栏保证人身安全、操作人员在机器人工作范围 外保证人身安全等，这些人机交互方式大多数都是串行、人机并没有真正接触 的工作方式。现有人机交互系统中，操作人员主要进行的工作为机器人工作原 料的装载、在安全距离外机器人运行的监督以及成品的检验。现有方案人机交 互过程依然受到空间限制，无法使人机交互摆脱空间限制随人走动，且识别图 像处理过程复杂。

本发明旨在发明一种基于视觉和惯导定位的工业人机交互系统及方法，以 克服上述缺点。

发明内容

为了解决上述技术问题，本发明提供一种基于视觉和惯导定位的工业人机 交互系统，可以消除人机交互中的空间限制和交互程度，提高人机交互的便利 性，且能够提高人机交互过程中的人机安全程度。

本发明的技术方案如下：

一种基于视觉和惯导定位的人机主动安全及协作系统，包括：

至少1个机械手，用于抓握以及加工目标工件；

至少1个机械臂，机械臂的底部为基座，机械臂末端安装所述机械手；

至少4个工业摄像机，用于获取操作人员工作区域的手臂及机械手的彩色 图像；

至少4个摄像机，用于对空间机械臂机械手的位置进行立体定位，采集摄 像机信息，包括机械臂工作环境的彩色图像信息、深度图像信息；

至少1组体感穿戴设备，穿戴于操作人员手臂位置，用于获取体感穿戴设 备信息，包括操作人员手臂的手肘，手腕，手指关节的移动方向的位移、速度、 加速度信息；

至少1台计算机，用于获取体感穿戴设备中的惯导传感器的信息并在操作 人员工作区域的小视场内，计算机通过获取体感穿戴设备信息、摄像机信息以 及工业摄像机的彩色图像，判断操作人员是否与机械臂及机械手发生碰撞。

所述体感穿戴设备包括：

安装在手臂关节位置及手掌的惯导传感器；

安装在手指关节及手指末端的主动光源。

所述计算机用于：

摄像机处获得的彩色图像信息与深度图像信息用于计算场景中无人区域移 动目标的空间位置信息、移动速度及方向信息，得到场景中移动目标与机械臂 及机械手的相对位置、相对速度信息，并对移动目标是否与机械臂及机械手是 否发生碰撞进行预测和判断；

处理惯导传感器获得的手臂关节及手掌的位置信息、速度信息、加速度信 息；

从操作人员工作区域的工业摄像机获取的彩色图像得到参与工作的手指关 节位置信息及机械手的位置信息。

一种基于视觉和惯性定位的人机主动安全及协作方法，包括以下步骤：

(1)将所述机械臂基座的中心点作为三维空间坐标系的原点建立世界坐标 系，将所述机械手的末端中心点作为原点建立末端坐标系；

(2)采用所述摄像机发射红外激光，采集彩色图像信息和深度图像信息， 以摄像头作为原点建立相机坐标系，对大视场中摄像机进行标定，建立摄像机 间的坐标变换；

(3)采用所述惯导传感器，采集操作人员手臂关节及手掌的位置信息；

(4)采用所述主动光源及工业摄像机，采集操作人员手指关节及手指末端 的位置及图像信息；

(5)采用所述计算机基于大视场中摄像机及工业摄像机获取的彩色图像、 彩色图像信息及深度图像信息，获取场景图像每个像素点的图像坐标及相机坐 标，通过光流估计方法检测移动目标在图像中的位置信息，采用图像坐标到世 界坐标的映射以及相机坐标系到世界坐标系的坐标变换，得到大视场内移动目 标的世界坐标中的位置信息；结合计算机获取的机械臂的位置信息，获得移动 目标与机械臂及机械手的相对位置和相对速度，判断移动目标是否与机械臂发 生碰撞，并指定机械臂及机械手的主动避让行为；

(6)采用所述计算机基于小视场内获取的图像信息、小视场内场景及操作 人员肢体的三维坐标信息、速度、加速度信息，采用积分运算得到操作人员的 手臂位置信息，通过主动光源获得的手指位姿信息，判断操作人员在工作区域 内是否与机械臂及机械手发生碰撞，并指定机械臂及机械手的主动避让行为。 所述大视场为机械臂最大工作空间周围的设置的包括摄像机和工业摄像机的成 像设备能够采集到的最大范围；所述小视场为完成具体工作时机械臂末端与操 作人员发生直接接触或间接接触的场景中成像设备能采集到的最大范围。

步骤(5)中所述判断移动目标是否与机械臂发生碰撞，包括：

移动目标与机械臂的最近点p、移动目标与机械臂的相对位置d、相对速度 v作为是否发生碰撞的评价指标，具体内容如下：

当d＜T_d，v＜T_v时，判定可能发生碰撞；

当d＜T_d，v＞T_v时，判定可能发生碰撞；

当d＞T_d，v＜T_v时，无干扰；

当d＞T_d，v＞T_v时，判定可能发生碰撞；

其中，T_d、T_v为在不同的应用时设置不同的数值。

步骤(6)中所述判断操作人员在工作区域内是否与机械臂及机械手发生碰 撞，包括：

移动目标与机械臂的相对速度v作为是否发生碰撞的评价指标，具体内容 如下：

当v≥T_v时，判定可能发生碰撞；

当v＜T_v时，无干扰；

其中，T_v为在不同的应用时设置不同的数值。

本发明具有以下有益效果及优点：

1.本发明避免人机交互工程中的人机碰撞概率，保证工作人员的人身安全。

2.本发明估计误闯入工作空间的目标对生产过程的影响，提高系统安全性。

3.本发明支持机械臂工作空间的实时检测，无需设置额外的防护栏，提高 空间利用率，降低设备成本。

4.本发明允许机械臂与人体发生直接接触，并实时估计机械臂对人体碰撞 的风险，可以扩大机械臂在工业生产中的应用范围。

5.本发明穿戴设备可以泛化到不同的人体关节迁移性较强，可实时提供各 个关节位置信息，为其他人机交互场景提供了解决方案。

附图说明

图1为本发明应用场景的工作区域整体示意图；

图2为本发明的非交互场景障碍物检测流程图；

图3为本发明的系统内坐标变化示意图；

图4为本发明的受限关节速度确定示意图；

图5为本发明的机械臂在线避障路径规划示意图；

图6a为本发明的穿戴设备示意图一；

图6b为本发明的穿戴设备示意图二；

图7为本发明的穿戴设备辅助流程图；

图8为本发明的交互场景避障流程图；

图9为本发明的末端关节速度确定示意图。

具体实施方式

下面结合附图对本发明进行详细说明。

基于视觉和惯导定位的工业人机交互系统，包括被控设备、场景图像采集 系统及穿戴在操作者手臂、手掌及指尖的辅助定位设备。

本系统将人机交互的场景可以被分为两类：非交互工作空间、交互工作空 间。其中交互工作空间内工作过程中操作人员与机械臂本会发生直接或间接地 接触。人机交互过程中威胁人及安全的主要问题为操作人员与被控设备之间的 碰撞，该系统在运行过程中的主要任务为预测即将发生的碰撞。针对两类不同 的场景本系统将保证人机安全的问题分成两个子问题：(一)非交互工作空间 避障及路径规划；(二)交互工作空间避障及路径规划。

协作方法在应用中具有两种工作场景：非交互工作空间、交互工作空间， 不同的工作场景所判断移动目标与机械臂是否发生碰撞的评价指标不同。所述 非交互工作空间为大视场场景；所述交互空间为小视场场景。大视场为机械臂 最大工作空间周围的设置的包括摄像机和工业摄像机的成像设备能够采集到的 最大范围；所述小视场为完成具体工作时机械臂末端与操作人员发生直接接触 或间接接触的场景中成像设备能采集到的最大范围；所述直接接触为人机交互 中操作人员与机械臂发生直接的物理接触；所述间接接触为人机交互过程中操 作人员与机械臂进行工件传递等对同一个目标同时进行操作时间接产生的接触。

(一)非交互工作空间避障及路径规划

人机协作场景的基本框架中，场景中存在一个或者多个机械臂，工作人员 的正常操作区域与机器人的活动区域有重叠，在重叠区域完成人机协作。在实 际应用过程中可能出现在正常操作区域之外的交互场景中的人与目标，我们称 之为非交互工作空间异物。在操作区域外的有异物入侵会引发人机安全问题， 本子系统采用计算机视觉检测技术，解决非交互工作i空间中的人机安全问题。

所述计算机视觉检测子系统连接到控制设备软件操作界面，检测系统利用 场景中安装的多组Kinect2.0成像设备实时采集非交互场景中的场景图像和场 景深度信息，然后判断场景中是否有异物侵入。计算机视觉检测子系统实时对 场景目标进行点云重构，并检测当前场景中目标与机械臂的最近点。该子系统 通可以通过实时获取点云确定最近点p非交互场景中的位置，与被控设备的相对 速度、相对位置信息。子系统可以通过获取的点p的位置与被控设备的相对位置、 相对速度等信息，并将其作为标准判断前景目标是否与机械臂发生碰撞。计算 机视觉检测子系统在运行过程中的状态可以分为以下几种：a.若判断异物与被 控设备会发生碰撞，根据系统设置的路径规划方法对机械臂进行路径规划；b.若判断异物与被控设备不发生碰撞时，被控设备正常运行，执行当前命令。c.若 未检测到异物侵入，被控设备正常运行，执行当前命令。

(二)交互工作空间避障及路径规划

操作区域为生产过程中操作人员身体部位应该频繁出现，并且正常过程中 必须与被控设备发生接触的区域，实际生产过程中执行人机交互操作的区域是 系统设计时进行规定的。受人机交互过程中人体不同部位的不同移动尺度限制， 操作区域内的目标定位及异常检测子系统采用了惯导传感定位系统对手臂、手 掌的关节位置进行标记，采用可见光定位技术在手指关节及尖端安装主动光源 进行定位。

控制器通过蓝牙与操作人员的穿戴设备进行通讯，可以获取操作人员手臂 关节位置、手掌关节位置，从而估计进入交互区域的人体部位三维空间位置。 操作系统周围的摄像头可以获取安装在手指关节及手指末端的不同颜色发光等 的图像，控制器通过可见光定位技术实时的获取操作人员手部的详细位姿信息。 操作区域内系统结合视觉成像设备与惯导传感器实时评估前景目标的移动速度， 将速度作为是否发生碰撞的依据。计算机视觉检测子系统在运行过程中的状态 可以分为以下几种：a.若相对速度过大，判断异物与被控设备末端会发生碰撞， 根据系统设置的路径规划方法对机械臂进行路径规划；b.若相对速度不超过 预定阈值，判断异物与被控设备不发生碰撞，被控设备继续执行当前操作命令。

如图1所示，应用场景分为非交互空间寄交互空间两个区域，系统在两个 区域采用不同的障碍检测及路径规划策略。

(一)非交互工作空间障碍物检测及机械臂路径规划：

自动化流程生产场地需要严格的管理和设定，机械臂周围出现的非必要人 员及目标都是与机械臂发生碰撞引发事故的安全隐患。本专利中将上述场景中 的协作机械臂的避障行为定义机械臂非交互工作空间中的障碍物检测。

如图1所示，我们利用机械臂非交互场景周围安装的Kinect2.0获取场景 RGBD图像，并进行图像拼接和场景重建，获取预设工作场景的三维信息。在机 械臂正常运行过程中，系统根据Kinect2.0获取的RGBD信息进行实时重建当 前工作场景。

如图2所示，通过系统根据Kinect2.0获取的RGBD信息对预设工作场景进 行重建，结合实时获得的工作场景可以对工作场景进行精准实施重建。世界坐 标系以机械臂基座中心所在的位置为基准点，系统已知机械臂的三维空间位置， 进行机械臂滤除后，系统可以实时获得场景中的障碍物信息，获取障碍物与机 械臂的最近点p，并实时评估点p与机械臂的相对位置及相对速度等信息。

场景内的成像设备已经进行标定，按照图3进行坐标变换，将成像设备获 取的图像转换到世界坐标系内。Kinect2.0可以获得场景内的深度图像与彩色图 像，获取的彩色图像及深度图像进行配准可以完成单Kinect2.0的三维场景重 建。标定后的成型设备间的关系已知，根据系统内坐标变换可以拼接多个成像 设备获得的信息完成整个场景的实时三维点云重建。

障碍物与机械臂相对速度确定：

世界坐标以机器人基座中心点为基准点，机械臂与控制系统直接连接，系 统可以实时获取机械臂各关节的运动信息、机械臂的三维空间信息及机械臂骨 架线。利用Kinect2.0实时获取的图像可以对场景中进行三维重建。遍历场景 的点云坐标与机械臂坐标可以获得障碍物与机械臂的最近点p。障碍物在以p为 球心，r为半径的球体内的点集构成障碍物距离机械臂最近的区域Z。如图4所 示，由p点机械臂骨架线l最近的一段l’最近点作垂线。在点p及对应骨架线 l’构成的平面A内，可以计算点p速度v相对骨架线l’的垂直分量v’。

点p与机械臂的相对位置d、相对速度v做为评价指标，我们将障碍物与 机械臂关系及分为以下几类：

其中，T_d、T_v根究经验在不同的应用场景设置不同的数值。

如图5所示，判定机械臂与障碍物发生碰撞时，系统对机械臂进行路径规 划。当预测发生碰撞时，为了不继续减小障碍物与机械臂间的最小距离，我们 控制机械臂关节使l’在平面A内的运动速度为v’。系统运行时实时更新点p 位置信息，从而实现机械臂整体远离障碍物避免碰撞。

机械臂关节及速度确定：

以完成当前任务为前提机械臂能够躲避障碍物进行路径规划，执行任务与 避障同时进行时。由于障碍物的存在部分关节受到限制为受限关节，其余关节 为冗余关节。系统利用冗余关节补充受限关节的自由度躲避障碍并维持末端执 行器的运动状态。机械臂在线避障过程中的路径规划可表示为以下流程：

机械臂关节与执行任务的在笛卡尔空间中的表示可以描述为：

其中x，q为任务及机械臂关节位置空间表示，J^#为雅可比矩阵J的伪逆， J∈R^m×n，m为完成任务所需自由度，n为机械臂自由度，m＜n，R为实数。

判定发生碰撞时，当前任务在笛卡尔世界坐标系中的坐标表示为x，受限关 节速度约束

及受限关节能够到达的空间坐标x′可由以下公式获得：

其中I∈R^n×n为单位矩阵，q_c，q_v为约束关节及冗余关节的空间表示，x为 任务的笛卡尔空间坐标表示，x′为受限关节的空间坐标表示， (JW)^#为雅可比矩阵(JW)的伪逆，W＝diag{W_ii}，i＝[1，…，n]为关节约束的示 性矩阵，关节i被约束时W_ii＝1，反之W_ii＝0。

冗余关节对任务x进行补偿，关节速度可有以下公式获得：

其中，J_c与J_v分别表示受限及冗余关节的雅可比矩阵表示，O为0矩阵，x_v为 补偿关节点的笛卡尔空间坐标表示。

如图4所示，系统运行过程中根据障碍物的当前运动实时更新点受限关节 及其速度。实际生产中，机械臂依靠末端执行器完成任务，机械臂末端的笛卡 尔坐标可表示为x_e，x_e的一阶导数x_e′为机械臂关节的速度可由以下公式获得

机械臂完成避障任务时关节速度可表示为

系统实时控制关节 避障及达到目标位置，进行实时路径规划。

(二)交互工作空间避障及路径规划：

穿戴设备及辅助原理：

如图6a～图6b所示，为系统设计的穿戴设备。交互场景中工作人员与机械 臂末端执行器存在直接或间接接触，人机交互过程中存在多种交互场景周围预 设相机不能确定的盲区。根据上述情况人机交互场景中工作人员需佩戴设计的 穿戴设备进行辅助定位。

穿戴设备包含两种传感器，关节惯导定位传感器佩戴在关节位置并附有主 动光源进行辅助定位。手部穿戴设备在关节及手指末尾安装有主动光源。穿戴 设备在系统运行过程中，时刻T开始，可以实时记录关节的运行轨迹。当工作 人员肢体出现遮挡时，视觉成像设备失效，可以利用惯导传感器记录关节轨迹， 辅助定位。

如图7所示，系统中以成像设备获取的图像进行三维成像确定的人体定位 为主，实时更新人体在三维空间中的位置。交互场景中多摄像机三维成像技术 可实时对主动光源位置进行定位，进而实时获取工作人员手部及上肢重要关节 的位姿、运动速度信息。当人体在成像设备中出现遮挡时，系统将遮挡发生时 记为起始时刻，以原有位姿为基础，对惯导穿戴设备获取的速度进行积分，计 算遮挡时间内工作人员上肢的位移，辅助估计人体在三维空间中的位置。

交互场景避障相对速度计算及避障流程

如图8所示，交互场景中系统由预设的相机进行实时场景三维重建。交互 场景以机械臂基座中心点为基点进行坐标变换后进行机械臂滤除可以获得场景 中前景目标的三维信息，由于可能存在多种盲区需要用惯导定位传感器进行补 全得到前景目标在笛卡尔坐标系中的三维坐标表示。

在交互工作空间内机械臂末端执行器与工作人员本身存在直接或间接地接 触，在交互场景中以相对速度作为评价指标判断是否发生碰撞。障碍物在以p 为球心，r为半径的球体内的点集构成障碍物距离机械臂最近的区域Z。世界坐 标以机器人基座中心点为基准点，机械臂与控制系统直接连接，系统可以实时 获取机械臂各关节的运动信息、机械臂的三维空间信息及机械臂骨架线。利用 Kinect2.0实时获取的图像可以对场景中进行三维重建。遍历场景的点云坐标与 机械臂坐标可以获得障碍物与机械臂的最近点p。图9展示了两种障碍及机械臂 末端执行器最近点的两种情况：(1)由p点执行器骨架线e最近点作垂线；(2) 及执行器末端与点p的直接连线。在点p及执行器骨架线e构成的平面A内， 对机械臂骨架线及障碍物的相对距离做微分可以计算点p速度v，其相对骨架线 e的垂直分量为v’。

图9所示为v’为障碍物与机械臂执行器末端地相对速度大小。点p与机械 臂的相对速度v做为评价指标，我们将障碍物与机械臂关系及分为以下几类：

其中，T_v根究经验在不同的应用场景设置不同的数值。

根据上述表格判断机械臂与障碍物可能发生碰撞时，为了不继续减小障碍 物与执行器间的最小距离，我们控制机械臂使机械臂末端执行器e在平面A内 的运动速度为v’。系统运行时实时更新点p位置信息，从而实现机械臂末端远 离障碍物避免碰撞。

Claims (4)

1.一种基于视觉和惯性定位的人机主动安全及协作系统，其特征在于，包括：

至少1个机械手，用于抓握以及加工目标工件；

至少1个机械臂，机械臂的底部为基座，机械臂末端安装所述机械手；

至少4个工业摄像机，用于获取操作人员工作区域的手臂及机械手的彩色图像；

至少4个摄像机，用于对空间机械臂机械手的位置进行立体定位，采集摄像机信息，包括机械臂工作环境的彩色图像信息、深度图像信息；

至少1组体感穿戴设备，穿戴于操作人员手臂位置，用于获取体感穿戴设备信息，包括操作人员手臂的手肘，手腕，手指关节的移动方向的位移、速度、加速度信息；

至少1台计算机，用于获取体感穿戴设备中的惯导传感器的信息并在操作人员工作区域的小视场内，计算机通过获取体感穿戴设备信息、摄像机信息以及工业摄像机的彩色图像，判断操作人员是否与机械臂及机械手发生碰撞；

所述计算机用于：

摄像机处获得的彩色图像信息与深度图像信息用于计算场景中无人区域移动目标的空间位置信息、移动速度及方向信息，得到场景中移动目标与机械臂及机械手的相对位置、相对速度信息，并对移动目标是否与机械臂及机械手是否发生碰撞进行预测和判断；

处理惯导传感器获得的手臂关节及手掌的位置信息、速度信息、加速度信息；

从操作人员工作区域的工业摄像机获取的彩色图像得到参与工作的手指关节位置信息及机械手的位置信息。

2.根据权利要求1所述的一种基于视觉和惯性定位的人机主动安全及协作系统，其特征在于，所述体感穿戴设备包括：

安装在手臂关节位置及手掌的惯导传感器；

安装在手指关节及手指末端的主动光源。

3.一种基于视觉和惯性定位的人机主动安全及协作方法，其特征在于，包括以下步骤：

(1)将机械臂基座的中心点作为三维空间坐标系的原点建立世界坐标系，将机械手的末端中心点作为原点建立末端坐标系；

(2)采用摄像机发射红外激光，采集彩色图像信息和深度图像信息，以摄像头作为原点建立相机坐标系，对大视场中摄像机进行标定，建立摄像机间的坐标变换；

(3)采用惯导传感器，采集操作人员手臂关节及手掌的位置信息；

(4)采用主动光源及工业摄像机，采集操作人员手指关节及手指末端的位置及图像信息；

(5)采用计算机基于大视场中摄像机及工业摄像机获取的彩色图像、彩色图像信息及深度图像信息，获取场景图像每个像素点的图像坐标及相机坐标，通过光流估计方法检测移动目标在图像中的位置信息，采用图像坐标到世界坐标的映射以及相机坐标系到世界坐标系的坐标变换，得到大视场内移动目标的世界坐标中的位置信息；结合计算机获取的机械臂的位置信息，获得移动目标与机械臂及机械手的相对位置和相对速度，判断移动目标是否与机械臂发生碰撞，并指定机械臂及机械手的主动避让行为；

(6)采用所述计算机基于小视场内获取的图像信息、小视场内场景及操作人员肢体的三维坐标信息、速度、加速度信息，采用积分运算得到操作人员的手臂位置信息，通过主动光源获得的手指位姿信息，判断操作人员在工作区域内是否与机械臂及机械手发生碰撞，并指定机械臂及机械手的主动避让行为；步骤(5)中所述判断移动目标是否与机械臂发生碰撞，包括：

移动目标与机械臂的最近点p、移动目标与机械臂的相对位置d、相对速度v作为是否发生碰撞的评价指标，具体内容如下：

当d＜T_d，v＜T_v时，判定可能发生碰撞；

当d＜T_d，v＞T_v时，判定可能发生碰撞；

当d＞T_d，v＜T_v时，无干扰；

当d＞T_d，v＞T_v时，判定可能发生碰撞；

其中，T_d、T_v为在不同的应用时设置不同的数值；

步骤(6)中所述判断操作人员在工作区域内是否与机械臂及机械手发生碰撞，包括：

移动目标与机械臂的相对速度v作为是否发生碰撞的评价指标，具体内容如下：

当v≥T_v时，判定可能发生碰撞；

当v＜T_v时，无干扰；

其中，T_v为在不同的应用时设置不同的数值。

4.根据权利要求3所述的一种基于视觉和惯性定位的人机主动安全及协作方法，其特征在于，所述大视场为机械臂最大工作空间周围的设置的包括摄像机和工业摄像机的成像设备能够采集到的最大范围；所述小视场为完成具体工作时机械臂末端与操作人员发生直接接触或间接接触的场景中成像设备能采集到的最大范围。

Images