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

田会方 谭树栋 吴迎峰

摘要：针对室内AGV的自主导航中位姿获取问题，为了得到精度较高、稳定可靠的位姿信息，提出了一种基于EKF算法融合多传感器数据的方法。通过建模分析，讨论各种位姿估计方法的优缺点，针对里程计和IMU的累计误差问题，借助UWB来消除，针对UWB非视距误差的问题，提出动态加权的思想，针对特殊的路段，借助里程计和IMU来改善。实验结果表明，多传感器融合能有效地克服各种传感器的局限性，优势互补，得到精确可靠的位姿信息。

关键词：里程计;IMU;UWB;扩展卡尔曼滤波;融合

中图分类号  TP391   文献标识码：A

文章编号：1009-3044（2022）20-0111-04

1 引言

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

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

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

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

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

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

[ηk，l=MlN*2π，ηk，r=MrN*2π]                   （1）

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

[Sk=Sk，l+Sk，r2=R*ηk，l+R*ηk，r2]                （2）

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

[Δθk=Sk，l-Sk，rd，rk=SkΔθk]                 （3）

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

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

[xk+1=xk+Δx=xk+Skcos（θk+Δθk2）yk+1=yk+Δy=yk+Sksin（θk+Δθk2）θk+1=θk+Δθk]           （4）

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

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

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

1） 速度、位移解算

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

[vk=vk-1+ak+ak-12*Δtsk=sk-1+vk-1*Δt+14（ak-1+ak）*Δt2]    （5）

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

2） 角度估计

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

[θk=θk-1+ωk+ωk-12*Δt]                （6）

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

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

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

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

2.3 基于UWB的测距定位分析

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

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

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

[（x-x1）2+（y-y1）2=d12（x-x2）2+（y-y2）2=d22（x-x3）2+（y-y3）2=d32]           （7）

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

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

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

3.1 EKF滤波介绍

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

[Xk+1=f（k，Xk）+WkZk=h（k，Xk）+VkWk～（0，Qk），Vk～（0，Rk）]                  （8）

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

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

预测部分：    [Xk|k-1=f（k，Xk-1|k-1）Pk|k-1=Fk-1Pk-1|k-1FTk-1+Qk-1]   （9）

更新部分：

[Kk=Pk|k-1HTk（HkPk|k-1HTk+Rk-1）-1Xk|k=Xk|k-1+Kk（Zk-h（k，Xk|k-1））Pk|k=（I-KkHk）Pk|k-1] （10）

3.2 数据融合滤波

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

[Xk+1=f（k，Xk）+Wk=xk+Skcos（θk+Δθk2）yk+Sksin（θk+Δθk2）θk+ΔθkΔθk+Wk]  （11）

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

[Zk=h（k，Xk）+Vk=xkykθkΔθk+Vk]              （12）

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

1） 预测过程：

①状态先验方程：

[Xk+1|k=f（k，Xk|k）=xk+Skcos（θk+Δθk2）yk+Sksin（θk+Δθk2）θk+ΔθkΔθk] （13）

②先验方程协方差矩阵：[Pk+1|k=FkPk|kFTk+Qk] （14）

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

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

[Qk=0.0100000.010000100000.01]       （15）

2） 更新过程：

①卡尔曼增益Kk：

[Kk=Pk+1|kHTk（HkPk+1|kHTk+Rk）-1]     （16）

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

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

②状态后验方程：

[Xk+1|k+1=Xk+1|k+Kk（Zk-h（Xk+1|k））]      （17）

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

[Pk+1|k+1=（I-KkHk）Pk+1|k]        （18）

以上为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在本次实验中的设定值如下：

[Rk1=100000100000100000.1，Rk2=1000000010000000100000.1]   （19）

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

5 结束语

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

参考文献：

[1] 罗坚铭.移动机器人机床上下料系统的精确定位技术研究[D].广州：广东工业大学，2018.

[2] 李晶.基于ROS的AGV自动导航控制系统开发[D].武汉：华中科技大学，2017.

[3] 吴鹏，李东京，贠超.一种惯性传感器与编码器相结合的AGV航迹推算系统[J].机电工程，2018，35（3）：310-316.

[4] Xia X，Xiong L，Huang Y J，et al.Estimation on IMU yaw misalignment by fusing information of automotive onboard sensors[J].Mechanical Systems and Signal Processing，2022，162：107993.

[5] 王泽华，梁冬泰，梁丹，等.基于惯性/磁力传感器与单目视觉融合的SLAM方法[J].机器人，2018，40（6）：933-941.

[6] 董兴波.基于UWB的高精度室内定位系统研究与实现[D].桂林：桂林电子科技大学，2021.

[7] 徐庆坤，王天皓，宋中越.UWB与里程计融合的室内移动机器人定位设计[J].机械设计与制造，2021（8）：295-299，304.

[8] 王文博，黄璞，杨章静.基于超宽带、里程计、RGB-D融合的室内定位方法[J].计算机科学，2020，47（S2）：334-338.

[9] 李义.基于UWB与IMU组合的室内移动机器人定位方法研究[D].成都：电子科技大学，2021.

[10] 吴和龙，Pei Xinbiao，Li Jihui，Gao Huibin，Bai Yue.Attitude estimation method based on extended Kalman filter algorithm with 22 dimensional state vector for low-cost agricultural UAV[J].High Technology Letters，2020，26（2）：125-135.

【通联编辑：梁书】