CN114459353A - 一种管道位姿测量装置及方法 - Google Patents

Abstract

本发明公开一种管道位姿测量装置及方法。装置包括导向动力组件、三个变径支架组件、周向旋转组件、检测组件和控制器。方法是：激光位移传感器在周向旋转组件的带动下周向旋转任意角度后，在管道内壁投射若干激光点，在算法中对这些激光点进行坐标转换和椭圆拟合，以此确定椭圆的轴线向量，最后在控制器计算出两管道轴线的相对位姿关系，以实现对大型管道的位姿估计。与传统机械检测相比，本发明提高了测量精度，实现了非接触式自动化操作，结构紧凑，实用性强。

Description

一种管道位姿测量装置及方法

技术领域

本发明涉及视觉传感测量技术领域，特别是涉及一种管道位姿测量装置及方法。

背景技术

在管道应用领域中实现管道间精密对接十分困难。对于不同直径的大型管道，在安装时，测量其位姿尤为重要。但在管道的安装场景中，由于恶劣的工作环境和庞大的工作量以及安装精度的要求，采用人工对接管道的精度不是十分理想。在人工智能技术快速发展的今天，也有采用管道机器人来对管道进行精密对接的应用，但是出于对不同的管径以及不理想的工作环境和应用成本的考虑，管道机器人也不适合普遍应用。因此，对于操作简单以及自动化程度高的管道对接技术要求越来越迫切。

在管道位姿检测方面，国内外研究者已经研究出了很多基于不同原理的检测机构。但现有的检测机构或多或少存在一些不足，比如体积较大或通用性不高，对于不同管径的管道不能普遍适用等。对现有专利检索发现，存在一种变径直管道自适应机器人，包括控制导向组件、四个自适应调整支架组件、方腔组件、动力驱动组件和激光测距组件。该专利利用动力伺服电机和滚珠丝杠来进行动力传递，以此使支撑杆和定向轮做上下升降运动，采用激光测距仪来对管道的直径进行实时监测来作为变径依据。该专利的变径支撑杆一侧只用了两个连杆交叉连接，这会使得在升降过程中运行不会十分平稳，并且该专利使用两个定向轮来与管道接触，没有采取锁死机构，容易打滑，同时其检测管道的位姿精度差，无法实现精准检测。

发明内容

本发明提供一种管道位姿测量装置及方法，本发明可以适用于不同管径，实现对管道的位姿进行检测，同时能实现平稳变径，以此来保证整个装置在固定时有一个平稳的状态来使检测组件能够更好的检测管道之间的相对位姿。

本发明的技术方案如下：

一种管道位姿测量装置，包括导向动力组件、变径支架组件、周向旋转组件、检测组件和控制器；

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

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

进一步，所述导向动力组件包括电机、丝杠光轴、滚珠螺母、第一固定座、第二固定座、滑动件；

所述电机安装在所述第一固定座上，所述电机的输出轴与所述丝杠光轴的一端连接，所述丝杠光轴的另一端依次穿过所述第一固定座、滑动件和第二固定座的中心孔，所述滚珠螺母套接在所述丝杠光轴上，且设于所述第一固定座与所述滑动件之间，所述滚珠螺母的端面与所述滑动件固定连接。

在传递动力时，驱动电机，动力由电机经过联轴器传出到丝杠光轴，丝杠光轴的转动带动滚珠螺母沿着轴向移动，进而推动滑动件进行直线往复运动，使其在导杆上沿轴向移动传递动力，为变径支架组件提供变径的动力。本发明的导向动力组件结构紧凑，动力传递简洁方便。

进一步，所述变径支架组件包括折叠连杆机构和贴合板；

所述折叠连杆机构由主连杆组和副连杆组交叉联接形成，所述贴合板的底部滑动连接有滑块，所述折叠连杆机构的主连杆组的一端与所述滑块铰接，另一端均与所述滑动件的边沿铰接，所述折叠连杆机构的副连杆组的一端与所述贴合板的底部铰接，另一端与所述第二固定座的边沿铰接。

在滑动件的动力的推动下，折叠连杆机构的主连杆组进行升降，其中主连杆组采用滑块机构来使变径支架组件变径时能够一端固定，一端游动，以此保证贴合板水平无倾斜升降，在主连杆组的带动下，副连杆组进行同步升降，以此带动贴合板稳定上下升降，可适应不同大小的管道，同时实现自动对心功能。

本发明的变径支架组件分为三组，沿管道轴向120°均匀分布，这样可以使贴合板很好的和管道进行稳定贴合、固定。每组变径支架组件进行双侧折叠连杆机构连接，这样可以在升降更加平稳的基础上，使变径支架组件受力均匀；贴合板采用天然橡胶制成，具有良好的减震和缓冲性能。

进一步，所述周向旋转组件包括舵机，所述舵机安装在所述第二固定座上，所述舵机驱动所述检测组件周向旋转。

在检测组件需要进行转动调整检测位置时，驱动舵机，进而使舵机带动检测组件进行同步转动。

进一步，所述检测组件包括三个激光位移传感器，三个所述激光位移传感器在所述支撑板上均匀分布。

实际工作时，本发明所提供的管道位姿测量装置中，电机接电转动，通过联轴器带动丝杠光轴转动，丝杠光轴带动滚珠螺母进行轴向平移，以此来推动滑动件的轴向滑动，使折叠连杆机构带动贴合板进行升降，实现在管道内的稳定变径，当贴合板和管道内壁贴合固定好后，启动激光位移传感器开始进行管道位姿测量工作。

测量工作具体是激光位移传感器在舵机的带动下周向旋转任意角度后，在管道内壁投射若干激光点，由这些激光点的坐标确定一个平面，该平面与管道内壁的截线形成一个椭圆，通过数据采集卡将点的坐标数据传给控制器，将这些点的坐标通过算法转化成相对于在支撑板上设定的基坐标系的三维坐标点，并进行椭圆拟合，以此确定椭圆的轴线向量，最后计算出两管道轴线的相对位姿关系，以实现对大型管道的位姿估计。

具体地，提供一种管道位姿测量方法，应用上述的管道位姿测量装置对管道进行测量，包括以下步骤：

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

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

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

S4：控制器以激光位移传感器所在的平面建立基坐标系{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}的三维点坐标在进行坐标系的转换过程中，因激光位移传感器内部的工具坐标系不能人为设定，因此还需对其进行一个手眼标定；

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

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

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

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

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

令

简化上式可得：

AX＝DX

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

进一步，步骤S5所述的求解椭圆方程、椭圆长短轴、偏角α和旋转角度θ的具体过程为：

根据椭圆一般方程：

Ax²+2Bxy+Cy²+Dx+Ey+F＝0

任意的椭圆方程由标准椭圆通过平移(x₀,y₀)和旋转角度θ得到：

x＝(X-X₀)cosθ+(Y-y₀)sinθ

y＝-(X-X₀)sinθ+(Y-y₀)cosθ

椭圆长短轴a、b通过以下公式求得：

偏角α通过以下公式求得：

旋转角度θ通过以下公式求得：

在算法中运用以上公式拟合椭圆并获取椭圆参数。

进一步，步骤S6所述的基坐标系{B}到坐标系{1}再到坐标系{2}的具体转换过程为：

式中，

为坐标系{1}相对于基坐标系{B}的变换矩阵，

为坐标系{2}相对于坐标系{1}的变换矩阵；

即管道轴线的坐标系{2}相对于基坐标系{B}的变换矩阵为

与现有技术相比，本发明具有以下有益效果：

本发明的装置采用导向动力组件来传递动力，推动周侧的三个变径支架组件进行变径运动，在保证结构紧凑的条件下，在工作过程中对变径支架组件提供动力，且三个变径支架周侧分布，即在管道内壁实现圆周分布，彼此间隔120°，以此确保变径机构的稳定性；本发明的周向旋转组件能周向旋转，可以使其搭载的检测组件在所要求的位置停止；本发明的装置操作方便、适应性强，变径支架组件能根据管径变化及时变径调整，可适用于不同管径管道位姿检测；本发明的方法精准适用，将激光位移传感器打出的点的坐标直接进行相关算法的分析计算，可以精确得出管道间的位姿关系。

附图说明

图1为管道位姿测量装置的整体结构示意图。

图2为导向动力组件的结构示意图。

图3为三个变径支架组件设置在导向动力组件周侧的结构示意图。

图4为周向旋转组件和检测组件进行连接的结构示意图。

图中：电机1、电机支座2、丝杠光轴3、滚珠螺母4、第一固定座5、第二固定座6、导杆7、滑动件8、丝杠防护罩9、贴合板10、主连杆组11、副连杆组12、轨道13、滑块14、舵机15、舵机安装板16、支撑轴17、支撑板18、轴承座19、固定连杆20、激光位移传感器21。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。

实施例1：

如图1-图4所示，一种管道位姿测量装置，包括导向动力组件、变径支架组件、周向旋转组件、检测组件和控制器(图中未示出)；

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

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

参阅图2，在本实施例中，导向动力组件包括电机1、电机支座2、丝杠光轴3、滚珠螺母4、第一固定座5、第二固定座6、导杆7、滑动件8、丝杠防护罩9；

三个导杆7设于第一固定座5与第二固定座6之间，滑动件8滑动套接在三个导杆7上，在第一固定座5、滑动件8、第二固定座6上等分的开有三个孔，以此与三根导杆7进行配合，使滑动件8能沿着导杆7进行轴向移动；电机1通过电机支座2安装在第一固定座5的外侧上，电机1的输出轴通过联轴器与丝杠光轴3连接，丝杠光轴3依次穿过第一固定座5、滑动件8和第二固定座6的中心孔，且末端通过轴承穿出第二固定座6用丝杠防护罩9进行遮盖，滚珠螺母4套接在丝杠光轴3上，且设于第一固定座5与滑动件8之间，滚珠螺母4的端面与滑动件8固定连接。

在传递动力时，驱动电机1，动力由电机1经过联轴器传出到丝杠光轴3，丝杠光轴3的转动带动滚珠螺母4沿着轴向移动来推动滑动件8，使其在导杆7的导向下沿轴向移动传递动力，进而为变径支架组件提供变径的动力。本发明的导向动力组件结构紧凑，动力传递简洁方便。

其中，导杆7使滑动件8的推动变径更稳定，起导向作用，并且在丝杠光轴3的末端装配了丝杠防护罩9，可避免周向旋转组件的电源线及信号线卷入丝杠光轴3，同时还起到防尘的效果。

参阅图1和图3，在本实施例中，变径支架组件包括两个折叠连杆机构和一个贴合板10；

折叠连杆机构由主连杆组11和副连杆组12交叉联接形成，贴合板10的底部两侧均设有轨道13，且轨道13上均设有滑块14；两个折叠连杆机构的主连杆组11的一端分别与两个滑块14铰接，两个折叠连杆机构的主连杆组11的另一端均与滑动件8的边沿铰接，两个折叠连杆机构的副连杆组12的一端分别与贴合板10的底部两侧铰接，两个折叠连杆机构的副连杆组12的另一端均与第二固定座6的边沿铰接。

其中，滑动件8、第二固定座6的边沿均凸出有三个均匀分布的铰接台，分别与三个变径支架组件的折叠连杆机构对应，每个变径支架组件中的两个主连杆组11的一端分别连接在滑动件8中一个铰接台的两侧，另一端分别连接在两个滑块14上，每个变径支架组件中的两个副连杆组12的一端分别连接在第二固定座6中一个铰接台的两侧，另一分别连接在贴合板10的底部边沿两侧。

在滑动件8的动力的推动下，折叠连杆机构的主连杆组11进行升降，其中主连杆组11采用轨道滑块机构来使变径支架组件变径时能够一端固定，一端游动，以此保证贴合板10水平无倾斜升降，在主连杆组11的带动下，副连杆组12进行同步升降，以此带动贴合板10稳定上下升降，可适应不同大小的管道，同时实现自动对心功能。

本发明的变径支架组件分为三组，沿管道轴向120°均匀分布，这样可以使贴合板10很好的和管道的内壁进行稳定贴合、固定。当接收到动力时，三个变径支架组件同时升降，动作具有一致性。本发明的每组变径支架组件进行双侧折叠连杆机构连接，这样可以在升降更加平稳的基础上，使变径支架组件受力均匀；贴合板10采用天然橡胶制成，具有良好的减震和缓冲性能。

参阅图1和图4，在本实施例中，周向旋转组件包括舵机15、舵机安装板16、支撑轴17、支撑板18、轴承座19和固定连杆20；

舵机安装板16安装在第二固定座6上，舵机15安装在舵机安装板16，三个固定连杆20的一端分别连接在第二固定座6的周侧，三个固定连杆20的另一端分别连接在轴承座19的周侧，通过固定连杆20使轴承座19固定于舵机15的输出轴的正上方，轴承座19与第二固定座6同轴设置，舵机15的输出轴通过法兰盘与支撑轴17的一端连接，支撑轴17的另一端穿轴承座19与支撑板18的底部连接。

在检测组件需要进行转动调整检测位置时，驱动舵机15，进而使舵机15带动支撑轴17进行转动，支撑轴17带动支撑板18上的检测组件进行同步转动。

在本实施例中，检测组件包括三个激光位移传感器21，三个激光位移传感器21在支撑板18上间隔120°均匀分布，用于对管道内壁投射激光点进行测量数据。

在本实施例中，控制器采用微型计算机，控制器与电机1、舵机15、激光位移传感器21均电性连接，控制器用于对电机1、舵机15、激光位移传感器21进行驱动控制，及采集激光位移传感器21的数据进行处理。

工作原理如下：

装置实际工作时，电机1接电转动，通过联轴器带动丝杠光轴3转动，丝杠光轴3带动滚珠螺母4进行轴向平移，以此来推动滑动件8沿导杆7的轴向滑动，使折叠连杆机构带动贴合板10进行升降，实现在管道内的稳定变径，当贴合板10和管道内壁贴合固定好后，启动激光位移传感器21开始进行管道位姿测量工作。

测量工作具体是；激光位移传感器21在舵机15的带动下周向旋转任意角度后，在管道内壁投射若干激光点，由这些激光点的坐标确定一个平面，该平面与管道内壁的截线形成一个椭圆，通过数据采集卡将点的坐标数据传给控制器，将这些点的坐标通过算法转化成相对于在支撑板18上设定的基坐标系的三维坐标点，并进行椭圆拟合，以此确定椭圆的轴线向量，最后计算出两管道轴线的相对位姿关系，以实现对大型管道的位姿估计。

具体地，本实施例提供一种管道位姿测量方法，应用上述的管道位姿测量装置对管道进行测量，包括以下步骤：

S1：控制器启动电机1，电机1通过丝杠光轴3驱动滚珠螺母4，使滚珠螺母4带动滑动件8进行轴向运动，滑动件8通过三个折叠连杆机构带动三个贴合板10进行同步升降，使三个贴合板10贴紧管道的内壁，以此完成变径，以使整个装置固定在管道内；

S2：控制器启动激光位移传感器21，使其在管道内壁投射若干激光点；

S3：控制器驱动舵机15，使舵机15通过支撑轴17带动支撑板18上的激光位移传感器21进行周向旋转任意角度后，令激光位移传感器21再次投射若干激光点，激光位移传感器21将数据传输给控制器；

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

S5：控制器基于激光点相对于基坐标系{B}的三维点坐标，进行椭圆拟合，并根据最小二乘法计算椭圆方程、椭圆的长短轴、以及椭圆与管道横截面的偏角α，确定椭圆的圆心(x_0,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即为需要求解的手眼关系矩阵。

在本实施例中，步骤S5所述的求解椭圆方程、椭圆长短轴、偏角α和旋转角度θ的具体过程为：

根据椭圆一般方程：

Ax²+2Bxy+Cy²+Dx+Ey+F＝0

上式中的A、B、C、D、E、F是椭圆一般方程的系数；

任意的椭圆方程由标准椭圆通过平移(x₀,y₀)和旋转角度θ得到：

x＝(X-X₀)cosθ+(Y-y₀)sinθ

y＝-(X-X₀)sinθ+(Y-y₀)cosθ

椭圆长短轴a、b通过以下公式求得：

偏角α通过以下公式求得：

旋转角度θ通过以下公式求得：

在算法中运用以上公式拟合椭圆并获取椭圆参数。

在本实施例中，步骤S6所述的基坐标系{B}到坐标系{1}再到坐标系{2}的具体转换过程为：

式中，

为坐标系{1}相对于基坐标系{B}的变换矩阵，

为坐标系{2}相对于坐标系{1}的变换矩阵；

即管道轴线的坐标系{2}相对于基坐标系{B}的变换矩阵为

与传统机械检测相比，本发明提高了测量精度，实现了非接触式自动化操作，结构紧凑，实用性强。

本发明的装置操作方便、适应性强，变径支架组件能根据管径变化及时变径调整，可适用于不同管径管道位姿检测。

本发明的方法精准适用，将激光位移传感器21打出的点的坐标直接进行相关算法的分析计算，可以精确得出管道间的位姿关系。

在设计时，需注意的是，变径支架组件的折叠连杆机构不应该局限于实施例中通过螺栓将折叠连杆机构交叉连接的搭建方法，其他搭建方法也是可行的。考虑到尽可能使结构紧凑、升降效果更加平稳，实施例中的折叠连杆机构交叉连接的搭建方法是比较合适的。

在设计时，在大型管道内壁采用了三个变径支架组件来固定机构，需注意的是，变径支架组件的数量不应该局限于实施例中的三组，其他数量也是可行的，考虑到材料成本和最终机构的稳定效果，实施例中的变径支架组件的数量是比较合适的。

在设计时，需注意的是，贴合板10和折叠连杆机构不能采用螺栓通过四个固定吊耳来进行连接。这样连接在实际工作中，贴合板10上升时，因为四个连接点均为固定连接，会造成贴合板10倾斜，在接触管道内壁时不能水平的与管道很好的贴合。因此本发明采用一侧用两个吊耳固定连接，另一侧采用轨道滑块机构来使变径组件变径时能够一端固定，一端游动，以此保证贴合板10水平无倾斜升降。需注意的是，游动的一端不应该局限于实施例中的轨道滑块机构，其他游动方式也是可行的，考虑到滑动的柔顺性，实施例中的轨道滑块机构是比较合适的。

实施例2：

本实施例与实施例1相似，所不同之处在于，变径支架组件设有四组，分别以90°间隔环绕导向动力组件设置，变径支架组件包括推动杆和支撑杆，推动杆的一端与滑动件8铰接，另一端与支撑杆的中部铰接，支撑杆的一端与第二固定座6铰接，另一端与贴合板10的底部固定连接，由滑动件8的轴线运动，通过推动杆推动支撑杆进行撑开或收拢，使贴合板10往外或往内，以此实现变径。

在本实施例中，贴合板10的外表面采用弧形面，以此与管道的内壁更好的贴合，增强稳定性。

实施例3：

本实施例与实施例1相似，所不同之处在于，检测组件包括四个激光位移传感器21，四个激光位移传感器21在支撑板18上间隔90°均匀分布。

在进行测量时，驱动周向旋转组件的舵机15带动激光位移传感器21进行周向旋转60°角度后，令激光位移传感器21再次投射若干激光点，之后再以实施例1中的测量方法进行位姿的确定。

本实施例中设定旋转的角度，进行定向采集，以此达到验证不同固定角度的管道位姿检测是否存在差异的目的。

需要说明的是，上述的实施例只是本发明所介绍的较为优选的实施例，本领域技术人员在此基础上，完全可以设计出更多的实施例。对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其他实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是与本文所公开的原理和新颖性特点相一致的最宽范围。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

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

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

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

2.根据权利要求1所述的管道位姿测量装置，其特征在于，所述导向动力组件包括电机(1)、丝杠光轴(3)、滚珠螺母(4)、第一固定座(5)、第二固定座(6)、滑动件(8)；

所述电机(1)安装在所述第一固定座(5)上，所述电机(1)的输出轴与所述丝杠光轴(3)的一端连接，所述丝杠光轴(3)的另一端依次穿过所述第一固定座(5)、滑动件(8)和第二固定座(6)的中心孔，所述滚珠螺母(4)螺纹套接在所述丝杠光轴(3)上，且设于所述第一固定座(5)与所述滑动件(8)之间，所述滚珠螺母(4)的端面与所述滑动件(8)固定连接。

3.根据权利要求2所述的管道位姿测量装置，其特征在于，所述变径支架组件包括折叠连杆机构和贴合板(10)；

所述折叠连杆机构由主连杆组(11)和副连杆组(12)交叉联接形成，所述贴合板(10)的底部滑动连接有滑块(14)，所述折叠连杆机构的主连杆组(11)的一端与所述滑块(14)铰接，另一端均与所述滑动件(8)的边沿铰接，所述折叠连杆机构的副连杆组(12)的一端与所述贴合板(10)的底部铰接，另一端与所述第二固定座(6)的边沿铰接。

4.根据权利要求3所述的管道位姿测量装置，其特征在于，所述周向旋转组件包括舵机(15)，所述舵机(15)安装在所述第二固定座(6)上，所述舵机(15)驱动所述检测组件周向旋转。

5.根据权利要求4所述的管道位姿测量装置，其特征在于，所述检测组件包括三个激光位移传感器(21)，三个所述激光位移传感器(21)在所述支撑板(18)上均匀分布。

6.一种管道位姿测量方法，其特征在于，应用权利要求1-5任一所述的管道位姿测量装置对管道进行测量，包括以下步骤：

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}进行输出到屏幕，完成管道位姿的测量。

7.根据权利要求6所述的管道位姿测量方法，其特征在于，步骤S4所述的激光点相对于基坐标系{B}的三维点坐标需经过激光位移传感器坐标系{C}相对于各自腕部坐标系{W}的转换、腕部坐标系{W}相对于基坐标系{B}的转换，具体过程为：

上式中，

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

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

8.根据权利要求7所述的管道位姿测量方法，其特征在于：激光点相对于基坐标系{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即为需要求解的手眼关系矩阵。

9.根据权利要求6所述的管道位姿测量方法，其特征在于：步骤S5所述的求解椭圆方程、椭圆长短轴、偏角α和旋转角度θ的具体过程为：

根据椭圆一般方程：

Ax²+2Bxy+Cy²+Dx+Ey+F＝0

任意的椭圆方程由标准椭圆通过平移(x₀，y₀)和旋转角度θ得到：

x＝(X-X₀)cosθ+(Y-y₀)sinθ

y＝-(X-X₀)sinθ+(Y-y₀)cosθ

椭圆长短轴a、b通过以下公式求得：

偏角α通过以下公式求得：

旋转角度θ通过以下公式求得：

在算法中运用以上公式拟合椭圆并获取椭圆参数。

10.根据权利要求6所述的管道位姿测量方法，其特征在于：步骤S6所述的基坐标系{B}到坐标系{1}再到坐标系{2}的具体转换过程为：

式中，

为坐标系{1}相对于基坐标系{B}的变换矩阵，

为坐标系{2}相对于坐标系{1}的变换矩阵；

即管道轴线的坐标系{2}相对于基坐标系{B}的变换矩阵为

Images