STM32F103ZET6单片机的叉车式AGV的寻迹算法※

石含飞，何勇灵

(北京航空航天大学 交通科学与工程学院，北京100191)



STM32F103ZET6单片机的叉车式AGV的寻迹算法※

石含飞，何勇灵

(北京航空航天大学 交通科学与工程学院，北京100191)

摘要:以单片机为系统控制器核心，针对磁敏传感器组寻迹的叉车式AGV,设计了传感器的阵列布置形式，并根据此形式以及磁导引叉车式AGV的寻迹约束条件和行走要求提出了路径的识别与跟踪算法。前进时采用分段模糊PID控制，PD系数被模糊控制器动态改变,以适应驱动轮的不同转速和前向位置偏差的大小。后退过程应用双闭环PID进行控制，姿态角和后向位置分别为内环和外环。在实际行走测试中，AGV稳定行驶时路径偏差可保持在10 mm内。

关键词:STM32F103ZET6；AGV；磁导引；模糊控制；PID控制

引言

综合考虑AGV车身长度为2 m、稳定行驶路径偏差不超过10 mm等约束条件和行走要求，以磁带作为导引路径，以STM32F103ZET6单片机为核心构建控制器，提出了一种基于模糊控制和PID控制的AGV智能寻迹算法，解决了叉车式AGV自动行进状态下路径的准确识别与快速跟踪问题。

1AGV基本结构

文中所讨论的为单舵轮叉车式AGV。轮系部分主要有一个转向驱动轮、两个从动轮、再辅以两个支撑轮，转向驱动轮与两个从动轮的布局方式和相对位置决定了AGV的运动形式[2]。叉车运动的前进与后退的方向定义如图1(a)所示。

e1为前向16位磁敏传感器感知的路径中心相对预设路径中心的偏移量，即前向位置偏差；e2为后向8位磁敏传感器感知的路径中心相对预设路径中心的偏移量，即后向位置偏差；α为两磁敏传感器感知的路径中心线相对于预设路径中心线的偏转角，即姿态角。前向与后向磁敏传感器的布置阵列如图1(b)所示，前向磁敏传感器为YF-9016(16路开关量传感器)。后向磁敏传感器为YF-9006(8路开关量传感器)，当磁敏传感器相应的位检测到磁条的有效信号时，该位输出高电平，位与位之间的距离L=10 mm，虚线上的7和4分别为磁条中心预设在前向磁敏传感器和后向磁敏传感器上的位置，即预设路径中心对应于传感器上的位置。

图1　叉车式AGV结构与磁敏传感器布置阵列

2路径识别

2.1磁条中心位置检测

AGV在运动过程中，磁条与传感器的相对位置会出现3种情形：①磁条被传感器完全覆盖，此时磁条中心位置是传感器连续获取信号的中心；②磁条处于传感器边缘，若此时传感器上检测到磁信号的位数少于3，则自动补齐传感器外面的位，使有效位数为5，然后再计算磁条中心位置；③磁条超出了传感器的感应距离，传感器丢线，此时沿用丢线之前的磁条中心位置作为当前采样周期的磁条中心位置，若连续丢线达到一定次数，则认为AGV失去自导引能力。

2.2有效磁条选择

AGV在行进过程中，同一时刻可能会检测到多块磁条的存在：①车子处在无分叉路段，则会比较当前所有磁条的中心位置，从多块磁条中选择中心位置最接近历史位置的一块磁条作为有效磁条；②AGV正在进入分岔路段，那么其会根据RFID标签信号选择最靠近分岔方向的那块磁条作为有效磁条，并将其中心位置视为新检测到的路径中心位置。

2.3磁条中心位置均值滤波平滑

经传感器信号计算出来的磁条中心位置，需进行平滑滤波，如此处理可屏蔽某些干扰信号，避免检测到的路径中心位置存在较大波动，降低转向轮的摆动频率与幅值，使车子行进更加平稳。具体方法是将当前采样周期检测到的磁条中心位置与前几次的采样值取均值作为当前磁条中心位置，即均值滤波。

3路径跟踪

路径跟踪过程是单片机接收RFID天线发送过来的RFID标签号，并据此判断AGV当前的位置，将要执行的动作(直行、后退、转弯、加速、加速、负载、空载)，以及何种路径跟踪模式，并可依据横向位置偏差e1和e2计算出转向轮的转向角。RFID即射频识别，亦称射频识别技术或无线识别技术，可通过射频信号获取目标信息，实现自动识别[3]，无需人工接触及光学可视即可完成信息输入和处理[4]。转向角的单位是°，取值区间为[-90°,90°]，转向角以逆时针方向为正，转向轮轴线垂直于AGV纵向轴线时转向角定为0°。AGV前进或者后退等最基本的控制算法都是基于位置式数字PID控制，其输出如下：

(1)

式中，Kp、Ki和Kd分别为比例系数、积分系数、微分系数，ej(0≤ej≤n)为第j次采样所计算出的偏差，Un为第n次采样偏差输入后的控制器输出。

3.1前进路径跟踪

前进路径跟踪采用的是分段模糊PID控制，在标准位置式数字PID的基础上，以驱动轮的转速ω=2 000rpm为边界，进行高速段和低速段的划分，高速PID作用于直道行走，低速PID控制AGV转弯，以下所述ω的单位皆为rpm(转/分钟)。此外，基于前向位置偏差对PID中的比例、微分系数进行模糊控制，实时动态调节这二者的值[5]，使PID控制适应不同的速度段与偏差范围，基于经验知识的模糊控制器不需要精确的数学模型且控制的鲁棒性较好[6]。转弯过程中，后向磁敏传感器一直处于丢线状态，而在走直道过程中依靠前向磁敏传感器的信号就可以稳定地控制AGV，因此前进过程中只用到了前向16位磁敏传感器。

3.1.1前进模糊控制器

由表1可知，比例作用强度模糊量化为三级：强、中、弱。微分作用强度模糊量化为二级：强、弱。强度等级可用数值进行表征，数值越大，相应的作用强度越大。

表1　比例、微分作用强度模糊等级量化

图2为比例作用强度隶属度曲线图，A(x)、B(x)、C(x)分别表示比例强作用、比例中作用、比例弱作用的隶属度函数。图3为微分作用强度隶属度曲线图，D(z)、E(z)分别表示微分强作用、微分弱作用的隶属度函数。

图2　比例作用强度隶属度

图3　微分作用强度隶属度

模糊决策采用“均值评分原则”，即各级作用强度值与相应的隶属度的积的累加和，代入表1中高速段和低速段PID的系数，得到模糊控制器的最终输出：

(2)

(3)

3.1.2前进PID控制器

低速下，AGV的实际速度和电机输出转速之间偏差较大，尤其是在电机有输出但是AGV未动时，转向角的积分值会迅速达到饱和状态。而AGV一旦动起来，转向

轮就会往积分饱和的方向转一个很大的角度，路径偏差变大，并出现较大超调，系统稳定性下降[7]。因此，低速时，一般通过减小积分系数(Ki)的方式来避免过度积分；而高速时，为了追求AGV行驶的平稳性，积分项的变化率也不宜过大。本文不采取调整积分系数的方式来改变积分项的大小和变化速率，而是根据驱动电机转速输出值的大小，调节前向位置累计偏差的衰减程度，前向位置累计偏差的计算式如下：

(4)

其中，k为衰减系数，sumn_e为从开始采样到第n个采样周期时的前向位置偏差累计值，sum(n-1)_e为从开始采样到第(n-1)个采样周期时的前向位置偏差累计值，k的取值如下：

(5)

其中，800rpm为电机的死区转速，即当电机在转而AGV未行走时电机输出的最大转速。

上述方法可以有效减小前向位置累计偏差的变化速率，但是并不能防止因为积分项而出现的超调，而且AGV在行进过程中，车轮与地面的滑动以及重心偏移产生的惯性负载，都会导致前向位置累计偏差的变化，可能会引起瞬时超调[8]，因此，对积分项的输出要进行限幅:

(6)

前进控制PID的积分系数恒定且Ki=0.023，结合式(2)～(6)的结果，可得PID控制器的输出如下：

3.2后退路径跟踪

后退路径跟踪使用的是固定系数的双闭环PID控制器，内环为AGV姿态角控制环，外环为AGV后向8位磁敏传感器的后向位置控制环。叉车式AGV的后退是为了取货和放货，其路径是直道，因为AGV转向轮在前，叉车车身长2 m，仅靠后向8位磁敏传感器采集的磁条信息无法控制叉车直线行走，故需要前向磁敏传感器配合进行姿态角调节。如图4所示，后退过程中后向位置控制环基于后向位置偏差的大小，输出预设姿态角，预设姿态角的值不一定为0(两磁敏传感器的中心线平行于AGV纵轴线)。如果偏差较大，如AGV过于靠近磁条右边时，需要修正，则给予转向轮一个正的转向角，即视输出的预设姿态角为正，而姿态角控制环则根据前向位置偏差和后向位置偏差，计算出AGV当前的姿态角，根据预设姿态角与当前姿态角的偏差给出转向轮的转向角。姿态角的表征方程如下：

(8)

x1和x2分别为前向和后向磁敏传感器在后退过程中经采样和滤波后所得的磁条中心位置，X和Y分别为在后退过程中预设路径中心位置对应于前、后向磁敏传感器上的位置，D表示两磁敏传感器的中心距离。

图4　后退路径跟踪双闭环PID控制框图

4试验结果

为直观呈现AGV实际行走路径与预设路径的吻合度，在布置好的磁条线路上进行测试。所用磁条宽度为3 cm，磁条被磁敏传感器完全覆盖时，磁敏传感器感应到磁信号的位数恒为5。小车走直道、小车过弯道的示意图略——编者注。

前进过程中预设路径中心对应于前向磁敏传感器上的位置为7，图5为前进过程中，前向磁敏传感器采集磁条信息后，经计算所得的实际路径中心与时间的关系图。

图5　AGV前进路径中心关系图

由图5可知，实际路径中心与预设路径中心在大部分时间内基本吻合，误差在10 mm以内，折线密集的地方是AGV处于转弯状态，在转弯的过程中轨迹方向时刻变化，导致AGV的前向位置偏差一直存在且处于变化状态[9]。图5中初始阶段偏差较大，这是由于外力把AGV移动到导引路径上时，磁条中心位置和前向磁敏传感器的上的位置7相距较远，经过一段时间调整后，前向磁敏传感器感知的路径中心位置稳定在预设路径中心附近。

图6为AGV后退走直道的过程，预设路径中心对应于后向磁敏传感器上的位置为4，对应于前向磁敏传感器上的位置为7。由图6可知，AGV寻迹效果良好，误差在10 mm以内,折线密集的地方是因为磁条没有贴直，导致AGV转向轮摆动频率过高。

图6　AGV后退路径中心关系图

结语

总体上，在走直道过程中，AGV会有轻微震荡，横向位置偏差在10 mm以内。过弯道时，通过AGV减速，并增大PID系数，可使横向位置偏差稳定在10 mm以内，但震荡频率相对走直道略高。控制器以STM32F103ZET6单片机为核心，相对于工业控制器具有低功耗的优势[10]，同时充分利用了单片机方便灵活、编程方式容易实现智能控制算法的优点。从现场试验的结果来看，本文设计的集模糊控制与PID控制于一体的算法寻迹效果良好，为磁导引叉车式AGV的自动控制提供了一种可供参考的方法。

参考文献

[1] 关宏.AGV整体集成系统结构设计[J] .物流技术，2004(4)：37-38.

[2] 耿牛牛.单舵轮AGV路径跟踪方法的研究[J] .制造业自动化，2011,33(4)：81-84.

[3] 游战清，李苏剑.无线射频识别技术(RFID)理论与应用[M] .北京：电子工业出版社，2004.

[4] 周晓光，王晓华，王伟.射频识别(RFID)系统设计、仿真与应用[M] .北京：人民邮电出版社，2008.

[5] 宋长会.移动机器人控制系统设计与研究[D] .哈尔滨:哈尔滨工程大学,2006.

[6] 廖华丽，周祥，董丰，等.基于模糊控制的AGV循迹算法[J] .哈尔滨工业大学学报.2005,37(7)：896-898.

[7] 陈盈.磁导引自动导航小车控制系统的设计[D] .武汉：湖北工业大学，2013.

[8] 程远.基于模糊控制的技术的AGV运动控制器设计[J] .无锡职业技术学院学报,2012,11(5)：41-44.

[9] 黄胄.基于PLC控制的AGV技术研究及其应用[D] .上海：华东理工大学，2013.

[10] 郑炳坤，赖乙宗，叶峰.磁导航AGV控制的设计与实现[J] .自动化与仪表,2014(3)：6-10.

石含飞(硕士研究生)，研究方向为嵌入式系统应用、AGV控制系统技术开发；何勇灵(教授)，研究方向为汽车电子软硬件控制技术、工业机器人技术。

Navigation Algorithm of Automated Guided Forklift Based on STM32F103ZET6※

Shi Hanfei,He Yongling

(School of Transportation Science and Engineering,Beihang University,Beijing 100191,China)

Abstract：Taking the STM32F103ZET6 MCU controller as the core of system,the sensor array design is layout for the magnetic sensor group tracking forklift AGV.According to the layout of sensors,tracing constraints and walking requirements,the identification path and tracking algorithm are proposed.AGV is controlled by segmentation fuzzy PID controller during the forward process.PD coefficient is dynamically adjusted by the fuzzy controller to adapt to different speeds of driving wheel and different size of forward position deviation.Double-loop PID controller is applied during the reverse process.The gesture angle and backward position are inner ring and outer ring respectively.In actual walking tests,the path deviation maintains less than 10 mm when AGV walks stably.

Key words:STM32F103ZET6;AGV;magnetic navigation;fuzzy control;PID control

收稿日期：(责任编辑：杨迪娜2015-07-06)

中图分类号:TP24

文献标识码:A