管道机器人直行与转弯模糊控制

2022-07-06高铭泽黄民唐凯郎需强高佳明

北京信息科技大学学报(自然科学版) 2022年3期

高铭泽，黄民，唐凯，郎需强，高佳明

(北京信息科技大学机电工程学院，北京 100192)

0 引言

目前很多商业化的管道机器人，能够通过视频遥控完成一些简单的清洁任务[1-3]。然而，若仅靠机器人摄像头的图像进行遥控运动，效率较低，可靠性差，并且可能由于距离或位置判断不准确而导致机器人发生碰撞，尤其是在转弯时碰撞几率更高。因此，需要设计智能控制算法，使机器人具有环境感知和自主位姿控制能力。Reyes-Acosta等[4]为了保证机器人在管道内直线运动不侧翻，引入了模糊控制，利用机器人板载的惯性测量单元实时感知姿态角，将偏移角度误差与误差的变化率作为控制器输入，轮速作为输出，取得了不错的效果，机器人在偏移30°时经过5 s左右可恢复直线运动。季艳波等[5]设计了一种单输出的模糊控制器，以机器人的横滚角和其变化率作为输入，两轮的差速变量值作为输出，偏航时能够自适应调节输出。任新新等[6]在管道机器人直线运动中采用模糊控制的基础上，分析了传感器数据采样周期长短对控制性能的影响，采用多次采样数据平均值作为控制器输入进行补偿，取得了较好的效果。朱彦昭等[7]针对转弯运动，通过建立管道机器人位姿数学模型，分析得出了机器人在转弯时各驱动轮的理想转速比，优化了转弯运动性能。王宁[8]提出了分层模糊控制算法，用以提升轮腿式管道机器人弯道运行的性能，三层模糊控制器中，第一层通过改变机器人的驱动体直径来调节机器人各车轮的受力，使每一个驱动轮受力均在一个范围内波动；第二层根据当前驱动体直径来输出驱动轮的速度；第三层以姿态角为输入，进一步调节驱动轮输出，最终获得较为稳定的运动控制。

衡量管道机器人性能的一个重要指标为管道通过性，而直行和转弯是管道机器人最基本的运动形式。本文在新风系统通风管道清洁场景下，针对管道机器人直行运动提出了一种双输入双输出的模糊控制器，使得机器人在直线运动偏航时迅速回正，大幅减少了回正时间，并且能使机器人始终保持在中线运动；针对转弯运动提出了一种双输入单输出的模糊控制器，在入弯偏航的情况下能实现平滑转弯。

1 管道机器人控制系统

本研究中的管道机器人原理样机如图1所示，控制系统结构如图2所示。

图1 管道机器人原理样机

图2 机器人控制系统结构

机器人为多节轮式串联的机械结构，相邻两个单元通过U型连接件连接，通过舵机控制，可以改变机器人位姿。机器人前进动力通过安装在头部和尾部的无刷电机提供。机器人的控制系统是以STM32微控制为核心的嵌入式系统。控制器通过向驱动器输出串口信号或脉宽调制波(pulse width modulation，PWM)来控制执行器动作。机器人头部安装了激光雷达、测距传感器、惯性测量单元。激光雷达能够获取前方地形的距离信息；两组激光测距传感器安装在机器人头部两侧，感知其与管壁的间距；惯性测量单元集成在机器人控制电路板上，能实时上传机器人姿态角数据。上位机界面运行在操作端的计算机上，摄像头图像和传感器的数据将显示在界面上，同时上位机可通过虚拟按钮发出控制指令。

机器人在运行时属于多单元、多驱动轮运动，因此在直行时很难实现完全同步，在误差的积累下容易偏航，仅根据机器人的摄像头图像遥控并不能迅速调正方向，并且可靠性较差。此外，机器人的转弯运动是依靠前驱动轮差速来实现的，通常有初始的差速量，并且运动中始终保持固定的差速值；但当入弯之前机器人已经偏航时，在转弯过程中遇到干扰或者管道结构不标准时，机器人有可能产生碰撞或转弯失败。综上所述，应设计合理的智能控制算法，使机器人在运动时根据管道地形自适应调节输出量，提高运动性能。

2 模糊控制器设计

本研究中采用的模糊控制算法，是一种根据专家经验进行模糊推理的控制方法，采用负大、负小、正大等模糊语言来表示变量的大小程度。模糊控制系统的特点是鲁棒性强，适应性强，而且有较好的抗干扰能力[9-10]。模糊控制系统主要由模糊化、模糊推理和解模糊组成，如图3所示。

图3 模糊控制系统结构

2.1 直行运动

机器人在管道内直线运动时，同时存在机器人偏移和头部偏航两种情况，如图4所示。

图4 机器人直行时偏移和偏航

图4中，x1和x2为机器人头部左右测距传感器的数据值，θ为偏航角，通过惯性测量单元来获得。令Ed=d1-d2=x1cosθ-x2cosθ，表示机器人与两侧管壁距离的差值，用来描述机器人偏移管道中线的程度，Ed为负表示中线向左偏移，为正表示向右偏移，并且绝对值越大表示偏离程度越大。设定偏移量最大为150 mm，则Ed的范围是[-150,150]；设定头部偏航最大角度为30°，则θ的范围是[-30°,30°]，向左偏航为正，向右偏航为负。将Ed和θ作为直行模糊控制器输入，左右驱动轮转速UL、UR为输出。定义模糊子集，输入和输出为7级的量化等级，范围均为[-3,3]，量化的过程就是将实际的传感器数值映射到 [-3,3]的范围内。语言变量以NB、NM、NS、ZO、PS、PM、PB来分别表示负大、负中、负小、几乎为零、正小、正中和正大。采用如图5所示的三角形隶属函数(trimf)计算输入的隶属度。

图5 三角形隶属度函数

模糊推理中的规则库是根据机器人的运动特性和运行环境条件来制定的，根据专家经验，设计了前左右驱动轮模糊控制规则如表1、2所示。根据控制经验，头部的偏航带来的影响要比机器人整体偏移中线的影响大，因此当偏航角θ偏移过大时，不论Ed的值有多大，此时都应快速增大差速量，使机器人头部迅速回正；当θ偏移量很小时，则根据Ed的大小调节差速量，并且尽量在低速段运行；当Ed、θ均很小时，控制目标是运动的稳定性，差速量和两轮转速应在小范围内调节。在直行过程中机器人始终沿着管道中线运动为最终的控制目标，控制结果是Ed和θ趋于0。

本研究中采用重心法解模糊。对于已经量化后的输入Ed和θ，每个输入量都隶属于两个相邻的模糊语言变量，例如当Ed=0.5时，隶属于ZO和PS，并且属于ZO和PS隶属度的和为1。因此每个输入对应了两个隶属度，根据模糊规则表，会对应着4个输出，总输出为这4个输出的和，得到的结果是一个范围在[-3,3]的值，再通过比例因子放大，得到电机的控制量，至此完成了一次直行模糊控制。

表1 模糊控制量UL取值表

表2 模糊控制量UR取值表

2.2 转弯运动

机器人在转弯过程中，偏航角会一直处于变化的状态，很难描述偏差的变化，因此不能作为模糊控制器输入。另外，由于存在初始的差速量，控制的目的应是根据位姿和地形调节差速量的大小。据此设计了一种双输入单输出的转弯模糊控制器，输入为Ed和其变化率Edc，输出为差速增量ΔU。设定输入和输出为7级量化等级，对于ΔU，PS、PM和PB表示增大差速量的程度，NS、NM和NB表示减少差速量的程度，ZO表示保持原差速值。输入输出的模糊化同样采用三角形隶属度函数，模糊论域均为[-3,3]，根据控制经验设计了如表3所示的模糊规则。由于采用了差速增量作为模糊控制器输出，因此左转和右转运动具有通用性，左转弯的Ed为左侧传感器数值减右侧传感器数值，而右转弯相反。

表3 模糊控制量ΔU取值表

3 性能测试

3.1 STM32程序实现

模糊控制算法通过C语言编写在STM32主控制器上，当机器人开始直行运动或转弯运动时，测距传感器和惯性测量单元的数据以100 Hz的频率上传，主程序每10 ms进行一次模糊控制计算，程序流程如图6所示。

图6 模糊控制主程序流程

系统首先采集测距传感器和姿态传感器的数据并处理，得到距离值和偏航角，通过量化函数将输入值量化到模糊论域之内，并进行限幅。隶属度函数的计算是根据相似三角形法，得到的隶属度值和其索引分别存于数组中。模糊规则采用一个7×7的数组存储，模糊推理根据隶属度和索引计算输出，最终得到了一个脉宽在1 500～2 000 μs的PWM信号，用于控制电机转速。

3.2 直行运动测试

机器人的初始位姿由(x1,x2,θ)来表示，x1、x2为实际获取到的左右测距传感器数据值，θ为偏航角。分别设定初始位姿为(120,120,15)、(90,170,-21)、(40,185,0)和(60,175,10)进行测试，实验中采用截面尺寸为300 mm×300 mm的矩形通风管道。实验结果如图7所示。

图7 不同初始位姿下直行运动实验结果

实验中跟踪了两传感器的数据和偏航角，图7(a)描述了初始位姿为(120,120,15)时的运动情况，机器人初始无中线偏移，只有头部15°的偏航。运动初始波动较大，机器人经过3.5 s使偏航角恢复在0°附近。图7(b)表明在(90,170,-21)初始位姿下，机器人在2 s时偏航角恢复到0°，2.5 s左右消除了中线偏移，并且偏航角近似呈线性变化，因此运动较为稳定。图7(c)初始位姿为(40,180,0)，表示初始时只有中线偏移并无偏航，因此调节在低速段进行，机器人在3 s时已经运行在管道中线，并且能一直保持。图7(d)描述了初始位姿为(60,175,0)时的运动状况，机器人初始向左偏移且头部也向左偏航，因此初始差速量很大，0.5 s时消除了偏航，之后逐渐自适应调节，在2.5 s左右恢复到管道中线运动。实验结果表明，所设计的模糊控制器能够显著提升机器人运动性能，使其在各种位姿下均能恢复直线运动。

3.3 转弯运动测试

对左转弯和右转弯分别进行了一次测试，如图8所示。左转弯时初始位姿为(180,40,0)，表示进入弯道时偏离中弧线右侧，靠近外侧管壁。右转弯初始位姿(175,60,0)，表示靠近内侧管壁。

图8 不同初始位姿下转弯运动实验结果

图8(a)描述了左转弯的运动情况，机器人的头部在1.5 s内完成了转弯，并且在0.5 s左右就消除了中线偏移，调节速度很快。图8(b)表明，在右转弯时，机器人经过1 s 之后能沿着中弧线运动，经过1.5 s左右完成90°转弯，转弯中距离右侧管壁最近时为40 mm，因此能够满足通过性要求。通过偏航角变化曲线可以看出，在转弯运动中偏航角没有波动，运动较为稳定，因此证明了所设计的转弯模糊控制器的有效性。