1、2023 年第 5 期仪 表 技 术 与 传 感 器InstrumentTechniqueandSensor2023No 5基金项目:国家自然科学基金(51971013)收稿日期:20221122基于惯性导航的管道探测蛇形机器人定位算法王晓铮1，2，王亚慧1，2，张成林1，2(1 北京建筑大学，机器人仿生与功能研究北京市重点实验室，北京100044;2 北京建筑大学电气与信息工程学院，北京102616)摘要:为满足管道探测蛇形机器人地下定位的精度需求，提出了一种基于扩展卡尔曼滤波(EKF)的双惯导(INS)+编码器里程计融合定位算法。系统建立了管道内部蛇形机器人及其运动轨迹的数学模型，以两套惯
2、导的位置信息为状态量，使用绕线盘+旋转编码器代替传统轮式里程计测得机器人在载体坐标系中每个采样时间的距离增量作为状态方程控制量，以惯导之间的距离作为量测量，融合扩展卡尔曼滤波算法建立了管道探测蛇形机器人双惯导定位模型，克服了单惯导定位误差累计的缺点。经实验验证:双惯导+地表编码器里程计融合定位方法能够有效提高蛇形探测机器人在地下管道内部定位的精度，且在不同运行条件下及惯导系统不同安装距离时均有效，具有较高的实用价值。关键词:管道探测蛇形机器人;融合定位算法;扩展卡尔曼滤波;惯性导航;编码器里程计;误差补偿中图分类号:TP242文献标识码:A文章编号:10021841(2023)05008406
3、Snake obot Localization Algorithm for Pipeline Detection Basedon Inertial NavigationWANG Xiao-zheng1，2，WANG Ya-hui1，2，ZHANG Cheng-lin1，2(1 Beijing University of Civil Engineering and Architecture，Beijing Key Laboratory of obotics Bionic and Functional esearch，Beijing 100044，China;2 College of Electr
4、ical and Information Engineering，Beijing University of CivilEngineering and Architecture，Beijing 102616，China)Abstract:To meet the demand for underground localization accuracy of pipeline detection snake robots，a dualINS with en-coder odometry fusion localization method based on EKF was proposed The
5、 system established a mathematical model for thesnake robots and its motion trajectory inside the pipeline The dualINS position information was used as the state quantities Thedistance increment of the robots at each sample time in the carrier coordinate system was measured as the state equation con
6、trolquantities by using a winding disk and a rotary encoder instead of a conventional wheel odometer The distance between two INSwas as the measurement quantities Fusion EKF algorithm was used to build a dualINS localization model for pipeline detectionsnake robots，which overcame the accumulated dis
7、advantage of single INS localization error It is experimentally verified that thefusion localization method of dualINS and surface encoder odometer can effectively improve the accuracy of the snake robots lo-calization inside an underground pipeline And it is effective under different operation cond
8、itions and at different installation dis-tances of the INS，which has high practical valueKeywords:pipeline detection snake robot;fusion localization algorithm;EKF;inertial navigation;encoder odometry;error compen-sation0引言管道作为燃气运输的主要方式，具有安全性高、运输方便等优点。然而随着全球燃气规模不断扩大，地下管线日益增多，异常复杂，难以进行日常巡检和维护，特别是管道内部的
9、探测1。为了预防管道事故，天津大学开发了一种高精度数据采集和监控系统，目前已应用于管线泄漏检查2;邓蕊等3、邢利辉等4 设计了一种管道探测蛇形机器人，将检测到的管道内部环境信息上传至上位机，以便工作人员确定是否进行管道维护或开挖。因此对于管道内机器人的定位是至关重要的。目前应用于管道无损检测中的定位技术有探地雷达法、电磁法和磁强计法等，然而这些方法的定位精度往往会受到外部环境影响。基于惯性器件的定位导航技术是一种完全自主式定位，具有短时间内定第 5 期王晓铮等:基于惯性导航的管道探测蛇形机器人定位算法85位精度高且不受环境影响的优点5。张成林等6 为检测燃气管道内部设计了一种基于离散卡尔曼滤波
10、算法的高精度捷联惯性定位系统，其定位误差最大不超过 4 7%。然而惯性器件的定位误差会随着时间的推移不断累积，因而在实际应用中大多采用与其他传感器相结合的组合定位导航方式对惯导数据进行修正7。刘鸿宇8 以微惯性测量单元与里程轮相结合完成地下管道机器人定位，有效提高了管线定位精度，但在工作过程中管道内壁往往存在焊缝或积存杂物等致使里程轮出现打滑甚至脱离与管道接触，从而影响测速导致定位误差增加。向泓铭9 为进行管道内部机器人的精准定位，融合视觉与惯性定位，解决了惯导误差积累问题。但是融合视觉定位依赖于信标建立，对于长直管道等无法建立信标的环境，难以利用周围环境来判断位置信息。此外，一些研究人员通过
11、滤波算法来减小惯性导航的定位误差。PZhang10 减少了由惯导系统和里程计安装位置以及比例因子引起的误差。Y Huang11 提出了一种用于里程计辅助惯性导航系统的卡尔曼滤波方法。实验结果表明，所提出的方法可以更好地估计从当前坐标系到初始坐标系的姿态矩阵。为保证定位精度，减小误差积累，并解决上述文献中存在的实际问题，本文提出了一种基于扩展卡尔曼滤波算法的双惯导+地表编码器里程计融合定位算法。系统以两套惯导的距离为约束，结合地表编码器里程计测得蛇形机器人位移增量信息，再以扩展卡尔曼滤波算法建立管道探测蛇形机器人定位系统模型，以补偿单惯导定位随时间产生的累积误差，从而提高了管道探测蛇形机器人的导
12、航定位精度，达到高精度导航定位的目的。1系统定位原理系统硬件单元连接如图1 所示。燃气管道探测蛇形机器人以行波运动在管道内部行走，惯导系统安装在“蛇身”，通过有线电缆将惯导解算信息传输到上位机监控中心，另外在信号传输线上加装一根尼龙绳并行。编码器里程计由地表上位于管道闸井入口处的绕线盘和旋转编码器组成，将尼龙绳缠绕于绕线盘上，随着蛇形机器人在管道内部的移动带动绕线盘转动，利用安装在绕线盘上的旋转编码器即可获得蛇形机器人在管道内部运行速度及位移增量。根据两套惯导系统输出的姿态角解算出蛇形机器人从载体坐标系到参考坐标系的方向余弦矩阵。同时通过地表上编码器里程计解算出蛇形机器人在图 1系统硬件连接单
13、元载体坐标系下的速度和位移增量，再利用方向余弦矩阵将其转换到参考坐标系下，并累计到两套惯导系统的初始位置上，获得双惯导系统在参考坐标系下的位置。将惯导系统初始位置作为状态量，编码器测得机器人位移增量作为控制量，惯导间的距离作为量测量引入扩展卡尔曼滤波器，经过扩展卡尔曼滤波获得两套惯导系统的位置，从而输出地下管道内部蛇形机器人位姿信息。融合算法定位原理如图 2 所示。图 2融合定位算法原理2双惯导+编码器里程计融合定位算法模型2 1坐标系建立建立管内蛇形机器人本体的数学定位模型12，并建立坐标系，如图3 所示。参考坐标系为东北天坐标系(OXnYnZn)，Xn指向北向，Yn指向东向，Zn垂直于Xn
14、轴和 Yn轴指向天向。载体坐标系(OXbYbZb)以蛇形机器人机身几何中心为坐标原点 O，其中 Xb为机器人的轴向方向，其与管道的轴向方向一致，Yb垂直于管道轴向，Zb为垂直方向。k 为惯导系统采样时间。为了分析蛇形机器人位置的变化，可以将蛇形机器人本体在不同坐标下的运动状态简化为向量 OF?，简化模型如图4 所示。以蛇形机器人机身上表面的几何中心为坐标原点 O，建立蛇形机器人的载体坐标系(OXbYbZb)。则向量 OF?在参考坐标系 On 下可以86Instrument Technique and SensorMay 2023图 3管道内蛇形探测机器人的数学模型及运动轨迹表示为OnFn?=r
15、XnrYn0T，在载体坐标系 Ob 下可以表示为 ObFb?=rXbrYb0T。图 4蛇形机器人本体简化模型假设 Ob 绕负 z 轴旋转 度角，坐标系定义为 rXb，rYb，rZb，表示如下:rXb=rXncos+rYnsinrYb=rXnsin+rYncosrZb=rZn(1)得到其变换矩阵如下:Cbn=cossin0sincos0001(2)同理，围绕 X 旋转 角，围绕 Y 轴旋转 角得到的变换矩阵如下:Cbn=1000cossin0sincos(3)Cbn=cos0sin010sin0cos(4)因此，得到从参考坐标系 On 到载体坐标系 Ob 的坐标变换矩阵 Cbn，方程如下所示13
16、:Cbn=CbnCbnCbn=coscos+sinsinsincossin+sincossinsincossincoscoscossinsincoscossinsinsinsincoscossincoscos(5)蛇形机器人载体坐标系到参考坐标系的姿态 Cnb矩阵可以表示为Cnb=(Cbn)1=(Cbn)T(6)将惯导系统的角度信息输入到式(6)中，便可以描述蛇形机器人姿态信息。2 2扩展卡尔曼滤波模型在单位采样周期内，地表上编码器里程计测量蛇形机器人在管道内部移动的距离增量为 S，则第 i 套惯导系统(i=1，2)在载体坐标系下的位移增 Sbi表示如下:Sbi=0sbiY0T(7)转换为参考坐标系下的位移增量 Sni如下:Sni=CnbSbi=Cnb SnXiSnYiSnZiT(8)整理式(8)得到机器人本体上两套惯导系统在参考坐标系下的位移增量 Sni如下:Sni=SnXiSnYiSnZi=sinicosicosicosisinis(9)则惯导系统在参考坐标系下的参考位置如下:Pni(k)=Ni(k)Ei(k)Ui(k)=Ni(k1)Ei(k1)Ui(k1)+Sni(k)(10)式
