复杂构件机械手扫查位姿的四元数转换方法

摘要

本发明提出一种实现复杂构件扫查成像检测的机械手位姿转换的方法：对五轴数控加工生成的数控代码进行后置处理，采用四元数法代替矩阵和欧拉角描述六自由度机械手末端执行器空间超声检测的位姿；针对复杂型面工件的专用性和特殊性，本发明采用水浸式超声检测，将探头固定于水槽中，机械手夹持被测工件相对超声探头进行复杂空间运动，进而对其实现超声无损检测。这种机械手位姿转换方法可有效地完成机械手超声扫查的后置处理，旋转变换计算更加简洁、方便。

说明书

一、技术领域

本发明提出一种复杂构件机械手扫查位姿的四元数转换方法，该方法运用于机械手的位姿转换，可有 效地完成机械手超声扫查的后置处理过程，旋转变换计算更加简洁、方便。

二、背景技术

在工程实践中，复杂型面构件的可靠性与其内部结构紧密相关，其内部结构复杂，超声传播规律复杂， 人工检测的难度大、效率低，其内部缺陷的无损检测一直是探伤技术领域的难题。

超声检测是相当有效的一种检测手段，可以比较精确地检测出被测构件内部的缺陷。目前，有一种自 动检测的方法可以有效解决人工检测带来的问题：采用水浸式检测方法，把水作为耦合剂，将超声探头固 定在水槽中，六自由度机械手握持被测构件在水槽中相对探头进行复杂空间运动，采用超声扫查方法对被 测构件内部缺陷进行检测。

然而这种方法存在的问题是，在超声扫查的过程中，需要对被测构件CAD数据模型进行轨迹规划， 但是数控软件得到的刀位数据并不能被机械手利用，需要对其进行后置处理。在笛卡尔坐标系中，被测构 件在坐标系下的位姿由三个位置自由度和三个姿态自由度组成。后置处理的任务就是对CAM系统所提供的 工件坐标系下的刀位数据进行处理，转换为机械手末端执行器的位置坐标及相应的回转角度。

常用的坐标转换方法有欧拉角法、旋转矩阵法和四元数法。欧拉角旋转时某些情况下可能导致的自由 度丧失，即所谓的“万向节锁”现象。用旋转矩阵法需要9个参数，计算繁琐且耗时。经万方数据库检索， 文献(肖文磊,等.切削加工机器人与CAD/CAM系统集成化[J].机械工程学报，2011)公开了一种将G代 码进行坐标转换的方法，但该方法只适用于机械手夹持刀具相对被测构件运动的情况。对上述描述的探头 固定，机械手夹持被测构件相对探头进行检测这种情况不适用。

由于运用欧拉角法或旋转矩阵法对被测构件进行旋转时，需要得到已知的被测点的自身坐标系才能完 成坐标转换，而CAM软件五轴加工生成的刀位文件只有被测点在自身坐标系中的位置和法线矢量，缺少被 测点的方位矢量和接近矢量，不方便在被测点建立局部坐标系，继而进行坐标转换。另一方面，对复杂型 面进行超声扫查检测过程中，我们需要保证被测构件的各个扫查点的表面法线方向与超声换能器的轴线方 向一致，即保证构件被测点旋转到超声换能器位置时候，法线矢量z_g与换能器法线方向n一致。这样一 来，坐标转换就有了一定的困难。

1843年，爱尔兰数学家哈密顿(Hamilton)发明了四元数。1985年，Shoemake将其引入计算机图形学 领域。从此四元数在计算机图形学、计算机动画、计算机视觉和虚拟实现等领域得到了广泛运用。经文献 检索发现，文献(谢荣,鲁海燕.基于四元数的船舶运动姿态仿真研究[J].中国造船，2011)采用四元数方 法对运动船舶模型的三维姿态及其旋转进行了描述，但不是针对机械手位姿转换的。

为解决以上的问题，本文提出了一种采用四元数对五轴数控加工生成的数控代码进行后置处理，继而 来描述六自由度机械手夹持被测构件相对探头运动过程中末端执行器空间超声检测的位姿，实现机械手超 声扫查的位姿转换的方法，这是一种全新的方法，具有广泛的应用前景。

三、发明内容

本发明的目的是提供一种用于机械手夹持被测构件相对探头运动过程中机械手的位姿转换方法。该方 法可解决欧拉角或矩阵法难以实现的坐标转换问题，可以实现复杂型面超声扫查的机械手位姿转换。

本发明的目的是这样实现的：

步骤1：求四元数q。超声探头自身坐标系为MX₁Y₁Z₁(∑₁)(M表示坐标原点，X₁表示X轴。Y₁表 示Y轴，Z₁表示Z轴，为简便说明，我们把上述坐标系用(∑₁)表示)，同理，被测工件坐标系为OX₂Y₂Z₂(∑₂) (O表示坐标原点，X₂表示X轴。Y₂表示Y轴，Z₂表示Z轴，为简便说明，我们把上述坐标系用(∑₂) 表示；同时，令被测构件坐标系与机械手末端执行器坐标系重合)。已知被测构件在∑₂坐标系下某一点A的 点位信息A(x,y,z,T_x,T_y,T_z)(前三个参数(x,y,z)表示被测点的位置坐标，后三个参数(T_x,T_y,T_z)表示该 点的法线矢量)，设构件坐标系∑₂原点O的点位信息为O(0,0,0,0,0,-1)，其中，前3个参数表示原点位置 (0,0,0)，后3个参数表示原点沿Z轴反方向一单位长度的法线矢量(0,0,-1)；同理，设∑₁坐标系原点M的 点位信息为M(0,0,0，0,0，-1)。

通过四元数法获得要使被测点A的法线矢量(Tx,Ty,Tz)旋转到与点M的法线矢量(0,0，-1)同向的四 元数q(q₀,q₁,q₂,q₃)；在确定了四元数q后，接下来需要对构件所有被测点的位置进行四元数q旋转变换。 此时，A点变为A′＝(x,y,z,Tx′,Ty′,Tz′)，O点变为O′(0,0,0,T′_Ox,T′_Oy,T′_Oz)。

步骤2：求平移向量p。计算出旋转后的被测点A′到点M的距离p＝(-x′,-y′,-z′)。将构件位移p。 此时，位移后的O″距离M的距离为p₀＝(p_x,p_y,p_z)＝O″+p。

步骤3：求机械手输入参数。将上述的四元数q通过四元数与欧拉角的转换公式变换为机械手需要的 欧拉角(α,β,γ)。这样，我们就得到了机械手夹持被测构件时，被测点A到达探头坐标原点M时候机械 手的六个参数(p_x,p_y,p_z,α,β,γ)。

本发明的优点在于：被测构件相对探头运动，运用四元数方法对NC代码进行后置处理，保证被测工 件每个被测点上的法线矢量z_g依次旋转到与探头法线方向n一致，可以有效解决CAM软件得到不完整所需 数据继而难以进行坐标转换这个问题。这种方法适用于复杂型面超声扫查的机械手位姿转换，旋转变换计 算更加简洁、方便。

四、附图说明

图1被测构件与超声探头在世界坐标系O₀X₀Y₀Z₀下的坐标转换关系图

图2机械手位姿的转换过程

图3水浸式超声脉冲反射法被测构件检测示意图

五、具体实施方式

下面对本发明的具体实施方式进行详细说明：

被测构件与探头的相对运动可以看成两个刚体的一般运动，可以通过绕某个轴的旋转和沿该轴的平移 实现。用四元数表示空间的旋转q；当考虑平移部分时，可设平移向量为p，这样就由四元数旋转q与平 移向量p构成了被测构件的整个变换过程。表达式为：

p_B＝qp_Aq^-1+p    (1)

其中，p_A为坐标转换前的值，p_B为坐标转换后的值。

被测构件与超声探头在世界坐标系O₀X₀Y₀Z₀下的坐标转换关系，为简便介绍该方法，我们将被测构 件简化成一刚体，如图1。设探头形心点的自身坐标系为用户坐标系MX₁Y₁Z₁；被测构件自身坐标系与机 械手末端执行器的坐标系重合，为OX₂Y₂Z₂；被测构件上任一被测点A处的坐标系为O_gX_gY_gZ_g。

步骤1：求四元数q。见图2(a),超声探头自身坐标系为MX₁Y₁Z₁(∑₁)(M表示坐标原点，X₁表 示X轴。Y₁表示Y轴，Z₁表示Z轴，为简便说明，我们把上述坐标系用(∑₁)表示)，同理，被测工件坐标 系为OX₂Y₂Z₂(∑₂)(O表示坐标原点，X₂表示X轴。Y₂表示Y轴，Z₂表示Z轴，为简便说明，我们把 上述坐标系用(∑₂)表示；同时，令被测构件坐标系与机械手末端执行器坐标系重合)。已知被测构件在∑₂坐标系下某一点A的点位信息A(x,y,z,T_x,T_y,T_z)(前三个参数(x,y,z)表示被测点的位置坐标，后三个 参数(T_x,T_y,T_z)表示该点的法线矢量)，设构件坐标系∑₂原点O的点位信息为O(0,0,0,0,0,-1)，其中，前 3个参数表示原点位置(0,0,0)，后3个参数表示原点沿Z轴反方向一单位长度的法线矢量(0,0,-1)；同理， 设∑₁坐标系原点M的点位信息为M(0,0,0，0,0，-1)。

通过四元数法获得要使被测点A的法线矢量(Tx,Ty,Tz)旋转到与点M的法线矢量(0,0，-1)同向的 四元数q(q₀,q₁,q₂,q₃)；在确定了四元数q后，接下来需要对刚体所有被测点位置进行式(3)四元数q旋 转变换，即得到图2(b)。此时，A点变为A′＝(x,y,z,Tx′,Ty′,Tz′)，O点变为O′(0,0,0,T′_Ox,T′_Oy,T′_Oz)。

步骤2：求平移向量p。计算出旋转后的被测点A′到点M的距离p＝(-x′,-y′,-z′)。将被测构件位 移p，即得到图2(c)。此时，位移后的O″距离M的距离为p₀＝(p_x,p_y,p_z)＝O″+p。

步骤3：求机械手输入参数。将上述的四元数q通过四元数与欧拉角的转换公式变换为机械手需要的 欧拉角(α,β,γ)。这样，我们就得到了机械手夹持被测构件时，被测点A到达探头坐标原点M时候机械 手的六个参数(p_x,p_y,p_z,α,β,γ)。

本发明采用水浸式脉冲反射法对复杂型面缺陷进行超声检测，见图3。

由超声检测原理知，检测时超声束沿着被测构件表面法向入射最优。扫查时一方面要保证超声换能器 与被测构件表面等距(该间距称作水声距)，另一方面要保证在各个扫查点超声换能器的轴线n与被测构 件的表面法线方向z_g一致。利用四元数法将CAM软件得到的被测构件每个被测点上的法线矢量z_g依次旋 转到与探头法线方向n一致，实现对复杂型面超声扫查的机械手位姿转换。

保证被测构件位置数据与超声检测数据对应性的实施方式。在对被测构件进行检测的过程中，为了有 效、准确、真实地表达被检构件各个部位的缺陷情况，要保证机械手的运动位置与超声换能器的检测信号 采集的对应性。本发明采用的方法是，在运用四元数法对被测构件的点位信息进行坐标转换后，实时传输 给机械手，机械手严格按照指定的坐标变换来运动，保证被测构件位置数据与超声检测信号数据的对应性。