一种基于虚拟现实带电作业机械臂防碰撞保护方法与流程

本发明属于电力技术领域，具体涉及一种基于虚拟现实带电作业机械臂防碰撞保护方法。

背景技术：

采用机器人自主完成带电作业，不仅可以将工作人员从危险的、繁重的、精神紧张的工作中解放出来，有效避免带电作业时人员伤亡事故的发生，使带电作业更加安全，提高作业效率；同时还可以提高电网的运行质量，进一步减少供电系统的人员投入，降低人员成本，具有巨大的经济效益和社会效益。

带电作业时采用多机械臂共同完成带电作业，如果没有系统、安全、可靠的防碰撞方法，那么多机械臂必然无法协调完成带电作业，甚至引发安全事故，给机器人和操作人员带来危害。

技术实现要素：

本发明提出一种基于虚拟现实带电作业机械臂防碰撞保护方法，能够使得机器人在带电作业时，多机械臂之间协调工作，互相配合、互不干扰，有序高效安全地完成带电作业。

为了解决上述技术问题，本发明提供一种基于虚拟现实带电作业机械臂防碰撞保护方法，根据真实作业场景建立虚拟作业场景，虚拟场景中机械臂模型的尺寸略大于机械臂实际尺寸，在虚拟场景中进行虚拟碰撞检测，以判断机械臂是否会发生碰撞；所述虚拟碰撞检测采用基于包围盒的虚拟碰撞检测方法，具体为：

步骤1，预处理阶段，对参与碰撞检测的机械臂模型进行特征采样，将构成机械臂模型的基本集合元素的包围盒的中心点作为特征采样点，根据两个待检测包围盒的特征对建立二维离散搜索空间；

步骤2，碰撞检测阶段，包括以下步骤：

步骤2-1，采用粒子群算法进行搜索，得到距离小于一定碰撞检测阈值的特征对；

步骤2-2，判断以上距离小于一定碰撞检测阈值的特征对所对应的包围盒是否相交，若包围盒不相交，则进入步骤2-4；若包围盒相交，则继续步骤2-3；

步骤2-3，判断机械臂的基本几何元素是否相交，若基本几何元素不相交，进入步骤2-4；若基本几何元素相交，则做出碰撞响应。

进一步，步骤2-1具体为：

步骤2-1-1，将两个机械臂基本几何元素的包围盒特征作为粒子，初始化粒子群，包括群体规模、每个粒子的位置和速度；

步骤2-1-2，计算每个粒子的适应度值；

步骤2-1-3，对每个粒子，比较其适应度值和个体极值p_best，如果适应度值小于粒子当前的个体极值p_best，则更新个体极值p_best；否则不更新；

步骤2-1-4，对每个粒子，比较其适应度值和全局极值g_best，如果适应度值小于粒子当前的全局极值g_best则更新全局极值g_best；否则不更新；

步骤2-1-5，根据以下公式更新粒子的速度和位置；

其中，k为迭代代次数；c₁,c₂为学习因子；r₁,r₂为均匀分布在[0,1]之间的两个随机数；ω为惯性权重；为粒子i的当前位置；V_i^k+1为粒子i的当前飞行速度；P_i是粒子i经历的最优位置，称为个体最优位置；P_g是到目前时间段为止，群体中所有粒子经历的最优位置，称为全局最优位置；

步骤2-1-6，判断是否满足粒子群算法结束条件，若满足，输出距离小于一定碰撞检测阈值的特征对，若不满足，则返回步骤2-1-2。

本发明与现有技术相比，其显著优点在于：

(1)本发明针对自主带电作业机器人，采用基于虚拟碰撞检测的防碰撞保护方法和基于深度传感器的防碰撞保护方法，多种方法结合，更好地监控机械臂安全、协调地完成带电作业；

(2)将粒子群算法应用到带电作业的虚拟碰撞检测中，可以实时地检测到发生干涉的元素，从而及时避免碰撞事故；

(3)虚拟场景中所建立的机械臂模型外围尺寸比实际机械臂大一些，首先在虚拟场景中检测到碰撞，然后及时对碰撞做出响应，避免真实场景中的机械臂发生碰撞。

(4)带电作业机器人作业时分为人工遥操作方式和机器人自主方式，当机器人自主作业时，采用本发明的机械臂防碰撞保护方法；当人工遥操作时，一方面操作者通过监控画面实时观测机械臂状态，另一方面，如果计算机检测到机械臂即将发生碰撞，立即干预人工操作，停止机械臂的一切动作。

附图说明

图1为本发明中带电作业机器人一种实施例的整体结构示意图；

图2为本发明中机器人平台的结构示意图；

图3为本发明的带电作业虚拟碰撞检测的流程图；

图4为采用粒子群算法的流程图。

具体实施方式

容易理解，依据本发明的技术方案，在不变更本发明的实质精神的情况下，本领域的一般技术人员可以想象出本发明基于虚拟现实带电作业机械臂防碰撞保护方法的多种实施方式。因此，以下具体实施方式和附图仅是对本发明的技术方案的示例性说明，而不应当视为本发明的全部或者视为对本发明技术方案的限制或限定。

结合附图，带电作业机器人包括绝缘斗臂车1、控制室2、伸缩臂3、机器人平台4。其中，绝缘斗臂车1上架设控制室2和伸缩臂3，伸缩臂3末端连接机器人平台4，机器人平台4与控制室2之间采用光纤以太网通信或者无线网络通信。

绝缘斗臂车1可供操作人员驾驶，从而将机器人平台4运输到作业现场。绝缘斗臂车1上装有支撑腿，支撑腿可以展开，从而将绝缘斗臂车1与地面稳固支撑。绝缘斗臂车1上装有发电机，从而给控制室2及伸缩臂3供电。

伸缩臂3设有沿伸缩方向的驱动装置，操作人员可以通过控制驱动装置，从而将机器人平台4升降到作业高度。该伸缩臂3由绝缘材料制成，用于实现机器人平台4与控制室2的绝缘。在本发明中，伸缩臂3可有由剪叉式升降机构或其他机构代替。

控制室2中设置有第二工控机、显示屏、第一主操作手、第二主操作手、辅助主操作手以及通信模块等。

机器人平台4包括绝缘子46、第一机械臂43、第二机械臂44、辅助机械臂42、第一工控机48、双目摄像头45、全景摄像头41、深度摄像头410、蓄电池49、专用工具箱47、通信模块。

机器人平台4的绝缘子46用于支撑第一机械臂43、第二机械臂44、辅助机械臂42，将这三个机械臂的外壳与机器人平台4绝缘。

蓄电池49为第一工控机48、第一机械臂43、第二机械臂44、辅助机械臂42、全景摄像头41、双目摄像头45、深度摄像头410、通信模块供电。

专用工具箱47是放置抓具、扳手等作业工具的场所。机械臂末端安装有工具快换装置。机械臂根据作业任务的类型到专用工具箱47中使用工具快换装置获取作业工具。

控制室2中第一主操作手、第二主操作手以及辅助主操作手是一种用于人工远程操作机械臂的操作装置，他们与第一机械臂43、第二机械臂44和辅助机械臂42构成主从操作关系。机械臂和主操作手具有相同的结构，只是主操作手尺寸规格比机械臂小，以便于操作人员操作。机械臂和主操作手拥有六个关节，每个关节都有光电编码器采集角度数据，各主操作手的微型控制器通过串口将六个关节的角度数据发送给第二工控机。

为了防止上述带电作业机器人各机械臂发生碰撞，本发明提出一种基于虚拟现实带电作业机械臂防碰撞保护方法，包括基于虚拟碰撞检测的防碰撞保护方法和基于深度传感器的防碰撞保护方法。

所述虚拟碰撞检测，即建立机械臂的运动虚拟场景，在虚拟场景中机械臂交互和运动时不可避免会发生碰撞，系统必须实时地检测到这些碰撞，避免场景中机械臂在运动过程中发生穿透等现象。虚拟场景中机械臂外围尺寸比实际机械臂大一些，当虚拟机械臂检测到碰撞时，实际机械臂还未发生碰撞，此时虚拟机械臂进行碰撞响应，一个机械臂向反方向移动，另一个物体(机械臂或带电作业现场其他物体)保持静止，从而有效避免了机械臂发生碰撞。进一步，虚拟碰撞检测分为两个步骤：

步骤1，预处理阶段，对参与碰撞检测的机械臂模型进行特征采样，对构成机械臂模型的基本集合元素的包围盒的中心点作为特征采样点。包括以下步骤：

步骤1-1，输入两个机械臂模型；

步骤1-2，用包围盒将机械臂模型包围住，对机械臂模型进行不断的划分，每一级划分都用一个简单的包围盒包围住，直到将机械臂模型细分成一些诸如三角形的基本几何元素；

步骤1-3，对机械臂基本几何元素的包围盒进行特征采样，如点、边、三角片、四面体等；

步骤1-4，建立二维离散搜索空间，即两个待检测包围盒的特征对构成的搜索空间。

步骤2，详细碰撞检测阶段，包括以下步骤：

步骤2-1，采用粒子群算法进行搜索，得到距离小于一定碰撞检测阈值的特征对；

步骤2-2，判断以上距离小于一定碰撞检测阈值的特征对所对应的包围盒是否相交。当包围盒不相交时，在包围盒里面的基本几何元素必然不会相交。当包围盒发生相交时，基本几何元素才会相交。

若不相交，进入步骤2-4；若包围盒相交，继续步骤2-3；

步骤2-3，判断机械臂的基本几何元素(点、边、三角片、四面体等)是否相交，若不相交，进入步骤2-4；若几何元素相交，做出碰撞相应；

步骤2-4，判断是否终止，终止则退出，不终止则回到步骤2-1。

进一步，步骤2-1中采用的粒子群算法，两个机械臂基本几何元素的包围盒特征对即为粒子，包括以下步骤：

步骤2-1-1，初始化粒子群，包括群体规模、每个粒子的位置和速度；

将待检测的随机碰撞检测特征对(即两个机械臂包围盒的特征，如点、边、三角片、四面体等)分别放在两个集合中，记为集合A和集合B，假设它们分别含有N_a和N_b个元素。判断两个机械臂模型是否发生干涉碰撞，第一步需要计算两个待检测包围盒之间是否至少存在一对特征对P(a_i,b_j)，其中a_i∈A(0＜i＜N_a)，b_j∈B(0＜j＜N_b)，使F(P)≤δ。a_i,b_j分别是集合A和B的基本几何元素的包围盒的中心特征点，它们在三维物体空间中具有坐标属性(x,y,z)。F为距离函数，δ为碰撞阈值。目标空间D是由特征对任意组合形成的二维离散空间，由于特征对具有对偶特性，D的大小为Na*Nb/2。

一个粒子P_k(a_i,b_j)代表两个机械臂包围盒采样特征的一个组合，每个特征在三维物体空间都具有一个位置。优化的目标是找到一个特征间距离最短的组合。每个粒子的速度和位置向量有两部分组成：

V＝{Va,Vb}＝{(v_ax,v_ay,v_az),(v_bx,v_by,v_bz)}

X＝{Xa,Xb}＝{(x_a,y_a,z_a),(x_b,y_b,z_b)}

其中，V是速度，Va、Vb分别是特征a、b的速度，v_ax,v_ay,v_az分别代表特征a在x、y、z轴上的速度，v_bx,v_by,v_bz分别代表特征b在x、y、z轴上的速度；X是位置，Xa、Xb分别是特征a、b的位置，x_a,y_a,z_a分别代表特征a在x、y、z轴上的位置，x_b,y_b,z_b分别代表特征b在x、y、z轴上的位置。

步骤2-1-2，计算每个粒子的适应度值；

适应度函数是两个包围盒中心点之间的欧氏距离，为了减少计算量采用距离的平方：

F(Xa,Xb)＝(x_a-x_b)²+(y_a-y_b)²+(z_a-z_b)²

其中，x_a,x_b,y_a,y_b,z_a,z_b是物体的空间坐标，F(Xa,Xb)是适应度函数。

在每次迭代后粒子的状态由它在两个模型上的速度和位置分量共同决定。粒子P在第k-1次迭代后的状态是P^k-1，经过一次进化后新的状态是P^k。

步骤2-1-3，对每个粒子，用它的适应度值和个体极值p_best比较，如果适应度值小于粒子当前的个体极值p_best，则更新p_best；否则不更新；

步骤2-1-4，对每个粒子，用它的适应度值和全局极值g_best比较，如果适应度值小于粒子当前的全局极值g_best则更新g_best；否则不更新；

步骤2-1-5，根据以下公式更新粒子的速度和位置；

其中，k为迭代代数；c₁,c₂是学习因子，一般取值2；r₁,r₂是均匀分布在[0,1]之间的两个随机数；ω为惯性权重，可以对算法的全局搜索能力和局部搜索能力进行平衡调整；X_i为粒子i的当前位置；V_i为粒子i的当前飞行速度；P_i是粒子i经历的最优位置，称为个体最优位置；P_g是到目前时间段为止，群体中所有粒子经历的最优位置，称为全局最优位置。

步骤2-1-6，满足结束条件(粒子的适应度值小于一定范围或到达最大循环次数)退出，得到距离小于一定碰撞检测阈值的特征对，否则回到步骤2-1-2。

所述基于深度传感器的防碰撞保护方法，利用深度传感器测量得到两个物体之间的位置，获得点云信息，计算两个物体点云数据的最小距离d，当d小于一定阈值时，认为两个物体发生碰撞，此时，一个机械臂向反方向运动，另一个机械臂保持静止，从而有效避免了带电作业时发生意外碰撞事故。

另外，带电作业机器人作业时分为人工遥操作方式和机器人自主方式。当机器人自主作业时，采用上述机械臂防碰撞保护方法；当人工遥操作时，一方面操作者通过监控画面实时观测机械臂状态，另一方面，如果计算机检测到机械臂即将发生碰撞，立即干预人工操作，停止机械臂的一切动作。