基于视觉导航AGV的定位算法及系统设计

李 斌，刘明兴，姚明杰，麻方达，李晓帆，符朝兴

(青岛大学机电工程学院，青岛 266071)

为解决生产车间人工运输物资效率低、工人劳动强度大、企业人力成本高等问题，采用AGV完成车间物流调度已成为最佳选择之一。AGV泛指采用导引方式运输物质的轮式移动机器人，最早出现于物流系统领域，此后，应用范围逐步向各领域扩展[1]。相比于传统运输车,AGV能够实现自动启停、避障、导引、寻迹、充电等功能，体现出当前工业发展下智能化、无人化的特点。视觉定位是通过摄像头获取环境信息，将信息返回至设备控制端实现设备的定位。视觉导航根据相机安装位置可分为全局视觉导航和局部视觉导航[2]，其中全局视觉导航相机安装在场景正上方，与车体分离；局部视觉导航相机一般安装于AGV车体之上。全局视觉对AGV的定位流程可分为识别和定位[3]，首先在全局图像中识别到AGV，随后确定AGV在世界坐标系中位姿。目标识别根据识别方式分为传统目标识别方式和基于深度学习的目标识别方式。传统目标识别方式根据原理不同可以分为基于图像特征匹配的识别方式和基于颜色的识别方式。如通过混合高斯模型和轮廓提取完成目标车辆位姿的粗估，实现了泊车AGV和目标车辆的粗对齐[4]。而基于车轮点云对称性特征的车辆位姿精确估计方法通过对目标车辆的位姿做出精确估计，实现了泊车AGV和目标车辆的精确对齐。Donmez等[5]在机器人上设计了三个标记点，根据标记点与目的地组成三角形的边长和夹角控制速度和方向，并与PID和fuzzy-PID进行了实验比较。对机器人做标记进行识别定位的方式还有设计标记卡、扫描二维码等[6-7]，如通过在移动机器人上设计的蝴蝶式标记进行定位，利用颜色外形等信息通过全局摄像头定位在足球机器人中应用较多[8]。传统目标检测方式原理简单、成本低、易实现，但需提前设计特征，对复杂环境检测效果不好。基于深度学习的识别方式分为基于回归的One-Stage和基于区域候选的Two-Stage方法[9]。One-Stage方法主要包括YOLO、SSD两种，Two-Stage方法主要包括R-CNN、Fast R-CNN、R-FCN等[10]。万元芳[11]分别采用传统图像分割和基于深度学习的图像分割方式识别机器鱼的位姿；段晓磊等[3]主要针对AGV遮挡丢失问题，利用长短期记忆网络(Long Short-Term Memory，LSTM)预测目标运动状态，支持向量机(Support Vector Machine，SVM)进行再识别和矫正。YOLO算法检测目标时，利用均值漂移跟踪目标对AGV进行定位[12]。基于深度学习的目标检测准确率和精度较高，对复杂环境检测优于传统方法，但是检测模型需要提前训练，速度相对较低。为此，本文选用基于深度学习的目标检测方法对AGV定位，训练YOLO检测模型，并通过卡尔曼滤波、ORB算法进一步提高定位速度，最后采用巴氏系数判断定位结果的准确性。

1 总体设计方案

1.1 方案设计

本系统由硬件系统及软件算法构成。硬件系统主要包括图像采集模块、AGV和PC上位机三个模块，整体控制流程如图1所示。图像采集模块将实时拍摄的车间生产场景传输到PC，PC负责处理图像和任务调度，并通过图像实时确定AGV位置和规划路径。PC规划好每个AGV行走路线后，通过局域网向AGV发送指令，控制其沿指定路径行走，并持续定位防止AGV偏离路线。在AGV到达目的地停止后，PC将AGV位姿记录到数据库中，在下次开始导航时，直接获得AGV位置姿态，缩小搜索范围。软件算法采用YOLOv4算法进行关键帧检测，非关键帧通过ORB特征点与卡尔曼滤波结合的方式定位。

图1 导航系统整体控制流程

1.2 硬件选择

1.2.1 摄像头选型 采用单目摄像头作为图像采集模块。车间模拟环境范围为4 m×6 m，摄像头悬挂高度为5 m。使用的摄像头为海康威视DS-IPC-T12-I，焦距为4 mm，分辨率为1 920×1 080。

1.2.2 仿真环境和硬件参数 本文通过基于深度学习的目标检测方式对AGV定位，不同的硬件和软件环境检测效果不同，采用的仿真环境和硬件参数见表1。使用MySQL数据库，记录AGV每次停车时位置信息，并记录AGV具体物流信息。

表1 仿真环境和硬件参数

1.2.3 运行空间数字化环境构建 全局视觉定位中涉及的坐标系有世界坐标系、相机坐标系、图像坐标系和像素坐标系[13]。世界坐标系用W表示，描述相机位置(m)；相机坐标系以光心为原点(m)，用字母C表示；图像坐标系中光心为图像中点(mm)；像素坐标系原点为图像左上角(pixel)，将图像从上到下作为x轴正向，从左到右作为y轴正向，以xoy表示。在全局视觉定位算法中，根据AGV在图像中的像素坐标和像素坐标与世界坐标的转化关系来确定AGV在世界坐标系下的位姿。

1.3 算法定位流程

基于目标检测的AGV定位算法整体流程如图2所示，间隔n帧通过YOLO检测AGV定位，中间帧通过卡尔曼滤波和ORB特征点相结合的方式定位。首先通过YOLO目标检测选定目标AGV的初始位置，并保存此时检测区域作为模板；后续帧利用卡尔曼滤波预测下一帧AGV出现位置，并通过特征点与AGV中心点相对位置矫正AGV定位位置，当间隔n帧后再次通过目标检测矫正定位结果，减小累计误差。

图2 基于目标检测定位流程图

2 定位算法实现

2.1 YOLOv4定位导航

2.1.1 YOLOv4算法 选择YOLOv4作为目标检测的方法对AGV进行定位。YOLOv4算法实现了端到端的目标检测，采用Darknet-53的网络结构提取图像特征，Darknet-53网络结构如图3所示。图像在训练过程中会得到3个不同尺度的特征图，小尺度特征图的感受野比较大，所以可以检测大尺寸目标，而大尺度的特征图适合检测小尺寸的目标。将特征图划分为S×S的网格，对于不同尺度和数量的特征图使用不同尺寸的先验框，每个网格负责检测中心点落在该网格内的目标。特征图中每个网格会预测3个边界框，每个边界框要预测的信息包括：边界框位置信息(x，y，w，h)，其中x、y为边界框中心点坐标，w和h为边界框的宽和高[14]；物体识别度Pr(object)，当目标中心点恰好落入该网格内，则Pr(object)=1，否则为0；voc数据集有20类，coco数据集有80类。

图3 Darknet-53

边框位置信息(x，y，w，h)可以通过先验框解码得到

>bx=σ(tx)+cx
by=σ(ty)+cy
bw=pwetw
bh=pheth

(1)

其中，bx,by,bw,bh为边界框相对于特征图的位置和大小，σ为激活函数sigmoid，σ(tx)和σ(ty)是先验框中心点与左上角格点的偏移量，pw,ph为先验框的宽和高，tw,th是尺度缩放。将计算结果乘以特征图采样率后即可得到正式检测框信息。

置信度可以度量框中是否存在对象的概率和边界框位置的准确度

(2)

2.1.2 YOLOv4算法的数据集训练 YOLO中损失函数包括分类损失、定位损失和置信度损失，通过加权三者得到总损失。IOU用来衡量模型检测计算结果的好坏程度，表示预测框与实际物体位置的交并比，当比值为1时说明两者完全重合。目标训练过程中损失(loss)曲线和Avg IOU曲线如图4所示(图中横坐标batches表示数据集的大小)。

图4 训程图(a)损失曲线;(b)区域平均IOU曲线

使用900张AGV图片作为数据集训练YOLOv4网络，训练次数为5 000次，由图4可知，loss趋近于0，IOU趋近于1，说明训练次数足够。

2.2 基于目标检测的AGV导航定位算法

直接使用YOLOv4对AGV定位时，速度较慢。定义初始位置及循环间隔n帧之后的帧数为关键帧，采用关键帧检测可以极大提高检测速度。提高效果与间隔帧数有关，间隔越多速度越快，但当中间间隔帧数过多时，无法对AGV进行检测和监控状态，容易偏离预定路线而发生撞击。为此，对间隔帧采用卡尔曼滤波和ORB特征点结合的方式定位，实现间隔帧状态下对AGV的检测和监控。由于关键帧能矫正检测位置，故对间隔帧定位算法的精度要求较低，但对定位速度有一定要求。

2.2.1 卡尔曼滤波原理 卡尔曼滤波(Kalman filtering)认为观测值可信度模型、控制模型符合正态分布，信号是线性变化的，在运算过程中仅通过上一时刻的最优估计和此时刻的测量值进行迭代，而不涉及过去时刻的状态值与观测值，有效减小了运算过程的计算量，因此具有计算量小、递推性好等优点。

由于卡尔曼滤波适用于信号是线性变化的系统上，当间隔帧数过大会导致AGV位置变化非直线，影响滤波效果，且间隔帧数越大影响越大，故应通过实验将间隔帧数控制在合适的范围内。

AGV在车间行驶速度较慢，卡尔曼滤波预测当前时刻状态比较准确，AGV定位过程主要关心中心点位置。若仅预测中心点位置，由于卡尔曼滤波不支持多步预测，当间隔帧数过多时，中心点位置会发生漂移。

2.2.2 ORB算法 为解决卡尔曼滤波多步预测不准确的问题，通过中心点与特征点间空间位置关系重新计算中心点位置，将其作为测量值输入到卡尔曼滤波中。特征点代表图像中带有特征性质的点，具备旋转或尺度不变性或小的仿射变换的不变性。特征点寻找技术包括SIFT、SURF、FAST、ORB等。由于ORB 算法速度快，受噪声影响较弱，因此选用ORB特征点。

ORB(Oriented Fast and Rotated Brief)算法可以对图像中的特征进行提取和创建向量，这些特征向量可以用来识别图像中的对象。ORB通过FAST算法从图像中检测特征点，对不同尺度的图片检测特征点[15]。ORB通过BRIEF算法(Binary Robust Independent Elementary Features)将一组特征点转换为二元特征向量，向量中仅含0、1两个值，可以高效存储在内存中，便于快速计算。为使描述具备旋转不变性，ORB以特征点为原点，以特征点和取点区域的质心的连线为x轴建立坐标系[16]。使用ORB算法分别在模板图像和目标图像上创建描述符，根据描述符之间距离可以进行匹配，在目标图像上识别目标。

2.2.3 卡尔曼滤波和ORB特征点的中心点定位 由于两次检测之间间隔帧数不会过大，因此认为AGV在图像上短时间内不会发生形变，AGV上各点相对位置不发生变化，AGV外形宽和高不变，即选择框大小不变。目标检测框选的AGV区域作为感兴趣区域，提取ORB特征。以预测中心为选择框中心，选择区域并提取特征，与目标检测结果匹配，通过中心点与目标检测区域特征点位置关系，计算中心点在目前帧的位置，将计算的中心点坐标作为测量值输入到卡尔曼滤波。

目标检测中心点O1坐标为(xo1,yo1)，O1在当前帧对应匹配对的特征点O2坐标为(xo2,yo2)，特征点A1的坐标为(xa1,ya1)，A1在当前帧对应匹配对的特征点A2坐标为(xa2,ya2)，特征点B1坐标为(xb1,yb1)，B1在当前帧对应匹配对的特征点B2坐标为(xb2,yb2)，求解当前帧AGV中心点坐标(x,y)，将A2O2看作由A1O1平移旋转而来

(3)

其中，T为平移矩阵，R为旋转矩阵，tx、ty分别为将A1点平移到A2点x、y轴的平移量，θ为平移后A1B1与A2B2夹角。A1、B1点距离不应太近，为保证计算的精确度，可选择多对特征点计算。

图5为间隔25帧使用YOLO检测一次时仅使用卡尔曼滤波与根据ORB特征点定位中心点效果对比，图中A边框为仅使用卡尔曼滤波结果，B边框为结合ORB特征点定位后的结果，边框中心点为预测或定位的AGV中心点位置。

图5 基于卡尔曼滤波和ORB特征点的中心点定位

与特征匹配方式相比，该方法利用卡尔曼滤波提前确定AGV位置，仅需对选择框进行调整移动即可准确定位AGV，不必搜索整幅图像进行匹配，比直接模板匹配更加快速。当两幅图像匹配不上时，使用YOLO检测定位目标，当YOLO也无法检测到AGV时，说明目标丢失，发送停止命令，直至再次检测到AGV。MOSSE目标追踪算法检测一帧图像花费21.46 ms，本方法花费时间为19.04 ms。当多AGV运行时，可根据数据库记录上次停车位置，选择目标AGV。定位过程可参考SORT算法进行多目标追踪，通过最大交并比IOU关联目标，使用匈牙利指派算法[17]关联检测框到目标，通过特征点和卡尔曼滤波进一步提高运行速度。

2.2.4 评价器设计 通过卡尔曼滤波结合ORB算法对AGV定位，可能由于ORB特征点匹配错位导致定位不准确，因此需要一个评价器来判断定位是否准确。巴氏系数(Bhattacharyya coefficient)可以测量两个样本之间的相似性，故通过巴氏系数计算定位结果与YOLO检测结果的相似度。当相似度小于阈值时，则认为本次定位结果不准确，重新使用YOLO进行识别定位。巴氏系数其中，p为模板的颜色直方图，q为待检测图像的颜色直方图。ρ(p,q)越大说明两图片越相似，当为同一张图像时ρ(p,q)=1。

(4)

将YOLO检测结果作为模板，将卡尔曼滤波结合ORB算法获取的非关键帧作为待评价区域，通过多组图片实验后选取0.9作为阈值。

3 结果与讨论

本节对改进算法(设计的AGV定位算法)进行定位精度、定位速度实验验证，确定改进算法是否可满足实际应用要求。在间隔一定帧数后重新检测的累积误差主要与检测精度和间隔帧的定位误差有关，可以直接反应在定位精度上。

3.1 图像去畸变

如图6所示，摄像头拍摄场景时，图像存在较大的畸变，产生定位误差，因此要对摄像头去畸变。本文使用张正友摄像机标定法结合OpenCV对摄像头进行标定，获取相机内参，提高精度。摄像机标定过程中使用的标定模板如图7所示。标定模板中共有69个角点，每个棋盘格宽度为2.75 cm。

图6 摄像头拍摄场景

图7 标定模板

相机内参为

(5)

矫正后模拟场地如图8所示，为检验AGV定位精度，需要计算每个像素点代表的实际距离。因此本文任选4个点，测量4点间实际距离和对应间隔像素数。经过计算得单位像素的宽度对应实际2.3 mm。

图8 去畸变后图像

3.2 误差校正

由于摄像头投影关系的影响，当AGV行驶至边缘时，在图像上测量的中心点与实际中心点位置像素坐标会不一致。AGV高度为29 cm，在行驶路径上任取6个点，实际坐标点与采集像素坐标点误差见表2。可知x轴方向误差近似保持不变，平均误差为9.83像素。y轴方向的测量坐标yc和y轴方向误差δ的拟合结果如图9所示。

图9 y轴方向误差拟合结果

表2 测量结果与实际结果对比

δ=-0.2417×yc+163.4

(6)

控制AGV从(532，1313)沿直线行驶至(513，839)，改进算法测得的路径坐标校正结果其中，(x,y)为校正后坐标，(xc,yc)为算法定位坐标。

(7)

3.3 定位速度

对仅使用YOLO、YOLO间隔2帧及改进算法三种定位方式实验验证。定位速度见表3，实验采用同一视频，花费时间为检测视频所花费的总时间，平均帧数为每秒检测帧数，由总帧数除以花费时间得到。

由表3可知，改进算法(间隔2帧)定位速度为24.96帧/s，考虑该算法对非关键帧也进行了定位，因此其定位速度慢于YOLO定位(间隔2帧)。但随着改进算法中间隔帧数逐渐增多，定位速度也逐渐加快，改进算法在间隔12帧时定位速度已达29.81帧/s，高于YOLO定位(间隔2帧)定位速度，在间隔24帧时定位速度比间隔2帧及YOLO定位速度都有明显地提高。

表3 基于目标检测定位速度对比

3.4 定位精度

对定位精度采用定位生成路径与实际路径的欧式距离做出评价，分别计算像素坐标系、世界坐标系下的最大误差、最小误差及平均误差。定位精度见表4，部分定位方式轨迹如图10至13所示(图中pixel表示像素)。

图10 YOLO定位(a)AGV实际路径与定位路径；(b)误差统计

图11 YOLO(间隔2帧)定位(a)AGV实际路径与定位路径；(b)误差统计

图12 改进算法(间隔2帧)定位(a)AGV实际路径与定位路径；(b)误差统计

图13 改进算法(间隔24帧)定位(a)AGV实际路径与定位路径； (b)误差统计

表4 基于目标检测定位精度对比

可知，改进算法(间隔2帧)定位精度为11.13 mm，相较于YOLO定位精度10.76 mm及YOLO(间隔2帧)定位精度10.52 mm都有降低。随着改进算法中间隔帧数逐渐增大，定位精度也逐渐降低，但下降很小，从(间隔2帧)时的11.13 mm下降到(间隔24帧)时的11.89 mm。此外由于间隔帧数过多，AGV运行的稳定性也有降低。由于YOLO算法在每次检测时都会重新生成选择框，选择框大小的变化及与真实选择框的差别，会导致定位中心点发生波动，但实验整体结果较稳定。

综上，较传统的YOLO算法，改进算法的定位精度和稳定性有一定下降，但其精度在间隔帧数增大时下降很小，从间隔2帧的11.13mm下降为间隔24帧的11.89 mm，且系统整体较稳定。较传统YOLO算法，改进算法定位速度有明显提高，从15.41帧/s提高到33.92帧/s，且随着间隔帧数的增多,定位速度提升更加明显，从间隔2帧的24.96帧/s提升为间隔24帧的33.92帧/s。故在满足定位精度及稳定性的前提下，使用基于改进算法设计的AGV导航可最大限度地提高定位速度，提升自动化效率。

4 结论

本文提出一种基于深度学习的目标检测及定位方法，采用YOLO算法进行关键帧检测，通过ORB特征点与卡尔曼滤波结合的方式定位非关键帧。该算法在满足定位精度的前提下，有效提高了定位速度。今后研究考虑将新算法扩展应用于不同场景中，满足实际应用对AGV导航定位速度的要求。