基于改进粒子滤波的超宽带与惯性测量单元融合定位算法

2020-12-04陈凤冉田春月孙毛毛曾宪锋

科学技术与工程 2020年30期

匡兵，陈凤冉，田春月，孙毛毛，曾宪锋

(桂林电子科技大学机电工程学院，桂林 541000)

定位技术用于在环境中获取物体位置。近年来，由于无线定位技术的普遍[1-2]，超宽带(ultra wide band，UWB)定位技术已经逐渐受到重视。物体实际定位过程中，其定位精度不仅受到设备自身影响，复杂环境对无线信号的遮挡更会加大定位误差[3]。单一的UWB定位导航技术，已经无法满足人们日益增长的生产及生活需求。为此，采用超宽带及惯性测量单元(inertial measurement unit，IMU)进行组合定位导航研究。

为了得到更加准确的定位信息，多传感器数据融合需要选择合适的融合算法[4-5]。传统数据融合常用算法包括：加权平均法[6]、卡尔曼滤波法[7-10](Kalman filter，KF)、Dempster-Shafer(D-S)证据推理、多贝叶斯估计法等。Gao等[11]通过采用随机加权估计来实现多传感器观测数据的最优加权融合，不过该方法没有考虑到传感器数据采集过程中因环境等因素干扰所带来的噪声误差；Li等[12]针对观测噪声的协方差不能准确知道或不断变化的多传感器动态系统，提出了一种模糊自适应KF滤波的多传感器数据融合算法，该算法利用模糊理论和协方差匹配技术对KF算法的观测噪声协方差进行自适应调整，有效地提高了KF对模型变化的适应能力。然而，该算法在模糊集合的选择根据经验进行不断的试验获得，这一过程需要花费大量的时间，不利于多传感器数据融合研究；Chen等[13]利用低成本惯性元件(IMU)与全球定位系统(global positioning system，GPS)相结合用于确定行人的准确位置信息，该算法通过行人在双脚交替行走处于静止姿势阶段时将脚速度设为零，来纠正IMU的漂移误差，然后利用扩展卡尔曼滤波算法(extended kalman filter，EKF)对GPS误差进行修正，补偿IMU随时间增长的累计误差，最终实验表明，基于EKF的定位在定位精度和鲁棒性方面均优于双积分和ZUPT方法。但是，该算法没有考虑到GPS在复杂环境或有高楼、隧道等遮挡物下，无法避免信号因丢失导致定位精度下降等问题[14]。

针对传统多传感器融合算法的不足，在UWB与IMU定位融合的基础上提出了一种基于改进自适应加权数据融合的粒子滤波[15]定位算法。该算法对于多传感器观测噪声协方差无法准确判断的问题，利用自适应最优加权融合算法中最小方差估计理论，对粒子滤波中粒子分布权重进行重新调整；为了降低算法融合中心的计算量，减轻传感器网络通信处理复杂度及易于实现算法故障排除和检测，该算法结构采用分布式数据融合[11]；为了避免因实际环境导致观测方差发散，利用阈值限制所求观测方差；最后，为了得到更加准确的融合信息，利用各传感器观测噪声协方差结合不同测量值，在粒子滤波后RMSE(root mean square error，RMSE)限制区间，求得各传感器最优加权因子。

1 改进自适应加权融合的粒子滤波算法

1.1 粒子滤波算法研究

粒子滤波(particle filtering，PF)是以贝叶斯原理及重要性采样为基本框架，其本质思想是利用一组样本或者粒子来近似表示系统的后验分布，进而使用近似估计来逼近非线性系统的状态[16]。一般PF的算法流程如下[17]，假设n为独立分布的样本数，T为采样时间，k为采样时刻，Xk表示k时刻目标的状态，X0:k={X0,X1,…,Xk}与Z0:k=(Z0,Z1,…,Zk)分别表示0～k时刻的所有状态与观测值，前者与上一时刻的Xk-1有关，后者相互独立，只与Xk有关。

k时刻的每个粒子重新计算权重为

(1)

归一化权重：

(2)

第三步选择阶段(重采样)：

重要性采样阶段中重要的密度函数选择没有考虑当前观测值。因此，先验概率密度函数和真实后验概率密度的样本会有较大的不同。经过多次递归算法，只有某些粒子的权值较大，而其他粒子的权值太小，这种现象称为退化现象[18]。针对这一现象的不足，介绍了重采样技术，假设neff为有效粒子数，nT为设定的有效粒子数阈值，则：

(3)

(4)

k时刻的状态估计值为

(5)

第四步k=k+1，返回第二步继续执行。

1.2 自适应加权算法

假设用N个传感器测量某个未知位置目标Y，观测值为{Yi}(i=1,2,…,N)。则t时刻第j个传感器的观测值为

Yj(t)=Y(t)+yj(t)

(6)

(7)

由此可得估计方差为

(8)

式(8)中：σi表示第i个传感器的噪声方差。假设μ为构造系数，则为求得式(8)中令σ2最小的Wi，构造函数为

(9)

求解最小值问题可通过辅助函数归结为对{Wi}(i=1,2,…,N)求解极值的问题，即

(10)

通过求解式(10)可得

(11)

1.3 改进自适应加权融合的粒子滤波

PF算法中过程噪声方差Q和观测噪声的方差R理论上对滤波器的滤波结果影响较大。图1为仿真过程中R取不同值时[17]所得状态估计误差标准差。从信噪比的角度来说，信噪比越小，真实信号越有可能被噪声所覆盖，使得粒子滤波降低甚至失去滤波效果。因此，信噪比越大噪声越小，滤波越容易[17]。在仿真中Q、R一般都是根据信噪比来确定，但在实际工程应用中，观察方差通常由传感器的固有方差参数指定或由经验数据确定，而不考虑环境干扰。这样的观测方差不能反映实际观测的不确定性，因此很难获得最佳的数据融合效果。

图1 不同观测方差下的状态估计误差标准差Fig.1 Error standard deviation of state estimation under different observation variances

Yi=hX+yi

(12)

式(12)中：yi(i=1,2,…,N)包含传感器的内部噪声和环境干扰噪声，h为已知常量。

如果各传感器的测量噪声均为相互独立服从正态分布的零均值高斯白噪声，则N个传感器的测量均值为[19]

(13)

(14)

(15)

由于yij与yik(j≠k)相互独立，则有：

(16)

将式(14)代入式(16)得

(17)

假设用N个传感器对未知状态进行n次测试，第i个传感器第k次测量值为Yik，则：

(18)

(19)

(20)

(21)

(22)

2 实验与结果对比分析

2.1 实验设备

采用超宽带(UWB)和惯性测量单元(IMU)进行组合定位导航研究。UWB传感器的主要定位模块是Decawave公司于2012年推出的DWM1000模块，如图2所示。由于DWM1000模块抗多径衰落能力强、支持高数据速率通信、功耗低等优点，在UWB定位领域被广泛应用。选用的IMU模块为维特智能公司生产的JY901姿态角度传感器，如图3所示。该传感器模块集成高精度陀螺仪、加速度计、地磁场传感器，采用高性能的微处理器和先进的动力学解算与卡尔曼动态滤波算法，能够快速求解出模块当前实时的运动姿态。

图2 UWB定位模块Fig.2 UWB positioning module

VCC为模块电源，3.3V或5V输入；RXD为串行数据输入；TXD为串行数据输出；GND为地线图3 IMU定位模块Fig.3 IMU positioning module

2.2 实验结果对比

为了验证算法的准确性及在某个传感器信号弱丢失状态下的可行性，分别进行了两组实验。第一组实验选用的是500 cm×600 cm的非空旷场地。为了降低UWB标签定位解算的计算量，3个定位基站的坐标分别为A(0，0)、B(0，300)、C(450，550)，基站与标签处于同一水平高度。第二组实验选用的是500 cm×900 cm的非空旷场地，为了使得实验过程中模拟出UWB弱信号现象，设置3个定位基站的坐标分别为A(0，0)、B(0，500)、C(300，600)，基站与标签处于同一水平高度。为了验证算法的稳定性，每组实验均在同一环境及路径下，进行10次仿真求取平均RMSE(计算精度为小数点后3位)与UWB实际测量RMSE做对比。

其中，为了降低改进PF算法的计算复杂度，减轻传感器网络通信处理复杂度及易于实现算法故障排除和检测，该算法结构采用分布式数据融合，并给出了该算法分布式数据融合随粒子数增多，分别进行10次运算所求取的平均运算时长的对比。为了得到更好地融合效果，实验中采用粒子数i=1 000时所得到的实验结果与EKF融合算法进行对比分析。

表1为第一组实验改进PF算法在该路径下进行10次仿真所统计的融合精度，图4为表1第5次实验所得改进PF算法融合轨迹与EKF融合轨迹对比图。由图4可知，通过EKF与改进PF算法融合后的轨迹与采样轨迹相比更加接近真实轨迹。为了更加直观地显现出EKF与改进PF算法两者之间的融合差异性，绘制了实验两者融合轨迹与UWB测量轨迹的RMSE曲线，如图5所示。

表1 第一组实验10次仿真所统计的融合精度Table 1 The fusion accuracy of the first group was calculated by 10 simulations

图4 实验融合轨迹对比Fig.4 Experimental fusion trajectory comparison

图5 实验所得RMSE曲线Fig.5 RMSE curve obtained from the experiment

由图6可得，通过对分布式融合运算时间拟合结果得出，当i=1 000时，改进PF算法定位跟踪的运算时间为85.918 ms，在非视距实验过程中，如果目标运动速度为20 cm/s的情况下，其由重采样技术等引起的运算复杂度导致的跟踪性误差为 1.718 cm，远小于UWB因非视距等因素导致的测量误差，满足实验实时性跟踪需求。

图6 分布式融合运算时间随粒子数增多的对比Fig.6 Comparison of distributed fusion operation time with increasing number of particles

从表2可以看出，与UWB原始测量定位相比，EKF与改进PF算法融合后的定位精度均有不同程度的提高。通过实际计算可以得出，改进PF算法融合后的X方向波动性与EKF相比降低了14.972%，Y方向波动性降低了19.798%，定位精度提高了18.178%。

表2 第一组实验X、Y方向波动及RMSETable 2 The first group tested X-and Y-direction fluctuations and RMSE

为了进一步验证算法在UWB通信弱信号或丢失情况下的可行性和准确性，在500 cm×900 cm环境下进行第二组实验。其中，表3为改进PF算法在该组实验路径下进行10次仿真所统计的融合精度。图7为表3第5次实验所得改进PF融合轨迹与EKF融合轨迹对比，图8为该次实验下EKF与改进PF算法融合后RMSE对比。

由图7、图8、表3可以看出，改进PF算法修正后的轨迹比EKF融合修正后的轨迹更好。由图7可以看出，所提出定位融合算法受到的影响更小，融合后的轨迹比EKF更加接近真实轨迹。