磁导引与UKF滤波定位的轮式AGV路径跟踪研究

尹建军，余承超，贺 坤，刘继展

（江苏大学现代农业装备与技术教育部重点实验室，江苏 镇江 212013）

1 引言

轮式自动导引车（Automated Guided Vehicle，AGV）适应于现代农业发展要求，逐渐应用于对靶喷药、物资搬运、果实采摘套袋、农作物信息收集等方面[1-2]。AGV作为果实采摘机器人的载体，其自主导航行走系统利用携带的传感器得到自身在作业环境中的位姿并实现导航运行，系统的稳定性将直接影响整个果实采摘环节。然而，目前国内有关采摘机器人自主导航行走系统的设计研究，无法满足实际采收作业环境的要求。因此，稳定可靠的自主导航行走系统具有重要的科学研究意义。

移动机器人作为典型的非线性系统，难以建立精准的数学模型，以单磁导引横向偏差和两次采样周期计算得到的AGV与磁条相对角度偏差作为模糊控制器输入，结合人类驾驶车辆经验制定规则库，得到AGV速度与导向角控制输出。自动导航系统采用的核心导航算法，根据机器人应用的传感器种类，主要包括[3-6]：图像处理、航位推算、模糊逻辑、神经网络、与遗传算法以及卡尔曼滤波器。考虑自主设计农业轮式AGV 采用航姿参考系统、磁导航传感器、后轮交流伺服电机编码器等传感器探测方法，结合常用数据融合方法优缺点，选用无迹卡尔曼滤波器（UKF）进行多传感器信息融合的AGV位姿定位方法。并通过试验验证算法的有效性。

2 四轮转向AGV运动学建模

自主设计的AGV为四轮转向后轮差速驱动形式，左、右前轮转角采用一个交流伺服电动机与一个伺服电动推杆实现左、右前轮转角独立控制，后轮采用两步进电动机实现两后轮独立转向并使用两交流伺服电动机作为行驶驱动。

AGV在全局坐标系下位姿定位运动学模型，如图1所示。

图1 AGV位姿估计模型示意图Fig.1 AGV Pose Estimation Model

图中：P—AGV 速度瞬心；M—AGV 质心；φ—前轮导向角；Δθ—AGV 旋转角；R—转向半径；ω—AGV 航向角速度；θ—车体姿态角，即全局坐标系∑xIOyI的x轴到AGV局部坐标系∑xROyR的xR轴的夹角，左右轮轮距W=550mm，前后轮轴距L=870mm。

AGV 离散化运动控制，在不同的采样时间点，根据AGV 当前位姿、目标位姿及系统输入信号进行姿态控制。输入信号为导向角φ及两后轮转速，设ti时刻AGV位姿为ξIi=(xi，yi，θi)，经过采样时间间隔Δt，AGV 围绕其瞬时旋转中心运动到当前位姿ξIi+1=(xi+1，yi+1，θi+1)，在此过程中AGV位姿变化量ΔξI=(Δx，Δy，Δθ)，对于图1所示的运动学模型，可以得到[7]：

式中：R=L/2tanφ，由式（2）可以得到任意采样时刻AGV当前位姿，从而为自主导航定位提供数学基础。

3 基于模糊控制器的四轮转向AGV路径跟踪研究

将磁导航传感器探测得到AGV与导航路径相对位置偏差e与通过磁导航传感器计算得到AGV与导航路径相对角度偏差α，作为模糊控制器的输入。针对单一磁导航传感器仅能直接得到位置偏差e，提出一种采用两次采样周期得到的位置偏差计算磁条相对角度偏差α的方法，原理示意图，如图2所示。AGV单周期瞬时运动看做直线运动，ti时刻对应磁条相对偏差ei，ti+1时刻为ei+1，AGV直线行进距离S。

图2 角度偏差检测示意图Fig.2 Angle Deviation Detection Diagram

将AGV位置偏差与相对角度偏差控制输入分别设置7个模糊等级：左大偏（NB）、左中偏（NM）、左小偏（NS）、不偏（ZE）、右小偏（PS）、右中偏（PM）、右大偏（PB）。类似可以得到，导向角控制输出量：左大转（NB）、左中转（NM）、左小转（NS）、不转（ZE）、右小转（PS）、右中转（PM）、右大转（PB）；速度控制输出：L（1km/h）、S（1.5km/h）、M（2km/h）、B（2.5km/h）、V（3km/h）。

其中，横向偏差e的论域为［-9cm，-9cm］；角度偏差α论域为［-36°，36°］；导向角控制量Δφ论域为［-π/3，π/3］；行驶速度控制量v论域为［0.1m/s，0.3m/s］。依据以上输入输出关系制定98条模糊控制规则，在MATLAB模糊控制箱中生成AGV控制曲面，如图3所示。

图3 MATLAB模糊控制箱Fig.3 MATLAB Fuzzy Control Box

4 基于多传感器信息融合的AGV位姿定位方法

轮式AGV 主要面向温室、果园等环境果实的采摘作业时，作业环境复杂，使用单一传感器获取周围及路径信息，往往具有片面、局部、抗干扰性差、可信度低等问题。因此本系统采用多种传感器对AGV 姿态观测并进行信息融合得到AGV 实际运行位姿及路径信息。传感器包括：AH100B 型航姿参考系统（AHRS）、CA-16D 型磁导航传感器、后轮交流伺服电机编码器等。将AGV 初始位姿及磁导航传感器、航姿传感器、后轮交流伺服电机编码器等传感器测量信息，通过UKF卡尔曼滤波器进行信息融合获取系统导航参数的动态过程，准确显示AGV运动状态演变。

无迹卡尔曼滤波器（UKF）是一种典型的非线性估计方法，该方法通过一种非线性变换-U变换并结合标准卡尔曼滤波来进行非线性模型的状态与误差协方差更新，由于UKF没有线性化过程，因此对于非线性模型其估计结果拥有更高的估计精度[8-9]。

以AGV在全局坐标系中位姿估计模型推导得到的坐标（x，y）与航姿参考系统的航向角θ作为UKF卡尔曼滤波器的状态向量，以两后轮编码器得到AGV里程数据及AGV导向角φ作为系统控制输入量，AGV系统的状态方程可以表示为：

式中：X（t）—AGV的状态向量，状态向量所对应的方差阵为Pk；U（t）—系统控制输入量，表示AGV两后轮编码器得到的里程数据及导向角；wk-1—高斯白噪声。

根据四轮转向AGV位姿估计模型，则AGV系统状态方程可以表示为：

设k时刻AGV所在全局坐标系下位置为[xk，yk，θk]T，根据如图2所示磁导航传感器输出计算得到的横向偏差e及航姿参考系统输出的航向角θ转化为AGV 系统位姿，选取Zk=[x y θ]T作为观测量，则量测方程Zk可以表示为：

式中：vk—服从高斯分布的零均值白噪声序列。

根据UKF 卡尔曼滤波器基本模型及推导的AGV 系统状态方程与量测方程，四轮转向AGV组合定位UKF滤波算法流程图，如图4所示。主要包括系统初始化、样本点计算、状态更新、量测更新、滤波增益计算、循环转跳等步骤。

图4 四轮转向AGV组合定位UKF滤波流程图Fig.4 Four-Wheel Steering AGV Combined Positioning UKF Filter Flow Chart

5 四轮转向AGV行走系统试验研究

5.1 AGV测控系统硬件结构设计

AGV控制系统硬件主要包括：传感器系统、工控机、PLC、电动机驱动系统、无线遥控、供电系统等，如图5所示。采用主从分布式控制结构，上位机工控机基于LabVIEW 平台实时进行多传感器数据信息的获取、处理、存储、显示等，下位机PLC与工控机通过串行通信接收AGV 四轮转向运动控制信号，完成前进、后退、左转、右转、加减速等运动，并反馈驱动电机编码器信息[10]。

图5 AGV硬件系统Fig.5 AGV Hardware System

5.2 AGV测控系统软件设计

四轮转向AGV位姿估计程序采用LabVIEW软件设计，其主要完成与PLC程序通讯获取16位磁导航传感器以及后轮驱动电机编码器信息采集，通过串口获取航姿参考系统（AHRS）航向角信息，最后采用UKF滤波器对采集信息进行融合得到AGV位姿状态估计，如图6所示。

图6 UKF滤波程序设计流程图Fig.6 UKF Filtering Program Design Flow Chart

5.3 四轮转向AGV行驶系统试验研究

为了验证四轮转向AGV位姿定位多传感器信息融合算法的有效性，设计了圆角矩形磁导引轨迹位姿定位实验，并分别进行四轮转向AGV仅使用后轮驱动电机编码器反馈及使用后轮驱动电机编码器反馈、航姿参考系统与磁导航传感器信息融合位姿定位试验，受实验条件限制，只进行了室内位姿定位实验，如图7所示。实验设备：四轮转向AGV、3cm宽磁导航磁条、卷尺等。

图7 圆角矩形导引轨迹位姿定位试验Fig.7 Positioning and Orientation Test of Rounded Rectangle Guide Tracking

试验过程中上位机以100ms采样间隔实时记录航姿参考系统航向角、两后轮编码器反馈与磁导航传感器信息，并实时计算2种定位方式的位姿估计结果。

当AGV仅使用后轮驱动电机编码器进行位姿推断定位时，随着AGV向前行进，如图8（a）、图8（b）所示。AGV位姿随着导航时间不断偏离预定义轨迹，在初始时位姿与理论轨迹比较接近，当在第一个圆角ɑ进行转弯时AGV位姿误差开始变大，并且转弯过程中AGV转角误差不断累积；当转弯完成后其行进方向已与预定义航向出现10°左右偏差，并且随着行进位置偏差最大达到425mm，经过多次转向，AGV运行轨迹相对预定义轨迹出现较大变形；当AGV使用后轮驱动电机编码器、航姿参考系统与磁导航传感器信息融合位姿定位，AGV 能够有效沿着预定义的圆角矩形行进，且在行进过程中AGV位姿误差小于48mm，航向误差小于4°，表明采用UKF滤波器进行多传感器信息融合的方法能够有效提高AGV位姿定位精度，证明了本AGV位姿定位算法的有效性。

图8 圆角矩形导引试验结果Fig.8 Results of Rounded Rectangle Guidance Test

6 结论

基于自主设计的四轮转向AGV进行了位姿估计的运动学模型分析，开展了基于模糊控制器的磁导航路径跟踪控制研究和多传感器信息融合的AGV位姿定位方法，实现四轮转向AGV自主导航路径跟踪。控制试验表明，UKF卡尔曼滤波器AGV位姿定位算法可实现48mm以内的位姿误差和小于4°的航向误差，可以为农用轮式AGV应用提供参考。