CN106003031A - 无线传输三核十轴履带式高速天然气管道机器人控制系统 - Google Patents

Abstract

本发明公开了一种无线传输三核十轴履带式高速天然气管道机器人控制系统，所述的控制器采用三核控制器，包括ARM、FPGA和DSP，所述的ARM、FPGA和DSP通过无线装置进行通讯连接。通过上述方式，本发明提供的无线传输三核十轴履带式高速天然气管道机器人控制系统，自主研发了基于ARM+FPGA+DSP的全新三核控制模式。控制器以ARM为处理器核心，由FPGA实现八轴永磁同步电机和两轴直流电机的伺服控制，DSP实现图像采集数字信号的实时处理并与ARM通讯，把ARM从复杂的工作当中解脱出来，实现八轴三相永磁同步电机的实时位置采集，并响应DSP中断，实现数据通信和存储实时信号。

Description

无线传输三核十轴履带式高速天然气管道机器人控制系统

技术领域

本发明涉及大型管道机器人的领域，尤其涉及一种无线传输三核十轴履带式高速天然气管道机器人控制系统。

背景技术

天然气管道的输送介质属于易燃、易爆物质，介质中含有的硫化氢、 二氧化碳、游离水、粉尘等杂质，使敷设的管道处于内外腐蚀条件，甚至有的时候内部会产生阻塞情况发生。再加上环境、地质、气象和水文灾害、管材及设计缺陷、操作失误乃至人为破坏等因素，管道的安全受到众多因素的威胁。

1989年6月4日，前苏联的一条输气管道发生泄漏，两列对开火车在距离泄漏点1 公里外的铁路线上通过时，火车磨擦产生火花引起了泄漏的天然气体爆炸，造成600多人死亡，数百公顷森林烧毁；2000年8月，美国新墨西哥州东南部一条720mm口径的输气管道发生天然气爆炸，引起连天大火并至少造成 10人死亡，在30多公里以外的地方都可以看见巨型火球冲上天空，爆炸后地面留下一道长25m、深6m的大坑；我国的石油天然气管道亦曾多次发生事故，管道发生爆破、泄漏、停输等事故不仅造成巨大财产损失，而且也危害到生态环境。

管道机器人是一种可以沿着管道内部或者是外部自动行走，携带一种或多种传感器和操作机械，在操作人员的控制下或者是计算机自动控制下完成一系列管道作业的机电一体化系统。管道机器人的研究开始于上个世纪四十年代，到了70年代由于微电子技术、计算机技术、自动化技术的发展和进步，国外管道机器人技术于90年代初得到了迅猛发展，研制了许多实验样机，并取得了大量的研究成果。

用管道机器人巡检天然气管道，不仅可以提高管道检测的效率，而且对于改善劳动条件，减轻劳动强度，提高作业效率，降低作业成本，保障人身安全都有着十分重要的意义。但是国内还没有采用管道机器人巡检天然气管道，天然气管道爆炸时有发生，造成了巨大的经济损失和环境污染。

一个实用的天然气管道机器人必须具备以下几个部分：

1）图像采集系统：图像采集系统可以发现管道内已经出现的问题，并可以为工作人员提供管道受损和阻塞情况，为更换管道或者是清理管道提供可靠依据；

2）损伤采集系统：损伤采集系统可以及时发现管道系统外壁出现的异常情况，避免了管道而长期破损导致抗压能力减弱，最终导致天然气大量泄露而产生爆炸事故发生；

3）湿度探测和阻塞物探测：如果湿度过大，天然气管道的输送介质易于形成管道腐蚀，同时部分物质会堆积起来产生阻塞情况；

4）电机：执行电机是管道机器人的动力实施部件，它实时转化电源的能量，根据管道机器人微处理器的指令来执行机器人在天然气管道内的相关行走动作；

5）算法：算法是天然气管道机器人的灵魂，由于天然气管道是一个封闭的管道，内部情况非常复杂，天然气管道机器人必须采用一定的智能算法才能准确的从管道内一点到达另外一点，形成点对点的巡检，并实时存储采集图像、管道水汽信息、管道阻塞信息、管道受损情况和受损位置信息；

6）微处理器：微处理器是天然气管道机器人的核心部分，是天然气管道机器人的大脑；管道内所有的信息，包括管道内的湿度、阻塞情况、管道损伤信息以及损伤位置信息、电机状态信息、电池状态信息等都需要经过微处理器处理并做出相应的判断；

7）无线装置：为了能够及时发现和处理问题，全自动管道机器人必须采用无线装置，实时传输巡检的图像采集和损伤采集结果，并能通过总站二次确定问题所在。

国内对管道机器人的研究只是刚刚起步，均是采用单核控制器，均处于实验室样机设计阶段，离大规模使用具有一定的距离，主要面临以下问题：

（1）受控制技术影响，所有的管道机器人均采用单核控制器，控制器的计算能力较弱，管道机器人无法快速处理实时环境，且机器人行走速度较低，巡查管道速度较慢，且稳定性较差；

（2）对于采用电机驱动的管道机器人所携带的能源均采用可充电蓄电池，这些蓄电池均是通过简单的串联和并联后形成高压大电流能源系统，均没有保护电路，寿命较短，正常工作时经常出现异常甚至干扰到管道机器人的工作；

（3）对于采用步进电机或者是直流电机驱动的管道机器人来说，受电机自身效率的影响，能源利用率较低，导致机器人在管道内移动距离较短；

（4）对于采用步进电机或者是直流电机驱动的管道机器人来说，受电机功率密度的影响，由于所采用的电机体积均较大，最终导致机器人的体积较大，重量较重，严重影响了管道机器人的使用范围；

（5）无论是基于矢量控制或者是基于转子磁场定向控制算法永磁同步电机的伺服控制，除了要进行多次的坐标变换与反变换外，还要进行电流及速度的闭环控制，因而实现比较复杂且实时性要求较高；采用DSP技术或者是ARM技术并以软件方式来实现，系统开发周期比较长，而且该算法占用的处理器时间比较多，影响了DSP或者是ARM的处理功能；采用专用运动控制芯片虽然可以减少处理器处理时间，但是其内部的PID调节只能满足单一的要求，无法满足管道机器人在复杂环境中的应用；

（6）管道机器人运动状态自我调整能力较差，受控制方式影响，机器人在管道内的姿态参数识别较差，机器人无法识别自我平面与管道主平面的夹角，机器人无法实时根据外围环境调整自己的PID参数，导致机器人行走时出现倾斜，甚至有时会翻车，导致任务失败；

（7）对于有阻塞的天然气管道，普通的轮式机器人与地面接触面积较少，越障能力较弱，有的时候甚至无法越过障碍物，最终无法巡检完成任务；

（8）目前大部分管道机器人，对管道中的环境识别较差，均是通过分析存储的采集图像来分析管道情况，实时情况辨别较差；

（9）对于采用六轮动力驱动的管道机器人来说，机器人的动力调整能力较两轮、四轮动力驱动有所提高，可以满足管道机器人简单工况下的加速和爬坡功能，但是遇到具有一定坡度的管道或者是大型障碍物时，需求功率较大，六轮动力就显示出动力不足的弱点出来，使得系统动态性能降低；

（10）由于三轴加速度计通过二次积分才能得到机器人行走时形成的偏角，三轴陀螺仪经过一次积分才能得到机器人行走时形成的偏角，由于积分的存在使得管道机器人在密闭管道中惯性导航时的位置有时候会出现一定错误。

永磁同步电动机的定子与普通电励磁同步电动机具有相同的定子结构，只是转子上以钕铁硼稀土永磁材料作为磁极取代了同步机的励磁磁极和励磁绕组，使电动机的结构较为简单，且省去了容易出故障的集电环和电刷，实现了无刷化，提高了电动机运行的可靠性。因为不需励磁电流，因此可以省去励磁绕组的铜耗，极大提高了电机的效率；稀土永磁材料的使用使得电机功率密度较高，所以电机的体积可以做得更小，适合体积要求比较高的场合。永磁同步电动机除了有明显的节能效果外，还具有转速准确、噪声低的特性，稀土永磁同步电机基于转子磁场定向或者是基于矢量控制系统能够实现高精度、高动态性能、大范围的调速或定位控制，这些特性使得稀土永磁同步电机特别适合使用于管道机器人这些要求比较特殊的机器人控制系统中。

履带式移动机构是轮式移动机构的拓展，履带本身起着给车轮连续铺路的作用。相对于轮式机构，履带式移动机构有着诸多优点，如：支承面积大，接地比压小；滚动阻力小，通过性能较好；越野机动性好；履带支承面上有履齿，不易打滑，牵引附着性能好，有利于发挥较大的牵引力；变位履带式移动机构通过改变履带的位置或履带的机构形式以达到适应不同环境的要求，两履带的夹角可以调节，以适应不同的作业管径。

发明内容

本发明主要解决的技术问题是提供一种无线传输三核十轴履带式高速天然气管道机器人控制系统，自主研发了基于ARM+ FPGA+DSP的全新三核控制模式。，控制器以ARM为处理器核心，由FPGA实现八轴永磁同步电机和两轴直流电机的伺服控制，DSP实现图像采集数字信号的实时处理并与ARM通讯，把ARM从复杂的工作当中解脱出来，实现八轴三相永磁同步电机的实时位置采集，并响应DSP中断，实现数据通信和存储实时信号。

为解决上述技术问题，本发明采用的一个技术方案是：提供了一种无线传输三核十轴履带式高速天然气管道机器人控制系统，包括电池、控制器、永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J、永磁同步电机K、直流电机A、直流电机B、基于CCD图像采集单元、图像存储单元、湿度采集单元、基于霍尔效应管道探伤采集单元、无线装置以及管道机器人，所述的电池单独提供电流驱动所述的控制器，所述的控制器采用三核控制器，包括ARM、FPGA和DSP，所述的ARM、FPGA和DSP通过无线装置进行通讯连接，所述的基于CCD图像采集单元和图像存储单元均与DSP和FPGA通讯连接，所述的湿度采集单元和基于霍尔效应管道探伤采集单元均与ARM和FPGA通讯连接，所述的ARM和FPGA分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号分别控制所述的永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K的信号合成之后再控制管道机器人的运动，所述的DSP和FPGA分别发出第七控制信号和第八控制信号，由所述的第七控制信号和第八控制信号分别控制所述的直流电机A和直流电机B的信号合成之后与基于CCD图像采集单元通讯连接。

在本发明一个较佳实施例中，所述的电池采用锂离子电池。

在本发明一个较佳实施例中，所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号均为PWM波控制信号。

在本发明一个较佳实施例中，所述的ARM采用STM32F746；所述的FPGA采用QUICKLOGIC；所述的DSP采用TMS320F2812。

在本发明一个较佳实施例中，所述的管道机器人包括机器人壳体、前方激光位移传感器、磁导航传感器、左岔口传感器、右岔口传感器、数字磁罗盘、三轴加速度计、三轴陀螺仪以及同步带，所述的前方激光位移传感器分别安装在机器人壳体的前端，所述的左岔口传感器和右岔口传感器分别位于前方激光位移传感器下方的左右两端，所述的同步带分别设置在机器人壳体的左右两侧边并分别与永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K连接，所述的磁导航传感器、数字磁罗盘、三轴加速度计和三轴陀螺仪分别依次设置在机器人壳体上并位于永磁同步电机X和永磁同步电机Y之间，所述的直流电机A和直流电机B均设置在机器人壳体上并分别位于左岔口传感器和右岔口传感器的上方，所述的无线装置位于机器人壳体的后端。

在本发明一个较佳实施例中，所述的激光位移传感器包括前方激光位移传感器、左激光位移传感器和右激光位移传感器，所述的前方激光位移传感器设置在机器人壳体正前方的中间位置，所述的左激光位移传感器和右激光位移传感器分别斜向设置在机器人壳体正前方的左右两端。

在本发明一个较佳实施例中，所述的同步带采用八轴八轮驱动模式，是由一根内周表面设有等间距齿的封闭环形履带和相应的带轮所组成。

在本发明一个较佳实施例中，所述的高速天然气管道机器人控制系统还设置有上位机程序、基于ARM主运动控制程序、基于DSP从运动控制程序、基于霍尔效应管道损伤探测以及无线传输，所述的上位机程序还包括管道读取、位置定位和电源信息，所述的基于ARM主运动控制程序还包括基于FPGA八轴永磁同步电机伺服控制、数据存储和I/O控制，所述的基于DSP从运动控制程序还包括基于FPGA两轴直流电机伺服控制和基于DSP图像采集，所述的基于霍尔效应管道损伤探测和无线传输分别与基于霍尔效应管道探伤采集单元和无线装置通讯连接。

在本发明一个较佳实施例中，所述的高速天然气管道机器人控制系统还包括光电编码器，所述的光电编码器分别安装在永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J、永磁同步电机K、直流电机A和直流电机B上。

本发明的有益效果是：本发明的无线传输三核十轴履带式高速天然气管道机器人控制系统，为了提高能源的利用率和减少机器人体积，本系统用效率和功率密度均较高的永磁同步电机替代了步进电机、直流电机等电机；为了提高系统加速要求，系统为这些特种工况加入了两个功率较小的永磁同步电机起到加速助力作用，增加系统动力特性；为了提高系统一般爬坡要求，系统为这些特种工况又加入了两个功率较小的永磁同步电机起到爬坡助力作用，增加系统动力特性；为了提高系统跨越大型障碍物的要求，系统为这些特种工况再次加入了两个功率较小的永磁同步电机起到越障助力作用，增加系统动力特性；为了提高运算速度，保证自动管道机器人系统的稳定性和可靠性，本发明在基于ARM的控制器中引入FPGA和数字信号处理器DSP，形成基于ARM+ FPGA+DSP的全新三核控制器，此控制器充分考虑电池在这个系统的作用，把控制系统中工作量最大的八轴永磁同步电机和两轴直流电机伺服系统交给FPGA完成、电池监控、路径读取、偏差处理等交给ARM处理，充分发挥ARM数据处理速度相对较快的特点，而图像数据采集和存储等功能交给DSP完成，这样就实现了ARM、FPGA与DSP的分工，同时三者之间也可以进行通讯，实时进行数据交换和调用。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图，其中：

图1 是本发明带有无线传输三核十轴履带式高速天然气管道机器人控制系统一较佳实施例的原理图；

图2是管道机器人二维结构示意图；

图3是图1的程序图；

图4是管道机器人巡检原理图。

具体实施方式

下面将对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

如图1所示，本发明实施例包括：

一种无线传输三核十轴履带式高速天然气管道机器人控制系统，包括电池、控制器、永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J、永磁同步电机K、直流电机A、直流电机B、基于CCD图像采集单元、图像存储单元、湿度采集单元、基于霍尔效应管道探伤采集单元、无线装置以及管道机器人，所述的电池单独提供电流驱动所述的控制器，所述的控制器采用三核控制器，包括ARM、FPGA和DSP，所述的ARM、FPGA和DSP通过无线装置进行通讯连接，所述的基于CCD图像采集单元和图像存储单元均与DSP和FPGA通讯连接，所述的湿度采集单元和基于霍尔效应管道探伤采集单元均与ARM和FPGA通讯连接，所述的ARM和FPGA分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号分别控制所述的永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K的信号合成之后再控制管道机器人的运动，所述的DSP和FPGA分别发出第七控制信号和第八控制信号，由所述的第七控制信号和第八控制信号分别控制所述的直流电机A和直流电机B的信号合成之后与基于CCD图像采集单元通讯连接。其中，所述的电池采用锂离子电池；所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号均为PWM波控制信号。

上述中，所述的ARM采用STM32F746；所述的FPGA采用QUICKLOGIC；所述的DSP采用TMS320F2812。

STMicroelectronics 所生产的全新STM32F7 MCU系列产品，是全球第一个量产且拥有32位元ARM Cortex-M7处理器的微控制器。Cortex-M7是Cortex-M系列产品中最新推出且效能最高的处理器核心，全新STM32F7 MCU是ST的STM32 MCU系列产品中效能最高的一项产品，结合了Cortex-M7核心与高阶外围装置，可提升应用程序效能、新增新功能、延长电池寿命、确保安全以及尽可能减少使用外部元件以节省成本与空间等无可比拟的优点。

STM32F7系列产品包括STM32F745 及STM32F746，这两项产品都配备拥有浮点运算单位及DSP扩充功能的Cortex-M7核心，运算速度最高 216MHz。STM32F7 MCU系列产品将ARM Cortex-M7效能超越早期核心(譬如Cortex-M4)的优势运用到极致，效能达到将近DSP 两倍。

FPGA采用了逻辑单元阵列LCA（Logic Cell Array）这样一个新概念，内部包括可配置逻辑模块CLB（Configurable Logic Block）、输出输入模块IOB（Input Output Block）和内部连线（Interconnect）三个部分。FPGA的基本特点主要有：采用FPGA设计ASIC电路，用户不需要投片生产，就能得到合用的芯片；FPGA可做其它全定制或半定制ASIC电路的中试样片；FPGA内部有丰富的触发器和I/O引脚；FPGA是ASIC电路中设计周期最短、开发费用最低、风险最小的器件之一；FPGA采用高速CHMOS工艺，功耗低，可以与CMOS、TTL电平兼容。上述特点使得用户可以根据自己的设计需要，通过特定的布局布线工具对FPGA内部进行重新组合连接，在最短的时间内设计出自己的专用集成电路，这样就减小成本、缩短开发周期。由于FPGA采用软件化的设计思想实现硬件电路的设计，这样就使得基于FPGA设计的系统具有良好的可复用和修改性。这种全新的设计思想已经逐渐应用在高性能的交流驱动控制上，并快速发展。上述特点使得FPGA特别适合伺服运动控制中，特别是对于本发明采用的八轴天然气管道巡检伺服控制结构，可以大大减少STM32F7控制器的伺服程序编写。

TMS320F2812是基于代码兼容的C28x内核的新型高性能32位定点数字信号处理器，C28x内核的指令执行周期达到了6.67ns，最高运行频率可以达到150MHz，F2812集成有许多外设，提供了整套的片上系统，其片上外设主要包括2×8路12位ADC(最快80ns转换时间)，2路SCI，1路SPI，1路McBSP，1路eCAN接口等，并带有两个事件管理模块(EVA、EVB)。另外，该器件还有3个独立的32位CPU定时器，以及多达56个独立编程的GPIO引脚。F2812采用统一编址方式，芯片内部有18K的SARAM，包括MO、M1、L0、L1、H0共5个存储块，各存储块保持独立，可以在同一机器周期对不同的RAM块进行访问，从而减少流水线时延。而且F2812内部有128K字的FLASH，地址空间3D8000h～3F7FFFh，适用于低功耗、高性能的控制系统。此外F2812提供了外部存储器扩展接口(XINTF)，方便进行系统扩展，其寻址空间可以达到1MB；上述特点使得F2812在具备数字信号处理器卓越的数据处理能力的同时，又具有适于控制的片内外设及接口，可广泛应用于各种高性能的系统控制中，上述特点使得TMS320F2812特别适合巡检机器人的图形采集、图像存储和位置信息存储。

为了能够准确采集管道内部损伤信息，本控制器为CCD图像采集系统加入了基于直流电机A和直流电机B的定位伺服系统，当DSP通过存储图像比较发现疑似区域后，由主站通过无线控制直流电机A调整CCD图像采集系统升降的高度，然后由直流电机B调整CCD图像采集系统旋转的角度，使得CCD的中心对准疑似物体的中心，然后由主站实时分析疑似区域，及时发现和处理这一区域，DSP实时存储该区域采集到的图像和位置信息，为管道机器人巡检管道结果分析提供判断依据。

为了能够精确导引管道机器人进行自动检测，本发明采用两套传感器导航模式（一套磁导航传感器导航，一套激光位移传感器导航），本发明的管道机器人二维结构如图2所示：所述的管道机器人包括机器人壳体K、前方激光位移传感器、磁导航传感器ME1、左岔口传感器ME2、右岔口传感器ME3、数字磁罗盘M1、三轴加速度计A1、三轴陀螺仪G1以及同步带T，所述的前方激光位移传感器分别安装在机器人壳体K的前端，所述的左岔口传感器ME2和右岔口传感器ME3分别位于前方激光位移传感器下方的左右两端，所述的同步带T分别设置在机器人壳体K的左右两侧边并分别与永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K连接，所述的磁导航传感器ME1、数字磁罗盘M1、三轴加速度计A1和三轴陀螺仪G1分别依次设置在机器人壳体K上并位于永磁同步电机X和永磁同步电机Y之间，所述的直流电机A和直流电机B均设置在机器人壳体K上并分别位于左岔口传感器ME2和右岔口传感器ME3的上方，所述的无线传感器I位于机器人壳体K的后端。其中，所述的激光位移传感器包括前方激光位移传感器LSF、左激光位移传感器LSL和右激光位移传感器LSR，所述的前方激光位移传感器LSF设置在机器人壳体K正前方的中间位置，所述的左激光位移传感器LSL和右激光位移传感器LSR分别斜向设置在机器人壳体K正前方的左右两端。

磁导航传感器ME1时刻检测管道中的磁条，并依此传感器为第一导航判据，当磁条不存在或导航偏离距离较大时，左前方激光位移传感器传感器LSL和右前方激光位移传感器传感器LSR共同作用判断前进方向与左右管壁的距离，并作为管道机器人直线运动提供导航依据，前方激光位移传感器传感器LSF为管道机器人前进障碍物的判别提供依据以及停车做判据。左岔口磁传感器ME2和右岔口传感器ME3分别检测地面岔口标志，然后分别作为交叉管道左转或者是右转的判据，并在此位置可以对天然气管道机器人进行精确补偿，这对于管道机器人巡检管道计算位置至关重要。

数字磁罗盘M1是利用地磁场固有的指向性测量空间姿态角度的，它可以测量载体三维姿态数据：水平航向、俯仰、横滚，可以广泛地用于需要获取平台（或载体）姿态角度的场合，数字磁罗盘M1具有体积小、航向精度高、倾斜范围宽、频响高、低功耗的优点，很适合用于既对航向精度有较高要求同时又对功耗、体积有限制的场合。为了提高天然气管道机器人在密闭管道行走过程中导航的稳定性，实现姿态的自动调整以及自主导航能力，并减少加速计A1和三轴陀螺仪G1长期积分带来的误差，本发明在天然气管道机器人伺服硬件系统中采用三轴加速度计A1+三轴陀螺仪G1+数字磁罗盘M1的惯性导航系统。在管道机器人行走管道期间全程开启三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1，三轴加速度计用来测量管道机器人三个前进方向的角加速度，三轴陀螺仪G1用来测量管道机器人三个前进方向的角速度，数字磁罗盘M1用来测量管道机器人空间姿态角度。当管道机器人的姿态发生变化超过设定阀值时，在一个新的采样周期控制器就立即对其位置补偿，避免管道机器人在行走过程中因为倾斜过大而翻到现象的发生，提高了其快速行走导航时的稳定性；管道机器人在密闭管道中可以不依赖于任何外部信息就能够得到其在导航坐标系中的加速度、速度、偏航角和位置等信息，所产生的导航信息连续性好而且噪声非常低，极大增强了管道机器人的自主惯性导航能力。当管道机器人读到交叉管道入口时，管道机器人要实现左转90或者是右转90的动作，在这种情况下，三轴加速度计和三轴陀螺仪配合数字磁罗盘可以准确计算出机器人旋转的角度，保证了其转弯的准确性。当管道机器人处于爬坡巡检时，三轴加速度计A1和三轴陀螺仪G1配合数字磁罗盘M1可以精确测量爬坡角度，ARM根据这一角度可以精确计算出爬坡所需功率，保证管道机器人能够按照指定速度完成巡检管道任务。当管道机器人处于爬越障碍物巡检时，三轴加速度计A1和三轴陀螺仪G1配合数字磁罗盘M1可以精确测量出爬越障碍物的角度，ARM根据这一角度可以精确计算出爬越所需功率，保证管道机器人能够按照指定速度完成爬越障碍物的任务。

同步带T传动是由一根内周表面设有等间距齿的封闭环形胶带和相应的带轮所组成。运动时，带齿与带轮的齿槽相啮合传递运动和动力，是一种啮合传动，因而具有齿轮传动、链传动和平带传动的各种优点。同步带T传动具有准确的传动比，无滑差，可获得恒定的速比，可精密传动，传动平稳，能吸震，噪音小，传动效率高，不需润滑，无污染，特别适合在不允许有污染和工作环境较为恶劣的场合下正常工作，结构紧凑特别适用多轴传动，因此本发明采用同步带技术形成八轴八轮驱动模式。

本发明为了解决国内管道机器人存在的问题，研发了一种由八台稀土永磁同步电机差速驱动的八轮无线传输履带式三核天然气管道机器人，八台稀土永磁同步电机的伺服控制算法由FPGA完成，增加系统计算的快速性，其中两个功率较大的永磁同步电机X和永磁同步电机Y为管道机器人正常速度巡检时提供能量需求，另外六个功率较小的永磁同步电机功率相等，其中的两只小功率永磁同步电机Z和永磁同步电机R配合两只较大永磁同步电机X和永磁同步电机Y在管道机器人加速巡检时提供动力，两只小功率电机U和电机W配合永磁同步电机X、永磁同步电机Y、永磁同步电机U和电机W在管道机器人爬坡或者是爬越小型障碍物时提供动力，两只小功率电机J和永磁同步电机K配合永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U和永磁同步电机W在管道机器人大型跨越障碍物或者是需求功率较大时提供动力，并且各个电机的功率通过控制器实现能量利用率最优，左右侧的多个轮分别通过变位履带结构机械链接，基于两轴直流电机的伺服系统为CCD图像采集系统提供定位功能，天然气管道机器人依靠其携带传感器进行巡检大型天然气管道。

本发明在吸收国外先进控制思想的前提下，自主研发了基于ARM+ FPGA+DSP的全新三核控制模式。本次设计的控制器原理图如图1：控制器以ARM为处理器核心，由FPGA实现八轴永磁同步电机和两轴直流电机的伺服控制，DSP实现图像采集数字信号的实时处理并与ARM通讯，把ARM从复杂的工作当中解脱出来，实现八轴三相永磁同步电机的实时位置采集，并响应DSP中断，实现数据通信和存储实时信号。

如图3所示，所述的高速天然气管道机器人控制系统还设置有上位机程序、基于ARM主运动控制程序、基于DSP从运动控制程序、基于霍尔效应管道损伤探测以及无线传输，所述的上位机程序还包括管道读取、位置定位和电源信息，所述的基于ARM主运动控制程序还包括基于FPGA八轴永磁同步电机伺服控制、数据存储和I/O控制，所述的基于DSP从运动控制程序还包括基于FPGA两轴直流电机伺服控制和基于DSP图像采集，所述的基于霍尔效应管道损伤探测和无线传输分别与基于霍尔效应管道探伤采集单元和无线装置通讯连接。

为达上述目的，本发明采取以下技术方案，为了提高能源的利用率和减少机器人体积，本系统用效率和功率密度均较高的永磁同步电机替代了步进电机、直流电机等电机；为了提高系统加速要求，系统为这些特种工况加入了两个功率较小的永磁同步电机起到加速助力作用，增加系统动力特性；为了提高系统一般爬坡要求，系统为这些特种工况又加入了两个功率较小的永磁同步电机起到爬坡助力作用，增加系统动力特性；为了提高系统跨越大型障碍物的要求，系统为这些特种工况再次加入了两个功率较小的永磁同步电机起到越障助力作用，增加系统动力特性；为了提高运算速度，保证自动管道机器人系统的稳定性和可靠性，本发明在基于ARM的控制器中引入FPGA和数字信号处理器DSP，形成基于ARM+ FPGA+DSP的全新三核控制器，此控制器充分考虑电池在这个系统的作用，把控制系统中工作量最大的八轴永磁同步电机和两轴直流电机伺服系统交给FPGA完成、电池监控、路径读取、偏差处理等交给ARM处理，充分发挥ARM数据处理速度相对较快的特点，而图像数据采集和存储等功能交给DSP完成，这样就实现了ARM、FPGA与DSP的分工，同时三者之间也可以进行通讯，实时进行数据交换和调用。

如图4所示，对于本文设计的ARM+ FPGA+DSP三核控制器，在电源打开状态下，ARM控制器先对管道机器人电池的SOC（荷电状态）进行判断，如果电池能源较低，控制器会发出报警信号；如果电池能源较高，先由主站通过无线装置把巡检天然气管道长度和半径等信息输入给ARM，然后自动引导管道机器人到管道检测口，管道机器人先进入自锁状态，等待入口阀门F1打开，当前方激光位移传感器LSF确定阀门打开后，管道机器人进入待检缓冲区域，然后入口阀门F1关闭，入口阀门F2打开，管道机器人进入管道检测区域；管道机器人携带的图像采集系统、湿度采集系统和管道损伤探测装置、无线装置均开启，管道机器人按照设定速度沿着巡检路线快速巡检，ARM把磁导航传感器ME1参数输入到FPGA，FPGA把这些磁导航传感器ME1参数转化为管道机器人在指定巡检轨迹下永磁同步电机X、永磁同步电机Y、永磁同步电机Z和永磁同步电机R的PWM波，实现四台永磁同步电机的实时伺服控制；DSP通过FPGA实现两轴直流伺服定位系统使CCD实时有效采集管道信息并存储，如果DSP对巡检某一个位置有疑问，将通过无线向主站发出中断请求并与ARM通讯，并通过无线装置传输疑似区域图像，由无线控制ARM发出停车指令使管道机器人停车，然后主站通过无线装置实现CCD二次图像采集和损伤探测，并由DSP实时记录和存储。如果管道机器人完成左转或者是右转巡检辅助管道再次返回主管道时，ARM将通过FPGA开通助力永磁同步电机Z和永磁同步电机R，FPGA按照磁导航传感器ME1参数和速度要求二次调整管道机器人永磁同步电机X、永磁同步电机Y、永磁同步电机Z和永磁同步电机R的PWM输出，实现四台永磁同步电机的实时伺服控制，为了减少能源消耗，在返回主通道前DSP将关闭CCD的信息采集；如果管道机器人在巡检过程，三轴加速度计、三轴陀螺仪和数字磁罗盘检测到管道机器人处于爬坡巡检管道，ARM将通过FPGA开通助力永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W，按照磁导航传感器ME1参数和速度要求再次调整管道机器人永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U和永磁同步电机W的PWM输出，实现六台永磁同步电机的实时伺服控制；如果管道机器人在巡检过程中要攀越障碍物时，三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1检测出管道机器人处于攀越角度，ARM将通过FPGA开通助力永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机J和永磁同步电机K，FPGA按照磁导航传感器ME1参数和速度要求调整管道机器人永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K的PWM输出，实现八台永磁同步电机的实时伺服控制。

参照图1，图2，图3和图4，其具体的功能实现如下：

1）管道机器人电源打开后，ARM会对电池SOC进行判断，如果电池SOC较低时，ARM将禁止FPGA工作，八台永磁同步电机PWM波被封锁，同时报警传感器将工作并发出报警信号，ARM通过无线装置向总站发出更换电源中断请求；如果电池SOC正常，管道机器人进入待工作状态，等待工作命令；

2）总站通过无线装置把天气然管道长度、半径和管道地形图等信息、已经上次检录信息传输给ARM，然后无线装置引导管道机器人到管道检测的开始端，为了精确导航管道机器人在封闭管道中的行走，ARM首先开启管道机器人基于三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的惯性导航模式；

3）管道机器人ARM开始通过磁导航传感器ME1读取地面导航磁条，磁导航传感器ME1的反馈值与实际设定中心值比较，ARM把此偏差输入给FPGA，FPGA把这个偏差参数转化为管道机器人在指定巡检轨迹下永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U和永磁同步电机W要运行的距离、速度和加速度，FPGA然后再结合电机的电流反馈、光电编码器反馈、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，经内部伺服控制算法得到六台永磁同步电机控制的PWM波控制信号，使管道机器人沿着导航磁条快速前进，根据外围环境FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；在管道机器人前进过程中前方激光位移传感器LSF将工作，ARM实时检测管道机器人与前方入口阀门F1的距离D，在可靠停车范围内ARM通过FPGA让管道机器人自动停车，然后原地自锁；

4）当前方激光位移传感器LSF检测到入口阀门F1打开时，管道机器人将开启自动巡航模式，控制器ARM将实时记录管道机器人沿着磁条运动的距离，当确定管道机器人完全进入待检区域后，入口阀门F1将再次关闭，天然气泄露装置检测到入口阀门F1完全关闭后，入口阀门F2将打开，此时前方激光位移传感器LSF将二次判断前方入口阀门F2的状态，确定前方阀门打开无误后，管道机器人开始进入巡检区域开始检测天然气管道内部实际情况；

5）管道机器人进入管道检测区域后，主站通过无线控制装置开通基于DSP控制的CCD图像采集系统，DSP同时启动两轴直流伺服定位系统；ARM先根据磁导航传感器ME1的反馈，读取管道机器人实际的位置信息、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的数值，然后与设定位置相比较，确定管道机器人偏离中心距离和倾斜的角度，ARM此偏差信号输入给FPGA，FPGA把这个偏差参数转化为管道机器人在指定巡检轨迹下永磁同步电机X和永磁同步电机Y要运行的距离、速度和加速度，FPGA然后再结合电机的电流反馈、光电编码器反馈、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，经内部伺服控制算法得到两台永磁同步电机控制的PWM波控制信号，并通过驱动电路实时调整管道机器人姿态，使管道机器人稳定运行在磁条中心附近，如果总站通过反馈的采集图像发现管道机器人较大幅度偏离了导航轨道，将通过无线控制装置优先校正管道机器人的姿态，同时根据外围环境，FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；ARM并实时记录机器人已经运行的直线距离，距离修正传感器S时刻检测地面修正标志，一旦读取到修正装置，ARM记录的位置距离信息要以修正标志的位置信息为准，消除A1机器人在行走时的所造成的位置误差；

6）管道机器人进入检测区域后，如果ARM通过三轴陀螺仪A1的反馈发现管道机器人处于爬坡巡检状态，为了保证管道机器人能够按照指定速度完成巡检管道任务，按照系统速度和加速度要求，ARM首先计算出爬坡所需功率，FPGA结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整管道机器人永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U和永磁同步电机W的PWM控制信号，使管道机器人满足常速巡检管道要求；在爬坡探测过程中，ARM根据磁导航传感器ME11的反馈，读取管道机器人实际的位置信息、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的数值，并与设定位置相比较，确定管道机器人偏离中心距离和倾斜的角度，FPGA再结合电机电流反馈、光电编码器反馈、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，根据其内部三闭环伺服控制程序得到六台永磁同步电机的PWM波控制信号，并通过驱动电路实时调整管道机器人姿态，使管道机器人稳定运行在磁条中心附近，如果总站通过反馈的采集图像发现管道机器人较大幅度偏离了导航轨道，将通过无线控制装置优先校正管道机器人的姿态；ARM并实时记录管道机器人已经运行的直线距离，距离修正传感器S时刻检测地面修正标志，一旦读取到修正装置，ARM记录的位置距离信息要以修正标志的位置信息为准，消除管道机器人在行走时的所造成的位置误差，当ARM通过三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1发现管道机器人已经完成爬坡动作进入正常巡检环境，FPGA控制器封锁永磁同步电机Z、永磁同步电机R、永磁同步电机U和永磁同步电机W的PWM控制信号，使系统进入永磁同步电机X和永磁同步电机Y驱动状态；

7）管道机器人进入正常检测区域后，如果管道机器人在主管道巡检过程中左岔口传感器ME2读到地面转弯标志，ARM首先根据地面装置修正管道机器人在管道中的位置信息，消除管道机器人行走误差；FPGA按照系统速度和加速度要求，结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整管道机器人永磁同步电机X和永磁同步电机Y的PWM控制信号，使管道机器人在距离R内停车，然后FPGA结合三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈使管道机器人原地旋转左90度，管道机器人进入左岔道进行巡航；在左岔道探测过程中，前方激光位移传感器LSF时刻开启并检测疑似堵塞物并检测与前方终点的距离；ARM根据前方激光位移传感器LSF的反馈实时记录前进距离，并在距离岔道终点前有效范围内使管道机器人有效停车，管道机器人在三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1控制下原地旋转180度准备返回主通道；由于左岔道已经巡检完毕，为了使管道机器人快速返回到主管道中，FPGA开启助力永磁同步电机Z和永磁同步电机R，使系统进入加速返回状态；在整个返回过程中，FPGA按照系统速度和加速度要求，结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整管道机器人永磁同步电机X、永磁同步电机Y、电永磁同步电机Z和电永磁同步电机R的PWM控制信号，使管道机器人能够加速沿着地面磁条回到主管道；进入正常巡检环境后，FPGA封锁永磁同步电机Z、永磁同步电机R的PWM控制信号，使系统进入永磁同步电机X和永磁同步电机Y驱动状态；

8）管道机器人进入正常检测区域后，如果管道机器人在主管道巡检过程中岔口右传感器ME3读到地面转弯标志，ARM首先根据地面装置修正管道机器人在管道中的位置信息，消除管道机器人行走误差；FPGA按照系统速度和加速度要求，结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整管道机器人永磁同步电机X和永磁同步电机Y的PWM控制信号，使管道机器人在距离R内停车，然后FPGA结合三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈使管道机器人原地旋转右90度，管道机器人进入右岔道进行巡航；在右岔道探测过程中，前方激光位移传感器LSF时刻开启并检测疑似堵塞物并检测与前方终点的距离；ARM根据前方激光位移传感器LSF的反馈实时记录前进距离，并在距离岔道终点前有效范围内使管道机器人有效停车，管道机器人在三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1控制下原地旋转180度准备返回主通道；由于右岔道已经巡检完毕，为了使机器人快速返回到主管道中，FPGA开启助力永磁同步电机Z和永磁同步电机R，使系统进入加速返回状态；在整个返回过程中，FPGA按照系统速度和加速度要求，然后结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整管道机器人永磁同步电机X、永磁同步电机Y、永磁同步电机Z和永磁同步电机R的PWM控制信号，使管道机器人能够加速沿着地面磁条回到主管道；进入正常巡检环境后，FPGA封锁永磁同步电机Z、永磁同步电机R的PWM控制信号，使系统进入永磁同步电机X和永磁同步电机Y驱动状态；

9）管道机器人进入正常检测区域后，如果管道机器人在主管道巡检过程中左岔口传感器ME2和右岔口传感器ME3同时读到地面转弯标志，说明管道机器人进入了十字管道交叉口，ARM首先根据地面装置修正管道机器人在管道中的位置信息，消除管道机器人行走误差；FPGA按照系统速度和加速度要求，然后结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整管道机器人永磁同步电机X和永磁同步电机Y的PWM控制信号，使管道机器人进入右岔道进行巡航，ARM根据前方激光位移传感器LSF的反馈实时记录前进距离，并在距离岔道终点前有效范围内使机器人有效停车，机器人在三轴加速度计、三轴陀螺仪和数字磁罗盘控制下原地旋转180度返回主通道，FPGA开启助力永磁同步电机Z和永磁同步电机R，使系统进入四轮加速返回状态；当右激光位移传感器LSR和左激光位移传感器LSL的反馈出现较大幅值变化时，说明管道机器人已经进入十字口位置，此时ARM开始修正管道机器人行走的误差，保证管道机器人位置距离信息正确，当右激光位移传感器LSR和左激光位移传感器LSL的反馈再次出现较大幅值变化时，说明管道机器人已经进入左岔道口管道，此时ARM开始修正管道机器人行走的误差，保证管道机器人位置距离信息正确，FPGA按照系统速度和加速度要求，结合电机电流、光电编码器、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，实时调整机器人永磁同步电机X和永磁同步电机Y的PWM控制信号，使管道机器人进入左岔道进行巡航，ARM根据前方激光位移传感器LSF的反馈实时记录前进距离，并在距离岔道终点前有效范围内使管道机器人有效停车，机器人在三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1控制下原地旋转180度返回主通道，FPGA开启助力永磁同步电机Z和永磁同步电机R，使系统进入四轮加速返回状态；当右激光位移传感器LSR和左激光位移传感器LSL的反馈出现较大幅值变化时，说明管道机器人已经返回十字口位置，ARM开始修正管道机器人行走的误差，保证管道机器人位置距离信息正确，控制器让管道机器人行走一段距离后稳稳停在十字路口，然后在三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1控制下左转90度重新回到主管道检测区域；

10）如果管道机器人在正常运动过程中受到外界干扰或者是遇到磁条断裂，磁导航传感器ME1无法读取到地面磁条信息，ARM将通过无线控制装置与总站通讯，右激光位移传感器LSR和左激光位移传感器LSL将开启工作，二者将测得的距离输入给ARM控制器，控制器然后与设定值相比较得到偏离中心的位置，ARM将此偏差信号输入给FPGA，FPGA把这个偏差参数转化为管道机器人在指定巡检轨迹下永磁同步电机X和永磁同步电机Y要运行的距离、速度和加速度，FPGA然后再结合电机的电流反馈、光电编码器反馈、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，经内部伺服控制算法得到两台永磁同步电机控制的PWM波控制信号，并通过驱动电路实时调整管道机器人姿态，使管道机器人稳定运行在管道平面中心附近，根据外围环境FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；ARM实时记录机器人已经运行的直线距离，距离修正传感器S时刻检测地面修正标志，一旦读取到修正装置，ARM记录位置距离信息要以修正标志的位置信息为准，如果管道机器人在行走过程中误读了地面修正标志，主站将通过无线传输装置与ARM通讯，并自动修改管道机器人的位置信息，消除管道机器人行走时的位置误差；

11）在管道机器人运动过程中，图像采集中的CCD时刻开启，DSP并实时存储CCD采集到的图像，DSP把采集的图像与设定的标准管道信息进行比对，如果二者比对结果出现较大误差，为了防止误操作，DSP立即通过无线装置向主站发出中断请求，并与ARM通讯， ARM立即响应DSP中断，并通过FPGA让管道机器人停车，DSP通过FPGA内部三闭环直流伺服控制程序调整直流电机A和直流电机B的输出定位CCD位置，使CCD图像采集中心对准疑似图像中心，CCD二次采集管道信息并通过无线传输装置实时传输，主站实时分析该疑似区域，DSP二次存储该图像，疑似处理完毕后无线控制重新开启管道机器人，FPGA开启永磁同步电机X、永磁同步电机Y、永磁同步电机Z和永磁同步电机R使管道机器人处于加速阶段，当达到设定速度后FPGA封锁永磁同步电机Z和永磁同步电机R的PWM控制信号，使管道机器人处于永磁同步电机X和永磁同步电机Y的两轴驱动状态并沿着管道导航标志继续前行；

12）在管道机器人运动过程中，湿度采集系统中的湿度传感器时刻开启，ARM实时存储湿度传感器采集到的湿度信息，并与设定的标准管道湿度信息进行比对，如果二者比对结果出现较大误差，为了防止误操作，ARM立即通过无线装置向主站发出中断请求，并通过FPGA内部伺服控制程序主动调整两台永磁同步电机X和永磁同步电机Y的PWM波控制信号，降低管道机器人的速度使其慢速通过有疑问的区域，根据外围环境FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；同时ARM立即向DSP发出中断请求，DSP立即响应ARM中断，通过FPGA内部三闭环直流伺服控制程序调整直流电机A和直流电机B的输出使其定位CCD位置，使CCD图像采集中心对准疑似水图像中心，并加大CCD管道采集信息中液态水的比对，DSP存储该区域的水汽疑似图像和实际位置信息，并通过无线传输装置实时传输，主站实时分析该疑似区域，确认无误后，DSP二次存储该图像和位置信息，主站通过无线装置重启ARM，控制管道机器人继续前行，当通过疑似区域后，ARM与FPGA通讯，FPGA开启永磁同步电机X、永磁同步电机Y、永磁同步电机Z和永磁同步电机R使机器人处于加速阶段，当达到设定速度后FPGA控制器封锁永磁同步电机Z和永磁同步电机R的PWM控制信号，使管道机器人处于永磁同步电机X和永磁同步电机Y的两轴驱动状态并沿着管道导航标志继续前行；

13）在管道机器人运动过程中，前方激光位移传感器LSF时刻开启，ARM实时处理前方位置信息，当在管道运行前方有异常物时，前方激光位移传感器LSF探测值将出现异常，ARM与FPGA通讯，并通过FPGA内部伺服控制程序主动调整两台永磁同步电机X和永磁同步电机Y的PWM波控制信号，降低管道机器人的速度使其慢速驶向障碍物，根据外围环境FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；同时ARM立即向总站和DSP发出中断请求，DSP立即响应ARM中断，并通过FPGA内部三闭环直流伺服控制程序调整直流电机A和直流电机B的输出使其定位CCD位置，使CCD图像采集中心对准疑似阻塞物图像中心，CCD二次采集管道信息并通过无线传输装置实时传输，主站实时分析该疑似区域，DSP存储该区域的堵塞疑似图像和实际位置信息，由于本次设计的管道机器人是多轮履带式结构，所以ARM控制管道机器人可以通过越过障碍物并可继续前行；ARM根据图像反馈结果计算出越障需求的功率，然后管道机器人通过FPGA同时开启永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K，同时右激光位移传感器LSR和左激光位移传感器LSL开启为机器人跨越障碍物位置定位提供导航依据，当通过疑似区域后，FPGA关闭永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K的驱动信号，使管道机器人处于永磁同步电机X和永磁同步电机Y的两轴驱动状态并沿着管道导航标志继续前行；

14）在管道机器人运动过程中，基于霍尔效应的管道探伤传感器将工作，当管道运行前方探测值出现异常，ARM与FPGA通讯，并通过FPGA内部伺服控制程序主动调整两台永磁同步电机X和永磁同步电机Y的PWM波控制信号，降低管道机器人的速度使其慢速驶向管道损伤疑似区域，根据外围环境FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；同时ARM立即向总站和DSP发出中断请求，DSP立即响应ARM中断，并通过FPGA内部三闭环直流伺服控制程序调整直流电机A和直流电机B的输出定位CCD位置，使CCD图像采集中心对准疑似损伤管道部位，CCD二次采集管道信息并通过无线传输装置实时传输，主站实时分析该疑似区域，DSP存储发现疑似管道损伤图像，如未发现管道损伤图像，DSP将记录疑似损伤实际位置信息，并标记外部损伤。当通过疑似区域后，ARM与FPGA通讯，FPGA控制器开启永磁同步电机X、永磁同步电机Y、永磁同步电机Z和永磁同步电机R使管道机器人处于加速阶段，当达到设定速度后FPGA控制器封锁永磁同步电机Z和永磁同步电机R的PWM控制信号，使管道机器人处于永磁同步电机X和永磁同步电机Y的两轴驱动状态并沿着管道导航标志继续前行；

15）如果管道机器人在正常运动过程中通过ARM通过右激光位移传感器LSR和左激光位移传感器LSL的反馈发现管道机器人处于非等径管道中时，控制器将舍弃磁导航传感器ME1的反馈信息，右激光位移传感器LSR和左激光位移传感器LSL将测得的距离实时输入给ARM，ARM将于FPGA通讯，FPGA控制器然后与设定值相比较得到偏离中心的位置， FPGA根据其内部伺服控制程序得到两台永磁同步电机的PWM波控制信号，并通过驱动电路实时调整管道机器人姿态，使管道机器人稳定运行在管道平面中心附近，根据外围环境FPGA实时调整内部伺服控制程序的PID参数；ARM实时记录管道机器人已经运行的直线距离，距离修正传感器S时刻检测地面修正标志，一旦读取到修正装置，ARM记录位置距离信息要以修正标志的位置信息为准，消除管道机器人行走时的位置误差；

16）在管道机器人运动过程中，ARM会时刻储存所经过的管道所处的位置或者是经过的参考点，FPGA把这个位置参数转化为管道机器人在指定巡检轨迹下永磁同步电机X和永磁同步电机Y要运行的距离、速度和加速度，FPGA然后再结合电机的电流反馈、光电编码器反馈、三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的反馈，经内部伺服控制算法得到两台永磁同步电机控制的PWM波，使管道机器人按照设定速度快速前行，同时根据外围环境，FPGA实时调整内部伺服控制程序的PID参数，使系统轻松实现分段P、PD、PID控制和非线性PID控制；

17）在运动过程中如果管道机器人发现距离求解出现死循环将向ARM发出中断请求，ARM会对中断做第一时间响应并原地自锁，然后向总站发出中断请求；总站通过无线装置重新复位ARM，传输新的位置等信息，ARM根据管道磁条导航标志反馈、右激光位移传感器LSR和左激光位移传感器LSL的反馈，实时调整永磁同步X电机和电机Y的速度，管道机器人从新的位置开始巡检；

18）装在永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J、永磁同步电机K、直流电机A和直流电机B上的光电编码器会输出其位置信号A和位置信号B，光电编码器的位置信号A脉冲和B脉冲逻辑状态每变化一次，ARM（或DSP）内的位置寄存器会根据电机的运行方向加1或者是减1；光电编码器的位置信号A脉冲和B脉冲和Z脉冲同时为低电平时，就产生一个INDEX信号给ARM（或DSP）内部寄存器，记录永磁同步电机的绝对位置，然后换算成管道机器人在管道检测系统中的具体位置；

19）管道机器人在运行过程ARM根据其内部算法实时计算电池SOC，如果控制器发现电源能量较低时，ARM会与DSP通讯，并通过DSP关闭CCD图像采集工作和图像存储工作，并通过FPGA内部三闭环伺服控制系统程序调整永磁同步电机X和永磁同步电机Y的PWM输出，管道机器人在无线控制下以较慢的速度驶向出口处，保证管道机器人能够顺利到出口处；

20）在管道机器人巡检过程中，如果伺服控制器检测到永磁同步电机的转矩出现脉动，由于本发明采用的永磁同步电机控制是基于矢量控制，因此FPGA会很容易补偿此干扰，减少了电机转矩对管道机器人运动过程的影响；

21）当管道机器人驶向出口阀门的过程中，其携带的前方激光位移传感器LSF会时刻检测其与阀门之间的位移，当确定出口阀门F3在打开状态，管道机器人将开启巡航模式，控制器ARM实时记录管道机器人已经沿着磁条运动的距离，当确定管道机器人完全进入出口待检区域后，出口阀门F3将关闭，天然气抽吸装置将抽吸待检区域的天然气情况，当天然气泄露装置未检测到待检区域有天然气残留时，出口阀门F4将打开，此时前方激光位移传感器LSF将二次判断前方出口阀门F4的状态，确定前方阀门打开无误后，管道机器人驶出检测管道，回到检测终点，等待下一个检测命令。

本发明具有的有益效果是：

1：在管道机器人运动过程中，充分考虑了电池在这个系统中的作用，基于ARM+ FPGA+DSP三核控制器时刻都在对其状态进行监测和运算，既避免了由于大电流放电而引起的锂离子电池过度老化现象的发生，又可以有效预测电池的能量，为管道机器人巡检提供了有效保证；

2：由FPGA处理管道机器人八只永磁同步电机基于转子磁场定向的伺服控制以及两轴直流电机的伺服控制，使得控制比较简单，大大提高了运算速度，解决了控制算法占用ARM和DSP运行周期较长的问题，缩短了开发周期短，并且程序可移植能力强；

3：本发明基本实现全贴片元器件材料，实现了单板控制，不仅节省了控制板占用空间，而且有利于管道机器人体积和重量的减轻；

4：本发明的管道机器人导航系统采用永磁同步电机替代了传统管道机器人系统中常用的步进电机、直流电机、直流无刷电机，由于其体积较小，效率较高，使得管道机器人体积可以进一步缩小，能源利用率大大提高；

5：由于永磁同步电机采用矢量控制，使得调速范围比较宽，调速比较平稳，即使在低速阶段电机的脉动转矩也非常小，有利于提高系统的动态性能；

6：由于本控制器采用DSP处理图形采集和存储的大量数据与算法，由FPGA处理八轴永磁同步电机和两轴直流电机的伺服控制，把ARM从繁重的工作量中解脱出来，有效地防止了程序的“跑飞”，抗干扰能力大大增强；

7：在控制中，FPGA可以根据机器人外围运行情况适时调整八轴永磁同步电机和两轴直流电机伺服内部的PID参数，实现分段P、PD、PID控制和非线性PID控制，使系统满足实时工况要求换；

8：管道机器人上配备有湿度采集系统，可以轻易地检测出隧道里的湿度异常区域，可有效查出管道水滴的存在；

9：管道机器人上配备有图像采集系统，可以轻易地检测出管道内部管道腐蚀等异常情况，并有效存储其图像；

10：基于无线图像传输可以使总站工作人员准确判断疑似区域，同时无线遥控技术可以很好的解决管道机器人出现的突发状况，比如求解死机、重启管道机器人等；

11：基于DSP的图像存储功能使得管道机器人完成任务后方便工作人员读取巡检结果，可以轻易的从存储结果中读取管道受损信息和具体位置，然后及时维修；

12：管道机器人上配备有基于霍尔效应的损伤探测采集系统，可以轻易地检测出管道外部管道腐蚀和损伤等异常情况，有利于及早发现问题管道；

13：三轴陀螺仪G1的加入可有效探测管道机器人偏离管道平面的倾斜角度，ARM会时刻对此角度进行监测并通过FPGA相应调整永磁同步电机的PWM波输出，并时刻通讯主站，遇到紧急情况可以由总站接管控制，有效控制了管道机器人的姿态；

14：磁导航传感器和激光位移传感器的加入使得系统导航具有一定的冗余度，极大提高了管道机器人的稳定性，并且使得此管道机器人可以巡检非等径的复杂管道；

15：履带的加入有效增加了管道机器人在管道中接触的面积，使机器人可以有效通过具有障碍阻塞物区域，提高了环境适应性；

16：基于直流电机A和直流电机B的两轴CCD图像采集定位系统可以使图像采集更可靠，为主站进行管道损伤和阻塞信息实时分析提供了可靠依据；

17：地面转弯导航标志配合左右侧激光位移传感器使得系统可以轻松读出管道信息，有利于管道机器人在复杂管道中的定位和位置误差的消除；

18：三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的加入可有效精确测量管道机器人转弯时的角度，为管道机器人在复杂管道中的转弯导航提高了可靠依据；

19：多台助力永磁同步电机的加入使得系统的动力性能调整具有可选择，使得机器人可以满足不同工况下的动力要求，使得机器人的适应能力加强；

20：三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的加入可有效精确探测管道机器人爬坡时的角度，ARM会时刻对此角度进行检测，并有效开启助力电机为管道机器人巡检具有坡度的管道提供动力需求；

21：三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的加入可有效精确探测管道机器人爬越大型障碍物时的角度，ARM会时刻对此角度进行检测，并有效开启多个助力电机为机器人穿越障碍物提供动力需求；

22：三轴加速度计A1、三轴陀螺仪G1和数字磁罗盘M1的加入可有效精确测量管道机器人在直线导航发生的速度和方向偏移，为管道机器人在复杂管道中的惯性导航提高了可靠依据。

综上诉述，本发明的无线传输三核十轴履带式高速天然气管道机器人控制系统，为了提高能源的利用率和减少机器人体积，本系统用效率和功率密度均较高的永磁同步电机替代了步进电机、直流电机等电机；为了提高系统加速要求，系统为这些特种工况加入了两个功率较小的永磁同步电机起到加速助力作用，增加系统动力特性；为了提高系统一般爬坡要求，系统为这些特种工况又加入了两个功率较小的永磁同步电机起到爬坡助力作用，增加系统动力特性；为了提高系统跨越大型障碍物的要求，系统为这些特种工况再次加入了两个功率较小的永磁同步电机起到越障助力作用，增加系统动力特性；为了提高运算速度，保证自动管道机器人系统的稳定性和可靠性，本发明在基于ARM的控制器中引入FPGA和数字信号处理器DSP，形成基于ARM+ FPGA+DSP的全新三核控制器，此控制器充分考虑电池在这个系统的作用，把控制系统中工作量最大的八轴永磁同步电机和两轴直流电机伺服系统交给FPGA完成、电池监控、路径读取、偏差处理等交给ARM处理，充分发挥ARM数据处理速度相对较快的特点，而图像数据采集和存储等功能交给DSP完成，这样就实现了ARM、FPGA与DSP的分工，同时三者之间也可以进行通讯，实时进行数据交换和调用。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (9)

1.一种无线传输三核十轴履带式高速天然气管道机器人控制系统，其特征在于，包括电池、控制器、永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J、永磁同步电机K、直流电机A、直流电机B、基于CCD图像采集单元、图像存储单元、湿度采集单元、基于霍尔效应管道探伤采集单元、无线装置以及管道机器人，所述的电池单独提供电流驱动所述的控制器，所述的控制器采用三核控制器，包括ARM、FPGA和DSP，所述的ARM、FPGA和DSP通过无线装置进行通讯连接，所述的基于CCD图像采集单元和图像存储单元均与DSP和FPGA通讯连接，所述的湿度采集单元和基于霍尔效应管道探伤采集单元均与ARM和FPGA通讯连接，所述的ARM和FPGA分别发出第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号，由所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号分别控制所述的永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K的信号合成之后再控制管道机器人的运动，所述的DSP和FPGA分别发出第七控制信号和第八控制信号，由所述的第七控制信号和第八控制信号分别控制所述的直流电机A和直流电机B的信号合成之后与基于CCD图像采集单元通讯连接。

2.根据权利要求1所述的高速天然气管道机器人控制系统，其特征在于，所述的电池采用锂离子电池。

3.根据权利要求1所述的高速天然气管道机器人控制系统，其特征在于，所述的第一控制信号、第二控制信号、第三控制信号、第四控制信号、第五控制信号、第六控制信号、第九控制信号和第十控制信号均为PWM波控制信号。

4.根据权利要求1所述的高速天然气管道机器人控制系统，其特征在于，所述的ARM采用STM32F746；所述的FPGA采用QUICKLOGIC；所述的DSP采用TMS320F2812。

5.根据权利要求1所述的高速天然气管道机器人控制系统，其特征在于，所述的管道机器人包括机器人壳体、前方激光位移传感器、磁导航传感器、左岔口传感器、右岔口传感器、数字磁罗盘、三轴加速度计、三轴陀螺仪以及同步带，所述的前方激光位移传感器分别安装在机器人壳体的前端，所述的左岔口传感器和右岔口传感器分别位于前方激光位移传感器下方的左右两端，所述的同步带分别设置在机器人壳体的左右两侧边并分别与永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J和永磁同步电机K连接，所述的磁导航传感器、数字磁罗盘、三轴加速度计和三轴陀螺仪分别依次设置在机器人壳体上并位于永磁同步电机X和永磁同步电机Y之间，所述的直流电机A和直流电机B均设置在机器人壳体上并分别位于左岔口传感器和右岔口传感器的上方，所述的无线装置位于机器人壳体的后端。

6.根据权利要求5所述的高速天然气管道机器人控制系统，其特征在于，所述的激光位移传感器包括前方激光位移传感器、左激光位移传感器和右激光位移传感器，所述的前方激光位移传感器设置在机器人壳体正前方的中间位置，所述的左激光位移传感器和右激光位移传感器分别斜向设置在机器人壳体正前方的左右两端。

7.根据权利要求5所述的高速天然气管道机器人控制系统，其特征在于，所述的同步带采用八轴八轮驱动模式，是由一根内周表面设有等间距齿的封闭环形履带和相应的带轮所组成。

8.根据权利要求1所述的高速天然气管道机器人控制系统，其特征在于，所述的高速天然气管道机器人控制系统还设置有上位机程序、基于ARM主运动控制程序、基于DSP从运动控制程序、基于霍尔效应管道损伤探测以及无线传输，所述的上位机程序还包括管道读取、位置定位和电源信息，所述的基于ARM主运动控制程序还包括基于FPGA八轴永磁同步电机伺服控制、数据存储和I/O控制，所述的基于DSP从运动控制程序还包括基于FPGA两轴直流电机伺服控制和基于DSP图像采集，所述的基于霍尔效应管道损伤探测和无线传输分别与基于霍尔效应管道探伤采集单元和无线装置通讯连接。

9.根据权利要求1所述的高速天然气管道机器人控制系统，其特征在于，所述的高速天然气管道机器人控制系统还包括光电编码器，所述的光电编码器分别安装在永磁同步电机X、永磁同步电机Y、永磁同步电机Z、永磁同步电机R、永磁同步电机U、永磁同步电机W、永磁同步电机J、永磁同步电机K、直流电机A和直流电机B上。