一种轨道式巡检机器人导航定位模块及粗精复合定位方法
阅读说明：本技术 一种轨道式巡检机器人导航定位模块及粗精复合定位方法 (Navigation positioning module of rail-mounted inspection robot and coarse-fine composite positioning method ) 是由 胡鹏博 程力 曹庆鹏 林俤 雷磊 刘晴 黄哲 陈浩 何卿 于琪 于 2020-12-28 设计创作，主要内容包括：本发明公开了一种轨道式巡检机器人导航定位模块及粗精复合定位方法,包括传感器模块、组合导航解算模块和电源模块,传感器模块、组合导航解算模块设于机器人本体中,通过控制中心控制机器人本体在管廊轨道中行进,通过传感器模块获取管廊轨道中定位孔信息,通过组合导航解算模块解算,控制中心确定机器人本体在管廊中的位置。本发明无需额外基站和无线设备,只需要在轨道上进行等间隔或不等间隔打孔,并在机器人上安装相应的光电传感器和MEMS惯性传感器即可,减小了使用和维护成本；能够获得机器人自身运动状态监测、轨道变形和管廊沉降数据；能够对轨道和管廊环境变化进行更多数据的监测。(The invention discloses a track type inspection robot navigation positioning module and a coarse and fine composite positioning method, which comprise a sensor module, an integrated navigation resolving module and a power supply module, wherein the sensor module and the integrated navigation resolving module are arranged in a robot body, the robot body is controlled to advance in a pipe gallery track through a control center, positioning hole information in the pipe gallery track is obtained through the sensor module, the integrated navigation resolving module resolves, and the control center determines the position of the robot body in a pipe gallery. According to the invention, no additional base station and wireless equipment are needed, only equal-interval or unequal-interval punching is needed on the track, and the corresponding photoelectric sensor and the MEMS inertial sensor are installed on the robot, so that the use and maintenance cost is reduced; the motion state monitoring, track deformation and pipe gallery settlement data of the robot can be obtained; the monitoring of more data can be carried out to track and piping lane environmental change.)
技术领域
本发明属于导航定位技术领域，涉及一种使用光电标签、MEMS惯性传感器进行组合定位导航的方法。
背景技术
在我国管廊巡检机器人运行过程中，机器人的精确定位始终是核心内容之一，优秀的定位方案可以有效提高巡检效率和精度。
常规的轨道机器人巡检，主要靠里程计进行相对位置定位，由于里程计会产生累计误差，还需靠外部辅助无线标签进行绝对位置校准。常见的校准方式有UWB无线定位方式和RFID标签定位方式，其中UWB定位精度0.1m左右，需要管廊内的无线基站具备UWB定位功能，RFID无源标签定位精度较高，可达0.02m，但需要每隔一段轨道布置一个RFID标签，还需要专门的读卡设备。
传统管廊巡检机器人在行走时需要依靠上述基站或大量RFID标签来进行辅助定位，增加了使用和维护成本，另一方面，传统的管廊巡检机器人也缺乏自身机械运动状态和轨道倾斜沉降的观测数据，因此智能的故障诊断技术也是管廊巡检机器人未来的发展方向。
发明内容
为解决现有技术中存在的上述缺陷，本发明的目的在于提供一种适用于管廊内轨道式巡检机器人的组合导航方法。采用光电/里程计/MEMS惯导组合进行粗精复合定位，可以获得巡检机器人的准确运动位置和姿态信息，在标定时，记录MEMS传感器获取的机器人姿态序列并存储，实际运行过程中，记录一段实时姿态序列，低通滤波后和存储序列进行匹配，从而对巡检机器人进行粗定位，再由光电传感器经过轨道孔位进行精定位，以获取精确的绝对位置信息。最后将改进的传感器结合起来进行光电/里程计/MEMS组合导航。其MEMS传感器陀螺和加计采样数据经过小波分析可获得机器人自身的机械运动状态诊断信息和管廊轨道变形、沉降等信息。
本发明是通过下述技术方案来实现的。
一种轨道式巡检机器人导航定位模块，包括：
传感器模块，通过光电传感器识别管廊轨道中定位孔，通过MEMS惯性传感器敏感机器人本体在轨道上的姿态变化信息，通过里程计获取机器人本体的相对移动信息，将获取的信息传递至组合导航解算模块；
组合导航解算模块，用于对传感器的数据进行解算，数据融合，解算后的导航数据传输至外部控制中心；
电源模块，为机器人本体提供电源供电；
传感器模块、组合导航解算模块设于机器人本体中，通过控制中心控制机器人本体在管廊轨道中行进，通过传感器模块获取管廊轨道中定位孔信息，通过组合导航解算模块解算，控制中心确定机器人本体在管廊中的位置。
对于上述技术方案，本发明还有进一步优选的方案：
优选的，机器人本体包括行走轮、传感器模块、电源模块和组合导航解算模块，传感器模块包括MEMS惯性传感器、里程计和光电传感器，里程计设在行走轮上，行走轮挂在管廊轨道上；管廊轨道上设有定位孔，光电传感器设于机器人本体底座上，位于管廊轨道两侧行走轮之间。
优选的，所述光电传感器包括发光二极管和光电探测器，发光二极管在管廊轨道一侧，光电探测器在管廊轨道另一侧，用于识别管廊轨道上的定位孔。
优选的，组合导航解算模块，包括DSP核心电路、存储器和串口通信模块，光电传感器和里程计分别通过cap1口和cap2口与DSP核心电路连接；MEMS惯性传感器通过扩展串口与DSP核心电路连接；DSP核心电路通过串口连接外电路。
本发明进而提供了一种轨道式巡检机器人导航粗精复合定位方法，包括：MEMS惯性传感器计算的姿态序列粗定位+光电传感器过轨道孔位精定位：
a、轨道打孔：在每段轨道等间隔打孔，进行孔位位置坐标标定；
b、运行标定：
MEMS惯性传感器每隔一段里程计距离ΔL采集当前机器人本体的包括方位、俯仰和滚转的姿态信息(αi,βi,γi)；机器人本体姿态信息被轨道倾斜状态调制的姿态序列存储在机器人存储器中；标定时获得的姿态序列与标定点位置一一对应；
c、姿态序列信息采集
标定完成后，实际运行过程中，通过MEMS惯性传感器等间隔ΔL采集当前位置的方位、俯仰和滚转序列；
d、姿态序列粗匹配定位
由解调的MEMS惯性传感器实时姿态序列与标定时存储的姿态序列匹配进行粗定位，由每段轨道的定位孔光电信号进行精确定位，采用粗精复合定位的方式来校正里程计累计误差，获得机器人本体绝对运动位置信息；
e、光电传感器精匹配定位
标定时每经过一个定位孔，光电传感器输出的脉冲位置信号对应到了姿态序列经低通滤波后的波形中；
在姿态序列粗匹配后的剩余误差ΔL1远小于打孔间隔的情况下，机器人本体获得的一个光电脉冲信号对应的孔位位置，此时完成精匹配定位；
f、位置修正
首次匹配完成后，机器人本体定位出在轨道上的具体位置，运行到下一个定位孔后经过光电探测器、里程计累积输出位置和标定位置对比，得到累积误差ΔLi，到达光电定位孔后将机器人本体绝对位置修正为光电定位孔标定位置，从而消除误差ΔLi的影响。
优选的，对姿态序列粗匹配定位步骤如下：
设存储的经低通滤波的标定姿态俯仰数据为S1，方位数据和横滚数据分别为S2和S3，大小为M×1；机器人本体实时运动俯仰数据位为T1，大小为N×1，方位数据和横滚数据分别为T2和T3，则其俯仰匹配相似度为：
上式中，R1(i)为最新采样俯仰数据匹配到第i个存储标定数据点时的相似度，S1,i(j)为俯仰存储标定数据中第i个存储标定点起，与最新采样数据相对应的第j个点，T1(j)为最新采样数据段的第j个点；
类似，可得到方位相似度R2(i)和横滚相似度R3(i)，整体相似度如下式所示：
其中，i＝1,...,M-N+1，相似度最大位置即为匹配的机器人粗定位位置。
优选的，在机器人本体运行过程中，系统对机器人本体姿态数据(S1,S2,S3)和加计数据(a1,a2,a3)持续采集，并保存到监控中心，监控中心对每次巡检后的机器人本体姿态和加计数据进行小波变换分析，并对比历史分析数据，诊断轨道倾斜、轨道异物、机器人机械故障、机器人本体行走轮磨损状态信息，对轨道和机器人本体进行维护。
本发明由于采取以上技术方案，其具有以下有益效果：
本发明无需额外基站和无线设备，只需要在轨道上进行等间隔或不等间隔打孔，并在机器人上安装相应的光电传感器和MEMS惯性传感器即可，在首次使用时提前对孔位和轨道姿态数据进行提前标定，减小了使用和维护成本。
本发明采用光电/里程计/MEMS组合导航，可获取巡检机器人准确的位置和姿态信息，相比传统机器人只能进行单一定位的方式，能够获得机器人自身运动状态监测、轨道变形和管廊沉降数据。能够对轨道和管廊环境变化进行更多数据的监测。
附图说明
此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，并不构成对本发明的不当限定，在附图中：
图1为组合导航模块组成层级图；
图2为组合导航模块基本硬件组成；
图3为管廊巡检机器人及光电传感器安装位置；
图4为光电传感器示意图；
图5为姿态序列匹配定位过程；
图6为MEMS惯性传感器信号处理过程；
图7为精匹配定位过程图示。
图中：1、管廊轨道；2、光电探测器；3、定位孔；4、发光二极管；5、行走轮；6、MEMS惯性传感器；7、组合导航解算模块；8、电源模块。
具体实施方式
下面将结合附图以及具体实施例来详细说明本发明，在此本发明的示意性实施例以及说明用来解释本发明，但并不作为对本发明的限定。
如图1所示，本发明的轨道式巡检机器人导航定位模块，包括：传感器模块，包括多个传感器，具体包括MEMS惯性传感器、里程计和光电传感器，通过多个传感器识别管廊轨道中定位孔，将获取的信息传递至组合导航解算模块。
组合导航解算模块，主要由DSP核心电路、存储器和串口通信模块，用于对传感器的数据进行解算，数据融合，解算后的导航数据通过串口通信模块RS232口送给外部控制中心，外部控制中心内部存储有管廊地图，可配合导航数据显示车辆位置。
电源模块，主要为设备提供电源供电。可实现5V转3.3V，5V转1.8V，5V转1.3V电平转换。
如图2所示，传感器模块、组合导航解算模块设于机器人本体中，通过控制中心控制机器人本体在管廊轨道中行进，通过传感器模块获取定位孔信息，通过组合导航解算模块解算，控制中心确定机器人本体在管廊中的位置。
如图2所示，本发明的轨道式巡检机器人导航定位模块作为管廊巡检机器人的组成部分，安装于机器人本体上，机器人本体中设有行走轮5，传感器模块、电源模块8和组合导航解算模块7。各传感器模块在机器人中的安装位置如图2所示。里程计设在行走轮5上，行走轮5挂在管廊轨道1上；管廊轨道1上设有定位孔3，光电传感器包括发光二极管4和光电探测器2，光电探测器2和发光二极管4分别设于机器人本体底座上，位于管廊轨道1两侧行走轮5之间。
MEMS惯性传感器6、电源模块8和组合导航解算模块7分别安装于机器人本体底座上，MEMS惯性传感器6用于敏感机器人姿态和位置信息。MEMS惯性传感器包括三轴MEMS陀螺、三轴MEMS加计、磁强计等，可获得导航模块的三轴角速率、加速度、磁航向等信息，传感器信息经过RS232扩展串口送入DSP，经导航解算可得到载体位置、速度、姿态信息。
如图3所示，光电传感器2部分发光二极管在轨道一侧，用于提供光源，光电探测器2在轨道另一侧用于探测穿过轨道孔位的来自发光二极管4的光线。在轨道孔位没有出现时，二极管发出的光线被轨道遮挡，探测器输出低电平信号，当机器人运行，孔位处于发光二极管和光电探测器之间，光电探测器接收到发光二极管光线，输出高电平。巡检机器人连续运行情况下，光电探测器输出脉冲信号。
光电传感器在经过轨道孔位的时候输出一个脉冲信号，并将该信号传递给DSP，DSP会结合里程计信息计算两个脉冲信号(对应轨道上两个孔位)之间的距离，并将一系列的距离信息存入数据库中。
里程计用于计算机器人运动时行走的里程。里程计会随着行走轮5的旋转输出相应的脉冲给DSP，DSP可根据累计脉冲数和行走轮半径计算出巡检机器人行走的里程数。
各模块之间的连接关系如图4所示。电源模块为传感器模块和组合导航解算模块供电，光电传感器信号通过DSP的cap1捕捉口和DSP连接，当巡检机器人本体通过轨道上的孔位时，光电传感器输出的脉冲被DSP接收，产生相应的cap捕捉中断，DSP可以根据存储的标定位置和中断信号对里程计产生的里程进行相对位置校正。里程计通过DSP的cap2捕捉口和DSP连接，随着行走轮旋转，里程计输出的脉冲被DSP捕捉计数，再结合行走轮半径计算机器人本体行走里程。MEMS惯性传感器通过RS232扩展串口与DSP连接，将敏感到的三轴角速率、加速度、磁航向等信息送入DSP进行解算。
下面通过本发明方法来说明本发明的效果。
在机器人本体快速运动，行走轮打滑的情况下，匹配精度将降低，可能造成误匹配等情况，因此改进的方法是采用MEMS惯性传感器计算的姿态序列进行粗定位，再采用光电传感器过轨道定位孔进行精定位的方式。
如图5所示，该方法的操作过程包括：轨道打孔、机器人本体运行标定、姿态序列信息采集、姿态序列粗匹配、光电传感器精匹配和位置修正。
a、轨道打孔
为了便于轨道加工，保持一致性，可在每段轨道固定位置打孔，即等间隔打孔，并提前进行孔位标定。
b、运行标定
MEMS惯性传感器测量巡检机器人本体在运行过程中的姿态(α,β,γ)，包括方位、俯仰和滚转，每隔一段里程计距离ΔL采集当前机器人本体的姿态信息(αi,βi,γi)，由于轨道铺设时不会完全水平，因此机器人本体的姿态信息会被轨道倾斜状态调制，初始全段标定完成后，被调制的机器人本体的姿态序列会存储在机器人存储器中。
c、姿态序列信息采集
标定完成后，开始巡检作业，巡检机器人本体运行过程中，通过MEMS惯性传感器等间隔ΔL采集当前位置的方位、俯仰和滚转序列。
d、姿态序列粗匹配定位
信号处理过程如图6所示，在组合定位过程中，由解调的MEMS惯性传感器信号匹配进行粗定位，由每段轨道的定位孔光电信号进行精确定位，采用这种粗精复合定位的方式来校正里程计累计误差，获得机器人本体绝对运动位置信息。以下对姿态序列粗匹配定位进行详细描述。
设存储的经低通滤波的标定姿态数据S1(俯仰数据)的大小为M×1，方位数据和横滚数据分别为S2和S3，大小也为M×1。最新采样的经低通滤波的机器人实时运动俯仰数据位T1，大小为N×1，最新采样的方位数据和横滚数据分别为T2和T3，以俯仰数据匹配为例。其俯仰匹配相似度可描述为：
上式中，R1(i)为最新采样俯仰数据匹配到第i个存储标定数据点时的相似度，S1,i(j)为俯仰存储标定数据中第i个存储标定点起，与最新采样数据相对应的第j个点，T1(j)为最新采样数据段的第j个点。
用同样的方法可求得方位相似度R2(i)和横滚相似度R3(i)，整体相似度如下式所示：
计算所有的R(i)，i＝1,...,M-N+1，相似度最大位置即为匹配的机器人本体粗定位位置。
e、光电传感器精匹配定位
标定时每经过一个定位孔，光电传感器输出的脉冲位置信号对应到了姿态序列经低通滤波后的波形中。轨道孔位绝对位置初始标定时已经确认。
在姿态序列粗匹配后的剩余误差ΔL1远小于打孔间隔的情况下，粗匹配完成后，巡检机器人本体获得的一个光电脉冲信号对应的孔位位置被唯一确定，此时完成精匹配定位。如图7所示。
f、位置修正
首次匹配完成后，巡检机器人本体定位出在轨道上的具体位置，运行到下一个孔位后经过光电探测，里程计累积输出位置和标定位置对比，会有一个累积误差ΔLi，到达光电孔位后将机器人本体的绝对位置修正为光电定位孔标定位置，从而消除误差ΔLi的影响。
另外，在机器人本体运行过程中，系统会对机器人本体的姿态数据(S1,S2,S3)和加计数据(a1,a2,a3)持续采集，并保存到监控中心，监控中心运维软件会对每次巡检后的机器人姿态和加计数据进行小波变换分析，并对比历史分析数据，可诊断轨道倾斜、轨道异物、机器人本体机械故障、机器人本体行走轮磨损等状态信息，以便及时对轨道和机器人本体进行维护。
本发明的轨道式巡检机器人导航定位模块及方法采用姿态序列+光电粗精复合定位。相对无线定位及RFID标签定位模式，采用伪随机编码进行孔位间距匹配定位，或者采用MEMS惯性传感器信号匹配进行粗定位，采用每段轨道的孔位光电信号进行精确定位的方式，无需额外的辅助标签和基站，节约了成本，可靠性高。集成了MEMS惯性传感器后，可采集机器人本体运动过程中的姿态信息、振动信息和轨道倾斜信息，有利于进行机器人故障诊断和维护。
本发明并不局限于上述实施例，在本发明公开的技术方案的基础上，本领域的技术人员根据所公开的技术内容，不需要创造性的劳动就可以对其中的一些技术特征作出一些替换和变形，这些替换和变形均在本发明的保护范围内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：一种便捷式水文水资源勘测装置