基于自适应卡尔曼滤波的RFID/SINS组合导航研究

张一康，陈燚涛，刘 芳

(武汉纺织大学 机械工程与自动化学院，湖北 武汉 430200)

0 引言

自动导引车(Automated Guided Vehicle,AGV)作为自动仓储和自动物流的关键设备能够实现无人运输的功能,其关键技术是利用导航系统提供较为精确的位置信息[1]。组合导航系统是将多个导航系统结合在一起,弥补各自的不足,从而实现更精准的导航定位。其中,GPS/捷联惯导系统(Strap-down Inertial Navigation System,SINS)组合导航由于精度较高和定位范围广等优点被广泛运用[2]。但是AGV小车普遍应用在厂房或者室内环境,无法稳定地接收到GPS信号,甚至在复杂的室内环境下完全无法接收到卫星信号[3-4],导致组合导航系统完全由惯性测量单元(Inertial Measurement Unit,IMU)发挥作用,使整套定位导航系统精度迅速降低。

为了解决室内接收不到GPS信号的问题,文献[5]运用机器学习的方法弥补惯性导航运行一段时间后定位精度变差的问题,但是增加了整套导航系统的计算量和设备负担,并且和室内组合导航系统相比其定位精度较差。文献[6]采用机器视觉辅助的方式替代原有的GPS信号组成新的组合导航系统,通过将快速准确的图像匹配应用到视觉里程计中,可以有效减少惯性导航系统的定位误差。文献[7]采用激光雷达和MEMS微惯性组合的方式进行室内定位与导航,提出了以SLAM定位结果为观测量的激光雷达/MEMS惯性的组合导航方案。

上述针对于室内组合导航的改进方案,本质都是寻找可以提供类似于GPS信号的定位装置,实现室内导航,但是改进方案都需要较高的成本。RFID室内定位具有信号穿透性强、成本低廉等诸多优点[8-10],因此,本文利用RFID将位置信息通过阅读器和上位机传输到惯性导航系统中,并使用自适应卡尔曼滤波(Adaptive Kalman Filtering, AKF)[11]的方法,在降低整体导航成本的前提下,提高了导航系统的定位精度。

1 RFID/SINS组合导航

在RFID/SINS组合导航系统中, RFID定位系统作为辅助系统,主要功能是提供对应时刻的位置信息,并与惯导系统的位置信息对比,通过卡尔曼滤波(Kalman Filtering, KF)进行过滤校正,其输入为RFID和SINS解算位置之差[12]。本文采用的组合导航系统仿真流程如图1所示。

图1 RFID/SINS组合导航仿真流程Fig.1 RFID/SINS combination navigation simulation flowchart

1.1 RFID辅助导航系统

在RFID/SINS组合导航系统中,RFID定位系统主要提供AGV的位置坐标,定位设备主要包含RFID阅读器和无源电子标签。通过RFID阅读器发射电磁波与标签交互后,根据收到的反馈信号强度判断AGV的位置,或者直接通过指纹定位算法估计AGV的位置坐标[8]。因此,通过将RFID定位系统获取位置信息作为观测量,并与惯性导航获取的位置信息比较导入KF中,即可对原有的导航轨迹进行校正。

1.2 组合导航系统数学建模

在设计组合导航系统之前,需要建立动态的特性状态方程[13]。本研究为二维平面内的导航定位,不需要考虑三维立体空间的参数变量。因此基于惯性导航的状态方程有12个变量,分别为SINS系统位置误差(δφ,δλ)、速度误差(δVE,δVN)、姿态角误差(φx,φy,φz)、三轴陀螺仪零漂(εx,εy,εz)和东、北向加速度零漂(∇x,∇y)。组合导航系统的状态变量如式(1)所示。系统噪声W(t)主要为陀螺仪和加速度计的高斯白噪声误差,如式(2)所示:

X(t)=[δφ,δλ,δVE,δVN,φx,φy,φz,εx,εy,εz,∇x,∇y]12×1,

(1)

W(t)=[ωgx,ωgy,ωgz,ωax,ωay]。

(2)

整个RFID/SINS组合导航系统的状态方程可以表示为式(3)。在组合导航系统中分别可以用式(4)和式(5)表示。

(3)

(4)

(5)

式中:F(t)为系统状态转移矩阵,对状态变量的运动建模;G(t)为控制矩阵,将输入转化为状态;FN为系统动态矩阵,由惯性导航系统的2个位置误差、2个速度误差和3个姿态误差组成[14]。

系统的量测噪声主要反映量测量和状态量之间的关系,在RFID定位系统中主要参数有东、北向的位置信息(xER,yNR),并与惯导系统中的位置信息(xE,yN)共同组成量测量。量测向量Z(t)则可以表示为式(6),对应的量测方程如式(7)所示,其中H(t)为量测矩阵(式(8)),V(t)为量测噪声,包含东、北向位置误差(式(9)):

(6)

Z(t)=H(t)X(t)+V(t),

(7)

(8)

V(t)=[EEEN]。

(9)

2 AKF优化分析

2.1 KF参数更新

KF是一种利用线性系统状态方程,通过系统输入输出观测数据,对系统状态进行最优估计的算法。传统KF参数更新的流程为,预测阶段用前一时刻的状态估计值推算当前时刻的状态,获得先验状态估计值,然后在更新阶段对预测值进行修正,获得后验状态估计值。通过不断地进行“预测—实测—修正”的更新顺序,最终达到一个平衡状态[15]。通过上述给出的系统状态方程和量测方程,利用KF进行参数的时间更新和量测更新,可以得到KF的基本方程如下[16]。

先验状态估计值:

(10)

状态一步预测均方误差:

(11)

滤波增益:

(12)

后验状态估计值:

(13)

后验状态估计均方误差:

(14)

2.2 AKF优化

AKF是一种能够动态调整KF参数的算法,对于含有噪声参数和不确定性信号的滤波有更好的估计性能。在RFID/SINS组合导航系统中,RFID定位系统用于提供离散的定位信息,惯性导航系统提供连续的运动状态信息。RFID直接或间接依靠电磁波信号特征进行定位计算,由于在室内复杂环境下存在各类障碍物,增加了电磁波信号噪声和不确定性,进而降低了AGV组合导航系统的导航精度和稳定性[17]。因此本文选用AKF融合导航信息。

AKF在进行状态估计的同时可以通过量测输出,在线实时的估计系统的噪声参数Rk。在AKF中噪声参数和量测输出相互关联,可以通过式(15)得出,其中,βk为自适应参数(式(16)),b为自适应因子,一般取值0.9～0.999[18]。

(15)

(16)

AKF流程如图2所示。

图2 AKF流程Fig.2 AKF flowchart

在RFID/SINS组合导航实现过程中,初始位置和姿态分别通过RFID定位系统和IMU磁力计获取计算得到,在AGV开始运动后采集RFID器件和惯性传感器信息,通过位置定位算法计算推导RFID阅读器的位置坐标,惯性导航系统则通过惯性导航姿态解算算法和运动状态估计算法输出导航参数。将2个导航系统的输出参数输入组合导航系统,运用AKF对下一时刻预估参数进行纠偏和校正,从而适应不同的运动状态,得到更好的导航精度和稳定性。

3 仿真实验

为了验证提出的AKF的效果,本文根据数学模型使用Matlab软件进行仿真,部分仿真参数如表1所示。RFID标签布置在AGV行驶的路径上,依据其速度每隔一定的距离布置RFID标签阵列。轨迹路线示意如图3所示。惯性传感器IMU采样频率为10 Hz,当AGV运动到RFID标签阵列处时,进行RFID辅助定位获取位置信息,并与IMU测量的位置信息进行比对,通过KF进行过滤和修正。在整个移动过程中,AGV每次经过RFID标签阵列时进行位置纠偏,直至AGV小车到达终点完成路径运动。

表1 仿真参数

图3 不同路径路线导航示意Fig.3 Different path route navigation schematic diagram

本文针对于室内复杂环境设计了2种不同路径的仿真方案,仿真时间为300 s。AGV正常行驶速度设置为1 m/s,进行转弯时,会以-0.2 m/s2的加速度将速度提前降到0.4 m/s,并且会在转弯后恢复至原有速度。第一次仿真轨迹为直线轨迹,第二次仿真轨迹为曲线轨迹。

对于上述2种轨迹,分别使用标准KF和AKF的RFID/SINS组合导航系统进行仿真,并输出导航系统在每个采样时刻的定位误差曲线。图4和图5分别为AGV小车沿直线轨迹行驶,传统KF和AKF在经纬方向上的定位误差曲线,其中δλ为纬度距离误差,δL为经度距离误差。图6和图7为曲线轨迹的定位误差曲线。

(a)KF纬度方向误差

(b)KF经度方向误差

(a)AKF纬度方向误差

(a)KF纬度方向误差

(a)AKF纬度方向误差

通过上述仿真结果可以看出,AGV直线轨迹运行和曲线轨迹运行所表现的整体导航性能变化不大,主要的不同点是AGV在转弯时会导致导航系统的定位误差产生较大的变化。单次导航过程中AGV的平均定位误差如表2所示。通过不同KF下的定位误差均方根,可知AKF比标准KF在RFID/SINS组合导航系统中定位精度更高,其平均定位精度提高了9.36%,并且在单次导航过程中其定位误差分布较为稳定, AKF相比于KF稳定性提升了6.64%。

表2 单次导航平均定位误差统计

为了保证仿真的准确性和可靠性,本文后续进行蒙特卡洛实验,以模拟AGV在复杂室内环境下的导航定位情况。在相同的轨迹下进行30次仿真。分别记录每次仿真的纬、经度方向上的定位误差,并计算平面内的几何定位误差和误差均方根值。不同滤波下的几何定位误差均方根曲线如图8和图9所示。为了清晰直观地了解30次仿真数据的整体情况,表3列出了多次仿真不同轨迹下的定位误差均方根平均值。

表3 不同滤波下定位误差均方根值统计

图8 直线轨迹几何定位误差均方根曲线 Fig.8 Root mean square curve of geometric positioning error of straight trajectory

图9 曲线轨迹几何定位误差均方根曲线Fig.9 Root mean square curve of geometric positioning error of curve trajectory

通过图8、图9和表3可以看出,在直线和曲线轨迹下,AKF组合导航在多次导航过程的定位误差均方差都比KF组合导航要小,说明基于AKF的组合导航其导航稳定性得到了较大提升,并且AKF组合导航定位误差均方根比较集中、上下浮动较小。因此,基于AKF的RFID/SINS组合导航系统在单次导航过程和多次导航过程中,其定位精度和稳定性都有一定的提升。综上所述,AKF在RFID/SINS组合导航系统中有更好的导航性能。

4 实验验证

4.1 实验环境

为了验证提出的基于AKF的RFID/SINS组合导航在实际使用过程中的准确性,本文设计并进行了实验。在实验中,采用的IMU测量模块为WHEELTEC的N200系列九轴传感器,其航向陀螺零偏稳定性为2 (°)/h,加速度零偏稳定性为0.000 2g。RFID阅读器为圆极化天线,工作频率为915 MHz,RFID标签为H47型,实验布置示意如图10所示。IMU测量模块和RFID阅读器均通过USB接口和上位机进行数据传输,IMU数据采集频率为10 Hz。实验装置如图11所示,实验场地示意如图12所示。

图10 RFID标签布置及参考点位置示意Fig.10 Schematic diagram of RFID tag arrangement and reference point location

图11 组合导航AGV结构简图Fig.11 Sketch of combined navigation AGV structure

图12 实验场地和预定路径示意Fig.12 Schematic diagram of the experimental site and intended path

4.2 实验结果分析

在跑车实验中,通过获取RFID和惯导IMU传感器的各项导航参数,分别使用KF和AKF对AGV导航精度进行处理,可以绘制AGV小车在移动过程中的位置精度数据图。KF和AKF下组合导航经纬度方向位置误差曲线如图13和图14所示。

(a)KF纬度方向误差

(b)KF经度方向误差

(a)AKF纬度方向误差

通过上述RFID/SINS组合导航系统的位置误差曲线图,可以计算该实验环境下AGV的整个移动过程中的平均位置误差和误差稳定性,其中位置误差稳定性通过位置误差均方差值来反映位置误差变化的幅度。组合导航系统输出及数据处理结果如表4所示。L和λ分别表示地球坐标系中的纬度和经度。KF和AKF下组合导航系统部分轨迹对比如图15所示。

表4 组合导航系统位置误差及其均方差输出

实验结果表明,AGV在整个导航运行过程中,除了在转弯时会产生较大的误差偏移,其余过程的位置误差较小且稳定。由表4和图13可以看出,在KF和AKF条件下RFID/SINS组合导航系统平均定位误差分别为0.08、0.072 m,在室内环境下均能够提供高精度的导航定位。应用AKF的组合导航,定位精度提升了10%,导航稳定性提升了7.4%,验证了上述仿真结果。

5 结束语

本文基于AKF的RFID/SINS组合导航系统,有效解决了室内惯性导航系统误差发散和滤波中噪声参数不断变化的问题。

针对室内环境无法接收GPS信号导致GPS/SINS组合导航精度变差和外界噪声的不确定影响,通过RFID辅助定位系统替代原有的GPS信号,实现室内组合导航与定位,应用AKF将噪声参数与量测输出参数关联并实时获取较为准确的噪声参数。

通过实验和仿真计算发现,RFID/SINS组合导航系统能够较好地实现室内高精度的定位与导航,实验结果显示组合导航系统的平均定位误差保持在0.07 m左右。应用AKF滤波其平均定位误差减少了10%,稳定性提升了7.4%。