[发明专利]一种带电作业机器人的远程遥控控制系统及其控制方法

权利要求书

1.一种带电作业机器人的远程遥控控制系统，其特征在于：包含遥操作器、主控电脑、带电作业机器人、无线通讯模块；

遥操作器与主控电脑连接进行数据传输，发送控制指令，主控电脑与带电作业机器人通过无线通讯方式进行信号传输，主控电脑将接收到的控制指令解析并发送给带电作业机器人，带电作业机器人根据指令完成相应的动作。

2.根据权利要求1所述的一种带电作业机器人的远程遥控控制系统，其特征在于：所述遥操作器包括底座、小型六自由度机械臂、辅助按键；

辅助按键包括左右臂切换按键、机械夹手抓取按键、机械夹手松开按键和作业任务按键；

底座包含电路控制板，与六个可活动关节和所有按键相连接，用于根据各个关节活动角度计算末端相对于底座的空间坐标和旋转姿态；

所述作业任务按键包括电缆剥皮按键、并沟引流线按键、安装绝缘罩按键，用于支持扩展，对应新增工作内容。

3.根据权利要求1所述的一种带电作业机器人的远程遥控控制系统，其特征在于：所述带电作业机器人包括两个六自由度机械臂、环境探测模块、高压带电作业工具、机械夹手；

环境探测装置包含多个无死角拍摄的高清摄像机，高压带电作业工具包括电缆剥皮工具、并沟接引流线工具、安装绝缘罩工具和工具支持扩展。

4.根据权利要求1所述的一种带电作业机器人的远程遥控控制系统，其特征在于：主控电脑中包括带电作业机器人控制动作解析模块、任务存储模块和状态监控模块；

其中，动作解析模块，用于解析从遥操作器接收到的信息；

任务存储模块，用于保存所有遥操作按键指令与相应动作的映射关系；

状态监控模块，用于实时显示机器人环境探测模块所拍摄的画面。

5.一种基于权利要求1至4任一项所述的带电作业机器人的远程遥控控制系统的控制方法，其特征在于：具体包含如下步骤：

步骤1，操作员在主控电脑中设置遥操作移动控制比例系数ratio；

步骤2，操作员控制遥操作器活动关节的末端移动和转动，遥操作器以1000Hz频率向主控电脑发送末端空间坐标和旋转姿态信息，以两个1*3的向量表示：

空间坐标：Pz＝[x，y，z]；

旋转姿态：Rz＝[Rx，Ry，Rz]；

其中，Pz和Rz都是以遥操作器的基座坐标为基准，Rz为旋转向量表达法；

从机械臂以125Hz频率向主控电脑发送机械臂各个关节的位置信息，以1*6向量表示；

Pjoint＝[θ1，θ2，θ3，θ4，θ5，θ6]

θ1，θ2，θ3，θ4，θ5，θ6分别表示机械臂从下至上的六个关节的旋转角度；

步骤3，每隔8ms获取一次遥操作器的末端空间坐标Pzⁱ和旋转姿态Rzⁱ以及从机械臂各个关节位置向量Pjointⁱ；

记前后两次的空间坐标分别为P_z^i-1和Pzⁱ，

moveⁱ＝Pzⁱ-Pz^i-1

moveⁱ即为从i-1到i时刻内的遥操作器末端的空间移动向量；

步骤4，根据从机械臂的物理结构参数，利用机械臂的正运动学算法求解出Pjointⁱ所对应的机械臂末端的空间坐标Pcⁱ和旋转姿态Rcⁱ；

步骤5，根据罗德里格斯公式求得Ozⁱ对应的旋转矩阵Rzⁱ，由于遥操作器与从机械臂的基座坐标基准可能不一致，且遥操作器末端初始工作姿态的旋转姿态与从机械臂末端初始工作姿态的旋转姿态也可能不一致。假设遥操作器末端初始工作姿态的旋转向量为Rz，从机械臂末端初始工作姿态的旋转向量为Rc。

遥操作器末端初始工作姿态与从机械臂末端初始工作姿态的映射关系式为：

R_reflect*Rc*Pc＝Rz*Pc

所以有

R_reflsct＝Rz*Rc^-1

Rc^-1为Rc的逆矩阵；

Rcⁱ⁺¹＝R_rsflsct*Rzⁱ＝Rc*Rz^-1*Rzⁱ

(6).根据从机械臂的物理结构参数，利用机械臂广义逆运动学解析算法，根据空间坐标Pcⁱ⁺¹及旋转矩阵Rcⁱ⁺¹求解出对应六个关节的坐标Pjointⁱ⁺¹，将Pjointⁱ⁺¹作为运动目标以125Hz频率发送给从机械臂控制柜，由控制柜完成各个电机的控制，实现机器人的空间移动和转动；

机械臂广义逆运动学解析算法：

由机械臂关节坐标得到机械臂其次变换矩阵

求解关节角1：

m＝d6*a_y-p_y

n＝d6*a_x-p_x

求解关节角5：

θ5＝±acos(a_x*sin(θ1)-a_y*cos(θ1))

求解关节角6：

mm＝n_x*sin(θ1)-n_y*cos(θ1)

nn＝o_x*sin(θ1)-o_y*cos(θ1)

θ6＝atan2(mm，nn)-atan2(sin(θ5)，0)

求解关节角3：

mmm＝d6*(sin(θ6).*(n_x*cos(θ1)+n_y*sin(θ1))+cos(θ6).*(o_x*cos(θ1)+o_y*sin(θ1)))-d6*(a_x*cos(θ1)+a_y*sin(θ1))+p_x*cos(θ1)+p_y*sin(θ1)

nnn＝p_z-d1-a_z*d6+d5*(o_z*cos(θ6)+n_z*sin(θ6))

求解关节角2：

s2＝((a3*cos(θ3)+a2)).*nnnn-a3*sin(θ3).*mmmm/(a2²+a3²+2a2*a3*cos(θ3))

c2＝(mmmm+a3*sin(θ3).*s2)/(a3*cos(θ3)+a2)

θ2＝atan2(s2，c2)

整理关节角1、关节角5、关节角6、关节角3、关节角2；

求解关节角4：

θ4＝atna2(-sin(θ6).*(n_x*cos(θ1)+n_ysin(θ1))-cos(θ6)).*(o_x*cos(θ1)+o_y*sin(θ1))，o_z*cos(θ6)+n_z*sin(θ6))-θ2-θ3

由此计算出8组解，以前后关节运动量最小为确定唯一解的依据。

步骤7，通过视频监控判断带电作业机器人是否移动到位，通过选择遥操作器上面的作业任务按键选择机器人作业内容，遥操作器将控制指令发送给主控电脑，主控电脑通过查询任务存储模块发送相应的任务动作指令给带电作业机器人。

6.根据权利要求5所述的一种基于带电作业机器人的远程遥控控制系统的控制方法，其特征在于：所述正运动学算法具体如下：

根据机械臂结构参数确定机械臂DH参数表中的坐标系绕x轴旋转角度、坐标系沿x轴移动距离a、坐标系沿z轴移动距离d，坐标系绕z轴旋转角度为未知量：建立相邻关节之间的齐次变换矩阵：

pc＝[p_x p_y p_z]

RM通过罗德里格斯公式可以变换成旋转向量Rc

Pcⁱ⁺¹＝Pc+moveⁱ*ratio。