基于EKF多传感器融合的自动导航车(AGV)位姿估计

田会方，谭树栋，吴迎峰

(武汉理工大学机电工程学院，湖北武汉430070)

1 引言

在传统生产制造业中，产品的生产过程，真正加工和制造所占用的时间很少，大部分时间消耗在搬运、装卸和仓储等物流环节。随着社会的发展，劳动力成本不断提高，企业想要提高自身的竞争力，就需要对生产的物流环节进行改进优化[1]。AGV(自动导航车)是连接物流与生产的重要桥梁，是工业自动化中不可或缺的重要设备和技术，因其具有较强的机动灵活性、高度的作业重复性和安全可靠性等优势，可以有效地提高生产效率并降低成本[2]。

AGV 在物流搬运与工业生产等领域有着越来越广泛的应用，在生产车间环境中，传统的导轨式AGV 有着诸多的限制，因此AGV 的自主导引能力越来越关键，AGV 的定位是实现自主导航功能的前提。因此获取AGV 的位置和姿态信息，并通过算法提高位姿的精度是本文研究的重点。

2 里程计、IMU、UWB的模型分析

2.1 基于轮式里程计的位姿估计

里程计算法建立的运动模型主要依赖于小车轮上的编码器，通过编码器的脉冲计数来估算得到小车的位姿。本文的小车底盘采用的是两轮差速驱动，底盘后方两个同构驱动轮为其提供动力，前方的万向轮起支撑作用，把小车完全视为刚体，不考虑任何力的影响，建立运动模型图如图1[3]。

图1 里程计航迹推算模型

小车的速度、位置以及偏航角都可以基于两轮的编码器测得的脉冲数来计算得到。编码器一周产生的脉冲数记作N，k时刻测得的脉冲数记作M，则有左右两轮的转角公式为：

小车的采样时间内的行驶距离Sk可以由转角以及驱动轮半径R来表示：

因此，小车的偏航角变化量Δθk，可以通过两轮的间距d和每个轮子的行驶距离来计算得到，进而可以计算得到小车的转动半径rk：

这里对模型进行一种假设，小车沿着圆弧的割线运动，即先转过一半的角度Δθk/2，然后沿此方向直线运动Sk，最后再转一半的角度Δθk/2，这种方法叫作割线模型。

因此，k+1 时刻的小车的位姿信息Pk+1=(xk+1,yk+1,θk+1)可以表示为：

此方法在建模过程中会对模型进行一定的假设优化，因此存在不可避免的误差；编码器位姿估计需要车轮直径和回转中心到驱动轮中心线的距离，在测量过程中也会存在误差；同时运动过程中小车可能会出现轮子打滑、侧移等现象，也会引起偶然误差。

2.2 基于惯性单元的位姿估计

惯性导航系统基于惯性测量单元(IMU) 来实现的，利用IMU数据结合目标物体的初始位置和方向来确定其运动姿态。IMU 主要由加速度计和陀螺仪组成，其中，加速度计用于输出小车在载体坐标系下的三个坐标轴方向上的线性加速度信息，而陀螺仪用于输出载体小车相对于世界坐标系的三个坐标轴方向的角速度信息[4]。

图2 IMU惯导模型

1)速度、位移解算

积分运算就是将采集到的加速度计数据进行积分处理，能够得到运动目标的速度信息，进而得到位移信息。加速度计推算的离散模型为：

式中，vk表示k 时刻的瞬时速度，ak表示k 时刻加速度计测得的加速度数据，Sk表示从计时开始到k时刻的总位移。

2)角度估计

陀螺仪是IMU 中最为关键的模块，陀螺仪的精度决定了IMU单元的上限和价格，陀螺仪可以测得运动物体相对于自身的三个轴向的角速度数据，那么陀螺仪推算的姿态角变化的离散模型为：

式中θk表示k 时刻的姿态角，ωk表示k 时刻陀螺仪测得的角速度数据。二维平面行驶的AGV，这里只考虑偏航角yaw。

对实验用到的消费级加速度计采集到的原始数据分析时，由于精度较低的原因以及AGV小车在运输过程中加速度比较小，因此加速度的误差对于积分计算得到的速度和位移信息有较大的误差，且不可避免，对导航影响较大，因此本文对位置信息的获取不采用加速度计的方式。

对于陀螺仪采集到的角速度数据，可以一次积分得到角增量，结合上一时刻的航向角可以得到下一时刻的航向角信息，由于航向角的误差对速度的二次的，对于位置的影响是三次的，因此减小航向角误差能够极大地提高导航的精度以及稳定性。然而陀螺仪本身存在零偏误差，就是当车体静止且水平时陀螺仪的读数不为零[5]。因此需要与其他的传感器配合使用来获得相对精准的航向角信息。

与陀螺仪配合使用的传感器通常有磁力计，磁力计是测量磁场强度的传感器，将磁力计作为观测量引入导航系统可以抑制陀螺仪数据对航向角信息的误差累计，但是，磁力计的使用对环境有一定的要求，环境中存在一些干扰磁场，因此也会产生行进过程中的误差，而且生产车间中对磁力计的干扰影响比较大，因此通过与陀螺仪等传感器的配合使用可以有效降低误差。

2.3 基于UWB的测距定位分析

超宽带技术(UWB)是一种新兴的非正弦窄脉冲通信方式，其特点是带宽极宽，穿透能力强，传输速率高，有较强的分辨能力和抗干扰能力，同时功耗很低，抗多径效果好，定位精度高，可达到厘米级别，因此在室内定位方面获得了较高的关注[6]。

本文采用的是基于TOA的测距定位法，基本思想是通过测量目标标签与基站之间的信号飞行时间，从而得到标签与基站之间的距离，然后通过三边定位算法得到标签在世界坐标系下的坐标位置[7]。下面介绍三边定位算法的原理。

图3 TOA三边定位法

已知三个基站点A(x1,y1)，B(x2,y2)，C(x3,y3)，目标节点为D(x,y)，三个基站到D 的距离分别为d1，d2，d3，根据几何关系列出方程组：

在实际的工厂环境中，影响UWB 系统的定位精度是多方面的，包括硬件本身的性能、定位算法的精度以及环境因素等。这里主要考虑工厂环境中的非视距误差以及多径效应误差等。

非视距误差(NLOS)的产生主要是工厂环境中存在障碍物遮挡，比如设备、货架、墙壁等，使得信号传播并不是理想状态下的直线传播方式，而是信号传播发生反射、折射等形式，在这种情况下障碍物材质以及厚度都会带来不同的影响，对UWB信号的削弱导致最终定位受到影响[8]。对于多路径效应误差，UWB技术对其有一定的抑制能力，并且相比非视距误差，其影响要小得多，因此本文忽略多路径的影响。

3 基于扩展卡尔曼滤波的信息融合

3.1 EKF滤波介绍

扩展卡尔曼滤波是在卡尔曼滤波基础上对于非线性问题的扩展，利用线性化技巧将非线性系统转化为近似的线性化模型，然后应用卡尔曼滤波实现对目标的滤波估计[9]。非线性系统的状态方程和观测方程可以表示为：

其中，Xk表示k时刻的状态变量，Zk表示k时刻对应状态的观测值，f(•)和h(•)为非线性函数，过程噪声Wk和观测噪声Vk为高斯白噪声，服从均值为0，协方差分别为Qk和Rk的正态分布，而且Wk和Vk两两互不相关。

EKF以卡尔曼滤波为基础，核心思想是将非线性函数进行泰勒级数展开，忽略其二阶及以上的高阶项，进而得到近似的线性模型[10]。算法实现过程如下：

3.2 数据融合滤波

选取Xk=[xk,yk,θk,Δθk]T作为AGV 系统的状态量，状态量为k时刻里程计运动模型的x、y坐标以及航向角和航向角变化量，建立的系统状态方程为：

观测量为UWB 定位系统测得的坐标信息(xk,yk)，以及陀螺仪测得的角增量信息△θk和磁力计测得的航向角信息θk，因此系统的观测方程为：

对于上述状态方程的过程噪声Wk，主要是由于基于里程计的运动模型中建模过程产生的高斯白噪声；对于观测方程的过程噪声Vk主要来自传感器的测量过程，有陀螺仪的随机游走噪声和磁力计受到其他磁场干扰产生的噪声，以及UWB 定位模块受到环境因素干扰产生的噪声，均满足正态分布，且彼此之间互不相关。对于这个典型的非线性系统，可以通过扩展卡尔曼滤波来处理噪声。

1)预测过程：

①状态先验方程：

其中状态转移矩阵Fk由f函数一阶泰勒展开线性化得到。

过程噪声协方差矩阵Qk由里程计系统模型中的误差得到，通常里程计模型的过程噪声设置为wx=wy=w△θ=0.1，wθ=1，所以：

2)更新过程：

①卡尔曼增益Kk：

其中观测矩阵Hk由f函数一阶泰勒展开线性化得到。

观测噪声协方差矩阵Rk是传感器测量过程中产生的噪声。对于UWB 模块会受到环境因素的干扰，因此可以采取动态更新权重的方式来设置对应的Rk分量，对于特殊路段，可能受到遮挡而产生的非视距误差的程度不同，设置不同的值，而这些路段的坐标信息会存储在导航系统中，当AGV 行进到该范围时，更改对应的值。对于磁力计和陀螺仪模块可以设定一个固定的值，通过对数据融合过程的效果来确定Rk矩阵的各个值。

②状态后验方程：

③将先验协方差矩阵更新为状态后验估计值的协方差矩阵：

以上为AGV 定位系统的EKF 算法的一个计算周期，各个时刻EKF 对非线性系统的处理过程就是不断循环这个计算周期。通过EKF 滤波算法，对传感器信息进行数据融合，最终得到AGV在世界坐标系下的位姿信息。

4 实验与分析

本实验采用搭载JY901B惯导模块、DWM1000超宽带模块以及电机自带的霍尔编码器等传感器，以及STM32F103单片机来进行传感器数据的采集，将数据通过串口传输到PC电脑，并在电脑上进行数据处理融合。实验步骤如下：先将三个UWB基站模块放置在实验场地的预置的坐标点，然后将AGV 小车放置于起始坐标点，控制小车从起始点出发顺时针方向经过一个长为6m，宽为6m的矩形的四条边并回到起始点。

在工厂环境中，即使将UWB基站放置在比较高的位置，也会存在遮挡的现象，因此会使UWB定位精度下降，因此本文在EKF算法的基础上，针对UWB影响较为严重的路段，位置误差变大，采用动态加权法来配置Rk的值，其实验效果图如下：

图4 为里程计模型的定位轨迹图；图5 为UWB 定位轨迹图，在A-B路段设置了障碍物遮挡，因此波动明显变大，与实际位置的误差也较大；图6为协方差矩阵Rk1为固定值的情况下，EKF算法得到的定位轨迹图，会发现在A-B路段与实际运行轨迹有一定的偏差；图7为协方差矩阵Rk2为动态加权的情况下，EKF 算法得到的定位轨迹图，可以看出受到遮挡情况下，其融合效果也比较理想。其中Rk1和Rk2在本次实验中的设定值如下：

图4 里程计定位轨迹图

图5 UWB定位轨迹图

图6 固定协方差矩阵的EKF定位轨迹

图7 动态协方差矩阵的EKF定位轨迹

其中，在Rk1的基础上，对于A-B受遮挡严重路段，UWB的误差变大，因此增大观测误差协方差矩阵的值，方差值越大表示越不准确，代表这段路程更信任里程计得到的位置信息。

5 结束语

本文考虑在室内环境下，能够实现AGV的定位问题，AGV可以通过自身的惯导系统来进行导航定位，但是里程计、IMU存在累计误差，长时间运行会有较大误差，因此需要结合其他传感器，考虑UWB技术的独特室内定位优势，因此提出了利用EKF 算法来融合多传感器数据，并且考虑了UWB 技术的非视距误差，采用动态的协方差矩阵来改善。通过实验结果分析，可以得出该方案能够提高单一定位系统的精度。