室内AGV导引和定位系统设计

0 引言

近几年来，AGV被广泛应用于物流搬运、分拣、无人泊车、安防巡逻等领域中。导引和定位技术作为AGV控制系统最重要的技术之一，关系到AGV的安全性、可靠性和高效性。

目前AGV主要通过视觉、磁感应、激光、惯性、无线定位以及组合方式进行导引和定位。利用Xilinx公司的ZYNQ-7020开发板Zedboard处理4路摄像机数据，并应用于复杂环境下的AGV自主导航，文献[3]基于机器视觉技术进行AGV导引，通过特征提取和SVM分类器对人工标识进行识别，对场地的改造和运行环境都较为苛刻。文献[4]设计了一种RFID与图像处理结合的AGV导引与定位技术，虽然定位精度高，实时性好，但是需要运行环境布满大量射频卡。文献[5]采用磁钉、陀螺仪和编码器进行室内AGV定位，需要对运行环境进行大量改造、安装磁钉，不适合推广，文献[6]利用激光雷达实时检测室内环境并对AGV进行定位，系统的定位精度高、实时性好，但是激光雷达的成本过于昂贵。文献[7]利用安卓设备开发了一种基于位置指纹算法和改进K最近邻匹配算法的Wifi信号强度指示定位系统，无法保证实时性。文献[8]和文献[9]设计一种基于WLAN和超宽带技术的RSSI定位系统，但是定位精度低，无法满足工程应用需求。

本文以ZYNQ-7000为核心，设计了融合CMOS传感器、Zigbee收发器、低成本MIMU传感器的AGV室内定位系统，实现低成本、高精度的AGV室内导引定位系统。

1 系统硬件设计

系统主要包括安装在AGV上的MIMU传感器、CMOS相机（安装在AGV前后左右方向上的正中间）、Zigbee端节点、以ZYNQ7000为核心的数据处理单元，以及安装布置在工作区域的Zigbee基站和指纹服务器，系统组成如图1所示。图1中Zigbee基站、服务器来自成熟产品，通过光纤以太网互联，本文主要围绕安装在AGV上的定位定向系统进行说明。

1.1 Zigbee端节点电路

Zigbee端节点电路主要由C2520外围电路及射频天线电路组成。C2520的四线SPI接口(MISO、MOSI、CSN、CLK）、芯片复位引脚（RSTN）和数字稳压器使能引脚（VREG_EN)与主控芯片XC7Z020的PL端连接，并且XC72020作为主设备，DCOUPL引脚外接100nF电容输出18V去耦电压，但不能供给其它设备使用。RBIAS引脚外接56KQ电阻作为参考电流的儒置电阻。射频天线电路由分立电容和电感组成，采用传输线平衡/不平衡变换的方式连接，外接50Q阻抗的天线负载。Zigbee 端节点电路原理如图2所示。

1.2 MIMU传感器电路

MIMU传感器电路主要由MPU-9150及外围电路组成。芯片不采用外部时钟输入的工作方式，因此CLKIN引脚接地。I2C总线（SDA、SCL)与XC7Z020的PL端自定义的MPU-9150数据采集IP核连接，并且MPU-9150作为从设备。系统不采用帧同步输入（FSYNC)功能，该引脚接地。引脚ADO接地，器件地址为0x68（电子罗盘的地址为0xOC）。MIMU传感器电路原理如图3所示。

1.3 CMOS传感器电路

CMOS传感器电路主要由OV7725及外围电路组成。系统采集RGB565格式图像，因此数据总线与D[9：2]相连。功耗选择模式（PWDN）和帧同步输入信号（FSIN）正常情况下接地即可。SCCB接口（SDA、SCL）的物理层本质是I2C，需要上拉电阻。输入时钟信号（XCLK）提供24MHz的时钟输入，SCL最高为400KHz，图像输出能够达到每秒10帧，满足系统需求。像素输出时钟（PCLK）、帧同步输出信号（VSYNC）、行同步输出信号（HREF）用于信号采集的控制和同步。OV7725的引脚接口按照功能可以分为控制接口和数据采集接口。控制接口（包括引脚XCLK、RST和SCCB接口）与XC7Z020的PL端自定义的OV7725控制IP核连接；数据采集接口（包括引脚PCLK、VSYNC、HREF和数据总线D[9：2])与XC7Z020的PL端自定义的OV7725图像数据采集IP核连接，1路CMOS传感器电路原理如图4所示。

1.4 XC7Z020电路

XC7Z020电路基于Xilinx的ZYNQ-7020开发板Zed-board，通过Vivado套件进行软硬件协同设计，PS端作为主控制核心运行算法。PL端采用IP核集成的设计思想与外围电路连接，电路如图5所示。PS与PL之间的连接基于AXI总线。VDMA作为DDR3与PL进行高速数据传输的机制，具有S2MM和MM2S两个数据端口。S2MM端口用于PL到DDR3的数据传输，MM2S端口用于DDR3到PL的数据传输。OV7725设备驱动IP核通过AXI总线与VDMA连接，并通过AXI-HP接口与PS连接，最终通过DDR3缓存与ARM进行数据共享。CC2520及MPU-9150驱动IP核挂在AXI总线上，并通过AXI-GP接口与PS连接。

2 算法及软件设计

根据系统功能划分，系统软件主要包括惯性导航、RS-SI定位、图像采集和处理、组合导航，系统软件总体设计流程如图6所示，通过定时器中断完成外围传感器的操作及算法处理。

1s定时中断：向基站发送定位请求帧，基站将信号强度值汇总至指纹库服务器，服务器进行指纹匹配并通过基站反馈定位坐标；

20ms定时中断：周期采集MIMU的惯性数据进行惯性导航解算；

100ms定时中断：采集CMOS数据并通过图像处理提取车道线与AGV的相对位置关系。

2.1 RSSI定位方法

RSSI定位[12]利用无线信号在空间传播的过程中会发生信号强度随着传播距离的增加而单调衰减的特性，通过建立信号强度与传输距离之间的函数关系，采用三点定位算法实现定位功能，但是受多径效应影响很大。由离散的坐标和信号强度数据组成的指纹库充分保留了每个坐标的多径特征，多径影响在同一坐标上具有唯一性，适合应用在复杂的室内环境。RSSI定位软件流程如图7所示。

系统捋10m×20m的平面区域按0.5m距离栅格化成若干个正方形，一共924个坐标点。定位系统在各个坐标上测量4个基站的信号强度，生成指纹数据库见表1。

系统采用1s定时中断的方式发起定位请求，基站受到定位请求帧后将信号强度通过光纤发送至指纹服务器，指纹服务器捋汇总的信号强度值与指纹数据库进行比较，采用最小欧式距离的方式匹配，匹配公式如下

其中，rssi_m是当前坐标发送Zigbee数据到第m个基站的信号强度，rssi_m,n是指纹数据库中第n个坐标发送Zigbee数据到第m个基站的信号强度，取d_n最小时所对应的坐标即为匹配坐标。

2.2 图像采集及视觉导引

图像采集、视觉导引软件的流程如图8所示。

系统在每次上电后都需要对OV7725进行配置。配置通过SCCB总线进行，采用三相写数据的方式进行寄存器配置：发送设备地址、回应、发送寄存器地址、回应、发送寄存器数值。OV7725的地址是0x42，寄存器的地址是0x00~0xAC，配置见表2。

寄存器配置完成后，从100ms定时中断进入图像采集流程。当第一个帧同步信号到来，进行写控制操作（包括写使能和写复位的电平操作）；当第二个帧同步信号到来，进行读控制操作完成FIFO的准备工作，然后控制读时钟完成一帧图像的8位并行数据读取，触发DMA传输。

图像处理由DMA结束中断触发，通过事先设定的感兴趣区域截断图像数据并进行ostu阀值分割，提取车道线像素坐标集（x^ostu，y^ostu）

其中，t_ostu是阀值分割算法得到的阀值。

车道线像素坐标集通过最小二乘法进行直线拟合后，得到直线参数k和b

2.3 RSSI/SINS/视觉组合导航算法

取惯导误差作为卡尔曼滤波器的状态估计量

其中，[ΦE,ΦN,ΦU]知，和是姿态角误差，[δV_E，δV_N，δV_U]是速度误差，[δL，δλ，δh]是位置误差，[ε_x，ε_y，ε_z]是陀螺误差，[∇_x，∇_y，∇_z]是加速度计误差。根据惯导误差方程，滤波器状态更新方程如下

其中，系统噪声满足W~N（0，Q）。

视觉导引的角度和无线节点定位的位置经过坐标系转换后得到[ ψ_vision,L_rssi,λ_rssi]与惯导的差值可作为滤波器的观测，观测方程如下

其中，量测噪声满足U~N(O，R）。

卡尔曼滤波器根据状态更新方程和观测方程对惯导误差进行最优估计和输出校正，并周期性反馈校正惯导参数，滤波器方程如下

3 实验结果与分析

实验在室内10m×20m的矩形区域内进行，区域内设计了闭环的车道线，由大圈和小圈组。AGV沿着车道线自动导引，大圈小圈交替进行3圈。

（1）视觉导引性能

某时刻的图像处理结果如图9所示，图像中的矩形区域为预先设置的感兴趣区域，经过阀值分割后成功提取出车道线的像素，直线拟合的结果与车道线中心相符，视觉导引满足应用需求。

（2）定位性能

图10给出RSSI和组合导航的定位结果。由于AGV自动导引的控制精度存在一定的误差，导致AGV的运行轨迹与车道线有一定的偏差。相比于RSSI定位结果，组合导航结果更平滑，精度更高。忽略导引控制精度的误差，组合导航的定位精度约为0.5m，满足应用需求。

4 结束语

本文设计的嵌入式室内AGV导引和定位系统，以ZYNQ7000SOC为核心进行数据采集和处理，实现低成本、高精度的组合导航功能。相比于现有的AGV导引和定位系统，该系统复用视觉导引的角信息来修正导航，具有更高的系统集成度，降低应用现场的改造需求和依赖度，达到降低综合成本的目的。

该系统经过室内动态实验，验证了视觉导引及定位的功能和精度，定位精度优于0.5m，能够普逼适用于室内AGV的导引和定位需求，具有较高的实用性和工程应用参考价值。