技术领域
[0001] 本
发明涉及机械手运动领域，特别涉及一种超声检测中机械手防碰撞方法。
背景技术
[0002] 镁
合金因其比重轻、比强度和比
刚度高、切削加工性好、耐
腐蚀等优越性能，在航空航天及军工领域应用广泛，如运载火箭、导弹壳体等。但在
挤压和
锻造等工艺过程中难免产生夹杂、孔洞等
缺陷，直接影响材料的强度及性能。因此，有效的检测出镁合金内部缺陷对保证其安全可靠性非常重要。
[0003] 超声
无损检测具有检测灵敏度高、检测深度大、缺陷
定位准确等优点，可有效实现镁合金弹壳的缺陷检测，然而新型镁合金弹壳具有规格多、型面复杂、变
曲率、变壁厚、批量大等特点，常用的龙
门式超声自动扫查系统难以满足其高柔性的技术要求。而基于机械手的超声自动检测系统具有柔性高、灵活性强及重复定位
精度高等优点，是满足如上需求的理想途径。但由于弹壳型面的复杂性以及机械手路径自动优化
算法等因素影响，导致关节变量存在不确定性，使机械手与
工件、夹具或
水槽等设备间存在碰撞的隐患，为了防止此类碰撞损伤，研究一种精确、实时的防碰撞策略是保证超声检测系统安全性和有效性的重要前提。
[0004] 目前，机械手的
碰撞检测主要分为基于外部
传感器的
硬件方法和基于
几何模型的
软件方法。Lu等在机械手终端加装
力传感器，通过碰撞力实现碰撞检测，这种方法只适应于如拾取、磨削等已碰撞的检测；陈琳等提出动态
包围体层次树算法，采用2叉树、K-D树、BSP’s等数据结构
加速碰撞检测，该软件方法在光线追踪和实时
渲染领域使用广泛，但难以满足超声检测高实时性的要求；Martínez-Salvador B等提出采用非均匀广义圆柱体表示模型，该方法能够处理任意形状的物体，但检测精度较低，在超声检测中采用较小水声距等情况下容易发生误检。因此，有必要研究一种高效、精确的防碰撞方法。
发明内容
[0005] 为了解决目前机械手运动碰撞检测方法性能不足的技术问题，本发明提供一种高效、精确的超声检测中机械手防碰撞方法。
[0006] 为了实现上述技术目的，本发明的技术方案是，
[0007] 一种超声检测中机械手防碰撞方法，包括以下步骤：
[0008] 步骤一，建立用于超声检测的机械手各个关节的
连杆坐标系，并建立相邻2个连杆坐标系之间的变换矩阵；
[0009] 步骤二，根据相邻2个连杆坐标系之间的变换矩阵与机械手自身的连杆参数和关节变量，建立超声检测机械手的运动学方程；
[0010] 步骤三，将检测系统中的设备分为静止体和碰撞体两类，并确定静止体的符号距离场，在碰撞体运动过程中，本方法采用的离散碰撞检测以时间步长为间隔，对碰撞
体模型与符号距离场进行碰撞检测；碰撞体上的点根据机械手运动学方程求解，将碰撞体上的点变换到符号距离场坐标系下的坐标点，通过搜索符号距离场确定变换后的坐标点与静止体的最短欧氏距离，并与安全
阈值比较，根据比较结果确定是否执行相应的碰撞策略。
[0011] 所述的一种超声检测中机械手防碰撞方法，所述的步骤一中，相邻的两个连杆坐标系{i-1}和{i}之间的变换矩阵 为：
[0012]
[0013] 其中，i＝1,2,...,6，li、αi、di和θi分别为连杆坐标系{i}中的连杆长度、连杆扭
角、
连杆偏距和关节变量角，Rot(Zi-1,θi)表示绕连杆坐标系{i-1}中的Z轴旋转θi角度的旋转矩阵，Rot(Xi,αi)表示绕连杆坐标系{i}中的X轴旋转αi角度的旋转矩阵，Trans(0,0,di)表示沿着X轴运动0距离，沿Y轴运动0距离，沿Z轴运动di距离的平移矩阵，Trans(li,0,0)表示沿着X轴运动li距离，沿Y轴运动0距离，沿Z轴运动0距离的平移矩阵。
[0014] 所述的一种超声检测中机械手防碰撞方法，所述的步骤二中，机械手的运动学方程 为：
[0015]
[0016] 其中，
[0017] 其中θj为连杆坐标系{j}中的关节变量角，j＝0,1,2,...,6。
[0018] 所述的一种超声检测中机械手防碰撞方法，所述的步骤三中，静止体的符号距离场通过下式表达：
[0019]
[0020]
[0021] 其中， p表示三维空间中的点，q表示静止体上的点，S表示闭合曲面即静止体的表面， 表示三维空间， 表示一维标量，sgn(p)表示当p在闭合曲面内部时取-1，在闭合曲面外面时取1，min{|p-q|}表示求出点p到闭合曲面s的最短距离。
[0022] 所述的一种超声检测中机械手防碰撞方法，所述的步骤三中，在计算碰撞体运动过程中，以机械手碰撞体上的点在当前时刻以相应速率运动，并结合当前时刻的符号距离场值D(p)，计算下一时刻的近似符号距离场值，令机械手最大运动速率为vmax，取d(ti)'＝d(ti-1)-γ×△t×vmax，当比较值δ＝d(ti)'-σ满足δ＞0，其中σ为安全阈值，则机械手碰撞体上的点不与静止体发生碰撞，然后令近似距离场值d(ti)'作为ti时刻的距离场值d(ti)，即d(ti)＝d(ti)'，继续进行再下一时刻的计算；否则，根据离散距离场检测进一步检测下一时刻准确的距离场值。
[0023] 所述的一种超声检测中机械手防碰撞方法，所述的步骤三中，计算碰撞体运动过程包括以下步骤：
[0024] 步骤(1)，根据以下二式计算近似距离场值以及δ值：
[0025] d(ti)'＝d(ti-1)-γv△t
[0026] δ＝d(ti)'-σ
[0027] 若δ＞0，令d(ti)＝d(ti)'，执行ti+1时刻的碰撞检测；否则，跳转到步骤(2)；
[0028] 步骤(2)，根据变换矩阵计算碰撞体的运动学方程；
[0029] 步骤(3)，根据步骤(2)结果，搜寻相应的距离场值，使用三线性插值计算d(ti)，与安全阈值σ作比较并判断是否发生碰撞干涉；
[0030] 步骤(4)，根据步骤(3)的判断结果，若发生碰撞则执行相应的碰撞策略；否则，跳转到步骤(1)执行ti+1时刻的碰撞检测。
[0031] 所述的一种超声检测中机械手防碰撞方法，所述的步骤(4)中，所述的碰撞策略包括停止设备运行和发出警报在内的防止机械手碰撞的预设方案。
[0032] 本发明的技术效果在于，根据镁合金弹壳超声检测过程中碰撞干涉的特点，在机械手防碰撞策略中引入了距离场并改进了其防碰撞算法。实际应用证明本发明在超声检测中的有效性，根据检测结果可指导超声检测路径优化。通过不同算法间的指标对比表明，本发明不仅提高了碰撞检测效率，同时能够满足镁合金弹壳超声检测系统防碰撞的高实时性要求。
附图说明
[0034] 图1为超声自动检测系统总体结构示意图；
[0035] 图2为机械手连杆坐标系示意图；
[0036] 图3为超声检测系统的
虚拟环境和距离场示意图，其中(a)为检测系统的虚拟环境示意图，(b)为检测系统虚拟环境俯视图，(c)为A-A截面示意图，(d)为B-B截面距离场示意图；
[0037] 图4为不同扫描角度下距离随时间变化曲线示意图，其中(a)为 时的示意图，(b)为 时的示意图，(c)为 时的示意图，(d)为 时的示意图；
[0038] 图5为算法改进前后的随时间变化曲线示意图。
具体实施方式
[0039] 镁合金弹壳的超声自动检测系统的总体结构如图1所示，采用水浸纵波反射法，三爪转盘夹持镁合金弹壳旋转，由机械手控制探
头架带动超声
探头，与镁合金弹壳表面保持垂直关系，并沿着镁合金弹壳
母线扫描，形成螺旋扫描路径，实时采集超声检测
信号并经由工控机分析处理，最终实现镁合金弹壳内部的缺陷
可视化[8]。因为
超声波具有很强的
指向性，检测过程中探头的型面
跟踪精度是有效提取缺陷信号的前提，因此6
自由度机械手精确的运动控制是整个系统的关键环节，而机械手运动学分析则是运动控制的
基础。
[0040] 如图2所示，本研究采用的史陶比尔TX60L机械手由6个
转动关节构成，按Denavit-Hartenberg方法和连杆坐标系原则，建立如图3所示的连杆坐标系，相邻2个连杆坐标系{i-1}和{i}(i＝1,2,...,6)之间的变换矩阵为：
[0041]
[0042] 式(1)中i＝1,2,...,6，li、αi、di和θi分别为连杆坐标系{i}中的连杆长度、连杆扭角、连杆偏距和关节变量角，关节变量角即表示机械手的关节转动多少度。Rot(Zi-1,θi)表示绕连杆坐标系{i-1}中的Z轴旋转θi角度的旋转矩阵，Rot(Xi,αi)表示绕连杆坐标系{i}中的X轴旋转αi角度的旋转矩阵，Trans(0,0,di)表示沿着X轴运动0距离，沿Y轴运动0距离，沿Z轴运动di距离的平移矩阵，Trans(li,0,0)表示沿着X轴运动li距离，沿Y轴运动0距离，沿Z轴运动0距离的平移矩阵。根据表1所示的连杆参数和关节变量，结合式(2)可建立超声检测机械手的运动学方程 该方程可用于后续的运动控制和碰撞检测。
[0043]
[0044] 其中，
[0045]
[0046] 表1D-H参数表
[0047]
[0048] 如图3所示，将检测系统中的设备分为静止体和碰撞体两类，静止体S的符号距离场由式(3)表示。
[0049]
[0050]
[0051]
[0052] 其中P表示三维空间中的点，q表示静止体上的点，S表示闭合曲面即静止体的表面， 表示三维空间， 表示一维标量，sgn(p)表示当p在闭合曲面内部时取-1，在闭合曲面外面时取1，min{|p-q|}表示求出点p到闭合曲面s的最短距离。
[0053] 碰撞体运动过程中，离散距离场检测以时间步长△t为间隔，对碰撞体模型与符号距离场进行碰撞检测。当时间t＝ti时，碰撞体上的点p'根据式(2)的机械手运动学方程求解，将点p'变换到距离场坐标系下的坐标点p通过搜索符号距离场D(P)确定点p与静止体的最短欧氏距离，与安全阈值σ比较。
[0054] 由于在机械手实际应用中，碰撞检测的效率要求较高，因此假设机械手碰撞体上点p'在ti-1时刻以速率v运动，ti-1时刻的距离场值为d(ti-1)，根据式(5)计算d(ti)时刻的近似距离场值d(ti)'，剔除大量碰撞体与静止体相分离的情况，减少计算量，加速离散距离场碰撞检测算法。
[0055] d(ti)'＝d(ti-1)-γv△t(5)
[0056] 其中γ为估计系数，一般取1～3。
[0057] δ＝d(ti)'-σ(6)
[0058] 令机械手最大运动速率为vmax，由于受关节
位置的限制，运动时的速率v≤vmax，近似取d(ti)'＝d(ti-1)-γ×△t×vmax，当式(6)满足δ＞0，则点p'不与静止体发生碰撞，令近似距离场值d(ti)'作为ti时刻的距离场值d(ti)，即d(ti)＝d(ti)'；否则，根据离散距离场检测进一步检测ti时刻准确的距离值d(ti)。加速的离散距离场碰撞检测具体过程如下所示：
[0059] (1)根据式(5)和式(6)计算近似距离场值以及δ值，若δ＞0，令d(ti)＝d(ti)'，执行ti+1时刻的碰撞检测；否则，跳转到步骤(2)；
[0060] (2)根据式(2)计算碰撞体的运动学方程；
[0061] (3)根据步骤2结果，搜寻相应的距离场值，使用三线性插值计算d(ti)，与安全阈值σ作比较并判断是否发生碰撞干涉；
[0062] (4)根据d(ti)判断碰撞的状态，若发生碰撞则执行相应的碰撞策略；否则，跳转到步骤(1)执行ti+1时刻的碰撞检测。
[0063] 本发明实验以图3(a)、图3(b)所示超声检测系统为实验对象，以AZ80镁合金壳体为检测对象，搭建系统的虚拟环境如图3(a)所示，根据第2节中的方法，设置距离场的
体素尺寸Voxel为1mm，即步骤(3)中距离场检测误差为1mm(此变量可根据实际的碰撞检测精度进行设置)，在扫描前建立距离场，图3(b)中静止体的A-A截面和B-B截面距离场如图3(c)和(d)所示。
[0064] 将实验过程中碰撞体的运动分为扫描运动和辅助运动两部分，扫描运动为机械手沿着母线运动的过程，辅助运动为两段母线间的过渡阶段及准备阶段和结束阶段，设实验中超声探头晶片中心与被测件表面间的距离，即水声距为20mm，扫描步距为1mm，机械手扫描速度为1mm/s(即扫描运动时，vmax＝1mm/s)，最大辅助速度为80mm/s(即辅助运动时，vmax＝80mm/s)，安全阈值σ应略大于距离场体素尺寸，故取值为3mm，扫描角度 即图3(b)所示探头轴线与x轴的夹角，以不同的角度开展超声C扫描实验，以0.04s为时间间隔进行距离场碰撞检测，实验结果如图4、图5及表2所示。
[0065] 在图4中，实线表示采用传统距离场算法时，整个碰撞体与静止体在不同时刻的最短距离，虚线表示探头中心与静止体在不同时刻的最短距离，扫描运动过程中，最短距离稳定于水声距，但当扫描试
块下部时，由于接近转盘导致最短距离变小，甚至小于安全阈值，而在辅助运动过程中最短距离变化较大，但距离值远大于安全阈值。从图4可以看出，距离场碰撞检测算法能够实时获取碰撞体与静止体的最短距离，可在危险时刻及时采取防范措施，保证系统的安全性。
[0066] 采用改进算法前后的距离场计算结果对比如图5所示，其中，实线和虚线分别表示改进前后整个碰撞体与静止体在不同时刻的最短距离。可见，在大于安全阈值的情况下，采用改进的近似距离场进行检测，可有效降低计算机械手正解运算及距离场的搜索
频率，在避免发生碰撞事故发生的前提下提高碰撞检测的效率，在小于安全阈值的情况下仍采用传统距离场算法进行碰撞检测，保证关键位置的碰撞检测精度。
[0067] 由表2可见，当 增大时，碰撞体与静止体在整个实验过程中的最短距离变小，当时开始小于安全阈值，当 时仅1.0013mm，但当 增大时，实验辅助时间变短，最短辅助时间较最长辅助时间减小了42.86％，由表2可知当 时实验效果比较理想。通过对表2距离场检测数据的分析，可知距离场碰撞检测算法除了能够实现防碰撞功能，还能对实验参数优化提供参考依据。
[0068] 表2不同扫描角度下实验数据分析表
[0069]
[0070] 为了考察改进的距离场算法的性能，在i3-2350M 2.3G上将改进算法与距离场算法和OpenCCD碰撞检测的算法进行对比，OpenCCD算法检测效率与三角片数成正比，距离场算法检测效率与碰撞体的模型
顶点数量成正比，表3为碰撞体与静止体模型的数据。表4为3种不同的碰撞检测算法下，以 为实验条件的碰撞检测结果，距离场算法检测效率比OpenCCD算法提高了97.17％，本发明提出的距离场改进算法检测效率比距离场算法提高了80.62％，可见本发明改进的距离场算法对超声检测中的碰撞干涉检查的效率更高。
[0071] 表3碰撞体与静止体模型数据
[0072]
[0073] 表4三种不同算法的检测结果
[0074]
