CN111633665A - 抓取电磁换能器协助无损检测的装置及其轨迹规划方法 - Google Patents

Abstract

本发明公开了一种抓取电磁换能器协助无损检测的装置及其轨迹规划方法，该装置包括两个机械臂和双目相机，两个机械臂分别设在检测台周边的机械臂支撑底座上；双目相机设在检测台周边的支撑架上；机械臂包括第一转轴、支撑柱、第一机械臂、固定装置、第一舵机、第二机械臂、第二舵机、第二转轴、机械爪、减速电机；支撑柱的底部通过第一转轴与支撑底座连接，第一机械臂固定在支撑柱的顶部，第一舵机的一端通过固定装置与第一机械臂连接，第一舵机的另一端与第二机械臂的一端连接，第二机械臂的另一端与第二舵机的一端连接，第二舵机的另一端通过第二转轴与机械爪连接；减速电机设在机械爪上，与机械爪的齿轮连接。

Description

抓取电磁换能器协助无损检测的装置及其轨迹规划方法

技术领域

本发明涉及无损检测技术领域，具体是一种抓取电磁换能器协助无损检测的装置及其轨迹规划方法。

背景技术

铝合金板材在生产加工、运输过程中难免会有裂纹、腐蚀等缺陷，通过检测、分析超声波在铝板中的传播后得到的波形信息，利用算法软件对铝合金板材缺陷进行成像，可直接观察到铝合金板材中的缺陷位置、大小等特征，从而可以合理选择铝合金板材的用途和使用方式。

电磁超声换能器有着非接触式、受环境影响小、可移动等优点，电磁超声换能器正逐步取代传统换能器。利用电磁超声换能器进行超声波无损检测成像检测，需要先在铝合金板材上划分一块几何区域，沿边界设置几十个检测点，一次一个检测点激发，剩余检测点放置接收换能器，依次将激发换能器放到所有的检测点，通过示波器或者其他接收设备将接收换能器接收到的模拟信号转化为数字信号和数据存储起来，根据所有接收到的数据信息得到关于这块区域缺陷的信息。超声波无损检测成像技术所需检测点数太多，且需要将换能器摆放到检测点的准确位置。凭人工手动划分区域、标记检测点位置，尺规工具在使用时会存在一定的误差，并且在检测的过程中，需要不停更换激发换能器和接收换能器的位置，并通过控制示波器软件逐个接收换能器的数据信息，费时费力，而且在某些时候换能器的位置可能摆放不准确，造成接收到的数据有一定的误差，而换能器位置不准确造成的误差和原本尺规划分区域造成的误差累积起来将对成像的效果有着很大的影响。

随着技术的发展、自动化水平的提高，出现许多代替人工的机械臂，可以完成重复性大的工作，大大提高效率，且可以精确地定位到三维空间上的某一点进行作业，大大减小了由于人工摆放产生的误差。而视觉机器人的出现又解决了机器人不能观察环境的缺点，利用相机的视觉可以帮助机器人精确确定目标的位置以及目标位置的变动。因此需要设计一款利用视觉相机和机械臂相结合的机器人，用以完成在超声波检测成像过程中电磁超声换能器的移动。并通过上位机的程序配合机械手摆放完成的信号完成接收换能器接收到的数据信息的存储，实现无损检测数据采集的自动化。

发明内容

本发明的目的在于克服现有技术的不足，而提供一种抓取电磁换能器协助无损检测的装置及其轨迹规划方法。

实现本发明目的的技术方案是：

一种抓取电磁换能器协助无损检测的装置，包括两个机械臂和双目相机，两个机械臂分别设在检测台周边的机械臂支撑底座上，检测台上放置待检测的金属板，两个机械臂配合夹取电磁换能器在待检测金属板上移动检测；双目相机设在检测台周边的支撑架上，用于采集待检测金属板、电磁换能器、机械臂的图像；

所述的机械臂，包括第一转轴、支撑柱、第一机械臂、固定装置、第一舵机、第二机械臂、第二舵机、第二转轴、机械爪、减速电机；支撑柱的底部通过第一转轴与支撑底座连接，第一转轴由安装在支撑底座上的电机控制旋转，可以使机械臂大范围转动，扩大检测范围；第一机械臂固定在支撑柱的顶部，第一舵机的一端通过固定装置与第一机械臂连接，第一舵机的另一端与第二机械臂的一端连接，第二机械臂的另一端与第二舵机的一端连接，第二舵机的另一端通过第二转轴与机械爪连接；减速电机设在机械爪上，与机械爪的齿轮连接。

所述的机械爪，其夹取部设有用于防滑、缓冲的橡胶。

所述的机械臂，由铝合金材质制作而成，防止在移动电磁超声换能器时被磁铁吸住。

所述的双目相机，为ZED双目相机，采集到的图像通过数据线传输至上位机。

一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，包括如下步骤：

1)双目相机将采集到待检测金属板、电磁换能器、机械臂的图像传输至上位机；

2)上位机通过双目视觉算法对图像信息进行处理，得到待检测金属板、电磁换能器、机械臂的几何信息，通过ROS软件将待检测金属板划分成若干个检测区域，以及得到换能器的摆放位置信息；

3)利用步骤2)得到的待检测金属板、电磁换能器、机械臂的几何信息建立障碍物模型；

4)对步骤3)得到的障碍物模型进行碰撞检测，设置机械臂的搜索空间，若机械臂在搜索空间内无碰撞发生，则进行对机械臂进行运动轨迹的规划；

5)建立人工势场，采用量子行为粒子群算法协助机械臂在检测区域逃离势能局部极小点，得到机械臂的运动轨迹；

6)通过模糊神经网络PID算法控制减速电机使机械臂按照运动轨迹将电磁换能器准确放到预定位置上，电磁换能器对待测金属板的某一检测区域完成检测；

7)将机械臂的位姿初始化，重复步骤1)-步骤6)完成剩余待测金属板检测区域的检测。

步骤3)中，所述的障碍物模型，包括障碍物和机械臂，建立方法包括如下步骤：

对于障碍物，假设障碍物模型中的基本形状是三角形，包围盒包围着障碍物模型的所有三角形，设包围盒内设第i个三角形的顶点矢量为p_i、q_i和r_i，则平均值u和协方差矩阵C定义如下：

上述公式(1)和公式(2)中，n为包围盒包围的三角形的数量，u为模型的均值，即包围盒的中心位置，

它们均为R³中的一个向量，例如

C为一个3×3的对称矩阵。协方差矩阵具有三个相互正交的特征向量，将其正规化操作后，作为一个基底，即可得到包围盒的方向轴，将所有顶点投影到方向轴上，计算在轴上的最大和最小值，来确定包围盒的大小；

对于机械臂，用球体表示关节，用圆柱体表示连杆，将机械臂模型进行简化，形成胶囊体结构。

步骤4)中，所述的碰撞检测，包括两个机械臂之间的碰撞检测，以及单个机械臂与障碍物的碰撞检测；

两个机械臂之间的碰撞，包括关节与关节之间、关节与连杆，连杆与连杆；当只考虑双机械臂之间的碰撞而不考虑外部环境时，通过计算双机械臂之间的最短距离来判断机械臂是否发生碰撞；

设在两个由空间点组成的集合F₁和F₂上，分别存在点p₁(p_1x,p_1y,p_1z)和点p₂(p_2x,p_2y,p_2z)，其距离为：

则两个集合之间的最短距离为d(F₁,F₂)＝min{||p₁-p₂||:p₁∈F₁,p₂∈F₂}，胶囊体A和胶囊体B用集合C_A和C_B表示，用集合L_A和集合L_B表示A和B的中间线段，球体的半径为r_A和r_B，设p_a和p_b为A和B上任意一点，集合C_A和C_B可表示为：

C_A＝{p_a：||p_a-x||≤r_A,x∈L_A} (4)

C_B＝{p_b：||p_b-x||≤r_B,x∈L_B} (5)

则A和B的距离与中心线段的距离为：

d(C_A,C_B)＝d(L_A,L_B)-r_A-r_B (6)

从公式(6)得到机械臂连杆之间的碰撞检测转化为胶囊体中心线段距离的计算，若d(C_A,C_B)＞0，则胶囊体没有发生碰撞；遍历完两个机械臂所包含的所有胶囊体后，若都不存在碰撞关系，则可以认为两个机械臂之间无碰撞；

单个机械臂与障碍物的碰撞检测，是采用分离轴定理进行检测，若找到一个平面，令胶囊体完全在平面的一侧，包围盒完全在平面的另一侧，则胶囊体和包围盒不重叠，则该平面为分离面，并且一定垂直于分离轴。

步骤4)中，所述的搜索空间，采用基于配置空间的间接映射路径搜索法，即根据当前关节角临近关节位型，通过正运动学得到对应关节及末端执行器三维坐标，其映射方法为：

p＝f(x),x＝(θ₁,…，θ_n) (7)

其中p表示对应关节角度配置下的连杆的三维坐标，x表示关节角度向量，而一组位型在搜索空间中是否可行则需要满足下式：

其中M(x)为机械臂位置，O_i表示第i个障碍，假设空间中存在m个障碍，则一组位型可行的条件是机械臂于空间中任意障碍的交集为空集。

步骤5)中，所述的人工势场，是将目标点视为引力源对机械臂末端执行器存在大小与两者间距离成正比的引力，障碍被视为斥力源对机械臂产生于两者距离成反比的斥力，机械臂的合势能为两者的代数和，其定义分别如下：

其中E_att表示系统引力值，U_rep表示第i根胶囊体与第r个障碍间的斥力，ξ、η分别为引力、斥力系数；d_ir为第i根胶囊体与第r个障碍间的最短距离；d₀为障碍斥力有效范围，则人工势场的总斥力势能为

人工势场的合势能为E_sum＝E_att+E_rep，设空间各位型组合(θ₁，…，θ_n)通过正运动学获得在该位姿下的关节坐标值，从而进行碰撞检测及引、斥力势能计算，机械臂从邻近网格中选取一个具有最小合势能的位姿进行移动，通过不断迭代最终获得一条避障轨迹。

步骤5)中，所述的量子行为粒子群算法，其优化方法是先利用波函数描述粒子的状态，使粒子处于量子束缚态，当粒子搜索完毕后，再利用蒙特卡洛法求解得到粒子的精确位置，其中处于量子束缚态的粒子是以一定的概率密度出现再空间任何点，在整个可行解空间中进行搜索，假设粒子群规模为M，则粒子的更新公式为：

p_k(t)＝r₁(t)·P_k(t)+[1-r₁(t)]·G(t) (11)

X_k(t+1)＝p_k(t)±α·|C(t)-X_k(t)|·ln[1/r₂(t)] (13)

X_k(t+1)为第k个粒子t+1次迭代的位置，X_k(t)为第k个粒子t次迭代的位置；p_k(t)为第k个粒子t次迭代的吸引子，P_k(t)为第k个粒子当前个体最好位置；G(t)为当前全局最好位置；C(t)为当前平均最好位置；α为收缩-扩张系数；r₁(t)和r₂(t)为介于[0，1]之间的随机数；

由于r₁(t)和r₂(t)为介于[0，1]之间的随机数，对端部关节角度变化量进行限制，限制公式如下：

α|C(t)-X(t)|ln[1/r₂(t)]∈(0,V_max) (13)

其中V_max为设定的最大单步容许变化量。

本发明提供的一种抓取电磁换能器协助无损检测的装置及其轨迹规划方法，该装置结构简单，通过使用机械臂和双目相机相互配合，获得待测电磁换能器的信息，可以有效的协助电磁换能器的无损检测；在轨迹规划时，上位机利用双目相机传输来的信息，通过程序对铝板进行区域划分，从而得到比人工划分更准确的换能器摆放位置，通过电机精度控制和换能器位置准确划分，机械臂可以将换能器更准确的摆放到设定位置，从而减小因人工划分区域和摆放造成的误差影响，通过将双臂作为障碍物之一代入轨迹规划算法，可以有效避免在机械臂移动过程中发生碰撞，可以更高效率、更优地将换能器摆放到指定位置。

附图说明

图1为一种抓取电磁换能器协助无损检测的装置的示意图；

图2为机械臂的结构示意图；

图3为轨迹规划方法的流程图；

图中：1.机械臂支撑底座 2.第一转轴 3.支撑柱 4.第一机械臂 5.固定装置 6.第一舵机 7.第二机械臂 8.第二舵机 9..第二转轴 10.双目相机 11.机械爪 12.减速电机。

具体实施方式

下面结合附图和实施例对本发明内容做进一步阐述，但不是对本发明的限定。

实施例：

如图1所示，一种抓取电磁换能器协助无损检测的装置，包括两个机械臂和双目相机10，两个机械臂分别设在检测台周边的机械臂支撑底座1上，检测台上放置待检测的金属板，两个机械臂配合夹取电磁换能器在待检测金属板上移动检测；双目相机10设在检测台周边的支撑架上，用于采集待检测金属板、电磁换能器、机械臂的图像；

如图2所示，所述的机械臂，包括第一转轴2、支撑柱3、第一机械臂4、固定装置5、第一舵机6、第二机械臂7、第二舵机8、第二转轴9、机械爪11、减速电机12；支撑柱3的底部通过第一转轴2与支撑底座1连接，第一转轴2由安装在支撑底座1上的电机控制旋转，可以使机械臂大范围转动，扩大检测范围；第一机械臂4固定在支撑柱3的顶部，第一舵机6的一端通过固定装置5与第一机械臂4连接，第一舵机6的另一端与第二机械臂7的一端连接，第二机械臂7的另一端与第二舵机9的一端连接，第二舵机8的另一端通过第二转轴9与机械爪11连接；减速机12电设在机械爪11上，与机械爪11的齿轮连接。

所述的机械爪11，其夹取部设有用于防滑、缓冲的橡胶。

所述的机械臂，由铝合金材质制作而成，防止在移动电磁超声换能器时被磁铁吸住。

所述的双目相机10，为ZED双目相机，采集到的图像通过数据线传输至上位机。

一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，如图3所示，包括如下步骤：

1)双目相机将采集到待检测金属板、电磁换能器、机械臂的图像传输至上位机；

2)上位机通过双目视觉算法对图像信息进行处理，得到待检测金属板、电磁换能器、机械臂的几何信息，通过ROS软件将待检测金属板划分成若干个检测区域，以及得到换能器的摆放位置信息；

3)利用步骤2)得到的待检测金属板、电磁换能器、机械臂的几何信息建立障碍物模型；

4)对步骤3)得到的障碍物模型进行碰撞检测，设置机械臂的搜索空间，若机械臂在搜索空间内无碰撞发生，则进行对机械臂进行运动轨迹的规划；

5)建立人工势场，采用量子行为粒子群算法协助机械臂在检测区域逃离势能局部极小点，得到机械臂的运动轨迹；

6)通过模糊神经网络PID算法控制减速电机使机械臂按照运动轨迹将电磁换能器准确放到预定位置上，电磁换能器对待测金属板的某一检测区域完成检测；

7)将机械臂的位姿初始化，重复步骤1)-步骤6)完成剩余待测金属板检测区域的检测。

步骤3)中，所述的障碍物模型，包括障碍物和机械臂，建立方法包括如下步骤：

对于障碍物，假设障碍物模型中的基本形状是三角形，包围盒包围着障碍物模型的所有三角形，设包围盒内第i个三角形的顶点矢量为p_i、q_i和r_i，则平均值u和协方差矩阵C定义如下：

上述公式(1)和公式(2)中，n为包围盒包围的三角形的数量，u为模型的均值(包围盒的中心位置)，

它们均为R³中的一个向量，例如

C为一个3×3的对称矩阵。协方差矩阵具有三个相互正交的特征向量，将其正规化操作后，作为一个基底，即可得到包围盒的方向轴，将所有顶点投影到方向轴上，计算在轴上的最大和最小值，来确定包围盒的大小；

对于机械臂，用球体表示关节，用圆柱体表示连杆，将机械臂模型进行简化，形成胶囊体结构。

步骤4)中，所述的碰撞检测，包括两个机械臂之间的碰撞检测，以及单个机械臂与障碍物的碰撞检测；

两个机械臂之间的碰撞，包括关节与关节之间、关节与连杆，连杆与连杆；当只考虑双机械臂之间的碰撞而不考虑外部环境时，通过计算双机械臂之间的最短距离来判断机械臂是否发生碰撞；

设在两个由空间点组成的集合F₁和F₂上，分别存在点p₁(p_1x,p_1y,p_1z)和点p₂(p_2x,p_2y,p_2z)，其距离为：

则两个集合之间的最短距离为d(F₁,F₂)＝min{||p₁-p₂||:p₁∈F₁,p₂∈F₂}，胶囊体A和胶囊体B用集合C_A和C_B表示，用集合L_A和集合L_B表示A和B的中间线段，球体的半径为r_A和r_B，设p_a和p_b为A和B上任意一点，集合C_A和C_B可表示为：

C_A＝{p_a：||p_a-x||≤r_A,x∈L_A} (4)

C_B＝{p_b：||p_b-x||≤r_B,x∈L_B} (5)

则A和B的距离与中心线段的距离为：

d(C_A,C_B)＝d(L_A,L_B)-r_A-r_B (6)

从公式(6)得到机械臂连杆之间的碰撞检测转化为胶囊体中心线段距离的计算，若d(C_A,C_B)＞0，则胶囊体没有发生碰撞；遍历完两个机械臂所包含的所有胶囊体后，若都不存在碰撞关系，则可以认为两个机械臂之间无碰撞；

单个机械臂与障碍物的碰撞检测，是采用分离轴定理进行检测，若找到一个平面，令胶囊体完全在平面的一侧，包围盒完全在平面的另一侧，则胶囊体和包围盒不重叠，则该平面为分离面，并且一定垂直于分离轴。

步骤4)中，所述的搜索空间，采用基于配置空间的间接映射路径搜索法，即根据当前关节角临近关节位型，通过正运动学得到对应关节及末端执行器三维坐标，其映射方法为：

p＝f(x),x＝(θ₁,...，θ_n) (7)

其中p表示对应关节角度配置下的连杆的三维坐标，x表示关节角度向量，而一组位型在搜索空间中是否可行则需要满足下式：

其中M(x)为机械臂位置，O_i表示第i个障碍，假设空间中存在m个障碍，则一组位型可行的条件是机械臂于空间中任意障碍的交集为空集。

步骤5)中，所述的人工势场，是将目标点视为引力源对机械臂末端执行器存在大小与两者间距离成正比的引力，障碍被视为斥力源对机械臂产生于两者距离成反比的斥力，机械臂的合势能为两者的代数和，其定义分别如下：

其中E_att表示系统引力值，U_rep表示第i根胶囊体与第r个障碍间的斥力，ξ、η分别为引力、斥力系数；d_ir为第i根胶囊体与第r个障碍间的最短距离；d₀为障碍斥力有效范围，则人工势场的总斥力势能为

人工势场的合势能为E_sum＝E_att+E_rep，设空间各位型组合(θ₁，...，θ_n)通过正运动学获得在该位姿下的关节坐标值，从而进行碰撞检测及引、斥力势能计算，机械臂从邻近网格中选取一个具有最小合势能的位姿进行移动，通过不断迭代最终获得一条避障轨迹。

步骤5)中，所述的量子行为粒子群算法，其优化方法是先利用波函数描述粒子的状态，使粒子处于量子束缚态，当粒子搜索完毕后，再利用蒙特卡洛法求解得到粒子的精确位置，其中处于量子束缚态的粒子是以一定的概率密度出现再空间任何点，在整个可行解空间中进行搜索，假设粒子群规模为M，则粒子的更新公式为：

p_k(t)＝r₁(t)·P_k(t)+[1-r₁(t)]·G(t) (11)

X_k(t+1)＝p_k(t)±α·|C(t)-X_k(t)|·ln[1/r₂(t)] (13)

X_k(t+1)为第k个粒子t+1次迭代的位置，X_k(t)为第k个粒子t次迭代的位置；p_k(t)为第k个粒子t次迭代的吸引子，P_k(t)为第k个粒子当前个体最好位置；G(t)为当前全局最好位置；C(t)为当前平均最好位置；α为收缩-扩张系数；r₁(t)和r₂(t)为介于[0，1]之间的随机数；

由于r₁(t)和r₂(t)为介于[0，1]之间的随机数，因此算法的增量可能会很大，以至于超出了机械臂的容许范围，为了使机械臂的运动更协调，必须对端部关节角度变化量进行限制，限制公式如下：

α|C(t)-X(t)|ln[1/r₂(t)]∈(0,V_max) (13)

其中V_max为设定的最大单步容许变化量。

Claims (10)

1.一种抓取电磁换能器协助无损检测的装置，其特征在于，包括两个机械臂和双目相机，两个机械臂分别设在检测台周边的机械臂支撑底座上，检测台上放置待检测的金属板，两个机械臂配合夹取电磁换能器在待检测金属板上移动检测；双目相机设在检测台周边的支撑架上，用于采集待检测金属板、电磁换能器、机械臂的图像；

所述的机械臂，包括第一转轴、支撑柱、第一机械臂、固定装置、第一舵机、第二机械臂、第二舵机、第二转轴、机械爪、减速电机；支撑柱的底部通过第一转轴与支撑底座连接，第一转轴由安装在支撑底座上的电机控制旋转；第一机械臂固定在支撑柱的顶部，第一舵机的一端通过固定装置与第一机械臂连接，第一舵机的另一端与第二机械臂的一端连接，第二机械臂的另一端与第二舵机的一端连接，第二舵机的另一端通过第二转轴与机械爪连接；减速电机设在机械爪上，与机械爪的齿轮连接。

2.根据权利要求1所述的一种抓取电磁换能器协助无损检测的装置，其特征在于，所述的机械爪，其夹取部设有用于防滑、缓冲的橡胶。

3.根据权利要求1所述的一种抓取电磁换能器协助无损检测的装置，其特征在于，所述的机械臂，由铝合金材质制作而成，防止在移动电磁超声换能器时被磁铁吸住。

4.根据权利要求1所述的一种抓取电磁换能器协助无损检测的装置，其特征在于，所述的双目相机，为ZED双目相机，采集到的图像通过数据线传输至上位机。

5.一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，其特征在于，包括如下步骤：

1)双目相机将采集到待检测金属板、电磁换能器、机械臂的图像传输至上位机；

2)上位机通过双目视觉算法对图像信息进行处理，得到待检测金属板、电磁换能器、机械臂的几何信息，通过ROS软件将待检测金属板划分成若干个检测区域，以及得到换能器的摆放位置信息；

3)利用步骤2)得到的待检测金属板、电磁换能器、机械臂的几何信息建立障碍物模型；

4)对步骤3)得到的障碍物模型进行碰撞检测，设置机械臂的搜索空间，若机械臂在搜索空间内无碰撞发生，则进行对机械臂进行运动轨迹的规划；

5)建立人工势场，采用量子行为粒子群算法协助机械臂在检测区域逃离势能局部极小点，得到机械臂的运动轨迹；

6)通过模糊神经网络PID算法控制减速电机使机械臂按照运动轨迹将电磁换能器准确放到预定位置上，电磁换能器对待测金属板的某一检测区域完成检测；

7)将机械臂的位姿初始化，重复步骤1)-步骤6)完成剩余待测金属板检测区域的检测。

6.根据权利要求5所述的一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，其特征在于，步骤3)中，所述的障碍物模型，包括障碍物和机械臂，建立方法包括如下步骤：

对于障碍物，假设障碍物模型中的基本形状是三角形，包围盒包围着障碍物模型的所有三角形，设包围盒内设第i个三角形的顶点矢量为p_i、q_i和r_i，则平均值u和协方差矩阵C定义如下：

上述公式(1)和公式(2)中，n为包围盒包围的三角形的数量，u为模型的均值，即包围盒的中心位置，

它们均为R³中的一个向量，例如

C为一个3×3的对称矩阵。协方差矩阵具有三个相互正交的特征向量，将其正规化操作后，作为一个基底，即可得到包围盒的方向轴，将所有顶点投影到方向轴上，计算在轴上的最大和最小值，来确定包围盒的大小；

对于机械臂，用球体表示关节，用圆柱体表示连杆，将机械臂模型进行简化，形成胶囊体结构。

7.根据权利要求5所述的一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，其特征在于，步骤4)中，所述的碰撞检测，包括两个机械臂之间的碰撞检测，以及单个机械臂与障碍物的碰撞检测；

两个机械臂之间的碰撞，包括关节与关节之间、关节与连杆，连杆与连杆；当只考虑双机械臂之间的碰撞而不考虑外部环境时，通过计算双机械臂之间的最短距离来判断机械臂是否发生碰撞；

设在两个由空间点组成的集合F₁和F₂上，分别存在点p₁(p_1x,p_1y,p_1z)和点p₂(p_2x,p_2y,p_2z)，其距离为：

则两个集合之间的最短距离为d(F₁,F₂)＝min{||p₁-p₂||:p₁∈F₁,p₂∈F₂}，胶囊体A和胶囊体B用集合C_A和C_B表示，用集合L_A和集合L_B表示A和B的中间线段，球体的半径为r_A和r_B，设p_a和p_b为A和B上任意一点，集合C_A和C_B可表示为：

C_A＝{p_a：||p_a-x||≤r_A,x∈L_A} (4)

C_B＝{p_b：||p_b-x||≤r_B,x∈L_B} (5)

则A和B的距离与中心线段的距离为：

d(C_A,C_B)＝d(L_A,L_B)-r_A-r_B (6)

从公式(6)得到机械臂连杆之间的碰撞检测转化为胶囊体中心线段距离的计算，若d(C_A,C_B)＞0，则胶囊体没有发生碰撞；遍历完两个机械臂所包含的所有胶囊体后，若都不存在碰撞关系，则可以认为两个机械臂之间无碰撞；

单个机械臂与障碍物的碰撞检测，是采用分离轴定理进行检测，若找到一个平面，令胶囊体完全在平面的一侧，包围盒完全在平面的另一侧，则胶囊体和包围盒不重叠，则该平面为分离面，并且一定垂直于分离轴。

8.根据权利要求5所述的一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，其特征在于，步骤4)中，所述的搜索空间，采用基于配置空间的间接映射路径搜索法，即根据当前关节角临近关节位型，通过正运动学得到对应关节及末端执行器三维坐标，其映射方法为：

p＝f(x),x＝(θ₁,...，θ_n) (7)

其中p表示对应关节角度配置下的连杆的三维坐标，x表示关节角度向量，而一组位型在搜索空间中是否可行则需要满足下式：

其中M(x)为机械臂位置，O_i表示第i个障碍，假设空间中存在m个障碍，则一组位型可行的条件是机械臂于空间中任意障碍的交集为空集。

9.根据权利要求5所述的一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，其特征在于，步骤5)中，所述的人工势场，是将目标点视为引力源对机械臂末端执行器存在大小与两者间距离成正比的引力，障碍被视为斥力源对机械臂产生于两者距离成反比的斥力，机械臂的合势能为两者的代数和，其定义分别如下：

其中E_att表示系统引力值，U_rep表示第i根胶囊体与第r个障碍间的斥力，ξ、η分别为引力、斥力系数；d_ir为第i根胶囊体与第r个障碍间的最短距离；d₀为障碍斥力有效范围，则人工势场的总斥力势能为

人工势场的合势能为E_sum＝E_att+E_rep，设空间各位型组合(θ₁，...，θ_n)通过正运动学获得在该位姿下的关节坐标值，从而进行碰撞检测及引、斥力势能计算，机械臂从邻近网格中选取一个具有最小合势能的位姿进行移动，通过不断迭代最终获得一条避障轨迹。

10.根据权利要求5所述的一种抓取电磁换能器协助无损检测的装置的轨迹规划方法，其特征在于，步骤5)中，所述的量子行为粒子群算法，其优化方法是先利用波函数描述粒子的状态，使粒子处于量子束缚态，当粒子搜索完毕后，再利用蒙特卡洛法求解得到粒子的精确位置，其中处于量子束缚态的粒子是以一定的概率密度出现再空间任何点，在整个可行解空间中进行搜索，假设粒子群规模为M，则粒子的更新公式为：

p_k(t)＝r₁(t)·P_k(t)+[1-r₁(t)]·G(t) (11)

X_k(t+1)＝p_k(t)±α·|C(t)-X_k(t)|·ln[1/r₂(t)] (13)

X_k(t+1)为第k个粒子t+1次迭代的位置，X_k(t)为第k个粒子t次迭代的位置；p_k(t)为第k个粒子t次迭代的吸引子，P_k(t)为第k个粒子当前个体最好位置；G(t)为当前全局最好位置；C(t)为当前平均最好位置；α为收缩-扩张系数；r₁(t)和r₂(t)为介于[0，1]之间的随机数；

由于r₁(t)和r₂(t)为介于[0，1]之间的随机数，对端部关节角度变化量进行限制，限制公式如下：

α|C(t)-X(t)|ln[1/r₂(t)]∈(0,V_max) (13)

其中V_max为设定的最大单步容许变化量。

Images