基于自适应联邦滤波的AGV定位研究

温尊旺，王尧尧，陈柏，姚佳烽

(南京航空航天大学 机电学院，江苏 南京 210016)

0 引言

随着“工业4.0”、“中国制造2025”等战略的提出，传统工业逐渐向自动化及智能化的方向发展。由于自动引导运输车(AGV)智能化高，灵活性强的特点，广泛应用于智能车间、仓储运输等领域[1]。常见的AGV室内定位方式主要有WiFi定位[2]、激光雷达、UWB定位[3]、视觉[4]、动作捕捉、电磁导航等。这些定位方式各有特点，在不同场合均有使用。

为了进一步提高定位精度，便有了多传感器的组合导航，即采用两种或两种以上的非相似性导航系统对同一导航信息做测量并解算以形成量测量[5]。COONEY J A等[6]介绍了麦克纳姆轮(Mecanum wheel)中的打滑问题，使用视觉航位推算来处理和计算车辆的速度及方向，以此来完成防滑的功能。QIN H L等[7]提出了基于小波阈值去噪和函数系数回归(FAR)建模的惯性组合数据处理方法，引入GPS姿态信息，提高了MEMS惯性传感器误差的估计精度。孙自飞等[8]提出一种RGB-D传感器对激光传感器进行辅助的自定位方法，具有较好的准确性和可靠性。李沛等[9]提出了一种基于视觉定位的AGV避障方案，将超声波传感器和红外传感器相结合，基于多传感器信息融合技术，实现避障算法。

本文基于Mecanum轮全向机器人设计了一种多传感器融合的低成本定位方案。结合各传感器的特点，采用自适应联邦卡尔曼滤波及多尺度融合的手段，提高了定位精度。

1 Mecanum轮全向机器人运动学分析

所设计的Mecanum轮全向机器人系统包含动力系统、传感器系统以及控制系统。动力系统包括电机驱动器、步进电机以及麦克纳姆轮；传感器系统包括编码器、加速度计、陀螺仪以及OpenMV，所用的传感器成本都较低。控制系统由STM32 F103组成，主要用于传感器数据读取、解算。

Mecanum轮全向机器人的安装方式主要为X型和O型，本文采用X型方式，如图1所示[10]。R为Mecanum轮半径；a、b分别为平台宽度、长度的一半；vx、vy、ωz分别为平台的x方向速度、y方向的速度以及角速度；v1、v2、v3、v4分别为4个麦克纳姆轮的分速度；α为轮毂轴和辊子轴的夹角。

图1 Mecanum轮全向机器人结构图

Mecanum轮全向机器人的运动可以分解为3个独立的变量，分别为：x轴的平移、y轴的平移以及绕z轴的旋转。如图1所示建立坐标系，假设小车的运动速度为V=[vx,vy,ωz]，对小车的运动速度进行运动分解，根据叠加原理，各个轮子的角速度为

(1)

式中：kx在理想状态=1/R；ky=1/(Rtanα)；kz=(atanα+b)/(Rtanα)。

2 传感器定位原理

为了获取准确的位姿，采用多种传感器实时获得与位姿相关的信息，包括：OpenMV单目相机通过AprilTag标签获得x、y方向的位置、z轴的姿态；通过4个电机的编码器获得当前的速度和角速度；陀螺仪得到z轴角度及角速度；MPU6050获得x、y方向的加速度。

2.1 坐标系规定及坐标变换

为了方便后续的计算，对各个传感器的坐标系进行规定，均采用右手定则，如图2所示。其中包括世界坐标系、AprilTag坐标系、OpenMv(单目相机)坐标系、AGV坐标系。相对于世界坐标系，AprilTag坐标系是不变的，其余坐标系都随着AGV坐标系而变化，假设初始位置AGV坐标系和世界坐标系重合。

图2 坐标系约定

(2)

2.2 AprilTag定位原理

AprilTag是一个二维平面目标基准系统，被广泛应用于视觉定位，当标签贴在目标物上时，便可识别出相机和标签之间的3D位姿关系[11]。为了保证在AGV运动过程中AprilTag信息不丢失，扩大识别范围，在天花板上贴4块ID不一样的标签。以天花板某一点O为原点，创建AprilTag坐标系，对称分布4块标签。其中AprilTag坐标系可以由世界坐标系绕x轴旋转180°后再平移得到，可以计算出旋转矩阵和平移向量为

(3)

式中：Cθ=cosθ；Sθ=sinθ；φ、θ、ψ分别为绕x轴、y轴、z轴的旋转角度；t1、t2、t3为水平偏移。

齐次变换矩阵为

(4)

在AGV运动过程中，若识别到多个标签时，为了提高识别精度，取最近的标签识别值为测量值。

2.3 里程计定位原理

4个麦克纳姆轮的电机上，均安装编码器，在AGV运动时，读取每个电机编码器的数值，根据式(1)计算出AGV当前的速度和角速度，通过航迹推算的方法来获得当前的位姿，若采样间隔为Δt，计算公式如下[12]：

(5)

从公式(5)可以看出，航迹推算的方法是非线性的，将其转化到世界坐标系下进行线性化。AGV在世界坐标系下的速度为

(6)

其中，速度和角速度可以根据式(1)计算，而式(6)也可以写成

(7)

2.4 加速度计定位原理

加速度计可以获得车体x、y、z方向的加速度，在静止状态下，x、y方向的加速度为0，z方向的加速度值为-g。为了减小加速度的误差，需要对加速度计进行校准和补偿。当加速度计静止时，误差为当前的测量值，表示为

(8)

校准后的实际加速度值为

(9)

将所测量的加速度值转换到世界坐标系下，计算为

(10)

3 组合导航系统设计

AGV的定位包括x、y方向的平移和z轴的旋转。单一的传感器难以获得较准确的位姿信息，单目相机获得位姿存在较大误差，且更新频率较慢，而通过航迹推算计算的位姿存在明显的累计误差。因此，把第2节介绍的多种传感系统进行组合，充分利用各导航的信息进行互补，最终使整个系统的精度和可靠性得到提高。

3.1 整体融合方案

整个系统的状态变量包括[xvxaxyvyayψωz]，如果将所有状态变量一起计算，每次进行矩阵运算次数是83，极大地增加了计算量。为了降低计算量，将状态变量分为[xvxax]、[yvyay]、[ψωz]单独估计。其中[ψωz]的估计采用陀螺仪和AprilTag标签偏航角的测量值进行融合。[xvxax]、[yvyay]的估计采用单目相机、里程计、加速度计，x、y方向的数据进行卡尔曼滤波融合。

根据传感器的特点，陀螺仪在一定时间内可具有较高的精度，但长时间存在积分误差，而单目相机在AGV速度较慢或者停止时具有较好的精度。因此，每次运动到任务点时，AGV速度较慢，此时用单目相机去校准陀螺仪。通过这样的融合，[ψωz]具有较好的全局精度。

3.2 经典卡尔曼滤波

以x方向为例，对单目相机和加速度计的数据进行卡尔曼滤波，估计最优的系统状态，假设系统的当前状态为x1,t，包括了位移、速度、加速度3个状态变量，x1,t-1为上一时刻状态，系统的状态方程和量测方程为

(11)

式中：F为上一时刻到下一时刻的变换矩阵；B为外部输入u1,t的输入控制矩阵；w1,t为系统的过程噪声；H1是系统的观测矩阵；v1,t为观测噪声；zx1,t为单目相机获得的x方向的位移量。其中：

(12)

卡尔曼滤波的步骤包括两步：预测和更新。预测过程计算如下[13]：

(13)

更新过程计算如下：

(14)

式中：Kx1,t为最优卡尔曼增益；Rx1为量测噪声；Px1,t为当前时刻协方差矩阵；I为单位阵。

3.3 局部滤波器的自适应卡尔曼滤波

根据卡尔曼滤波计算过程可知，只有当动态系统的参数和噪声特性参数都准确已知的条件下，经典卡尔曼滤波才能得到最优估计。然而，在实际系统中，一些参数无法获取或是实时变化的，可能导致滤波器的精度降低，甚至可能出现估计值和实际值的误差越来越大，从而引起滤波发散。为了消除这种误差，通过自适应滤波的方法，采用噪声估计器，实时修正和估计观测噪声的特性，从而达到提高滤波精度的目的。

在卡尔曼滤波中，局部滤波器的量测预测误差的公式计算为

(15)

量测噪声方向的更新过程为

(16)

当t趋于无穷大时，1/t趋于0，此时滤波器的自适应能力将逐渐减弱。为了始终保持自适应能力，将1/t替换为

(17)

当t趋于无穷大时，γt趋于1-c，始终保持一定的自适应能力，其中c取0.999。

3.4 多尺度融合及容错设计

在实际系统中，各个传感器的更新频率并不相同。如果卡尔曼滤波器的更新频率较大，单目相机的测量值有着严重的滞后；如果更新频率较小，则不能保证位置估计的实时性。因此，本文提出一种多尺度融合方法，兼有一定的容错能力。

里程计更新频率较快，以里程计的更新频率作为基准，通过串口中断检测单目相机是否更新。若更新，则按照式(13)进行计算；若不发生更新，则认为测量值不准确，完全信任预测值。整个过程可以表示为：

(18)

式中：当单目相机更新时δ(t)=1；没有更新时δ(t)=0。

通过上述方法，不仅实现了非均匀采样的多尺度系统的融合，也具有一定的容错能力，当标签被遮挡或者AGV行驶范围超出标签范围而无法识别时，卡尔曼滤波仍然能够起作用。当单目相机工作正常时，状态变量的精度会逐渐恢复到正常值。

3.5 联邦卡尔曼滤波

多传感器数据融合包括集中式和分散式滤波。集中式滤波即将所有的传感器数据放到同一个滤波器中进行滤波，分散滤波即利用子滤波器处理各系统的信息再经过全局滤波实现融合。集中式滤波虽然精度较好，但由于状态维数高，导致计算量较大，容错性低，且不便于故障排除。因此，本文采用联邦滤波器，虽然输出结果是次优的，但容错性得到极大的提升，计算量显著减少。

所设计的整体滤波器结构如图3所示，加速度计数据为参考系统，里程计和视觉定位数据为子系统，经自适应局部滤波器1、滤波器2后，进入主滤波器，最终得到系统的状态变量。

图3 滤波器结构图

将局部滤波器的信息融合为新的全局状态估计的过程为

(19)

得到全局估计后，再经反馈得到子滤波器的状态、协方差矩阵：

(20)

式中β1、β2为各子系统分配系数，这里均取为0.5。

4 仿真和实验

4.1 仿真与数据分析

根据上一节设计的自适应联邦卡尔曼滤波器，进行仿真验证。假设载体初始状态为x=[0 m 1 m/s 0 m/s2]，保持1 m/s的匀速运动，0.1 s更新一次。假定Openmv的方均根误差为1 m，里程计方均根误差为0.5 m/s，陀螺仪方均根误差为0.5 m/s2，经滤波后偏航角方均根误差为0.5°。Openmv未滤波前的误差、联邦卡尔曼滤波、自适应联邦卡尔曼滤波后误差的仿真结果如图4所示，并计算得到未滤波前的方均根误差约为1 m，联邦滤波后的方均根误差约为0.49 m，自适应联邦滤波后的方均根误差约为0.35 m。从仿真图和统计数据可以明显地看出，自适应联邦卡尔曼滤波器滤波后精度有明显的提升。

图4 自适应联邦滤波器仿真图

为了验证多尺度融合效果，考虑传感器的更新频率并进行仿真，仿真条件除更新频率外与上面仿真一致。假定里程计0.1 s更新一次，单目相机1 s更新一次，均采用自适应联邦卡尔曼滤波，分别使用多尺度融合和不使用多尺度融合。滤波后误差的仿真结果如图5所示，可以发现没有进行多尺度融合的滤波效果明显变差，这是因为更新较慢的单目相机存在滞后，通过多尺度融合减小了这种滞后。

图5 多尺度融合仿真图

4.2 实验与数据分析

为了进一步验证所设计的滤波算法，本节采用麦克纳姆轮全向机器人平台进行实验验证，对2.6 m×2.35 m的矩形轨迹进行跟踪，小车由A点出发，经过B、C、D点，最终回到A点。实验平台如图6所示，轨迹测量设备为激光跟踪系统，将靶球固定在AGV上，激光跟踪仪实时追踪靶球，获取靶球的位置。

图6 实验及测量平台

一共进行4组实验，实验结果如表1所示，由于里程计存在累计误差，虽然一开始具有较好的精度，但随着运动距离的增加，实际轨迹与期望轨迹越来越远，无法形成完整的闭合轨迹。采用单目相机定位未滤波得到的轨迹如图7所示，可以看出具有较大的跟踪误差且轨迹不平滑。采用联邦卡尔曼滤波的轨迹如图8所示，相比于没有滤波之前的轨迹得到了一些改善。采用自适应联邦卡尔曼滤波得到的轨迹如图9所示，通过对比轨迹图，可以看出定位精度有明显的改善。图10为在扰动下的轨迹，AGV初始位置并不在原点，但随着时间的推移，整个系统会趋于稳定，并得到一个较为准确的位姿估计。图11是3种滤波方式的误差比较，从图中可以看出自适应联邦卡尔曼滤波具有较好的滤波效果。根据表1可知，经自适应联邦卡尔曼滤波后最大误差从50.162 9 mm减小为15.603 0 mm，方均根误差从13.671 0 mm减小到5.922 4 mm，平均误差从10.370 9 mm减小到4.698 7 mm，最大定位误差减小了69%，方均根误差减小了56.6%，可以看出所设计的滤波器可以提高定位精度。

表1 几种滤波方式的误差统计 单位：mm

图7 未滤波轨迹

图8 联邦滤波轨迹

图9 自适应联邦滤波轨迹

图10 初始位置干扰后轨迹

图11 几种滤波方式的误差图

根据上述的仿真和实验的结果，可以推断出所设计的滤波器具有一定的容错能力。当单目相机被遮挡或超出工作范围时，此时单目相机数据具有严重的滞后性，但是里程计仍然能够进行工作，滤波器此时只信任里程计。尽管由于里程计的特性会导致误差逐渐增大，但是只要当单目相机重新正常工作，AGV将逐渐减小误差并回到正常的状态。

5 结语

本文设计了一套Mecanum轮全向机器人系统，推导了AGV的正逆运动学方程。建立了AGV及各传感器之间的坐标关系，基于单目相机的AprilTag定位、里程计定位、加速度计定位建立定位模型，并将传感器的非线性问题转换为线性问题。根据各传感器的特点，设计自适应联邦卡尔曼滤波器，并将不同更新频率的传感器进行多尺度融合。通过仿真和实验表明：所设计滤波器能够大幅提高AGV的精度，并具有良好的鲁棒性。