专利名称：微细管道机器人全程定位方法
技术领域：
本发明涉及一种微细管道机器人全程定位方法。
背景技术：
在石油、化工、能源、冶金、国防以及航空航天等行业中，有大量的孔类和管类零部件，诸如激光打印机喷嘴、原油或化工原料输送管道、核反应堆、蒸汽发生器热交换管，坦克用火炮炮膛、航空航天飞行器散热或气流场控制管路等，需要对其内轮廓形貌、尺寸，内壁缺陷以及管孔的同心度、空间分布等进行无损检测，以期对这些零部件的连续性、完整性和安全性进行无损评价，这对于提高产品性能质量、减少浪费、避免事故尤其是灾难性事故的发生，都具有十分重要的现实意义。
近年来，伴随着激光技术、光电技术和计算机技术的发展，已经出现了各种灵活、方便、多功能的光电内窥测量系统。这些系统的共同特点是采用主动光源照明，通过特殊的内窥成像光路及CCD摄像机，将包含被测参数几何特征的图像采集到计算机，从而进行图像处理及测量。Inari和Zhang W W等通过设计特殊的内窥成像光路，分别提出整圆成像法(whole-circle image method)和环形光切成像法(ring optical cutting image method)，一次性将整个管孔内截面成像到CCD摄像机上。但是，由于测量系统无法知道其自身的空间位置。所以，上述方法都只是得到了管孔的二维截面数据信息，只能通过对每个截面数据进行分析，得到测量检测的结论。无法得到管道内表面的整体形貌或三维模型。为了能够得到管道内表面的整体形貌或三维模型，我们提出对管道内表面进行三维重建，为此需要知道每个测量截面的空间位置。其思路是管道机器人将测量装置带入管道内部进行截面轮廓测量的同时，管道机器人对其自身所处空间位置和运动轨迹进行计算，即所谓全程定位。全程定位的意义主要是为了实现将截面测量和轴心轨迹(轴心线)联系起来，使得测量不仅可以检查管道的内部形貌，而且能够实现对管道内壁表面的三维重建。对此本文提出了一种切实有效的递推方法。首先，利用空间曲率传感器测量出机器人当前测量点的曲率和切向矢量方向，然后，根据前一测量点的空间位置、密切平面、切向矢量方向、机器人移动步距等参数计算当前空间位置。这样，只要给定初始测量点的空间位置、曲率、切向矢量方向，就可以实现机器人的全程定位。然后再通过曲线插值拟合，最终求出管孔轴心的空间曲线。最后，利用管孔轴心的空间曲线模型和具有确切空间位置截面轮廓曲线可以实现管道内表面的三维重建。
发明内容
本发明的目的是提供一种微细管道机器人全程定位方法。
它是给定起始点的空间位置、曲率和切矢量方向，通过微弯曲率传感器提供的当前测量点的曲率和相邻密切平面间的夹角，实现微细管道机器人的全程定位。
本发明主要是为了实现将截面测量和轴心轨迹(轴心线)联系起来，使得测量不仅可以检查管道的内部形貌，而且能够实现对管道内壁表面的三维重建。
图1是曲率传感器示意图；图2是密切平面示意图；图3是分段曲线示意图；图4是二维曲线示意图；图5是三维曲线示意图。
具体实施例方式
微细管道机器人全程定位方法的步骤为1)给定起始点的空间位置(坐标)、曲率、切矢量方向；2)通过微弯曲率传感器得到下一测量位置点的曲率和相邻密切平面间的夹角；3)按照本文提供的递推方法，求得当前测量点的空间位置和切矢量方向；4)在下一个测量位置时，前一测量点的空间位置和切矢量方向已知，可以求得当前点的空间位置和切矢量方向；5)逐级递推可以得到微细管道机器人的全程精确定位。
全程定位递推方法是针对管道机器人进行管道内壁表面形貌检测以及管道内壁表面三维重建而提出的一种递推方法。在已知起始测量点的空间位置、密切平面、切向矢量方向的条件下，依据空间曲率检测传感器所提供的曲率半径和相邻测量点密切平面间夹角，通过递推方法可以得到当前测量点的空间位置和切向矢量方向。我们分平面曲线型管孔和空间曲线型管孔两种情况讨论。
为了确定初始点的坐标，我们引入一段轴心轨迹为直线的导入装置。在递推方法的推导过程中我们假设曲率传感器不发生扭转运动。在计算过程中我们采用分段求解(假设每一段曲线都为平面曲线)，如图3所示，每一段圆弧曲线包含当前测量点两边各Δt/2(Δt为运动机构步距)的弧长。平面曲线型管孔对于平面二维曲线管孔，各测量位置处的密切平面夹角必然为0，我们可以通过所使用的管道机器人确定行走步距Δt，由曲率传感器计算当前测量点的曲率半径R。取其中的一段，建立局部坐标系，示意图如图4所示，其中，O’为当前测量点，O为前一测量点；OX/O’X’轴为圆弧的切线方向，OY/O’Y’轴为圆弧的法线方向(规定标号为单数微弯传感器曲率半径大，标号为双数曲率半径小时，法矢方向为正；否则为负)，OO’为平面曲线管孔轴心轨迹当前测量点所在的圆弧段，AO＝AO’＝R，Δt为OO’段的弧长，由此可以得到 OO’＝2Rsin(α/2) 这就是当前测量点在前一坐标系中的相对坐标。
当前测量点所确定的坐标系是前一测量点所确定坐标系经过坐标变换[8]得到的。首先进行平移变换，变换矩阵为T平移，然后再进行旋转变换，变换矩阵为T旋转。其中。 所以，在当前测量点下，圆弧终点的坐标系为[X′，Y′，1]＝[X，Y，1]·T平移·T旋转同时这也是下一测量点的初始坐标系。
在下一个测量坐标系中，x’o’y’为起点坐标系，通过上面的方法可以得到在x’o’y’坐标系中下一曲线段终点的坐标[a，b，1]，通过坐标变换[a′，b′，1]＝[a，b，1]·T旋转-1·T平移-1可以求得下一曲线段终点在xoy坐标系中的坐标(a’，b’1)，以此递推可以得到所有测量点在起点坐标系中的坐标值。空间曲线型管孔对于空间三维曲线管孔，我们可以通过所使用的管道机器人预测行走步距Δt，由曲率传感器计算当前点的曲率半径R以及当前点和前一测量点密切平面之间的夹角θ。
如图5所示建立局部坐标系，平面圆弧段OO’在X’O’Y’平面内(密切平面)，已知前一测量点坐标系XOYZ，XOY为密切平面，要求在XOYZ坐标系中O’点的坐标，首先对XOYZ坐标系进行坐标轴的旋转，其中Y轴与Y’轴的夹角为α，Z轴与Z’轴的夹角为当前测量点与前一测量点的密切平面夹角θ， θ为曲率传感器提供的相邻测量点密切平面间的夹角)。
旋转方法1)绕Y轴顺时针旋转θ角TY旋转； 2)绕Z轴逆时针旋转α角TZ旋转。 这样将空间三维问题转化为平面二维问题，在旋转后的坐标系中求出o′点的坐标(a，b，0，1)，然后再将(a，b，0，1)点的坐标进行逆旋转变换到XOYZ坐标系中，旋转方法1)绕Z轴顺时针旋转α角，旋转矩阵为TZ旋转-1；2)绕Y轴逆时针旋转θ角，旋转矩阵为TY旋转-1。得到旋转点的坐标为(a’，b’，c’，1)。这样就求出了在起点坐标系中当前测量圆弧段终点的坐标。
＝[X，Y，Z，1]·TY旋转·TZ旋转·T平移为下一测量点的初始坐标系。其中， 在下一个测量坐标系中，X’O’Y’Z’为起点坐标系，通过上面的方法可以得到在X’O’Y’Z’坐标系中下一曲线圆弧段终点的坐标(a，b，c，1，)通过坐标变换[a′，b′，c′，1]＝[a，b，c，1]·TZ旋转-1·TY旋转-1·T平移-1可以求得下一曲线段终点在XOY坐标系中的坐标(a’，b’，c’，1)，以此递推可以得到所有测量点在起点坐标系中的坐标值。
权利要求
1.一种微细管道机器人全程定位方法，其特征在于给定起始点的空间位置、曲率和切矢量方向，通过微弯曲率传感器提供的当前测量点的曲率和相邻密切平面间的夹角，实现微细管道机器人的全程定位。
2.根据权利要求1所述的一种微细管道机器人全程定位方法，它的步骤为1)给定起始点的空间坐标、曲率、切矢量方向；2)通过微弯曲率传感器得到下一测量位置点的曲率和相邻密切平面间的夹角；3)求得当前测量点的空间位置和切矢量方向；4)在下一个测量位置时，前一测量点的空间位置和切矢量方向已知，求得当前点的空间位置和切矢量方向；5)逐级递推得到微细管道机器人的全程精确定位。
全文摘要
本发明公开了一种微细管道机器人全程定位方法。它是给定起始点的空间位置、曲率和切矢量方向，通过微弯曲率传感器提供的当前测量点的曲率和相邻密切平面间的夹角，实现微细管道机器人的全程定位。本发明主要是为了实现将截面测量和轴心轨迹(轴心线)联系起来，使得测量不仅可以检查管道的内部形貌，而且能够实现对管道内壁表面的三维重建。
文档编号B25J9/10GK1392029SQ0213627
公开日2003年1月22日 申请日期2002年7月25日 优先权日2002年7月25日
发明者柯映林, 刘刚, 李江雄 申请人:浙江大学