基于惯性和视觉复合导航的自动导引小车研究与设计

摘要：自动导引小车(AGV)作为智能制造的重要物料搬运设备，其导航精度及路径柔性配置性能制约了其在智能制造领域的应用范围。针对这一问题，提出了基于惯性和视觉的AGV复合导航方法，利用两种导航技术的互补优势，实现高精度导航定位，提高了AGV对工作环境的适应能力和可靠性。在此基础上，利用二维码标签构建了AGV工作栅格地图，实现了AGV行驶路径的柔性配置。测试表明复合导航方法具有纵向5mm、侧向10mm、方向偏差1°的导航定位精度。

关键词：自动导引小车(AGV)；组合导航；二维码；累积误差

0 引言

自动导引小车(automated guided vehicle，AGV)作为柔性度较高的自动化搬运设备在智能工厂及智能制造等领域发挥着越来越重要的作用[1-2]。多台AGV可组成柔性物料搬运系统[3]，搬运路线可根据制造工艺流程的变化而动态调整。AGV的导航定位精度及路径调整能力一直是其在很多工业领域推广应用的难点。目前，AGV导航方法主要有磁导航[4]、激光导航[5]、视觉导航[6-7]、信标导航及其衍生的复合导航方法。磁导航路径设置柔性差，激光导航成本相对较高且在货架密闭场所容易丢失信号。针对目前AGV主流导航技术的优缺点，本文开展复合导航技术研究[8-9]，充分发挥复合中各单项导航定位技术的优势，解决AGV行驶中的定位精度和路径柔性调整等关键性问题。

本文提出视觉与惯性导航相结合的复合导航定位方法，利用含有AGV位置信息的二维码标签构建AGV行驶的栅格地图，AGV行驶中到达二维码站点时，利用车载视觉传感器估计AGV相对于二维码标签的纵向、侧向和方向的偏差，并将其作为AGV离开站点时惯性导航初始偏差值，消除站点之间惯性导航产生的累积误差，提高导航定位精度。为提高视觉导航的实时性，降低导航控制器的计算量，AGV行驶在栅格地图中2个二维码标签之间时，视觉传感器停止工作，开启惯性导航，实时估计AGV位姿及偏差，及时纠正行驶姿态。通过2种定位方法的有机切换和互补，既可实现高精度定位，又可利用二维码标签动态调整实行路径，并且可在关键性站点利用二维码标签进行二次定位，实现关键性工位的特殊定位需求。

1 复合导航方法设计

1.1 复合导航技术框架

AGV在相邻的2个二维码标签之间行驶时，通过安装在车体上的编码器和惯性传感器估计AGV的位姿，但惯性导航积分计算会产生累积误差。针对这个问题，本文在惯性导航的基础上，引入二维码标签定位，消除惯性导航在2个二维码标签站点间的累积误差。复合导航技术框架如图1所示，当AGV行驶至二维码站点，利用视觉识别系统计算AGV相对于二维码标签的纵向、侧向和方向的偏差；当AGV离开站点时，以此偏差作为惯性导航的初始偏差，结合编码器获取的质心速度v和惯性传感器获取的横摆角速度ω，实时估算AGV在站点间行驶的位姿，并推算此时的纵向偏差、侧向偏差和方向偏差。

图1 复合导航技术框架
Fig.1 Composite navigation technology framework

图1中的导航方式判断及偏差输出模块，根据AGV在全局坐标中的位置对导航方式进行选择，以确定导航控制依据的是视觉传感器识别二维码标签计算的偏差，还是惯性导航估算的偏差。

1.2 二维码导航方法

本文利用如图2所示的编码规则对二维码进行初始编码，利用二维码标签生成车间栅格地图，构建AGV的行驶路径信息。

图2 二维码标签编码方法
Fig.2 Coding method of two-dimensional barcode label

选择基于欧氏距离的A*算法[10]对AGV的行驶路径进行规划，如图3所示，其中，箭线表示规划的AGV行驶路径。

图3 车间栅格地图构建示意图
Fig.3 Schematic diagram of the construction of workshop grid map

利用二维码标签构建AGV行驶路径L=(L0，L1，L2，…)，Ln为路径中的第n个二维码站点信息，Ln=(xn, yn, Δl, Δd, Δφ, vt, φt)，其中，xn、yn分别为第n个站点的X轴坐标和Y轴坐标；Δl、Δd、Δφ分别为纵向偏差(mm)、侧向偏差(mm)和方向偏差(°)，并且在规划的理想路径中均为0；vt为目标站点速度，mm/s；φt为目标站点偏转角度，(°)。

AGV在行驶中识别二维码标签分三个步骤：首先检测二维码标签上的3个特征点；然后检测与拟合二维码边框的边缘直线，若能检测到4条相互垂直的直线，则确认其为二维码边框；最后根据3个特征点在图像中的位置计算AGV相对于二维码标签的纵向偏差、侧向偏差和方向偏差，如图4所示。

图4 二维码偏差估计示意图
Fig.4 Schematic diagram of the estimation of two-dimensional bar code deviation

根据检测到的3个特征点像素坐标位置，估计AGV相对二维码标签的偏差。纵向距离偏差、侧向距离偏差、方向偏差分别为

Δl=Kp_d(r2+r3-urow)/2

(1)

Δd=Kp_d(vcol-c2+c3)/2

(2)

(3)

式中，Kp_d为像素距离到物理距离的标定系数，mm/pixel；(r1, c1)、(r2, c2)、(r3, c3)分别为左上、右上、左下的特征角点的像素行列坐标;urow、vcol分别为图像的行、列方向上的像素数量。

航向确定方法如下：r1<r3且r2≤r3且c1<c2且c3≤c2时，AGV方向朝北；r1<r3且r3<r2且c3<c2且c3<c1时，AGV方向朝西；r3<r1且r3≤r2且c2<c1且c2≤c3时，AGV方向朝南；r2<r3且r2<r1且c1<c3且c2≤c3时，AGV方向朝东。

定义LR为AGV的实际行驶路径，即LR=(L0R，L1R，L2R，…)。LnR为实际行驶路径中的第n个二维码站点标签检测信息，LnR=(xn, yn, Δl, Δd, Δφ, vt, φt)，其中，纵向偏差Δl、侧向偏差Δd、方向偏差Δφ分别通过式(1)～式(3)计算获得，为行驶过程中的实际偏差数值。

1.3 惯性导航方法

当AGV行驶在相邻的2个二维码站点之间时采用惯性导航，通过编码器实时获取左右轮转速nl和nr，利用惯性传感器获得AGV横摆角速度ω。ω、nl、nr的精度对导航精度有显著影响，采用卡尔曼滤波方法对nl、nr、ω进行滤波处理，消除传感器测量随机噪声，系统参数状态方程和参数观测方程分别为

(4)

(5)

式中，Xk为系统k时刻的状态；Zk为系统k时刻的测量值；Wk为随机噪声；Vk为测量噪声；nl,k、nr,k分别为k时刻AGV的左轮转速和右轮转速，r/s；nl,k-1、nr,k-1分别为k-1时刻AGV的左轮转速和右轮转速，r/s；ωk、ωk-1分别为k时刻、k-1时刻AGV的横摆角速度，rad/s；Δnl,k、Δnr,k分别为 k时刻的左轮轮速变化量和右轮轮速变化量，r/s；C为左右轮速到横摆角速度的换算系数，C=2πd/D；d为车轮半径，mm；D为轮距，mm。

利用滤波后的横摆角速度ωk和左右轮转速nl,k、nr,k对AGV的位置进行估计。AGV在行驶中的位姿估计方程为

(6)

式中，vk为滤波后的AGV质心速度，mm/s；vk=πd(nl,k+nr,k)/2；T为系统的采样时间，s。

根据式(6)，可得AGV行驶中的实时偏差估计值：

(7)

2 运动控制系统设计

2.1 AGV运动学模型

本文设计的AGV采用两轮差速驱动，运动学模型如图5所示，AGV行驶过程中的位姿是运动控制的基础。位姿是指绝对坐标系中AGV的位置和姿态可表示为P=[x y θ]T，其中，x、y分别表示相对于绝对坐标系Y轴和X轴的距离，θ为左右车轮连线的中垂线与Y轴的夹角。

图5 AGV运动学模型示意图
Fig.5 Schematic diagram of AGV kinematics model

假设t时刻右轮速度vr小于左轮速度vl，AGV行驶轨迹为圆弧向右转向，质心速度为v，在采样时间T内，转过的角度Δθ (rad)为

(8)

式中，R为轨迹圆半径。

AGV的X、Y方向的速度分量以及时间间隔T内的横摆角速度为

(9)

规定初始位姿P=[xyθ]T=[x0y0θ0]T，得到任意时刻位姿与左右轮速度之间的变换公式：

(10)

2.2 控制方法设计

根据AGV的运动学模型，通过纵向偏差估计得到AGV质心速度控制量vc，利用侧向偏差估计得到期望横摆角速度ωc，进一步分解计算得到左右轮控制转速，实现对AGV运动轨迹的控制。图6为控制系统框架图。

图6 AGV控制系统框架图
Fig.6 Framework diagram of AGV control system

2.2.1 纵向偏差控制方法

AGV纵向偏差采用闭环控制方式，定义ad为实时动态控制加速度。运动过程中，ad根据当前速度vp、目标速度vt以及纵向偏差距离Δl实时计算调整：

(11)

计算得到ad后，将其与最大加速度amax、安全系数η和最大加速度的乘积ηamax进行比较，得出在各种工况下的控制速度vc：

(12)

AGV在下一站点需要停车(vt=0)的情况下，逼近二维码站点时以缓慢速度匀速行驶，vs为此时的匀速行驶速度。

如图7所示，时间段0～t1内，AGV以最大加速度进行加速行驶；时间段t1～t2内，AGV匀速行驶；时间段t2～t3内，AGV以最大加速度进行减速行驶，并在t3时刻速度减到vs，之后匀速行驶，当视觉传感器采集到二维码标签时(t4)进行减速直到停止(t5)。

图7 纵向控制方法示意图
Fig.7 Schematic diagram of vertical control method

2.2.2 侧向偏差控制方法

根据文献[11]的路径跟踪控制方法，在AGV行驶过程中实时构造虚拟行驶路径y(x)，如图8所示。根据AGV的行驶条件对该虚拟行驶路径进行求解，得到AGV实时调整的期望横摆角速度。AGV的质心侧偏角相对较小，假设AGV的实际质心速度与纵轴方向一致。

图8 侧向偏差控制方法示意图
Fig.8 Schematic diagram of lateral deviation control method

AGV行驶的虚拟路径方程为

y(x)=A+Bx+Cx2+Dx3+Ex4

(13)

式中，A～E为虚拟轨迹方程系数。

局部坐标系下虚拟路径的构造条件如下：起始点为原点，斜率为0，曲率ρ=ω/v；预瞄点坐标为(xe, ye)，斜率为tanφe，φe为AGV在预瞄点时的方位偏差，rad。根据构造条件求解规划的AGV行驶轨迹方程：

(14)

已知路径的曲线方程y(x)、车辆质心在曲线上的坐标(x, y)、速度v，速度方向及行驶曲率与车辆当前所在曲线上点一致。定义ωd=vρ为车辆的运动轨迹为曲线方程时的理想横摆角速度，分析此时的理想横摆角速度变化率

与时间的变化规律。由式(14)与ωd=vρ可得

(15)

预瞄点坐标与侧向、方向偏差的关系为

ye=Δd+xetanΔφ

(16)

联立式(15)、式(16)可得理想横摆角速度变化率：

(17)

最终得到期望横摆角速度：

(18)

对控制速度vc(通过纵向偏差控制得到)、期望横摆角速度ωr(通过侧向偏差控制得到)进行抑制，以防止AGV在具有很大的质心速度的同时具有相对较大的横摆角速度，发生甩尾等险情。根据抑制后的控制速度和期望横摆角速度计算左右轮控制转速：

ncl=(vc-ωrd)/D

(19)

ncr=(vc+ωrd)/D

(20)

式中，ncl、ncr分别为左轮和右轮的控制转速。

3 试验车设计及测试

3.1 试验车测试

实验平台采用的差速轮式AGV的基本参数如表1所示。在Z字形和八边形的典型路径对AGV进行车间跟踪测试。

表1 差速轮式AGV基本参数
Tab.1 Basic parameters of differential wheel AGV

基本参数数值总质量(kg)60尺寸(mm×mm)1200×800两轮间距(mm)780最大空载速度(m/s)1.2最大加速度(m/s2)0.8

3.2 Z字形路径测试及分析

Z字形路径主要测试AGV在行驶带转弯的直线路径时的跟踪性能和精度，测试中行驶速度为0.6 m/s。图9所示为规划的Z形行驶路径，行驶中按箭头的指示方向跟踪轨迹。控制程序有偏差记录模块，负责记录实时数据，并保存到文件。图10、图11所示分别为Z字形路径的侧向偏差曲线和角度偏差曲线。测试表明：执行Z字形路径任务时，侧向偏差最大值为22 mm，均值为8 mm，方向偏差最大值为1°，到达终点时纵向偏差为5 mm，侧向偏差为8 mm，方向偏差0.5°。

图9 Z字形规划路径示意图
Fig.9 Schematic diagram of Z-shaped path

3.3 八边形路径测试及分析

八边形路径主要测试AGV在正常速度下的连续转弯跟踪精度，测试中的行驶速度0.8m/s。图12所示为规划的八边形行驶路径，行驶中按箭头的指示方向跟踪轨迹。图13、图14所示分别为八边形路径的侧向偏差曲线和方向偏差曲线。

图10 Z字形路径侧向偏差变化曲线
Fig.10 Lateraldeviation variation curve of Z-shape path

图11 Z字形路径方向偏差变化曲线
Fig.11 Direction deviation variation curve of Z-shape path

图12 八边形规划路径示意图
Fig.12 Schematic diagram of octagon path

图13 八边形路径侧向偏差变化曲线
Fig.13 Lateraldeviation variation curve of octagon path

图14 八边形路径方向偏差变化曲线
Fig.14 Direction deviation variation curve of octagon path

测试中，AGV能够在正常速度下以有限离散点逼近弧形轨迹，并连续转弯，执行八边形路径任务时,侧向偏差最大值为21 mm，均值为7 mm，方向偏差最大值为1°;到达任务终点时,纵向偏差为5 mm，侧向偏差为5 mm，方向偏差为0.6°。

4 结论

本文提出一种基于二维码和惯性传感器的AGV复合导航方法，将视觉导航和惯性导航有机地结合，充分发挥两种导航技术的优势，提高了AGV轨迹跟踪精度。针对惯性导航传感器存在的漂移及累计误差，研究设计了基于卡尔曼滤波和视觉识别补偿的综合误差消除和补偿方法，实现精确位姿推算。利用虚拟路径和离散二维码标签实现了AGV行驶路径的快速柔性调整，便于工厂快速构建适应于新生产工艺的物料配送系统。

参考文献：

[1] BACIK J, DUROVSK Y F, BIROS M, et al. Pathfinder- Development of Automated Guided Vehicle for Hospital Logistics[J]. IEEE Access, 2017, 5:26892-26900.

[2] HOSSAIN S G M, JAMIL H, ALI M Y, et al. Automated Guided Vehicles for Industrial Logistics Development of Intelligent Prototypes Using Appropriate Technology[C]∥The 2nd International Conference on Computer and Automation Engineering. Singapore, 2010: 237-241.

[3] HERRERO-PEREZ D, MARTINEZ-BARBERA H. Modeling Distributed Transportation Systems Composed of Flexible Automated Guided Vehicles in Flexible Manufacturing Systems[J]. IEEE Transactions on Industrial Informatics, 2010, 6(2): 166-180.

[4] PANG Y, LOPEZ DE LA CRUZ A，LODEWIJKS G. Bipolar Magnetic Positioning System for Automated Guided Vehicles[C]∥2008 IEEE Intelligent Vehicles Symposium. Eindhoven, 2008: 883-888.

[5] XU Z, HUANG S, DING J. A New Positioning Method for Indoor Laser Navigation on Under-Determined Condition[C]∥2016 Sixth International Conference on Instrumentation & Measurement, Computer, Communication and Control. Harbin, 2016: 703-706.

[6] OSMAN K, GHOMMAM J, SAAD M. Vision Based Lane Reference Detection and Tracking Control of an Automated Guided Vehicle[C]∥2017 25th Mediterranean Conference on Control and Automation. Valletta, 2017: 595-600.

[7] 张建鹏，楼佩煌，钱晓明，等. 多窗口实时测距的视觉导引AGV 精确定位技术研究[J]. 仪器仪表学报，2016，37(6)：1356-1363.

ZHANG Jianpeng, LOU Peihuang, QIAN Xiaoming, et al. Research on Precise Positioning Technology by Multi-window and Real-time Measurement for Vision Navigation AGV[J]. Chinese Journal of Scientific Instrument, 2016; 37(6):1356-1363.

[8] LI X，ZHANG W. An Adaptive Fault-tolerant Multisensor Navigation Strategy for Automated Vehicles[J]. IEEE Transactions on Vehicular Technology, 2010, 59(6): 2815-2829.

[9] WANG Hongyu, ZHANG Xiaoxia, LI Xiaolin, et al. GPS/DR Information Fusion for AGV Navigation[C]∥ World Automation Congress 2012. Puerto Vallarta, Mexico, 2012: 1-4.

[10] BELL M G H. Hyperstar. A Multi-path Astar Algorithm for Risk Averse Vehicle Navigation[J]. Transportation Research Part B, 2009, 43(1): 97-107.

[11] 王家恩，陈无畏，王檀彬，等. 基于期望横摆角速度的视觉导航智能车辆横向控制[J]. 机械工程学报，2012，48(4)：108-115.

WANG Jiaen, CHEN Wuwei, WANG Tanbin, et al. Vision Guided Intelligent Vehicle Lateral Control Based on Desired Yaw Rate[J]. Journal of Mechanical Engineering, 2012, 48(4):108-115.

基于惯性和视觉复合导航的自动导引小车研究与设计 (cmemo.org.cn)