CN111702787B - 人机协作控制系统及控制方法 - Google Patents

Abstract

本发明提供了一种人机协作控制系统及控制方法，所述人机协作控制系统包括是机械手臂模块、集成控制柜及移动小车模块，其中：所述移动小车模块承载所述机械手臂模块及所述集成控制柜并移动；所述集成控制柜用于采集周围环境，并对采集的周围环境信息进行传输及数据处理，以形成控制指令，并根据所述控制指令控制所述移动小车模块的运行，以及所述机械手臂模块的动作。

Description

人机协作控制系统及控制方法

技术领域

本发明涉及机器人技术领域，特别涉及一种人机协作控制系统及控制方法。

背景技术

动作捕捉可以精确快速地捕捉到人体在空间的坐标和姿态，通过对刚体的计算，在电脑上实时复现三维动态的运动图，然后将运动轨迹数据化，通过机器人运动学计算出电机的转动角度，从而控制机械手臂运动。

在科技飞速发展的今天，动作捕捉技术已经十分成熟，其主要被应用在动漫和游戏制作中较多，在工业上的应用反而比较少。现如今，常用动作捕捉技术按照原理可分为机械式，惯性式，光学式等。机械式动作捕捉依靠机械装置和传感器来跟踪和测量运动轨迹，其成本低，精度高，但是由于设备往往对穿戴者限制很大，不适合复杂的活动；惯性式动作捕捉通过捕捉姿态信号，对运动进行解算，从而捕捉人体动作，其优点是便携，操作简单，基本不受限制，缺点是测量精度低，很难得到绝对的空间坐标。

机器人远程控制技术在航空航天、深海探索、危险环境作业等方面具有不可替代的作用。基于互联网的飞速发展，对远程的即时通讯和响应提供了条件，让人们可以坐在办公室里工作。工业上关于机械手臂的操作主要是通过计算机编程和手动控制器实现的。计算机编程主要应用于日常流水线的操作，对意外情况的处理能力较弱；而手动控制器虽然能让工程师在意外发生时进行即时的机械手臂动作控制，但对于工业上常用的六自由度手臂而言，手动控制仅能实现简单动作，对于复杂环境和复杂动作下的机械动作很难进行精确控制，例如转动和移动同时发生时，仅仅靠人眼和经验很难判断出手臂运动的最终位置。

发明内容

本发明的目的在于提供一种人机协作控制系统及控制方法，以解决现有的远程人机协作系统精度低可靠性低的问题。

为解决上述技术问题，本发明提供一种人机协作控制系统，所述人机协作控制系统包括是机械手臂模块、集成控制柜及移动小车模块，其中：

所述移动小车模块承载所述机械手臂模块及所述集成控制柜并移动；

所述集成控制柜用于采集周围环境，并对采集的周围环境信息进行传输及数据处理，以形成控制指令，并根据所述控制指令控制所述移动小车模块的运行，以及所述机械手臂模块的动作；

在所述的人机协作控制系统中，所述集成控制柜包含智能感知模块、机器视觉模块、机械手臂控制器、动作捕捉模块及远程传输控制模块，其中：

所述智能感知模块用于感知障碍物，并将所述障碍物信息发送至所述远程传输控制模块；

所述机器视觉模块用于对周围环境进行拍摄，把周围环境画面传输给所述远程传输控制模块；

所述动作捕捉模块安装有摄像采集周围环境的相机，机械手臂控制器和远程传输控制模块用于数据处理、控制所述机械手臂模块，以及远程传输控制模块用于数据的传输和指令的接收；

在所述的人机协作控制系统中，所述动作捕捉模块包括动作捕捉相机及标记点，所述标记点通过对人体手臂的标记进行关节和刚体的建模，所述动作捕捉相机采集人体手臂运动姿态和数据，传输给所述机械手臂控制器；

所述机械手臂控制器对人体手臂姿态和数据进行数据处理，形成控制指令，并将所述控制指令传输给所述远程传输控制模块，再由所述远程传输控制模块发给所述机械手臂模块，以实现最终的人机协作。

可选的，在所述的人机协作控制系统中，所述智能感知模块包括激光传感器和声波传感器，所述激光传感器通过激光反射侦测周围的障碍物，所述声波传感器通过声波反射侦测周围障碍物；

所述远程传输控制模块根据所述障碍物信息建立三维周围环境图，将所述三维周围环境图发给所述移动小车模块和所述机械手臂控制器，并遇到障碍物时向所述远程传输控制模块发出危险信号，以保证所述移动小车模块和所述机械手臂模块工作时的安全和避障。

可选的，在所述的人机协作控制系统中，所述机器视觉模块包括摄像机和深度学习相机，其中：

所述深度学习相机基于深度学习算法对周围物体进行识别，识别后调用已有数据库模型，修正所述三维周围环境图，在所述三维周围环境图中操控所述机械手臂模块进行工作；

所述摄像机对周围环境进行拍摄，并将周围环境画面传输给所述远程传输控制模块，所述远程传输控制模块根据所述周围环境画面进行实时监控。

可选的，在所述的人机协作控制系统中，所述动作捕捉模块对人体手臂空间运动轨迹进行捕捉并微分，求取人体手臂空间运动轨迹的线速度，利用运动学公式将单位时间内的人体手臂空间运动轨迹的线速度转化为所述机械手臂模块的电机转动角速度，以实现通过人眼反馈控制机械手臂模块的启动和停止。

本发明还提供一种基于上述人机协作控制系统的人机协作控制方法，所述人机协作控制方法包括：

控制所述移动小车模块移动到目的地；

在所述移动小车模块移动期间，所述智能感知模块自动感知周围环境，为所述移动小车模块的避障和移动轨迹规划提供数据；

所述移动小车模块移动到目的地后，所述智能感知模块先对周围环境进行仔细扫描并建立三维周围环境图，所述机器视觉模块通过深度学习相机和人眼识别，对周围物体进行定义，调动已有数据库模型，修正三维周围环境图；

工程师在所述动作捕捉模块下，穿戴带有所述标记点的装备进行运动轨迹记录，通过对空间坐标内的运动轨迹进行微分，得到单位时间内的空间速度向量，通过机器人逆运动学求解出机械手臂模块中的各个电机的转动角速度，作为各个电机的启停控制条件，从而控制机械手臂模块进行即时运动；

在所述机械手臂模块运动过程中，通过三维周围环境图和人眼反馈对所述机械手臂模块进行控制，所述机械手臂模块靠近障碍物时，自动停止并重新根据目标点规划避障轨迹；人眼反馈控制通过动作捕捉相机对周围环境进行拍摄，通过人眼观察和对人手的运动速度的控制，以达到对电机转动角速度控制。

可选的，在所述的人机协作控制方法中，所述机械手臂模块包括前三轴与后三轴，其中，所述前三轴能够空间移动，所述后三轴能够实现抓取或装配物体时的姿态，所述深度学习相机安装于所述前三轴的末端，用于侦测所述人体手臂空间运动轨迹的线速度、所述后三轴各个轴之间的距离、转动空间方向，以及所述后三轴中相邻轴的空间错位角度。

可选的，在所述的人机协作控制方法中，所述人机协作控制方法还包括：

构建动作捕捉系统坐标系，所述动作捕捉系统坐标系与所述前三轴的首端坐标系相同；

第i坐标系的参数包括：

Z_i轴：为移动或转动方向；

X_i轴：为同时垂直于相邻两个移动轴或转动轴的直线方向，X_i的指向为前段指向后段；

Y_i轴：方向与X_i轴及Z_i轴垂直，依据右手定则选择Y_i的指向；

α_i-1:以X_i-1方向看，Z_i-1和Z_i的夹角；

a_i-1：沿着X_i-1的方向，Z_i-1和Z_i的距离；

θ_i：以Z_i方向看，X_i-1和X_i方向的夹角；

d_i：沿着Z_i的方向，X_i-1和X_i的距离；

计算第i坐标系与第i-1坐标系的转换矩阵T为：

其中，c_i＝cosθ_i，s_i＝sinθ_i，sα_i-1＝sinα_i-1，cα_i-1＝cosα_i-1；

所述前三轴为移动轴，所述后三轴为转动轴，计算各个轴的电机角速度；

通过动作捕捉系统坐标系，确定所述机械手臂模块运动目的坐标P及运动轨迹，对运动轨迹进行微分，得到一个空间速度V，设为V(V1，V2，V3)，P设为是第四坐标系的原点；

利用机器人运动学的逆运动，求解每个电机的转动角度速度wi。

可选的，在所述的人机协作控制方法中，求解每个电机的转动角度速度wi包括：

其中，

为机械手臂模块运动目的坐标P在第三坐标系的位置；

由

可知，

设

则f₁＝a₃c₃+d₄sα₃s₃+a₂；

f₂＝a₃cα₂s₃-d₄sα₃cα₂c₃-d₄sα₃cα₂-d₃sα₂；

f₃＝a₃sα₂s₃-d₄sα₃sα₂c₃+d₄cα₃cα₂+d₃cα₂；

设

则g₁＝c₂f₁-s₂f₂+a₁；

g₂＝s₂cα₁f₁+c₂cα₁f₂-sα₁f₃-d₂sα₁；

g₃＝s₂sα₁f₁+c₂sα₁f₂+cα₁f₃+d₂cα₁；

其中：Δt为采样时间，设置所述采样时间1s；

另外，z＝g₃＝(k₁s₂-k₂c₂)sα₁+k₄；

其中，k₁(w₃)＝f₁；

k₂(w₃)＝-f₂；

k₄(w₃)＝cα₁f₃+d₂cα₁；

由于：

得到w₃；再解出w₂；w₁；

通过机器运动学公式

利用欧拉角求解出w₄，w₅，w₆的值。

在本发明提供的人机协作控制系统及控制方法中，通过移动小车模块承载机械手臂模块及集成控制柜并移动；集成控制柜用于采集周围环境，并对采集的周围环境信息进行传输及数据处理，以形成控制指令，并根据控制指令控制移动小车模块的运行，以及机械手臂模块的动作，实现了机械手臂模块提供智能化识别能力，集成控制柜可以实现机械手臂的控制，周围环境的采集，以及数据的传输和指令的接收。移动小车模块实现了移动，障碍物感知，及避障等功能。

附图说明

图1是本发明一实施例人机协作控制系统示意图；

图2是本发明另一实施例人机协作控制方法示意图；

图中所示：1-机械手臂模块；2-集成控制柜；3-移动小车模块。

具体实施方式

以下结合附图和具体实施例对本发明提出的人机协作控制系统及控制方法作进一步详细说明。根据下面说明和权利要求书，本发明的优点和特征将更清楚。需说明的是，附图均采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施例的目的。

本发明的核心思想在于提供一种人机协作控制系统及控制方法，以解决现有的远程人机协作系统精度低可靠性低的问题。

为实现上述思想，本发明提供了一种人机协作控制系统及控制方法，所述人机协作控制系统包括是机械手臂模块、集成控制柜及移动小车模块，其中：所述移动小车模块承载所述机械手臂模块及所述集成控制柜并移动；所述集成控制柜用于采集周围环境，并对采集的周围环境信息进行传输及数据处理，以形成控制指令，并根据所述控制指令控制所述移动小车模块的运行，以及所述机械手臂模块的动作。

<实施例一>

本实施例提供了一种人机协作控制系统，如图1所示，所述人机协作控制系统包括移动小车模块3、智能感知模块、机器视觉模块、机械手臂模块1、远程传输控制模块、动作捕捉模块和机械手臂控制器。移动小车模块3负责搭载智能感知模块，机器视觉模块，机械手臂模块1，远程传输控制模块，并将智能感知模块，机器视觉模块，机械手臂模块1，远程传输控制模块移动到目的地。

如图1所示，1是机械手臂模块，2是集成控制柜，3是移动小车模块。机械手臂模块1前三轴负责移动，后三轴负责抓取或装配物体时的姿态，手臂第三移动轴末端可加装深度学习相机，以提供智能化识别能力。集成控制柜2中包含机械手臂控制器(即主控制器)，负责周围环境摄像采集的相机(即动作捕捉模块)，以及远程传输控制模块，机械手臂控制器(即主控制器)和远程传输控制模块负责机械手臂的控制，以及数据的传输和指令的接收。移动小车模块3包含移动小车模块、智能感知模块及机器视觉模块。智能感知模块包含声波传感器，激光传感器，负责移动，障碍物感知，及避障等。

智能感知模块包括激光传感器和声波传感器，负责侦测周围的障碍物，建立三维的周围环境图以保证移动小车模块和机械手臂模块工作时的安全和避障，遇到障碍物时朝远程传输控制模块发出信号；机器视觉模块包括摄像机和深度学习相机，深度学习相机基于深度学习下对物件的识别，方便智能执行一些简易的操作命令；摄像机对周围环境进行拍摄，把画面传输给远程传输控制模块，方便实时监控。

动作捕捉模块包括动作捕捉相机及标记点，通过对人体手臂的标记，进行关节和刚体的建模，采集运动姿态和数据，传输给主控制器，再由主控制器进行数据处理，传输指令给远程传输控制模块，再由远程传输控制模块发给机械手臂模块，从而实现最终的人机协作。

具体的，现如今的动作捕捉技术大多数都是捕捉运动结束后地目的地坐标点，其工作流程都是在人体运动结束后，把最终的空间坐标点发给机械手臂实施运动。这种方法一方面需要计算机先判断出最终的目的地，排除中间可能存在的无效的停留点，然后才能决定手臂的运动轨迹，其缺点是无法即时与人形成联动；另一方面，由于机械手臂和人所处的环境不一样，机械手臂的应用环境往往较为复杂，通过机械手臂复现人的动作可能撞到周围的物体，存在一定的安全隐患。

为了解决上述的不足，本实施例通过动作捕捉模块对人体手臂运动轨迹进行捕捉并微分，求取线速度，将单位时间内的空间运动轨迹的线速度转化成电机转动的角速度，实现通过人眼的反馈控制机械手臂的动和停，并且达到与人即时联动的目的；其次，在移动小车模块上装有智能感知模块，通过激光传感器和声波传感器，对周围环境进行扫描，形成三维的周围环境图，让机械手臂模块进行轨迹规划时，能自动避开周围的障碍物。

为了达到机械手臂与人手的即时协调运动，并保证操作的安全性，必须实行的操作有：以动作捕捉设备采集人体的运动轨迹并进行微分，求取线速度；通过传感器感知及深度学习相机识别后调用已有数据库模型，建立周围环境三维模型；把空间线速度分解，利用运动学公式转化成移动电机的转动角速度；在三维模型中操控机械手臂进行工作；所需的参数有：人体运动线速度，各个转动轴之间的距离，转动空间方向，相邻转动轴的空间错位角度。

首先控制移动小车模块移动到目的地。在移动小车模块移动期间，智能感知模块自动感知周围的环境，为移动小车模块的避障和移动轨迹规划提供数据。移动到目的地后，智能感知模块先对周围的环境进行仔细扫描并建立三维模型，结合机器视觉模块，通过深度学习相机和人眼识别，对周围物体进行定义，调动三维模型库数据，形成三维环境图。

工程师在动作捕捉模块下，穿戴带有标记点的装备进行运动轨迹记录，通过对空间坐标内的运动轨迹进行微分，得到单位时间内的空间速度向量，通过机器人逆运动学求解出机械手臂模块中的各个电机的转动角速度，作为各个电机的启停控制条件，从而控制机械手臂模块进行即时运动。

在机械手臂模块运动过程中，可通过三维模型和人眼反馈对机械手臂模块进行控制，三维模型控制是在之前建好的三维的环境图中，如果机械手臂模块的运动轨迹靠近障碍物时，自动停止并重新根据目标点规划避障轨迹；人眼反馈控制是通过移动小车模块上装的摄像机对周围环境进行拍摄，通过人眼观察和对人手的运动速度的控制，从而达到对电机转动角速度控制。

考虑到市面上机械手臂的型号和结构各异，本实施例仅对一种六自由度的机械手臂PUMA-560进行方法展示，其余的推算方法类似。

首先构建坐标系，动作捕捉模块坐标系最好与首段机械手臂的坐标系相同，便于运动数据的转换。如图2所示，关于首段机械手臂的坐标系和参数选择如下：Z_i轴:为移动或转动方向；X_i轴:为同时垂直于相邻两个移动或转动轴的直线方向，X_i的指向为前段指向后段；Y_i轴:方向与X_i轴及Z_i轴垂直，依据右手定则选择Y_i的指向；

α_i-1：以X_i-1方向看，Z_i-1和Z_i的夹角；

a_i-1：沿着X_i-1的方向，Z_i-1和Z_i的距离；

θ_i：以Zi方向看，X_i-1和X_i方向的夹角；

d_i：沿着Z_i的方向，X_i-1和X_i的距离；

由机器人运动学可知，不同坐标系的转换矩阵T为：

其中，c_i＝cosθ_i，s_i＝sinθ_i，sα_i-1＝sinα_i-1，cα_i-1＝cosα_i-1。

由于手臂的前三轴产生移动；后三轴为转动，负责抓取的姿态，所以计算前三轴的电机角速度。通过动作捕捉，确定手臂的运动目的坐标P及运动轨迹，对运动轨迹进行微分，可得到一个空间速度V，设为V(V1，V2，V3)，同时P也设为是第四坐标系的原点。可利用机器人运动学的逆运动，求解出机械臂的每个电机的转动角度速度wi。具体求解方法如下：

其中，

为点P在第三坐标系的位置。

由

可知，

设

则f₁＝a₃c₃+d₄sα₃s₃+a₂；

f₂＝a₃cα₂s₃-d₄sα₃cα₂c₃-d₄sα₃cα₂-d₃sα₂；

f₃＝a₃sα₂s₃-d₄sα₃sα₂c₃+d₄cα₃cα₂+d₃cα₂；

同理，设

则g₁＝c₂f₁-s₂f₂+a₁；

g₂＝s₂cα₁f₁+c₂cα₁f₂-sα₁f₃-d₂sα₁；

g₃＝s₂sα₁f₁+c₂sα₁f₂+cα₁f₃+d₂cα₁；

可得，

采样时间根据系统的精度和要求决定，为了方便下面的计算，取采样时间1s。

另外，z＝g₃＝(k₁s₂-k₂c₂)sα₁+k₄；

其中，k₁(w₃)＝f₁；

k₂(w₃)＝-f₂；

k₄(w₃)＝cα₁f₃+d₂cα₁；

利用

可得到w₃；然后通过之前的式子再解出w₂；w₁。

最后，可通过机器运动学公式

利用欧拉角求解出w₄，w₅，w₆的值。

为了达到机械手臂与人手的即时协调运动，并保证操作的安全性，必须实行的操作有：以动作捕捉设备采集人体的运动轨迹并进行微分，求取线速度；通过传感器感知及深度学习相机识别后调用已有数据库模型，建立周围环境三维模型；把空间线速度分解，利用运动学公式转化成移动电机的转动角速度；在三维模型中操控机械手臂进行工作；所需的参数有：人体运动线速度，各个转动轴之间的距离，转动空间方向，相邻转动轴的空间错位角度。所需装置：移动小车模块，智能感知模块，机器视觉模块，机械手臂模块，远程传输控制模块和动作捕捉模块。

首先控制移动小车移动到目的地。在小车移动期间，智能感知模块会自动感知周围的环境，为小车的避障和移动轨迹规划提供数据。移动到目的地后，智能感知模块会先对周围的环境进行仔细扫描并建立三维模型，结合机器视觉模块，通过深度相机和人眼识别，对周围物体进行定义，调动三维模型库数据，形成三维环境图。工程师在动作捕作系统下，穿戴标记装备进行运动轨迹记录，通过对空间坐标内的运动轨迹进行微分，得到单位时间内的空间速度向量，通过机器人逆运动学求解出各个电机的转动角速度，作为电机的启停控制条件，控制机械手臂进行即时运动。在运动过程中，可通过三维模型和人眼反馈对手臂进行控制，三维模型控制是在之前建好的三维的环境图中，如果手臂运动轨迹靠近障碍物时，会自动停止并重新根据目标点规划避障轨迹；人眼反馈控制是通过移动小车上装的摄像机对周围环境进行拍摄，通过人眼观察和对人手的运动速度的控制，从而达到对电机转动角速度控制。

综上，上述实施例对所述人机协作控制系统及控制方法的不同构型进行了详细说明，当然，本发明包括但不局限于上述实施中所列举的构型，任何在上述实施例提供的构型基础上进行变换的内容，均属于本发明所保护的范围。本领域技术人员可以根据上述实施例的内容举一反三。

上述描述仅是对本发明较佳实施例的描述，并非对本发明范围的任何限定，本发明领域的普通技术人员根据上述揭示内容做的任何变更、修饰，均属于权利要求书的保护范围。

Claims (8)

1.一种人机协作控制系统，其特征在于，所述人机协作控制系统包括机械手臂模块、集成控制柜及移动小车模块，其中：

所述移动小车模块承载所述机械手臂模块及所述集成控制柜并移动；

所述集成控制柜用于采集周围环境，并对采集的周围环境信息进行传输及数据处理，以形成控制指令，并根据所述控制指令控制所述移动小车模块的运行，以及所述机械手臂模块的动作；

所述集成控制柜包含智能感知模块、机器视觉模块、机械手臂控制器、动作捕捉模块及远程传输控制模块，其中：

所述智能感知模块用于感知障碍物，并将所述障碍物信息发送至所述远程传输控制模块；

所述机器视觉模块用于对周围环境进行拍摄，把周围环境画面传输给所述远程传输控制模块；

所述动作捕捉模块安装有摄像采集周围环境的相机，机械手臂控制器和远程传输控制模块用于数据处理、控制所述机械手臂模块，以及远程传输控制模块用于数据的传输和指令的接收；

所述动作捕捉模块包括动作捕捉相机及标记点，所述标记点通过对人体手臂的标记进行关节和刚体的建模，所述动作捕捉相机采集人体手臂运动姿态和数据，传输给所述机械手臂控制器；

所述机械手臂控制器对人体手臂姿态和数据进行数据处理，形成控制指令，并将所述控制指令传输给所述远程传输控制模块，再由所述远程传输控制模块发给所述机械手臂模块，以实现最终的人机协作。

2.如权利要求1所述的人机协作控制系统，其特征在于，所述智能感知模块包括激光传感器和声波传感器，所述激光传感器通过激光反射侦测周围的障碍物，所述声波传感器通过声波反射侦测周围障碍物；

所述远程传输控制模块根据所述障碍物信息建立三维周围环境图，将所述三维周围环境图发给所述移动小车模块和所述机械手臂控制器，并遇到障碍物时向所述远程传输控制模块发出危险信号，以保证所述移动小车模块和所述机械手臂模块工作时的安全和避障。

3.如权利要求2所述的人机协作控制系统，其特征在于，所述机器视觉模块包括摄像机和深度学习相机，其中：

所述深度学习相机基于深度学习算法对周围物体进行识别，识别后调用已有数据库模型，修正所述三维周围环境图，在所述三维周围环境图中操控所述机械手臂模块进行工作；

所述摄像机对周围环境进行拍摄，并将周围环境画面传输给所述远程传输控制模块，所述远程传输控制模块根据所述周围环境画面进行实时监控。

4.如权利要求1所述的人机协作控制系统，其特征在于，所述动作捕捉模块对人体手臂空间运动轨迹进行捕捉并微分，求取人体手臂空间运动轨迹的线速度，利用运动学公式将单位时间内的人体手臂空间运动轨迹的线速度转化为所述机械手臂模块的电机转动角速度，以实现通过人眼反馈控制机械手臂模块的启动和停止。

5.一种基于权利要求1所述的人机协作控制系统的人机协作控制方法，其特征在于，所述人机协作控制方法包括：

控制所述移动小车模块移动到目的地；

在所述移动小车模块移动期间，所述智能感知模块自动感知周围环境，为所述移动小车模块的避障和移动轨迹规划提供数据；

所述移动小车模块移动到目的地后，所述智能感知模块先对周围环境进行仔细扫描并建立三维周围环境图，所述机器视觉模块通过深度学习相机和人眼识别，对周围物体进行定义，调动已有数据库模型，修正三维周围环境图；

工程师在所述动作捕捉模块下，穿戴带有所述标记点的装备进行运动轨迹记录，通过对空间坐标内的运动轨迹进行微分，得到单位时间内的空间速度向量，通过机器人逆运动学求解出机械手臂模块中的各个电机的转动角速度，作为各个电机的启停控制条件，从而控制机械手臂模块进行即时运动；

在所述机械手臂模块运动过程中，通过三维周围环境图和人眼反馈对所述机械手臂模块进行控制，所述机械手臂模块靠近障碍物时，自动停止并重新根据目标点规划避障轨迹；人眼反馈控制通过动作捕捉相机对周围环境进行拍摄，通过人眼观察和对人手的运动速度的控制，以达到对电机转动角速度控制。

6.如权利要求5所述的人机协作控制方法，其特征在于，所述机械手臂模块包括前三轴与后三轴，其中，所述前三轴能够空间移动，所述后三轴能够实现抓取或装配物体，所述深度学习相机安装于所述前三轴的末端，用于侦测所述人体手臂空间运动轨迹的线速度、所述后三轴各个轴之间的距离、转动空间方向，以及所述后三轴中相邻轴的空间错位角度。

7.如权利要求6所述的人机协作控制方法，其特征在于，所述人机协作控制方法还包括：

构建动作捕捉系统坐标系，所述动作捕捉系统坐标系与所述前三轴的首端坐标系相同；

第i坐标系的参数包括：

Z_i轴：为移动或转动方向；

X_i轴：为同时垂直于相邻两个移动轴或转动轴的直线方向，X_i的指向为前段指向后段；

Y_i轴：方向与X_i轴及Z_i轴垂直，依据右手定则选择Y_i的指向；

α_i-1：以X_i-1方向看，Z_i-1和Z_i的夹角；

a_i-1：沿着X_i-1的方向，Z_i-1和Z_i的距离；

θ_i：以Z_i方向看，X_i-1和X_i方向的夹角；

d_i：沿着Z_i的方向，X_i-1和X_i的距离；

计算第i坐标系与第i-1坐标系的转换矩阵T为：

其中，c_i＝cosθ_i，s_i＝sinθ_i，sα_i-1＝sinα_i-1，cα_i-1＝cosα_i-1；

所述前三轴为移动轴，所述后三轴为转动轴，计算各个轴的电机角速度；

通过动作捕捉系统坐标系，确定所述机械手臂模块运动目的坐标P及运动轨迹，对运动轨迹进行微分，得到一个空间速度V，设为V(V1，V2，V3)，P设为是第四坐标系的原点；

利用机器人运动学的逆运动，求解每个电机的转动角度速度wi。

8.如权利要求7所述的人机协作控制方法，其特征在于，求解每个电机的转动角度速度wi包括：

其中，

为机械手臂模块运动目的坐标P在第三坐标系的位置；

由

可知，

设

则f₁＝a₃c₃+d₄sα₃s₃+a₂；

f₂＝a₃cα₂s₃-d₄sα₃cα₂c₃-d₄sα₃cα₂-d₃sα₂；

f₃＝a₃sα₂s₃-d₄sα₃sα₂c₃+d₄cα₃cα₂+d₃cα₂；

设

则g₁＝c₂f₁-s₂f₂+a₁；

g₂＝s₂cα₁f₁+c₂cα₁f₂-sα₁f₃-d₂sα₁；

g₃＝s₂sα₁f₁+c₂sα₁f₂+cα₁f₃+d₂cα₁；

其中：Δt为采样时间，设置所述采样时间1s；

另外，z＝g₃＝(k₁s₂-k₂c₂)sα₁+k₄；

其中，k₁(w₃)＝f₁；

k₂(w₃)＝-f₂；

k₄(w₃)＝cα₁f₃+d₂cα₁：

由于：

得到w₃；再解出w₂；w₁；

通过机器运动学公式

利用欧拉角求解出w₄，w₅，w₆的值；上述k₁、k₂、k₃、k₄是k₁(w₃)、k₂(w₃)、k₃(w₃)、k₄(w₃)的简写。

Images