基于STM32的轨道式巡检机器人控制系统的设计

张申毅，樊绍胜，程嘉翊，黄 辉

(1.长沙理工大学电气与信息工程学院，湖南长沙 410114；2.湖南省中南桥梁安装工程有限公司，湖南怀化 418000)

0 引言

城市发展面临地下供电、通信、给排水、供气等管道交错复杂、维护不便等困难。 地下综合管廊集成了电力、通信、燃气、给排水等各种工程管道。地下综合管廊运维压力大，地下综合管廊空间狭长、管线密布，传统的人工检查效率低、无法发现各种细微故障；综合管廊往往温度较高、湿度较大，有时还会积聚毒害气体，作业环境比较恶劣；除此之外，人工巡检效率较低，巡检间隔较长，难以获知地下管线和相关设备的运行状况。

鉴于地下综合管廊人工巡检的缺陷与不足，使用轨道式机器人代替人工进行日常检查成为研究热点。因此，本文设计了一种基于STM32的轨道式巡检机器人控制系统，该控制系统具有安全性能好、定位精度高、响应速度快等优点。

1 机器人控制系统总体设计

轨道式巡检机器人的结构示意图如图1和图2所示：机器人主要由3部分组成： 顶部的行走部分，中部的控制部分和底部的工作部分。

图1 轨道式巡检机器人侧面示意图

图2 轨道式巡检机器人正面示意图

轨道式巡检机器人的控制系统总体架构如图3所示。该系统包括STM32微处理器及其最小系统、电源、CAN总线通信、光耦隔离、AD采样、485总线通信以及以太网通信等模块。主控模块主要完成的功能为：与上位机监控界面进行信息交互，接收、解析上位机监控系统发出的指令并分发至对应的任务线程执行相应操作；收集并整理各传感器数据、电机运行数据和机器人位置数据并实时发送至上位机。感知单元主要包括温度传感器、可燃性气体模组、超声波雷达和电流传感器组成；定位模块主要包括500线3通道增量式里程计、磁性开关、RFID收发器。电源模块包括48 V锂电池、24 V/12 V/5 V 3个电压等级的直流降压模块、无线充电模块接收端。

图3 轨道式机器人总体框图

2 系统硬件设计

轨道式巡检机器人控制系统硬件部分主要分为微处理器以及信号调理电路设计[1]。

2.1 微处理器

微处理器采用STM32F407ZGT6单片机[2]。该型微处理器具有32位高性能ARM Cortex-M4处理器和强大的时钟系统，其频率可达168 MHz；拥有丰富的I/O口资源。在本设计中主要用到的I/O口有：(1)5个AD采样端口，分别对机器人前/后超声波雷达、锂电池电压、充放电回路电流进行采样与处理；(2)8个逻辑输出端口，主要控制行走电机、关节电机与前/后LED探照灯、无线充电等设备固态继电器的通断；(3)2个485总线通信接口，分别与RFID收发器和气体传感器模组通信；(4)1个CAN总线通信接口，向行走电机、二自由度机械臂关节电机进行通信。

2.2 AD采样电路

由于微处理器I/O口只允许输入最高3.3 V电压，故需要对机器人各传感器输出信号进行分压与隔离采样。图4为0～5 V隔离采样电路，该电路由分压电路与电压跟随器组成[3]，为保证在输入较低电压的同时电压跟随电路也能准确跟随，同时考虑简化电路设计而尽量避免采用双电源供电，故选取AD806搭建电压跟随电路，该芯片采用5～24 V单电源供电，能够轨至轨输出，具有低噪声，高共模抑制比等优点。

图4 电压隔离采样电路

R801与R802构成分压电路，取R801=10 kΩ、R802=15 kΩ，分压公式为

(1)

经分压电路后，0～5 V输入信号将线性转换为0～3 V。分压电阻均采用高精密低温漂贴片电阻以确保电压采样的精确度。电压跟随器对分压后的信号进行隔离，其中D801为3.3 V齐纳二极管，用于过压保护，防止因AD采样电路输入电压过大或因干扰产生的过电压导致微处理器损坏。光耦隔离电路如图5所示。

图5 光耦隔离电路

2.3 RS485通信电路

轨道式巡检机器人配置2路RS485与气体传感器模组和RFID收发器进行通信，RS485通信电路如图6所示。本设计中采用致RS3485HT模块，该模块具有自动收发数据功能，集成电源隔离、电气隔离于一体，隔离耐压达2 500 VDC，包含发送引脚及接收引脚，无需控制引脚即可实现自动换向功能。 为了防止485总线出现瞬时过压，485模块A、B和RGND引脚均增加了TVS管D301、D302和D303进行过压保护。

图6 RS485通信电路

3 系统软件设计

轨道式巡检机器人控制系统软件包括下位机底层控制软件和上位机监控软件设计两部分[4]。

3.1 下位机底层控制程序软件设计

3.1.1 控制程序软件设计

FreeRTOS是一个可裁剪、可剥夺型的多任务内核。FreeRTOS提供实时操作系统所需的所有功能，包括资源管理，同步，任务通信等[5]。在STM32F407ZGT6主控制板中搭载FreeRTOS实时操作系统可以基本上满足系统需要，根据轨道式巡检机器人各个模块的作业任务的不同可相应地在主控程序中创建多个线程分别进行处理，合理配置各个线程的优先级顺序。

机器人主控软件设计包括一个主线程及实时操作系统所创建的通信、感知与定位3个功能子线程。主控制板程序流程如图7所示。

图7 主程序流程图

机器人程序启动后，先初始化各模块，然后创建3个子线程分别为负责TCP通信、机器人感知和机器人定位。TCP通信子线程与上位机建立并保持通信，接收并解析上位机指令，再加载相应的功能子线程；感知子线程采集监测各传感器数据，并在给定周期内向上位机上传数据包；定位子线程通过CAN总线向行走电机，并通过RFID收发器与磁性开关对机器人进行精确定位，到达指定检测点后向机械臂关节电机下达角度运动命令对周围管线或设备进行拍照、录像、数据采集等操作。

3.1.2 机器人巡检程序软件设计

机器人在开机之后即开始对运动模块、感知模块与定位模块等进行初始化操作。在未接收到上位机下达的指令时，机器人处于休眠状态，只保持TCP通信子线程处于工作状态。当接收到巡检指令后，机器人唤醒感知与定位子程序并开始巡检作业。机器人按照给定的目标点行进，行进过程中若感知到周围出现异常参数时，则立刻上传异常参数与当前所在位置。若上位机下达继续巡检指令，则机器人继续向目标巡检点行进，达到巡检点后再次上传各参数与图片。机器人巡检程序流程如图8所示。

图8 机器人巡检程序流程图

3.2 上位机监控软件

轨道式巡检机器人的上位机测控软件在Visual Studio 2017软件中使用QT开发框架编写，并根据巡检作业要求制定上位机软件界面。上位机监控软件界面如图9所示：上位机窗口最右侧为机器人操作区域，从上到下分别为启停控制区(启动、停止、重启、充电)、行走电机控制区(前进、后退、停止)、机械臂控制区(旋转、复位)、双目云台控制区(复位、拍照、录视频)；界面中间为机器人实时位置、实时速度显示区与双目云台视频显示区；上位机窗口最左侧分别为机器人状态显示区与定位设定区。

图9 主机监控软件界面图

4 精准定位控制器设计

传统的轨道式巡检机器人在定位方式上采用单独里程计或配合RFID收发器进行定位。采用单独里程计进行定位时，由于增量式里程计存在累积误差，随着机器人的不断行进必然会导致定位误差不断累积，因此该定位方法精度较低；采用RFID收发器的定位方式时，虽然RFID定位准确度较高但由于机器人巡检距离较长导致RFID标签的布置成本较高。综合考虑后，从里程计累积误差的校正以及模糊控制器的设计2个角度提高机器人的定位精度与响应速度。

4.1 速度-位置双模式匹配运动控制方法

本文提出了一种速度-位置双模式匹配运动控制方法用于校正里程计的累积误差。速度-位置双模式匹配运动控制方法示意图如图10所示。

图10 速度-位置双模式匹配运动控制方法示意图

在轨道式机器人悬挂的工字轨道上每间隔1 m打入1颗定位螺钉，机器人搭载的磁性开关每靠近定位螺钉则产生一次触发信号，微处理器通过磁性开关的触发信号对里程计数据进行清零以消除累积误差。在管廊的重要巡检点布置RFID标签以提高机器人定位系统的可靠性。

当机器人获知上位机控制系统下达的目标行驶里程后即对目标里程进行速度-位置模式匹配：将目标里程分为速度模式下行进与位置模式下行进2个区间，并依靠磁性开关的触发信号判断速度-位置行进区间的切换点。当机器人行走至给定目标点前一个螺钉时，微处理器通过向行走电机发送停止指令再切换为位置模式下行走剩余里程。速度-位置双模式匹配运动控制方法流程图如图11所示。

图11 速度-位置双模式匹配运动控制方法流程图

4.2 模糊控制器设计

机器人定位精度除了与增量式里程计的累积误差有关外，还与当前行进速度密切关联。进行精确定位时若行进速度过快，由于机械结构及惯性作用导致机器人在停止过程中出现打滑；若行进速度过慢，机器人行进至目标点所需时间过长，实用性无法满足。因此，设计单输入单输出形式的模糊控制器实现行进速度跟随当前位置距离目标点的误差进行相应调整。

4.2.1 输入输出变量的确定

设机器人当前位置与目标位置的里程误差为，作为模糊控制器输入量[6]。里程误差e的基本域是[-20 m,20 m],模糊域选择为[-3，3]。取机器人在速度-位置双模式匹配运动控制过程中速度模式区间下的行进速度为v，作为模糊控制器输出量。取速度v的基本论域为[-80 cm/s,80 cm/s],模糊论域选择为[-6,6]。

取输入量e的模糊子集为{负远程，负中程，负近程，零，正近程，正中程，正远程}，用符号简记为{NF，NM，NN，ZO，PN，PM，PF}。取输出变量γ的模糊子集为{负快速，负中速，负慢速，零，正慢速，正中速，正快速}，用符号简记为{NQ，NM，NS，ZO，PS，PM，PQ}。为简化计算，输入量e与输出量v均采用三角形隶属度函数[7]。各变量的隶属度函数如图12以及图13所示。

图12 输入量隶属度函数

图13 输出量隶属度函数

4.2.2 模糊控制规则的设计

模糊控制规则如表1所示。进行精确定位时，机器人行进速度主要取决于当前位置到目标位置的里程误差[8],当机器人远离目标点时，行进速度相应地越大，提高响应速度；机器人靠近目标点附近时，行进速度进行相应调整，以便精确定位；机器人接近目标点时，行进速度相应较慢，防止较大速度下因惯性作用出现的打滑。

表1 模糊规则表

5 机器人样机试验与结果分析

为了测试机器人的定位精度误差与响应速度，需要对机器人进行检测[9]。实验平台为长度40 m的工字轨道，在工字轨道上每间隔1 m打入1颗定位螺钉，在该平台上对机器人进行实验。

5.1 机器人样机精确定位实验

在实验中通过向机器人给定目标位置，再测量机器人实际行驶里程与相应时间，精确定位实验结果如表2所示。由实验结果可知：机器人定位最大误差不超过3 cm，定位精度高，响应时间快，平均速度可跟随里程变化而变化。

表2 机器人精确实验结果

5.2 机器人实际作业效果

在确定机器人精确定位功能满足要求后，下一步对机器人进行实际巡检作业进行测试，检测机器人能否实时上传环境参数，能否对异常参数进行预警，并能准确定位故障点，能否有效检测电缆等设备过热。实验结果表明，使用本控制系统的轨道式机器人可高效、自动地完成地下综合管廊的巡检任务。实际作业效果如图14所示。

图14 机器人实际巡检效果

6 结束语

开发了一种基于STM32的轨道式巡检机器人控制系统并对其进行了测试。该系统采用STM32F407ZGT6为微处理器，内嵌FreeRTOS操作系统。采用上下位机结构并通过以太网通信实现远程无线信息交互。根据传统轨道式巡检机器人定位精度不足等缺点，提出了一种速度-位置双模式匹配运动控制方法并设计了模糊控制器以提高机器人定位精度与响应速度。实验测试表明该系统能够精确、快速实现定位，能够有效弥补人工作业的不足，提高了地下综合管廊巡检作业智能化程度。