轮式AGV沿葡萄园垄道行驶避障导航算法与模拟试验

谢永良 尹建军 余承超 贺 坤 胡旭东 李仁旺

(1.浙江理工大学机械与自动控制学院，杭州 310018; 2.浙江经济职业技术学院物流技术学院，杭州 310018;3.江苏大学现代农业装备与技术教育部重点实验室，镇江 212013)

0 引言

轮式自动导引车(Automated guided vehicle，AGV)目前已广泛应用于车间物料运输、仓储物流等领域，对企业柔性集成化生产模式起到至关重要的作用[1]。在农业生产领域，AGV主要从事果实套袋、施药、采摘、搬运等工作，为减少劳动力、 提高生产效率提供了一种解决方案[1-2]。因而，实现AGV按既定路线自主导航行驶和防碰避障的智能化局域路径优化逐渐成为国内外众多学者的研究重点[3-11]。

环境信息已知的AGV全局路径规划算法有可视图法、自由空间法、栅格法等[4-6]，但此类算法不适用于障碍物复杂多变、环境信息未知的非结构化果园环境。对于环境信息未知的局部路径规划算法，势场蚁群算法存在易陷入局部极小值问题[12]；遗传算法[13]计算量大，难以保证在复杂环境中的实时性和快速性；模糊逻辑算法[14]控制规则数目多达几十条时会占用大量系统资源。上述算法在静态障碍物的避障路径全局规划应用中取得了良好效果，但并没有考虑在规划过程中局域路径优化问题，在未知的动态障碍物环境中缺乏有效的规划方法。席裕庚[15]将预测控制的基本原理从控制问题推广到一般广义控制问题求解，利用机器人实时探测得到的局部环境信息，以滚动窗口方式实施优化与反馈相结合的在线路径规划。文献[16-17]运用该原理在动态不确定环境中取得了良好的避碰效果。

本文面向葡萄园自主行驶应用需求，针对轮式AGV沿葡萄园垄道循迹受到障碍物阻碍的问题，基于超声波测距与AGV航向偏角检测，应用滚动优化原理研究轮式AGV沿直线垄道行驶的避障导航策略和算法，以工控机与PLC构建主从控制的AGV试验系统，通过模拟试验来验证导航算法的有效性，以期为轮式拖拉机等前轮导向车辆无人化作业时沿垄道循迹的防碰预警与避障技术提供参考。

1 问题描述与分析

图1 葡萄园环境描述Fig.1 Environment description sketches of vineyard

若垄道无障碍，AGV沿垄道中线行驶，设定为预定轨迹。若垄道存在静态障碍物，则检测其与AGV运行轨迹的相对位置关系，判断是否执行避障策略。若垄道存在动态障碍物，任意相邻瞬时时间间隔内认为其以直线轨迹移动。由于AGV在果园环境下作业是一种慢动态场景，若能预测动态障碍物的当前时刻运行轨迹，找到预测碰撞点，则可引导AGV避让动态障碍物。

AGV果园自主行驶分为2种模式：全局预设路径的纠偏行驶和局域轨迹下的防撞避障行驶模式。对于前者，AGV保持在果园垄道匀速行驶，根据AGV右侧的测距传感器探测的距离和航向偏角纠偏行驶。对于后者，AGV模拟驾驶员避障经验，分为调节AGV航向偏角的绕行静态障碍物避障和调节AGV车速的动态障碍物避让。

2 AGV沿垄道行驶的运动学模型

对于前轮导向的轮式AGV纯滚动转向，左前轮转角φL与前轮期望导向角φ的关系[18-20]为

(1)

式中L——前、后轮轴距

w——车体宽度

以AGV前进方向为计量基准，左转时φ为正值，右转时φ为负值。

左前轮转角φL与两后轮中点处转弯半径R0满足[18-20]

R0=LcotφL+w/2

(2)

右前轮转角φR与两后轮中点处转弯半径R0满足[18-20]

(3)

前轮左转时，两后轮的转速比k为[18-20]

(4)

式中RR——右后轮转弯半径

RL——左后轮转弯半径

eL——左后轮与车架距离

eR——右后轮与车架距离

前轮右转时

(5)

设AGV运行速度为v，左、右后轮的转速nL、nR分别为[18-20]

(6)

式中r0——车轮半径

AGV路径跟踪过程中，由模糊控制器输出前轮期望导向角φ，由式(1)、(3)可确定左、右前轮转角φL、φR，由式(6)计算设定车速下左、右后轮转速nL、nR，实现AGV纯滚动转向与差速驱动的协同控制[18]。

3 AGV沿垄道行驶的防碰避障导航算法

滚动优化是预测控制基本原理之一[15]，将AGV自带传感器探测得到的有限局部信息看成一个滚动窗口，并在当前窗口内实施局部避障策略。在有限时域内每结束一个规划周期，AGV将刷新窗口内的动态信息，并重复执行滚动过程，结合窗口滚动和反馈机制实现AGV全局路径规划。滚动优化原理具有实时性强、适用于静态和动态障碍物检测条件，其滚动窗口算法原理如图2所示。

图2 滚动窗口算法原理图Fig.2 Algorithm principle diagram of rolling windows

以传感器可以探测得到的最大距离R为半径的圆形区域作为窗口Win(PC(t))，PC(t)为AGV当前时刻所在位置。AGV每次规划步长ε(与运行速度和规划时长有关)作为规划窗口的半径，其中0<ε

(7)

式中l——AGV本体长度

3.1 超声波传感器检测障碍物的配置方案

如图3所示，8路超声波传感器安装在距地高500 mm的AGV周边，探测范围0.01～5 m，波束角60°。

图3 8路超声波传感器配置方案Fig.3 Configuration scheme of 8-way ultrasonic sensors

单个超声波传感器在60°扇区内仅能获取障碍物距离信息而无法探测其方位角。为提高超声波传感器方位角测量分辨率，在AGV车头左、右角各交错布置3个超声波探头1、2、3和5、6、7，AGV车头正中水平布置超声波探头4，AGV车尾正中水平布置超声波探头0，形成AGV车头300°范围测距、车尾60°测距。在AGV车头180°范围内形成交叉重叠的测距与方位角检测，超声波传感器对单个障碍物的方位角测量分辨率提高到15°或30°，测量空间划分为9个区间，取每个区间夹角平分线对应的角度作为障碍物方位角。考虑到障碍物大小，同一障碍物有可能被2个或3个区间的超声波传感器探测到。若探头2、3、4或4、5、6同时返回信号视为单探头3或5感应。若探头3、4、5同时返回信号，则认为该障碍物不可逾越，AGV选择停车报警。若探头0检测到AGV后方障碍物距离车体小于0.5 m时，AGV发出声音预警防碰。这样，在超声波探头有限测量范围内的任意时刻，2个或2个以上障碍物位于单探头或双探头交叉区间，将所有障碍物视作单个障碍物，单障碍物的方位角θ判定如表1所示。

表1 单障碍物的方位角判定Tab.1 Azimuth determination of single obstacle (°)

3.2 AGV沿垄道行驶的前轮期望导向角获取

3.2.1基于超声波测距的垄道边界探测

如图4所示，为使AGV能够沿距离右侧道路边缘为d的路径行走，采用右侧超声波传感器探测路径边界。超声波传感器与AGV车头水平面的安装夹角为θR，超声波沿扇形中线发送，可以保证AGV在进入垄道后，传感器可探测到路径右侧柱桩及葡萄树主茎相对AGV的距离SR。AGV车身中线距右侧柱桩距离dR为

dR=SRcosθR+w/2

(8)

图4 预设路径的边界探测Fig.4 Boundary detection of default straight path

AGV车身中线相对预设路径的横向偏差Δd=d-dR，位置左偏为负，右偏为正。

3.2.2基于航姿传感器的AGV航向角检测

以大地正北方向为0°航向角，选取半程垄道按如图5a所示通过航姿传感器测取航向角作为预设航向角γ0。如图5b所示，沿AGV纵向中线安装航姿传感器，车体初始姿态沿垄道走向，γ表示AGV沿垄道行驶的方向角。将预设航向角γ0与AGV当前航向角γ的角度差Δγ作为AGV航向偏角。

图5 航向偏角测量Fig.5 Measurement of heading deflection angle

3.2.3基于模糊控制器的AGV前轮期望导向角

将AGV横向位置偏差Δd与航向偏角Δγ作为路径跟踪模糊控制器的输入，控制器的输出为前轮期望导向角φ。AGV航向偏角与横向位置偏差根据其误差分别设置5个模糊等级：左大偏(LB)、左偏(LS)、不偏(Z)、右偏(RS)、右大偏(RB)；前轮期望导向角设置7个等级：右大转(PB)、右中转(PM)、右小转(PS)、不转(Z)、左小转(NS)、左中转(NM)、左大转(NB)。则模糊控制器输入与输出对应的模糊集为

Δd={LB,LS,Z,RS,RB}
Δγ={LB,LS,Z,RS,RB}
φ={PB,PM,PS,Z,NS,NM,NB}

其中，航向偏角Δγ∈(-60°,60°)；横向位置偏差Δd∈(-0.5 m,0.5 m)；前轮期望导向角φ∈(-60°，60°)。

依据上述输入输出关系制定25条控制规则，模糊控制器输出的前轮期望导向角如图6所示，导向角连续平缓变化，可使AGV车身姿态调节平稳。

图6 模糊控制器输出的前轮期望导向角Fig.6 Expected steering angle of front wheel output by fuzzy controller

3.3 障碍物判别及轨迹预测

3.3.1同一障碍物匹配与类型判别

图7 障碍物检测Fig.7 Obstacle detection

如图7所示，以2个障碍物在车体左、右侧为例，AGV上超声波探头测得的位置数据有2个规划周期的保存时间。首先滤除与AGV相距d的道路两侧的探头数据，然后提取第1个规划周期滚动窗口内的2个障碍物位置数据ρ1,E1、ρ1,F1和第2个规划周期的ρ2,E2、ρ2,F2，将后一规划周期的数据ρ2,E2分别与前一规划周期的2个数据ρ1,E1、ρ1,F1组合计算两点之间的距离

(9)

式中ρ1、ρ2——障碍物方位距离

θ1、θ2——障碍物方位角

按式(9)计算得到HE1E2、HF1E2、HE1F2和HF1F2，判定min{HE1E2,HF1E2}对应的两点为同一障碍物，同理判定min{HE1F2,HF1F2}对应的两点为同一障碍物。

若|ρ2sinθ2-ρ1sinθ1|>δ，δ为设定的距离阈值，即2个规划周期内，存在同一障碍物到AGV现阶段运行轨迹的垂直距离发生改变，可以判定为动态障碍物，否则判定为静态障碍物。

3.3.2动态障碍物的运动轨迹预测

在AGV动坐标系中，障碍物以一定速度运动。将AGV模型化为点状质心，障碍物膨胀半径为r。因此，这种碰撞关系可以简化为点状AGV到障碍物直线运动轨迹的距离关系问题。相邻2个规划周期的障碍物位置数据ρ1与ρ2若满足|ρ1sinθ1|>|ρ2sinθ2|，则排除远离AGV的动态障碍物。由图7得动态障碍物的直线轨迹E1E2方程为

Aρcosθ+Bρsinθ+C=0

(10)

其中

A=ρ2sinθ2-ρ1sinθ1

B=ρ1cosθ1-ρ2cosθ2
C=ρ2cosθ2ρ1sinθ1-ρ1cosθ1ρ2sinθ2

3.4 AGV防撞避障策略与算法

3.4.1调节绕行偏角Δθ绕行障碍物的避障算法

来自AGV前方的障碍物阻碍包括静态障碍物和相向而来的动态障碍物，此时以改变AGV绕行偏角来避障[21]。如图8所示，以AGV质心为原点建立AGV动坐标系xOy。以AGV质心O为原点、AGV前进方向为极轴建立极坐标系(ρ,θ)，将障碍物的动坐标转换为极坐标表示，构建由距离ρ和方位角θ组成的超声波传感器测量模型。将θ方向上的障碍物按照半径r进行边缘膨胀，构建防碰撞安全保护区。

图8 绕行偏角确定Fig.8 Determination of heading deviation angle

(1)检测AGV车体与当前障碍物的相对角度

当0≤θ<π，且满足d=ρsinθ

Δθ=arcsin(r/ρ)-θ

(11)

当π≤θ<2π，且满足d=-ρsinθ

Δθ=2π-arcsin(r/ρ)-θ

(12)

(2)避障转向模型

为了实现前轮导向式AGV平滑连贯的避障转向，建立AGV避障转向模型，如图9所示。

图9 AGV避障转向模型Fig.9 Steering model of AGV obstacle avoidance

AGV在偏角转向时绕瞬心P做圆周运动，AGV前一时刻速度v所在直线与车体后轮轴线交于B点、后一时刻速度v′所在直线与车体前轮轴线相交于A点，切线AO和BO的交点O与P点的连线平分∠AOB，可得

(13)

由式(3)重新计算右前轮转角，则左前轮转角为[16]

(14)

再由式(6)得两后轮差速控制输出，实现AGV避障转向行驶。

(3)确定回归转向点与回归运动子目标点

为实现多次调节绕行偏角Δθ避障绕行，记录AGV累积姿态角θsum，θsum=∑Δθi，i为转向次数。如图10所示，调节绕行偏角Δθ的障碍物避障策略中，确定AGV以累积姿态角θsum直线行驶为执行绕行障碍物的第1阶段。当AGV到达回归转向点Pback时，即超声波传感器检测到障碍物与AGV之间的距离由减小到增大的转折点，表明AGV恰好开始远离障碍物，此时执行全局预设轨迹的回归运动，AGV回归的姿态角为-2θsum，左、右前轮转角由式(14)、(3)计算得到。

图10 调节Δθ的避障示意图Fig.10 Diagram of obstacle avoidance for adjusting Δθ

避障过程中，AGV横向位置偏差为

Δd=Tv1sinθsum

(15)

式中T——AGV规划周期，ms

v1——编码器测定的车速，m/s

当回归运动使横向位置偏差重新为零时，即为回归运动的子目标点所在位置Prec，AGV完成障碍物绕行的第2阶段，结束避障，再继续执行沿垄道预设轨迹行驶。

(4)调节绕行偏角Δθ的绕行避障算法流程

根据上述避障策略，调节绕行偏角Δθ的绕行避障算法流程，如图11所示。

图11 调节Δθ的避障算法流程图Fig.11 Flow chart of obstacles avoidance algorithm for adjusting Δθ

3.4.2调节Δv的动态障碍物避障策略

如图7所示，直线E1E2为障碍物当前时刻预测轨迹，由点到直线距离方程计算AGV质心与障碍物预测轨迹的垂直距离D为

(16)

当D≥r时，说明动态障碍物运动路径在AGV膨胀保护圆以外，对AGV无影响。

当D0，即预测轨迹与动坐标系x轴正半轴相交，说明障碍物与AGV预测碰撞点在车身前侧，AGV减速或停车，待传感器测不到此障碍物再恢复设定速度。

当D

3.4.3避障优先级

AGV默认以恒定速度沿垄道行驶，采用滚动窗口方式检测障碍物，设定AGV循迹、调节Δθ避障和调节Δv避障的决策优先级为：动态避障、静态避障、循迹，规则制定如下：

(1)无论AGV现阶段执行何种动作，检测到动态障碍物进入滚动窗口，碰撞预警程度最高，优先采取减速或停车规避。

(2)AGV预设行驶轨迹与滚动窗口中静态障碍物膨化边缘存在交集，优先执行绕行避障策略，忽略路径跟踪行驶的模糊控制器输出。

4 AGV避障控制系统设计与试验

4.1 AGV避障控制系统设计

如图12所示，轮式AGV控制系统由控制器模块、驱动模块、电源模块、传感器模块、通信模块等组成。

图12 AGV控制系统硬件组成Fig.12 Hardware composition of AGV control system

工控机作为主控制器，主要完成传感器数据采集、存储和处理，避障决策与导航；松下PLC作为AGV转向与差速驱动的控制器，接收工控机避障算法的输出指令，完成AGV前轮转向与后轮差速驱动的协同行驶控制。8路KS101B型超声波传感器通过I2C适配器构建数据传输的总线子系统。AH100B型航姿传感器(方位角测量精度0.1°)用于检测AGV航向偏角。

为防止超声波串扰，在每个规划周期以10 ms为间隔通过I2C总线顺序执行超声波距离数据的发送和接收指令。AGV前方左、右角安装的超声波传感器探测的障碍物与车体距离实际为物体表面到传感器安装位置的距离。因此，AGV质心与障碍物的相对距离ρ为实测值ρi加上补偿距离Δρ，即

ρ=ρi+Δρ

其中

利用LabVIEW平台编写AGV导航控制程序，包括主程序、沿垄道直线行驶的模糊控制器以及防碰避障子程序，主程序流程图如图13所示。

图13 AGV沿垄道行驶的导航主程序流程图Fig.13 Flow chart of main program for AGV navigating along ridge road

AGV沿垄道行驶的导航主程序主要完成系统初始化，在每个规划周期获取传感器信息，刷新滚动窗口，使AGV沿垄道纠偏行驶，并判断是否执行调节Δv或Δθ的避障子程序。

4.2 AGV避障试验与分析

4.2.1AGV试验系统

图15 AGV依次绕行2个大间距静态障碍物的避障试验过程Fig.15 Obstacle avoidance test process when AGV detoured in turn two large space static obstacles

为验证设计的AGV沿垄道行驶的避障导航算法的有效性，模拟葡萄园种植方式开展AGV导航试验。试验场地为宽3.5 m、长50 m的平整道路。距AGV右侧1.7 m处有0.8 m高的路界，距AGV左侧3.9 m处为墙面。沿道路纵向布置多个方锥形路障模拟实际葡萄园环境中的静态障碍物，方锥底座宽28 cm、高65 cm。如图14所示，试验平台为前轮转向、后轮差速驱动AGV，轴距L=93.5 cm，轮距49 cm，AGV总质量200 kg，行驶速度0.3 m/s。

图14 AGV试验系统Fig.14 Test system of AGV

4.2.2AGV沿垄道行驶的避障模拟试验

试验中以方锥形路障作为静态障碍物、行人作为动态障碍物，超声波传感器在2 m以内对障碍物探测稳定，以此设置滚动窗口半径为2 m，大于2 m的数据设为空值。试验现场如图15～17所示，避障数据如图18～20所示。AGV上的8路超声波传感器测距数据如图18a、19a、20a所示。记录AGV航向角随AGV沿垄道行驶时的采样数据，如图18b、19b、20b所示。根据测得的AGV航向角与后轮编码器记录数据，绘制的AGV实际运行轨迹如图18c、19c、20c所示，以AGV初始位置的质心O为原点建立静坐标系XOY，蓝色圆形为障碍物，黄色曲线为AGV质心每个规划周期的循迹与避障运行轨迹。

图16 AGV连续绕行2个小间距静态障碍物的避障试验过程Fig.16 Obstacle avoidance test process when AGV detoured continuously two small space static obstacles

图17 AGV绕行1个静态障碍物并躲避1个动态障碍物的避障试验过程Fig.17 Obstacle avoidance test process when AGV detoured a small static obstacle and avoided a dynamic obstacle

图18 AGV依次绕行2个大间距静态障碍物的避障数据Fig.18 Obstacle avoidance data when AGV detoured in turn two large space obstacles

图19 AGV连续绕行2个小间距静态障碍物的避障数据Fig.19 Obstacle avoidance data when AGV detoured continuously two small space static obstacles

图20 AGV绕行1个静态障碍物并躲避1个动态障碍物的避障试验数据Fig.20 Obstacle avoidance test data when AGV detoured a small static obstacle and avoided a dynamic obstacle

(1)AGV依次绕行2个大间距障碍物试验

如图15a所示，AGV从初始位置按预设航向角沿距离右侧边界1.7 m直线行驶，按照模糊控制器设计规则，此时以AGV与右侧边界横向偏差ΔdR作为模糊控制的输入。探头6、7检测的数据维持稳定。当探头3、4检测到左前方路障满足碰撞条件后，AGV实施右转绕行，如图15b所示，回正前轮转角后沿路障安全保护圆右侧切线直线行驶，此时探头4、3、2、1依次检测到障碍物，探头1、2、3、4、6、7距离数据逐渐减小，车体与障碍物及道路右侧边界的距离逐渐缩短。当探头1感测车体与障碍物距离数据增大，判断为AGV回归转向点，开始执行绕障碍物环形的回归转向运动，如图15c所示。当探头4、5检测到第2个障碍物，继续执行绕行程序,如图15d～15f所示。当AGV绕过第2个障碍物后，由模糊控制器纠偏行驶，由探头6判定车身和垄道右侧边界稳定保持1.7 m的设定距离后停车。

(2)AGV连续绕行2个小间距静态障碍物试验

图16给出AGV连续绕行2个小间距障碍物时的避障结果。如图16a所示，探头4、5依次检测到偏于垄道右侧的障碍物1，AGV左转绕行。如图16b所示，障碍物2进入滚动窗口，位于单探头或双探头交叉区间，AGV将2个小间距静态障碍物合并为同一障碍物继续左转绕行。障碍物1远离AGV，第45个采样点后采样点依次从探头5、6探测区间消失，同时障碍物2进入5、6探测区间，并逐渐靠近AGV。如图16c～16f所示，AGV由回归转向点直线行驶至回归子目标点，并纠正偏差，在垄道预设路径停车。

(3)AGV绕行1个静态障碍物并躲避1个动态障碍物试验

图17给出AGV绕行静态障碍物时加入1个动态障碍物的避障结果，动态障碍物的起点坐标(2.8，1.75)，终点坐标(2.8，-1.75)。如图17a、17b所示，AGV首先绕行垄道右前方静态障碍物，向回归转向点直线行驶过程中，以人为动态障碍物从AGV滚动窗口内横穿垄道。AGV根据同一障碍物匹配和类型判别算法，判定动态障碍物预测碰撞点在车体前侧，选择减速停车让行的避障控制策略。在图20c中，S点处停车避让，探头2、3、4、5依次检测到车体前侧的动态障碍物。如图17c、17d所示，待动态障碍物远离，AGV靠近静态障碍物，如图20a中的第100个采样点，判定合并为同一障碍物后，AGV恢复设定速度继续绕行，回到预设路径。

由上述模拟试验可知，AGV在避障行驶过程中可以局域寻找一条较优无碰路径，避障过程流畅，验证了本文提出的AGV避障导航算法的有效性。

5 结束语

针对轮式AGV沿葡萄园垄道直线行驶过程中可能存在障碍物阻碍，采用交叉布置的8路超声波传感器检测垄道边界与障碍物，提出了同一障碍物匹配与类型判据及动态障碍物轨迹预测模型。基于滚动优化原理提出AGV绕行静态障碍物、减速或停车避让动态障碍物的导航算法，模拟试验结果表明，该算法可导引AGV避免与垄道上的障碍物发生碰撞，验证了避障导航算法的有效性。