[发明专利]一种带电作业机器人机械臂协作力反馈主从控制方法及系统

权利要求书

1.一种带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，操作手、机械臂和工控机组成带电作业机器人力反馈主从控制系统；机械臂末端安装有六自由度力/力矩传感器；

主操作手力反馈控制时，工控机根据机械臂末端的受力数据，计算出主操作手各个关节的扭矩数据，通过主操作手的控制接口，控制主操作手各关节输出扭矩；

所述械臂末端受力数据的标量为六自由度力/力矩数据，包括两部分：一部分来自于机械臂与环境接触时产生的受力数据，此部分数据由机械臂末端的六自由度力/力矩传感器采集获得；另一部分来自于机械臂之间相互碰撞而产生的受力数据。

2.如权利要求1所述带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，对主操作手静力学建模，得到机械臂末端六自由度力/力矩数据到主操作手各个关节扭矩数据的转换关系，工控机根据所述转换关系计算出主操作手各个关节的扭矩数据。

3.如权利要求1所述带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，来自于机械臂之间相互碰撞而产生的受力数据通过以下方法获得：

步骤1，获取第一机械臂与第二机械臂的末端位姿，运用第二机械臂的基坐标系到第一机械臂的基坐标系的转换矩阵，将第二机械臂的末端位姿坐标转换到第一机械臂的基坐标系中；

步骤2，在第一机械臂的基坐标系中，以第一机械臂的末端位姿坐标为球心，创建包围球，包围球的半径为碰撞检测半径；

步骤3，在第一机械臂的基坐标系中，检测上述包围球与第二机械臂的末端位姿坐标的碰撞关系，若没有发生碰撞，则第一机械臂末端受力数据为零，若发生碰撞，则根据第一机械臂与第二机械臂的末端位姿计算碰撞深度矢量，然后进入步骤4；

步骤4，采用弹簧模型计算第一机械臂末端受力数据，

F＝-kx

其中，k为弹簧系数，x为碰撞深度矢量，F为受力数据，F为六维矢量，其标量为六自由度力/力矩数据；

步骤5，将步骤4得到的第一机械臂末端的六自由度力/力矩数据取反获得第二机械臂末端的六自由度力/力矩数据。

4.如权利要求1所述带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，机械臂位姿控制时，工控机根据主操作手各旋转关节的角度数据，计算出机械臂末端速度矢量的期望值，通过机械臂的控制接口，按照所述期望值控制机械臂运动；获取机械臂末端速度矢量期望值的方法为：

运用D-H建模方法对主操作手建模，得出主操作手的各个旋转关节的角度数据到主操作手末端位姿数据的转换关系；工控机接收主操作手发送的各个旋转关节的角度数据，根据所述转换关系计算得到主操作手末端位姿数据，然后将主操作手末端位姿数据通过微分运算得到主操作手末端速度数据；根据主操作手末端位姿数据和末端速度数据，使用速度前馈PID控制器，计算得到机械臂末端速度矢量期望值。

5.如权利要求1至4所述的任意一种带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，所述主操作手和机械臂均为一个或者多个自由度机械臂结构。

6.一种带电作业机器人机械臂协作力反馈主从控制系统，其特征在于，包括操作手、机械臂和工控机；机械臂末端安装有六自由度力/力矩传感器；

所述主操作手和机械臂均为一个或者多个自由度机械臂结构；所述主操作手能够采集每个旋转关节的角度数据，每个旋转关节具备力矩电机，能够输出扭矩；所述机械臂末端安装有六自由度力/力矩传感器，用于采集机械臂与环境接触时产生的六个方向的受力数据；

主操作手力反馈控制时，工控机根据机械臂末端的受力数据，计算出主操作手各个关节的扭矩数据，通过主操作手的控制接口，控制主操作手各关节输出扭矩；

所述械臂末端受力数据的标量为六自由度力/力矩数据，包括两部分：一部分来自于机械臂与环境接触时产生的受力数据，此部分数据由机械臂末端的六自由度力/力矩传感器采集获得；另一部分来自于机械臂之间相互碰撞而产生的受力数据。