[发明专利]一种管道位姿测量装置及方法

权利要求书

1.一种管道位姿测量方法，其特征在于，包括管道位姿测量装置，所述管道位姿测量包括导向动力组件、变径支架组件、周向旋转组件、检测组件和控制器；

三个所述变径支架组件分布在所述导向动力组件的周侧，且由所述导向动力组件驱动进行同步变径，所述周向旋转组件设于所述导向动力组件的端部，且与所述导向动力组件同轴设置，所述检测组件设于所述周向旋转组件上，由所述周向旋转组件驱动进行周向旋转；

所述导向动力组件、周向旋转组件、检测组件均与所述控制器电性连接；

应用上述的管道位姿测量装置对管道进行测量，包括以下步骤：

S1：控制器启动导向动力组件，导向动力组件驱动变径支架组件进行变径，以使整个装置固定在管道内；

S2：控制器启动检测组件开始进行检测，检测组件采用激光位移传感器(21)，激光位移传感器(21)在管道内壁投射若干激光点；

S3：控制器驱动周向旋转组件带动激光位移传感器(21)进行周向旋转任意角度后，令激光位移传感器(21)再次投射若干激光点，激光位移传感器(21)将数据传输给控制器；

S4：控制器以激光位移传感器(21)所在的平面建立基坐标系{B}，其z轴朝向为管道的轴线方向，进行激光位移传感器坐标系{C}相对于各自腕部坐标系{W}的转换、腕部坐标系{W}相对于基坐标系{B}的转换，以此来获取打到管道内壁的激光点相对于基坐标系{B}的三维点坐标；

S5：控制器基于激光点相对于基坐标系{B}的三维点坐标，进行椭圆拟合，并根据最小二乘法计算椭圆方程、椭圆的长短轴、以及椭圆与管道横截面的偏角α，确定椭圆的圆心(x₀,y₀)位置和长短轴的方向，根据长短轴求得旋转角度θ；

S6：控制器将基坐标系{B}进行平移和旋转，在椭圆圆心处得到坐标系{1}，坐标系{1}的圆心为椭圆圆心(x₀,y₀)，且x、y轴分别是椭圆的长轴和短轴，因坐标系{1}的原点在管道轴线上，所以把坐标系{1}的z₁轴绕椭圆短轴y₁轴旋转角度θ即得到管道轴线的坐标系{2}；

S7：控制器将得到的管道轴线的坐标系{2}进行输出到屏幕，完成管道位姿的测量；

其中，步骤S4所述的激光点相对于基坐标系{B}的三维点坐标需经过激光位移传感器坐标系{C}相对于各自腕部坐标系{W}的转换、腕部坐标系{W}相对于基坐标系{B}的转换，具体过程为：

上式中，为腕部坐标系{W}相对于基坐标系{B}的变换矩阵；R_z(σ)表示腕部坐标系{W}相对于基坐标系{B}的旋转矩阵，σ表示腕部坐标系{W}相对于基坐标系{B}旋转的角度；

激光位移传感器坐标系{C}相对于腕部坐标系{W}的变换矩阵通过手眼标定求出；上式中，腕部坐标系{W}相对于基坐标系{B}进行σ度旋转后，沿基坐标系{B}的x轴进行平移即得到腕部坐标系{W}；

激光点相对于基坐标系{B}的三维点坐标在进行坐标系的转换过程中，因激光位移传感器(21)内部的工具坐标系不能人为设定，因此还需对其进行一个手眼标定；

手眼标定的基本思路是获取不同拍摄角度下标定板图像，通过多次观察结果推导转换矩阵中旋转与平移分量，具体过程如下：

将标定板固定，在标定板上取一个靶点，将激光位移传感器(21)的安装支架末端连同激光位移传感器(21)装在UR3机器手的末端，控制机器手的末端带动激光位移传感器(21)运动到不同位置，使激光位移传感器(21)投射点在标定板靶点处，从而推导计算出手眼系统的R和T，对于标定过程任意两个拍摄角度，列出如下关系式：

其中，B表示UR3机器人的基坐标系，W表示安装激光位移传感器(21)位置的腕部坐标系，T表示UR3机器人的TCP工具的中心坐标系，C表示激光位移传感器坐标系；

B₁表示UR3机器人在第一次拍摄角度中的基坐标系，W₁表示激光位移传感器(21)在第一次拍摄角度中的腕部坐标系，T₁表示UR3机器人的TCP工具在第一次拍摄角度中的中心坐标系；

B₂表示UR3机器人在第二次拍摄角度中的基坐标系，W₂表示激光位移传感器(21)在第二次拍摄角度中的腕部坐标系，T₂表示UR3机器人的TCP工具在第二次拍摄角度中的中心坐标系；

令简化上式可得：

AX＝DX

上式中X即为需要求解的手眼关系矩阵。