[发明专利]一种基于虚拟现实技术的带电作业机器人机械臂协作力反馈主从控制方法及系统

权利要求书

1.一种基于虚拟现实技术的带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，操作手、机械臂和工控机组成带电作业机器人力反馈主从控制系统；主操作手力反馈控制时，工控机根据机械臂末端的受力数据，计算出主操作手各个关节的扭矩数据，通过主操作手的控制接口，控制主操作手各关节输出扭矩；

所述械臂末端受力数据的标量为六自由度力/力矩数据，包括两部分：一部分来自于机械臂与环境接触时产生的受力数据，另一部分来自于机械臂之间相互碰撞而产生的受力数据；前述两部分受力数据均采用基于虚拟现实的技术获得，其中，获得机械臂之间相互碰撞而产生的受力数据的过程为：

步骤1，获取第二机械臂六个关节的角度数据，根据第二机械臂外型模型数据，生成第二机械臂的虚拟现实数据；

步骤2，运用第二机械臂的基坐标系到第一机械臂的基坐标系的转换矩阵，将第二机械臂的虚拟现实数据转换到第一机械臂的基坐标系中；

步骤3，运用八分法对第二机械臂的虚拟现实数据进行逐步划分，构建八叉树；

步骤4，获取第一机械臂末端位姿数据，以该末端位姿数据为球心，创建包围球，该包围球的半径为所需碰撞检测的距离；

步骤5，对步骤4的包围球与步骤3的八叉树进行碰撞检测，若没有发生碰撞，则机械臂之间的受力数据为零，若发生碰撞，则获得虚拟现实场景中发生碰撞的数据点，根据发生碰撞的数据点坐标与第一机械臂末端位姿数据，计算碰撞深度矢量，然后进入步骤6；

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

F＝-kx

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

步骤7，将步骤6获得的六自由度力/力矩数据符号取反获得第二机械臂末端的六自由度力/力矩数据。

2.如权利要求1所述基于虚拟现实技术的带电作业机器人机械臂协作力反馈主从控制方法，其特征在于，获得机械臂与环境接触时产生的受力数据的过程为：

步骤1，运用八分法对机械臂作业场景对应的虚拟现实场景进行逐步划分，构建八叉树；

步骤2，获取机械臂末端位姿数据，以该末端位姿数据为球心，创建包围球，设置包围球的半径为所需碰撞检测的距离；

步骤3，对步骤2的包围球与步骤1的八叉树进行碰撞检测，若没有发生碰撞，则机械臂末端的六自由度力/力矩数据为零，若发生碰撞，则获取虚拟现实场景中发生碰撞的数据点，根据发生碰撞的数据点坐标与机械臂末端位姿数据，计算碰撞深度矢量；

步骤4，根据碰撞深度矢量计算机械臂末端六自由度力/力矩数据，即采用弹簧阻尼模型计算反馈力，

F＝-kx-cv

其中，k为弹簧系数，x为碰撞深度矢量，c为阻尼系数，v为机械臂末端速度矢量，F为反馈力，其标量为六自由度力/力矩数据。

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

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

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

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