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

权利要求书

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