速度自适应增量PID在AGV上的应用

程松贵 ，张文群 ，王福松 ，霍修军

（1.安徽江淮汽车集团股份有限公司，合肥 230000；2.合肥通用职业技术学院，合肥 230000；3.安徽江淮银联重型工程机械有限公司，合肥 230000）

0 引言

为了降低人力成本，许多企业采用AGV进行物流运输。磁导航AGV具有经济性可靠性高、环境适应性比较强的特点，故应用非常广泛。在工程项目中，经常出现磁导航AGV跑偏甚至出轨现象，特别是在拐弯段，分析其原因是磁道跟踪算法片面追求跟踪的精度及可靠性，忽视了复杂路径下对整车速度进行自动修正，机械惯性过大导致跟踪失败。为此本文基于差动转向AGV提出了整车速度自适应增量PID控制算法，该算法由整车速度模糊控制模块和增量PID跟踪控制模块组成，经验证效果良好。

1 差动AGV工作原理

图1所示为差动AGV的磁道跟踪示意图，主要由机械框架、直流无刷电机、减速机构、驱动轮及磁导航传感器组成，主要功能是牵引、差动转向及检测磁道。图2为差动转向运动模型，ωl、ωr分别为左右车轮的角速度；vl及vr分别为左右车轮的线速度；R为车轮半径。v及ω分别为差动转向机构在oxy平面内的直线速度及旋转角速度。运动方程描述为：

图1 AGV基本结构示意图

图2 AGV差动转向机构运动模型

由运动方程可以看出若ωl=ωr，则ω=0，即AGV直线行走；否则AGV有圆弧运动。差动转向AGV就是由基本的直线运动及圆弧运动合成的。要有效地跟踪磁道，关键技术就是控制ωl及ωr。但为了提高程序代码执行效率，本系统轮速度控制是直接给出vl和vr命令值，节省了转换运算环节。

2 车载控制系统硬件设计

车载主控模块CPU采用STM32F407系列单片机，该单片机内核为32位高性能Arm Cortex-M4处理器，时钟频率高达168 MHz；数据通信接口丰富：3个I2C接口，6个串口，3个SPI接口，2个CAN2.0等。这些为AGV系统主控模块的开发提供了足够的资源，该芯片外围配置一些光耦芯片、RS232电平转换、RS485电平转换芯片、CAN收发器芯片、EEPROM芯片及基本的IO电平转换电路，就构成了AGV主控模块电路，该电路经济、实用、可靠。

整车控制系统主体采用CAN架构，自STM32主控模块引出两路CAN总线，一路CAN通信速率为125 kHz，该路总线配置的模块有车载电池BMS系统及音乐喇叭等；另一路CAN总线通信速率250 kHz，配置地标传感器、磁导航传感器及电动机驱动器等。CAN总线分两路的主要目的是提高系统的兼容性、实时性，本系统外购件如电池BMS及音乐喇叭等只支持125 kHz，而从AGV跟踪的角度要求控制系的数据采集、处理、命令执行快速高效，尽量提高CAN总线的通信速率，但考虑到通信速率过高会降低系统的稳定性，故另一路通信速率设置为250 kHz，经系统测试完全可以保障AGV系统正常运行。总线接口模块之外，车载控制系统还配置了与STM32主控模块直接串口通信的触摸屏及无线收发模块，开关量控制的避障扫描模块等。

所有电气硬件中，与磁道跟踪控制技术直接相关的模块主要是磁导航传感器。磁导航传感器通过检测磁道获取磁道相对该传感器的位置信号或数据。本系统所用的磁导航传感器支持3个字节的数据量输出，磁道检测数据结构如图3所示，实际有效位S1到S16位。由表1可以看出，磁导航传感器输出的数据有无效位在干扰数据结果，不能直接表示AGV小车前进方向纵向轴线相对磁道中心线向左还是向右偏差多少，故不能直接用于跟踪控制算法，必须予以预处理。

表1 磁导航传感器数据结构

3 车载软件关键设计

3.1 跟踪数据预处理

要进行跟踪控制，首先要对磁导航传感器数据进行预处理，生成磁道相对磁导航传感器位置的一组数据。主要是确定S1到S16的起止编号Sl、Sr。Sl中l对应AGV前进方向磁道左侧left，Sr的r对应right。

若车载控制系统要求AGV沿磁道右侧进行跟踪，则需要获取Sr值，获取方法如下：把3个字节的数据向右（最低位）循环移位，直到bit0为1，才停止循环移位，Sr等于循环移位次数。

若车载控制系统要求AGV沿磁道左侧进行跟踪，则需要获取Sl值，获取方法如下：为了简化算法，首先判定第三字节的最低位是否为1，若是则Sl等于1，否则进入第二步；第二步，判定第二字节的bit7是否为1，若是则Sl等于2，否则进入第三步；第三步，执行循环左移位操作，直到第二字节的bit7为1时停止循环移位，Sl=循环移位次数+2，否则若循环左移位结束，bit7还是0，则Sl为0。

限于篇幅，本文不对磁道交叉等环境的数据预处理进行赘述。自适应速度模块及增量PID跟踪模块的数据源为偏差值e，计算过程如下：

e=Sset-Si。

式中：Sset为磁道边沿跟踪序号设定值；Si为磁道跟踪实际值Sl或Sr，车载控制系统根据工艺路径方向决定取Sl还是取Sr。

3.2 自适应速度模块

自适应速度模块采用两输入单输出的模糊控制器结构。模糊控制器的输入为偏差e和偏差率ec，对应模糊量E和EC，变量值有NB、NM、NS、ZO、PS、PM、PB。模糊控制器输出为整车速度修正值Δv，对应模糊量为U。根据工艺特点，系统只对整车设定值Vset做减速修正，故U的模糊值有NB、NM、NS及ZO。设计的模糊控制规则如表2所示，把该表以二维数组的形式编写到程序代码中，以供程序查询。

表2 模糊控制规则表

为提高程序代码运行的高效性，偏差e及变化率ec模糊值定义为1、2、3、4、5、6、7，Δv模糊值定义为1、2、3、4。部分C语言程序代码如下：

i对应E模糊值，j对应EC模糊值，k对应模糊控制规则表U模糊值，v为修正后的速度值。在连续拐弯区间，v取当前最小值。

3.3 增量PID跟踪模块

速度模块生成v值后，跟踪模块立即根据v值给AGV左右轮分配速度vl和vr。跟踪算法多采用传统的位置PID控制技术，该算法有一定的缺陷，由于位置PID控制器的输出控制量对应的是执行机构的实际位置偏差，如果位置传感器有故障，跟踪会由于误判造成大幅度的车身摆动，导致生产事故。为了提升AGV系统的安全性，本系统采用增量PID技术[2]，该算法不会产生积分失控现象。部分代码如下：

上述代码的调用采用定时器中断技术，采样及执行周期20 ms，在AGV小车低于30 m/min工艺环境下，本系统完全可以准确跟踪。代码中的Kp、Ki及Kd在AGV机械及工艺参数冻结后，进行反复的试验获取。

3.4 两模块有机集成

AGV正常运行时，增量PID跟踪模块一直周期性调用；而自适应速度模块的周期性调用只在ek绝对值大于设定偏差emax时（即导航路径拐弯段）触发；左右车轮速度差小于设定值且ek小于emax若干秒后，系统判定AGV进入直线区间，控制系统中止自适应速度模块的周期调用，整车速度恢复到Vset，以降低AGV控制系统的资源消耗。

经两模块的有机集成后，AGV在路径拐弯段能自主地进行减速，直线段能自主地恢复到调度系统要求的速度，解决了拐弯段路径跟踪跑偏现象。

4 结论

文章根据磁导航传感器的数据结构，提出了跟踪数据获取及预处理方法，并综合运用整车速度自适应模糊控制模块及增量PID跟踪模块，实现了不同负载下拐弯段的有效跟踪。该方案经济性、实用性及可靠性比较高，经长期的生产验证，效果良好。