CN110802578B - 一种可穿戴机器人上肢肢体的最大安全作业范围检测方法 - Google Patents

Abstract

本发明涉及一种可穿戴机器人上肢肢体的最大安全作业范围检测方法，属于机器人技术领域。根据机械臂实体模型得出机械臂D‑H模型与D‑H参数表，得出机械双臂的各个关节所有可能出现的位置与机械双臂作业范围；根据使用者佩戴可穿戴机器人上肢肢体的方式，求解危险范围；建立世界坐标系，求解出机械臂的最大安全作业范围再经对称处理得出机械双臂的最大安全作业范围。有益效果是：能检测可穿戴机器人上肢肢体佩戴在人体不同位置时机械臂末端静止状态下其余机械臂关节的关节角等位置状态、然后检测出可穿戴机器人上肢肢体的最大安全作业范围，进而对可穿戴机器人上肢肢体的整体结构进行评估。

Description

一种可穿戴机器人上肢肢体的最大安全作业范围检测方法

技术领域

本发明涉及机器人技术领域，特别是涉及一种可穿戴机器人上肢肢体的最大安全作业范围检测方法。

背景技术

20世纪以来，我国的工业化程度不断提高，机器人产业不断发展，机器与人类的生活联系越发紧密，对机器人的研究也逐渐深入。其中机械臂是工业自动化领域中一种高效可靠的自动化装置，也是人体工程学和机器人领域的热点研究方向之一,这些年来伴随着工业自动化领域的技术革新，机械臂逐渐发展起来,并得到了较快的发展。因其独特的操作灵活性,已在工业装配、安全防爆等领域得到广泛应用。但其发展仍然存在一定的局限性，当下的很多新兴行业的产品种类繁多、体积较小、对操作人员的灵活度和柔韧性有很高要求，而当下仅凭工业机器人操作机械臂是很难做到这一点。人机协作型机器人正是为改变这一格局而来。其中可穿戴机器人上肢肢体正是人机协作式机器人的一个重要的研究方向。可穿戴机器人上肢肢体是一类可穿戴于人体且拥有两个对称的安装于人身体两侧如腰部、背部、肩部等的机械臂的新兴人机协作式机器人。

一般情况下，在一个人无法独立完成某项工作时,往往需要他人提供帮助或者增加相关的作业工具来实现相关任务。虽然在通常情况下这些方法都可以顺利完成预定的活动指标，但是不可避免的会造成用工成本增加，时间，精力耗费等。另一方面，由于人们需求越来越多样化，面对的任务和环境也会趋向于复杂，个人技能和传统装备难免会遇到不同类型的挑战，而依托人类智力与机器设备动力的高度融合，强调人的智力与肢体直接参与机器人的控制，可使机器人帮助人解决部分难题，从而减轻人的应对压力。在解决经济性和可用性的同时，可穿戴机器人肢体也应充分考虑安全性和稳定性，解决在当今社会上人们普遍存在的对机器人与人保持近距离时可能造成的潜在危险，让可穿戴机器人上肢肢体不仅仅适用于工厂生产，更是能走进人们的生活，解决一些特殊人群从事特殊工作者，在日常生活中可能遇到的一些问题。

但是目前国内外关于可穿戴机器人上肢肢体的研发正处于起步与发展上升期，作为人机协作式机器人的一种，安全性能是衡量一款可穿戴机器人上肢肢体质量优劣的重要参数之一，可是并没有可以用来检测可穿戴机器人上肢肢体安全性能的方法。与由人远程操控或按预输入程序进行动作的机械臂相比较，工作时可穿戴机器人肢体须考虑机械臂操作不当时对人体造成伤害的影响，须设置与人体的安全距离。目前我国已有一些关于单个机械臂运动空间计算的方法，如西北核技术研究所发表的六自由度机械臂运动学及工作空间分析，该论文就描述了计算出六自由度机械臂末端位置的方法。但该论文并没有引入安全范围这一观念，无法直接用于可穿戴机器人这一类人机协作式机器人来求解最大安全作业范围。华南理工大学设计了一款肩部可穿戴功能辅助机械臂，是目前国内少有的与可穿戴机器人上肢肢体的相关的设计，其主要讲述了如何通过脚部来实现对机械臂运动的控制来解放人们的双手，及该机构的主要构成。但是缺少对其所研发装置的最大安全作业范围的计算方法，机构的组成中缺少相应的安全装置，无法对其穿戴者的安全提供保障，与其研究目的，为残障人士提供帮助相悖，是其最终实现产品化的一大阻碍。

因此，如何检测可穿戴机器人上肢肢体的最大安全作业范围具有重要的理论研究价值。在本发明中，提出了一种对可穿戴上肢机器人最大安全作业范围实施有效的检测方法。

发明内容

本发明提供一种可穿戴机器人上肢肢体的最大安全作业范围检测方法，以解决目前没有对可穿戴机器人上肢肢体最大安全作业范围进行检测的问题。

本发明采取的技术方案是，包括下列步骤：

步骤1：根据机械臂实体模型得出机械臂D-H模型与D-H参数表，并根据D-H参数表与正向运动学规律写出各连杆坐标系之间的齐次变换矩阵，并取合适跨度精度，求解机械臂的各个关节所有可能出现位置与机械臂作业范围，经对称处理，得出机械双臂的各个关节所有可能出现的位置与机械双臂作业范围；

步骤2：根据使用者佩戴可穿戴机器人上肢肢体的方式，求解危险范围；

步骤3：建立世界坐标系，求解机械臂各个坐标系原点在世界坐标系中的笛卡尔坐标，剔除机械臂作业范围内的危险末端端点，进而求解出机械臂的最大安全作业范围再经对称处理得出机械双臂的最大安全作业范围，即可穿戴机器人上肢肢体的最大安全作业范围。

本发明所述步骤1包括：

S1、根据机械臂实体模型建立机械臂D-H模型，根据机器人上肢肢体的关节i，关节扭角θ_i，连杆间距d_i，连杆长a_i，连杆扭角β_i确定D-H参数表；

S2、根据D-H参数通过正向运动学规律写出各连杆坐标系之间的齐次变换矩阵,并解出机械臂各个坐标系相对于机械臂基座坐标系的变换关系与机械臂的末端位置；

从连杆坐标系i-1到连杆坐标系i之间的坐标变换可以通过变换矩阵表示；

代入公式：

进行齐次变换，可得如下矩阵：

其中，关节扭角θ_i为绕Z_i-1轴，从X_i-1轴转到X_i轴的角度，连杆间距d_i为沿Z_i-1轴，从X_i-1轴到X_i轴的距离，连杆长a_i为沿X_i轴，从Z_i-1轴移动到Z_i轴的距离，连杆扭角β_i为绕X_i轴，从Z_i-1轴转到Z_i轴的角度；

机械臂各个坐标系相对于机械臂基座坐标系变换关系为：

其中

为机械臂各个坐标系原点在基座坐标系的笛卡尔坐标，即机械臂各个关节在基座坐标系的笛卡尔坐标；

S3、取合适的精度跨度，求解机械臂的各个关节相对于基座坐标系所有可能出现的位置与机械臂作业范围，经对称处理，可以得出机械双臂的各个关节的所有可能出现的位置与机械双臂作业范围。

本发明所述步骤2包括：

根据使用者佩戴可穿戴机器人上肢肢体的部位，求解危险范围，设总反应时间t为：

t＝t₁+t₂

其中，t₁为从传感器接收信号或者使用者发觉危险到处理器发出制动命令所需的最大时间；t2为机器执行制动命令最长响应时间；

求解出基本安全距离s：

s＝v×t

再将基本安全距离s附加10％的余量得出最小安全距离公式，求得最小安全距离S：

S＝s×(1+10％)

其中，v为机械臂末端在工作时的最大速度，其单位为毫米每秒；

以可穿戴机器人肢体两只机械臂基座的连线的中心为圆心半径为S的圆形即为佩戴者的危险范围。

本发明所述步骤3包括：

以两只机械臂的基座的连线的中心为原点，建立世界坐标系，将机械臂基坐标系向世界坐标系转化并得出变换关系与机械臂各个关节在世界坐标系中的笛卡尔坐标；

以两只机械臂基座的连线的中心为原点，建立世界坐标系W，基座坐标系与坐标系W之间的变换关系为：

其中，θ₀为绕Z_W轴，从X_W轴转到X₀轴的角度，d₀为沿Z_W轴，从X_W轴到X₀轴的距离，a₀为沿X₀轴，从Z_W轴移动到Z₀轴的距离，β₀为绕X₀轴，从Z_W轴转到Z₀轴的角度；

从而机械臂各个坐标系相对于世界坐标系W的变换关系为：

其中

为机械臂各个关节在世界坐标系中的笛卡尔坐标；

忽略连杆的宽度，近似认为：只要每组数据中各个关节的位置均在危险范围外，则认为机械臂整体都在危险范围外，机械臂此时为安全状态，称此时机械臂末端端点为安全末端端点；反之，若存在关节的位置在危险范围内，则认为机械臂此时为不安全状态，称此时机械臂末端端点为危险末端端点，根据此原则，剔除机械臂作业范围内的危险末端端点，根据此原则，从机械双臂作业范围M内剔除由机械双臂的危险末端端点组成的危险作业范围N，即得到机械臂的最大安全作业范围；

对坐标向量

取模，与步骤二中安全距离S比较，若有：

则认为机械臂为安全状态，反之则机械臂为不安全状态，并从机械臂工作范围内剔除该坐标向量对应的机械臂的末端端点，对每组坐标位置均进行如此判断，得出机械臂处在安全状态的所有末端端点坐标，即最大安全作业范围，再经对称处理得出可穿戴机器人上肢肢体的最大安全作业范围。

本发明的有益效果是：能检测可穿戴机器人上肢肢体佩戴在人体不同位置时机械臂末端静止状态下其余机械臂关节的关节角等位置状态、然后检测出可穿戴机器人上肢肢体的最大安全作业范围，进而对可穿戴机器人上肢肢体的整体结构进行评估。

附图说明

图1是本发明的流程图；

图2是机器人连杆D-H模型图；

图3(a)是可穿戴机器人上肢肢体的轴测图，机械臂置于使用者腰侧；

图3(b)是可穿戴机器人上肢肢体的主视图，机械臂置于使用者腰侧；

图3(c)是可穿戴机器人上肢肢体的轴测图，机械臂置于使用者肩部；

图3(d)是可穿戴机器人上肢肢体的主视图，机械臂置于使用者肩部；

图4是安全作业范围标示图，其中X为危险范围，M为机械双臂作业范围，N为危险作业范围；

图5是机械臂的三维模型。

具体实施方式

为了能够更清楚地理解本发明实施例的上述目的、特征和优点,下面结合附图和具体实施方式对本发明进行详细描述。

在下面的描述中阐述了很多具体细节以便于充分理解本发明实施例,所描述的实施方式是本发明一部分实施方式,而不是全部的实施方式。

除非另有定义,本文所使用的所有的技术和科学术语与属于本发明实施例的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的,不是旨在于限制本发明实施例。

对空间机械臂的安全性能的影响因素众多，本发明中通过可穿戴机器人上肢肢体的最大安全作业范围检测方法对可穿戴机器人上肢肢体的安全性能进行评价与检测。其中可穿戴机器人上肢肢体所装备的机械双臂的最大安全作业范围即为可穿戴机器人上肢肢体的最大安全作业范围。

本实施例如图3所示，其包括背包主板1、5自由度关节型机械双臂2。

5自由度关节型机械双臂2固定在背包主板1上面，以方便使用者进行穿戴。且所述5自由度关节型机械双臂2的左、右臂关于两臂基座连线中垂面具有对称性。

图1是本发明实施方式的机械双臂的最大安全作业范围检测方法的流程图。

由于对称性，对于机械双臂的左、右臂，只需计算出左臂或者右臂的最大安全作业范围即可求出机械双臂的最大安全作业范围，在不影响结果的前提下，下文所述机械臂即为机械臂右臂。

包括下列步骤：

步骤1，根据机械臂实体模型得出机械臂D-H模型与D-H参数表，并根据D-H参数表与正向运动学规律写出各连杆坐标系之间的齐次变换矩阵，并取合适跨度精度，求解机械臂的各个关节所有可能出现的位置与机械臂作业范围；经对称处理，得出机械双臂的各个关节的所有可能出现的位置与机械双臂作业范围；

其中步骤一可以详细分为以下三步：

S1，根据机械臂实体模型建立机械臂D-H模型，根据机器人上肢肢体的关节i，关节扭角θ_i，连杆间距d_i，连杆长a_i，连杆扭角β_i确定D-H参数表；

如图5所示，所述5自由度关节型机械臂2由机械臂基座201、机械臂基座旋转舵机202、机械臂主臂203、机械臂大臂弯曲舵机204、机械臂大臂205、机械臂小臂弯曲舵机206、机械臂小臂扭转舵机207、机械臂小臂208、机械爪模块209组成；其中机械臂基座201和机械臂大臂205各有一个自由度，分别完成旋转运动与弯曲运动；机械臂小臂208有两个自由度，完成弯曲与扭转运动，其中第五个自由度为机械爪完成张合运动的自由度，所有自由度均通过舵机驱动的齿轮传动机构实现；

图2为5自由度关节型机械臂2的D-H模型，其中由X₀、Y₀、Z₀构成的连杆坐标系为基座坐标系，结合图2进行说明；

首先根据实体建立机械臂的三维模型，再根据机械臂的构型特点建立对应的D-H模型，采用D-H参数法建立各个连杆的连杆坐标系；完成连杆坐标系的建立后,根据连杆间的坐标系关系,得到机械臂的D-H参数表如下，其中i的取值为1至n，在该实施例中n值取4：

S2,根据D-H参数通过正向运动学规律写出各连杆坐标系之间的齐次变换矩阵,并解出机械臂各个坐标系相对于机械臂基座坐标系的变换关系与机械臂的位置；

从连杆坐标系i-1到连杆坐标系i之间的坐标变换可以通过变换矩阵表示；

代入公式：

进行齐次变换，可得如下矩阵：

将D-H参数表中的参数带入上述矩阵，得出各坐标系之间变换关系为：

其中，关节扭角θ_i为绕Z_i-1轴，从X_i-1轴转到X_i轴的角度，连杆间距d_i为沿Z_i-1轴，从X_i-1轴到X_i轴的距离，连杆长a_i为沿X_i轴，从Z_i-1轴移动到Z_i轴的距离，连杆扭角β_i为绕X_i轴，从Z_i-1轴转到Z_i轴的角度；

机械臂各个坐标系相对于机械臂基座坐标系变换关系为：

其中

为机械臂各个坐标系原点在基座坐标系的笛卡尔坐标，即机械臂各个关节在基座坐标系的笛卡尔坐标；

据实施例得：

其中可得出

即为机械臂末端端点的在基座坐标系的笛卡尔坐标；

S3,取合适的精度跨度，求解机械臂相对于基座坐标系所有可能出现的位置与机械臂作业范围。当θ₁、θ₂、θ₃、θ₄的取值被确定下来便能确定矩阵

进而解出T₁、T₂、T₃、T₄的值，对θ₁、θ₂、θ₃、θ₄取合适精度跨度，如0.1°至1°，从初始位置开始，对它们所有可能出现的值进行遍历组合，一组θ₁、θ₂、θ₃、θ₄便能求解一组T₁、T₂、T₃、T₄，从而解得在一定精度下T₁、T₂、T₃、T₄的所有可能取值并求出相应各个坐标系原点在基座坐标系的笛卡尔坐标，即得到机械臂的各个关节所有可能出现的位置；其中由机械末端端点

组成的集合构成机械臂的作业范围，经对称处理，可得机械双臂的各个关节所有可能出现位置以及机械双臂的作业范围如图四中的圆形M区域所示；

步骤2，根据使用者佩戴可穿戴机器人上肢肢体的部位，求解危险范围；

在实施时，本模型计算得出的安全距离可以在机械臂进入危险区工作区时，为使用者提供足够的保护，这类危险区的风险会由任何一种机械危险产生，如挤压、剪切、切割或割断、缠绕、吸入或陷入、摩擦或磨损、刺穿或扎穿以及撞击等。

本实施例的上肢机器人肢体采用背负式附着在人体上，采用双手控制的方式对整个机构进行调控，根据以上限制条件，对机械臂末端在工作时的最大速度进行测量，得出最大速度v值，通过对实施例的控制装置进行反复测量,得出从传感器接收信号或者使用者发觉危险到处理器发出制动命令所需的最大时间t1；机器执行制动命令最长响应时间t2，进而代入公式：

t＝t₁+t₂

确定总反应时间t的大小；

s＝v×t

求解出基本安全距离s；

再将基本安全距离s附加10％的余量得出最小安全距离公式，求得最小安全距离S：

S＝s×(1+10％)

以可穿戴机器人肢体两只机械臂基座的连线的中心为圆心半径为S的圆形即为佩戴者的危险范围，如图4中X区域所示；

步骤三：建立世界坐标系，求解机械臂各个关节在世界坐标系中的笛卡尔坐标，剔除机械臂的危险位置，再经对称处理进而求解机械双臂的最大安全作业范围。

以两只机械臂基座的连线的中心为原点，建立世界坐标系W，基座坐标系与坐标系W之间的变换关系为：

其中，θ₀为绕Z_W轴，从X_W轴转到X₀轴的角度，d₀为沿Z_W轴，从X_W轴到X₀轴的距离，a₀为沿X₀轴，从Z_W轴移动到Z₀轴的距离，β₀为绕X₀轴，从Z_W轴转到Z₀轴的角度。

从而机械臂各个坐标系相对于世界坐标系W的变换关系为：

其中

为机械臂各个关节在世界坐标系中的笛卡尔坐标，据实施例得：

其中可以得到机械臂各个关节在世界坐标系中的笛卡尔坐标：

忽略连杆的宽度，近似认为：只要每组数据中各个关节的位置均在危险范围外，则认为机械臂整体都在危险范围外，机械臂此时为安全状态，称此时机械臂末端端点为安全末端端点；反之，若存在关节的位置在危险范围内，则认为机械臂此时为不安全状态，称此时机械臂末端端点为危险末端端点。根据此原则，从机械双臂作业范围M内剔除由机械双臂的危险末端端点组成的危险作业范围N，即得到整体机械臂的最大安全作业范围；

对坐标向量

取模，与步骤二中安全距离S比较，若有：

则认为机械臂为安全状态，反之则机械臂为不安全状态，并从机械臂工作范围内剔除该坐标向量对应的机械臂的末端端点。对每组坐标位置均进行如此判断，得出机械臂处在安全状态的所有末端端点坐标，即最大安全作业范围。经对称处理，机械双臂的最大安全作业范围如图4中从M里去除危险作业范围N后的范围所示。

Claims (2)

1.一种可穿戴机器人上肢肢体的最大安全作业范围检测方法，其特征在于，包括下列步骤：

步骤1：根据机械臂实体模型得出机械臂D-H模型与D-H参数表，并根据D-H参数表与正向运动学规律写出各连杆坐标系之间的齐次变换矩阵，并取合适跨度精度，求解机械臂的各个关节所有可能出现位置与机械臂作业范围，经对称处理，得出机械双臂的各个关节所有可能出现的位置与机械双臂作业范围；

步骤2：根据使用者佩戴可穿戴机器人上肢肢体的部位，求解危险范围；

设总反应时间t为：

t＝t₁+t₂

其中，t₁为从传感器接收信号或者使用者发觉危险到处理器发出制动命令所需的最大时间；t2为机器执行制动命令最长响应时间；

求解出基本安全距离s：

s＝v×t

再将基本安全距离s附加10％的余量得出最小安全距离公式，求得最小安全距离S：

S＝s×(1+10％)

其中，v为机械臂末端在工作时的最大速度，其单位为毫米每秒；

以可穿戴机器人肢体两只机械臂基座的连线的中心为圆心半径为S的圆形即为佩戴者的危险范围；

步骤3：建立世界坐标系，求解机械臂各个坐标系原点在世界坐标系中的笛卡尔坐标，剔除机械臂作业范围内的危险末端端点，进而求解出机械臂的最大安全作业范围再经对称处理得出机械双臂的最大安全作业范围，即可穿戴机器人上肢肢体的最大安全作业范围，包括：

以两只机械臂的基座的连线的中心为原点，建立世界坐标系，将机械臂基坐标系向世界坐标系转化并得出变换关系与机械臂各个关节在世界坐标系中的笛卡尔坐标；

以两只机械臂基座的连线的中心为原点，建立世界坐标系W，基座坐标系与坐标系W之间的变换关系为：

其中，θ₀为绕Z_W轴，从X_W轴转到X₀轴的角度，d₀为沿Z_W轴，从X_W轴到X₀轴的距离，a₀为沿X₀轴，从Z_W轴移动到Z₀轴的距离，β₀为绕X₀轴，从Z_W轴转到Z₀轴的角度；

从而机械臂各个坐标系相对于世界坐标系W的变换关系为：

其中

为机械臂各个关节在世界坐标系中的笛卡尔坐标；

忽略连杆的宽度，近似认为：只要每组数据中各个关节的位置均在危险范围外，则认为机械臂整体都在危险范围外，机械臂此时为安全状态，称此时机械臂末端端点为安全末端端点；反之，若存在关节的位置在危险范围内，则认为机械臂此时为不安全状态，称此时机械臂末端端点为危险末端端点，根据此原则，剔除机械臂作业范围内的危险末端端点，根据此原则，从机械双臂作业范围M内剔除由机械双臂的危险末端端点组成的危险作业范围N，即得到机械臂的最大安全作业范围；

对坐标向量

取模，与步骤二中安全距离S比较，若有：

则认为机械臂为安全状态，反之则机械臂为不安全状态，并从机械臂工作范围内剔除该坐标向量对应的机械臂的末端端点，对每组坐标位置均进行如此判断，得出机械臂处在安全状态的所有末端端点坐标，即最大安全作业范围，再经对称处理得出可穿戴机器人上肢肢体的最大安全作业范围。

2.根据权利要求1所述的可穿戴机器人上肢肢体的最大安全作业范围检测方法，其特征在于，所述步骤1包括：

S1、根据机械臂实体模型建立机械臂D-H模型，根据机器人上肢肢体的关节i，关节扭角θ_i，连杆间距d_i，连杆长a_i，连杆扭角β_i确定D-H参数表；

S2、根据D-H参数通过正向运动学规律写出各连杆坐标系之间的齐次变换矩阵,并解出机械臂各个坐标系相对于机械臂基座坐标系的变换关系与机械臂的末端位置；

从连杆坐标系i-1到连杆坐标系i之间的坐标变换可以通过变换矩阵表示；

代入公式：

进行齐次变换，可得如下矩阵：

其中，关节扭角θ_i为绕Z_i-1轴，从X_i-1轴转到X_i轴的角度，连杆间距d_i为沿Z_i-1轴，从X_i-1轴到X_i轴的距离，连杆长a_i为沿X_i轴，从Z_i-1轴移动到Z_i轴的距离，连杆扭角β_i为绕X_i轴，从Z_i-1轴转到Z_i轴的角度；

机械臂各个坐标系相对于机械臂基座坐标系变换关系为：

其中

为机械臂各个坐标系原点在基座坐标系的笛卡尔坐标，即机械臂各个关节在基座坐标系的笛卡尔坐标；

S3、取合适的精度跨度，求解机械臂的各个关节相对于基座坐标系所有可能出现的位置与机械臂作业范围，经对称处理，可以得出机械双臂的各个关节的所有可能出现的位置与机械双臂作业范围。

Images