基于UKF的UWB和GPS融合定位算法①

应保胜,周晓帅,方海龙,吴伟伟

(武汉科技大学 汽车与交通工程学院,武汉 430065)

随着智能交通和智能车辆的发展,汽车驾驶辅助系统、车辆安全等领域对高精度定位的需求日益加大.由高精度定位传感器与GPS 融合定位导航技术来改善定位性能成为重要的发展方向.GPS和IMU 组合定位系统采用卡尔曼滤波进行融合定位,IMU 模块对加速度和角加速度进行积分的同时,时间积累误差也逐渐增大[1],卡尔曼滤波器将产生滤波发散,进而导致定位漂移.超声波、RFID、WLAN和蓝牙等技术可以通过测距算法解算目标位置坐标,由于信号带宽和定位误差成负相关,定位技术的定位精度受信号带宽的限制,不适合与GPS 组成精确组合定位系统.mmWave雷达的分辨率特性,一般只能分辨目标的距离、角度和速度等信息,在高度上分辨能力差,多采用多输入多输出技术实现目标的定位.文献[2]考虑到雷达点稀疏性,提出基于mmWave 雷达和视觉传感器的空间注意力融合障碍物检测,有效对视觉特征进行融合;文献[3]同样结合摄像头采集车道特征实现车道级定位.上述方法都采用多输入方式将mmWave和视觉信号进行融合以提高感知性能,但在汽车复杂行驶工况下容错性差.

UWB 定位利用纳秒级非正弦窄脉冲载波传输数据的通讯技术进行定位,其频谱范围极宽,载波信号频率能达到GHz 级别,定位精度能达到厘米级[4,5],UWB可提供高精度位置感知网络中的优良解决方案.GPS定位系统虽然无法直接对目标载体的测姿方位角进行估算[6-8],但对载体的速度和运动方位角的估算切实可行,可与UWB 系统输出共同构成组合定位系统.

结合UWB的精度、成本优势和GPS 定位特点,可采用UWB 模块可与GPS 系统组成多传感器融合定位系统.对此类非线性多源数据融合系统[9-11],扩展卡尔曼滤波(extend Kalman filter,EKF)是常用的滤波手段,将非线性系统函数进行Taylor 级数展开进而进行卡尔曼滤波,缺点是产生高阶误差,降低系统精度,系统函数线性化过程中将产生更复杂的雅可比矩阵和海塞矩阵,增加系统的计算复杂度,滤波收敛精度收益较小,系统非线性较为严重时误差更大.针对卡尔曼滤波器的滤波发散问题,文献[12]中提出了一种基于衰减记忆的最优渐消滤波算法,但该算法利用一个渐消因子对高维系统进行自适应滤波,系统状态的变化不能被渐消因子准确描述,滤波器没有得到明显优化.在渐消因子滤波的基础上,文献[13]又提出次优渐消滤波,文献[14]提出多因子滤波等方法.上述渐消滤波虽然能减小状态模型误差,但渐消滤波对有粗差向量的鲁棒性不高,仍可能产生滤波发散.

为实现汽车定位系统的精确和可靠性,避免多源数据融合解算结果发散[15,16],减小滤波算法复杂度,本文提出基于无迹卡尔曼滤波(unscented Kalman filter,UKF)的UWB和GPS 融合定位算法,计算复杂度降低,能实现定位系统的高阶近似.在UWB 数据解算方面,优化多基站最小二乘定位算法,采用加权最小二乘法降低远距离节点造成测距系统噪声的影响.研究轨迹纠偏算法方面,基于UWB和GPS 多传感器物理模型,构建非线性系统方程,对构建的滤波模型进行系统噪声误差分析,优化滤波算法;UWB 定位系统获取的载体的位置坐标和GPS 获取到运动载体实时的速度和方位角组成观测向量,构建定位系统状态方程和观测方程,采用无迹卡尔曼滤波算法将融合的数据进行滤波,达到精确定位的目的.

1 系统架构

基于UWB和GPS的高精度组合定位系统物理层由UWB 定位模块和GPS 模块两个子系统组成.UWB模块通过串口输出测距数据由模块总成芯片程序通过最小二乘法解算目标载体的位置坐标;GPS 模块串口输出的NMEA 报文,经模块总成芯片程序预先烧录的解算程序解算目标载体的速度和方位角信息.如图1所示,基于UWB和GPS的融合定位系统架构包括:硬件物理层、数据解析层和滤波算法层.其中数据解析层是软硬件中间层,为算法层提供量测向量的数据来源;算法层将计算采样点经过非线性系统方程进行无迹变换,利用卡尔曼滤波对量测预测和均方差预测进行更新.

图1 UWB和GPS的融合定位算法架构

UWB 模块依据TOF 测距算法模型解算被测目标到各个基站间的距离,在最小二乘法的基础上进行优化,利用加权最小二乘法将UWB 基站测距值解算出待测坐标;GSP 报文信息解析出目标载体的速度v和方位角θ.UWB和GPS 组合定位系统是非线性系统.系统模型在扩展卡尔曼滤波算法的基础上进行优化,采用无迹卡尔曼滤波算法对目标轨迹进行高阶近似,通过一定规律的采样点计算和无迹变换,近似获得目标载体状态向量的均值和方差,对状态向量进行循环更新,实现对定位噪声的滤波,达到精确定位目的.

2 数据解析

定位坐标解算的条件是获得UWB 节点间的距离,UWB 节点由UWB 基站和标签模块组成,节点模块解算节点间距离.UWB 坐标解算采用最小二乘(least squares,LS)定位算法,如图2所示.设待测标签坐标为(x,y),标签到参考基站i的测量距离为d′i,其中,1≤i≤n,n为基站最大序号,标签到参考基站i的测量距离为di,如式(1)所示.

图2 UWB 坐标解算算法模型

测量距离和基站与标签间的真实距离差值记为ρi=d′i-di,利用最小二乘法最小化的值解算标签的坐标(x,y),利用式(2)解待定坐标X.

其中,

则最小方差解X如式(3)所示:

由于采用传统最小二乘定位算法计算标签坐标时将标签距离所有基站视为相同权重,实际情况是距离越大误差越大,本文将加权最小二乘(weighted least squares,WLS)定位算法用于UWB 坐标解算优化,具体解算公式如式(4)所示:

其中,ω是加权矩阵,其中距离观测噪声是均值为0,方差为σ2的高斯白噪声,其中加权矩阵ω 如式(5)所示:

为对比传统LS 定位算法和提出的WLS 定位算法的定位性能,在面积为5 m×5 m的正方形区域内分别对待测点(2,2)进行100 次定位仿真,假设LS 定位算法和WLS 定位算法都服从方差为σ2=0.01 m2,均值为0且相互独立分布的高斯白噪声.LS 定位算法和WLS定位算法仿真结果如图3所示.

图3 LS与WLS 定位算法性能

由仿真数据可知,WLS 定位算法定位误差为12.8 cm,而LS 定位算法误差为9.1 cm,WLS 定位算法和LS 定位算法精度提高28.9%.

3 构建和优化滤波器

由于UWB和GPS 融合定位系统为非线性系统,非线性系统可采用扩展卡尔曼滤波或无迹卡尔曼滤波算法构建融合定位系统滤波器.本文是在基于扩展卡尔曼滤波的UWB和GPS 融合定位算法基础上进行优化,采用无迹卡尔曼滤波的UWB和GPS 融合定位算法对此非线性系统进行滤波.

3.1 滤波器建模

构建卡尔曼滤波定位算法前构建系统和状态方程.在目标载体所在的平面建立坐标系,设向东是x的正方向,向北是y的正方向.目标的坐标和速度作为状态向量,假设系统过程噪声和量测噪声都服从均值为0的高斯分布,则设状态向量为X=[xyx′y′]T,其中,x、y分别是目标载体的分坐标;x′、y′分别是目标载体的分速度.对采集到的数据进行离散化建立状态方程如式(6)所示:

其中,F为状态转移矩阵,Γ为噪声驱动矩阵,W(k)为k时刻的过程噪声,其相应的协方差矩阵为Q=diag[σ2wx′,σ2wy′].X(k)和X(k+1) 分别为k和k+1时刻的状态向量,F、Γ、W(k)具体值如式(7)所示:

其中,T为采样周期,wx(k)和wy(k)分别为x、y轴的噪声分量.

以UWB 采集的目标载体坐标(x,y)和GPS 报文解析得的速度v和方位角θ 作为量测值,离散化数据建立量测方程如式(8)所示:

观测方程的各项参数为如式(9)所示.

其中,Z(k)为k时刻的观测向量,f[X(k)]为状态向量与观测向量的关系函数,V(k)为观测噪声,其相应的协方差矩阵为R.观测向量V(k)中,坐标量测噪声vx和vy分别服从均值为0,方差为σ2x和σ2y的高斯白噪声;速度和方位角量测噪声vv和vθ分别服从均值为0,方差为σ2v和σ2θ的高斯白噪声,V(k) 对应的协方差矩阵R=diag[σ2x,σ2y,σ2v,σ2θ].

为和观测向量建立联系,使用状态向量x′(k)、y′(k) 非线性表示速度v(k)和方位角θ (k),其中的x′(k)、y′(k) 分别为状态向量X(k)的速度分量.由于量测方程为非线性方程,使用扩展卡尔曼滤波需将f([X(k)])关于滤波值(k)进行一阶Taylor 级数展开,如式(10).

系统状态方程和量测方程建立后进行卡尔曼滤波,先对状态向量的均值和协方差矩阵进行初始化.取状态向量的初值为X(0)=[x(0)y(0)x′(0)y′(0)]T;协方差矩阵为4×4的单位矩阵I4;采样周期T为1 s,按照卡尔曼滤波核心公式进行预测和更新.

状态预测和状态均方差预测分别如式(11)所示:

其中,根据k时刻的状态向量(k|k) 预测k+1时刻的状态向量(k+1|k),根据k时刻的均方差矩阵P(k|k)预测k+1时刻的均方差矩阵P(k+1|k),通过状态转移矩阵F、过程噪声协方差矩阵Q和噪声驱动矩阵Γ 等信息进行状态一步预测和状态一步均方差预测.

在量测更新阶段,卡尔曼增益为:

状态向量更新为:

均方差矩阵更新为:

式(1 2) 中,通过k时刻的预测均方差矩阵P(k+1|k)、量测矩阵H(k+1)和量测噪声协方差矩阵R预 测k+1时刻卡尔曼增益K(k+1),K(k+1)表示预测均方差占量测误差和预测均方差之和的比重.式(13)和式(14)表示利用k+1时刻卡尔曼增益K(k+1)和量测值Z(k+1),对均方差矩阵P(k+1|k+1)和滤波值(k+1|k+1) 进行更新.式(13)中f((k+1|k))表示状态向量和量测向量的非线性函数关系.

3.2 优化滤波器

由于定位算法运行于嵌入式芯片,算法复杂度过大将增大硬件资源开销,算法复杂度将决定定位实时性等性能指标.扩展卡尔曼滤波将非线性系统函数进行一阶Taylor 级数展开,线性化过程中抛弃高阶项从而给系统带来非线性误差,且高阶EKF 算法复杂度大大提高.若利用UKF 直接对样本的后验概率密度分布进行近似,算法复杂度得以优化.针对提出的基于UKF的UWB和GPS 融合定位算法进行复杂度分析,并对比EKF 算法复杂度,得出算法复杂度优化的依据.

对算法浮点操作数(flops)的统计是分析算法复杂度的有效手段.一次flops 定义为两个浮点数的加减乘除运算,Cholesky 分解、指数运算和开方等效为相同运行时间的flops.本文中基本的运算flops 次数为:若A∈Rn×m,B∈Rn×m,计算A±B需要进行nm次flops;若A∈Rn×m,B∈Rm×l,计算AB需要进行2mnl-nl次flops;若A∈Rn×n,计算A-1需要进行n3次flops;若A∈Rn×n,计算chol(A)需要进行次flops.下文中n为状态维数,l为量测维数.

UKF 将UWB 模块解算的载体坐标和GPS 报文解算的速度和方位角信息作为UWB和GPS的组合定位算法数据源,基于UKF的UWB和GPS 融合定位算法架构如图4所示.

图4 基于UKF的UWB和GPS 融合定位算法架构

假设目标载体在平面坐标系做匀速直线运动,定义在k时刻状态向量为X(k)=[x(k)y(k)x′(k)y′(k)x′′(k)y′′(k)]T,其中包含目标在坐标系内的坐标、速度和加速度,设定位系统运动过程受外界因素导致的误差,即过程噪声为W(k),采样周期为T,Tw(k)为加速度引起的过程噪声,为加速度积分引起的过程噪声,为加速度二次积分引起的过程噪声,无迹卡尔曼滤波定位算法系统方程如式(15)所示.

设定位系统状态方程为X(k+1)=FX(k)+ΓW(k),则状态转移矩阵F和噪声驱动矩阵Γ 如式(16) 所示,W(k)=[wx(k) wy(k)]T表示均值为0、协方差为的过程噪声.

在基于无迹卡尔曼滤波的UWB和GPS 融合定位算法系统中,假设定位系统在k时刻量测值为Z(k),其中包含标签相对于基站的真实坐标 (x,y)和GPS 定位系统确定的速度v和方位角θ,则离散化的系统的量测方程如式(17)所示:

其中,Z(k)为k时刻状态向量的量测值,Z(k)=[x(k)y(k)v(k) θ(k)]T,V(k)为k时刻 状态向量的量测值,V(k)=[vx(k)vy(k)vv(k)vθ(k)]T,坐标量测噪声vx和vy分别服从(0,σ2x)和(0,σ2y)的高斯白噪声;速度和方位角向量测噪声vv和vθ分别服从(0,σ2v)和(0,σ2θ)的高斯白噪声,量测噪声V(k) 对应的协方差矩阵R=diag[σ2x,σ2y,σ2v,σ2θ].

用上述确定的系统状态方程和量测方程进行无迹卡尔曼滤波,选取目标载体的初始坐标和滤波初值如式(18)所示,滤波框架如式(19)所示.

计算增益K(k) 进行l3+(2nl2-nl)次flops,计算状态向量估计(k) 进行2nl+l次flops,计算协方差矩阵PX(k) 进行2nl2-nl+2n2l次flops.

设已知3 个UWB 基站BS.A、BS.B和BS.C 分别位于 (0,10)、(10,0)和(10,10)处,目标载 体的初始状 态向量为X(0)=[0 0 0.2 0.2 0.05 0.05]T,状态协方差矩阵P(0)=I6.非线性量测方程无迹变换,分布系数α=1,比例因子κ=0,均方差系数γ=α2(n+κ)-n,状态向量维数n=6.状态预测过程中,计算采样点如式(20),状态一步预测和均方差矩阵一步预测如式(21).

计算采样点 χ (k-1) 进行次flops,计算状态一步预测(k/k-1)和均方差矩阵一步预测PX(k/k-1) 分别进行2n2+2n次 flops和4n3+5n2+2n次flops.

量测预测过程中,采样点更新如式(22)所示,量测均值更新、量测均方差矩阵更新和状态量测协方差矩阵更新如式(23)所示.ηi为非线性变换后的状态向量,Wim和Wic分别为采样后均值和协方差的加权系数.

计算量测样本点 χ(k/k-1) 进行次flops,采样点预测ηi(k/k-1) 进行4n2l-l次flops,量测均值更新(k/k-1)进行2nl+2l次flops、量测均方差矩阵更新PZ(k/k-1) 进行4nl2+2nl+3l2+2l次flops,状态量测协方差矩阵更新PXZ(k/k-1) 进行4n2l+2n2+2nl+2n次flops.

4 实验分析

为评估优化后的UKF 算法复杂度带来系统滤波实时性的提升,实验将UKF 定位算法和EKF 算法分别写入到实验平台的控制器,将滤波结果以日志文件的形式从串口输出,根据日志中每帧滤波结果的时间戳,计算两种算法滤波时间 Δt,验证UKF 有比EKF 更好的实时性.如表1所示为UKF和EKF 滤波时间.由表2可知,EKF 平均滤波时间为63.3 ms,UKF 平均滤波时间为37.9 ms,表明基于UKF的UWB和GPS 融合定位算法由较高的实时性,效率更高.

表1 各组实验数据和性能

表2 EKF和UKF 滤波时间 (ms)

为评估提出的基于无迹卡尔曼滤波的UWB和GPS 融合定位算法定位精度,实验平台车辆由UWB定位标签、GPS 定位模块以及GPS/IMU 组合定位模组构成.采用GNW-MKS-4051-A 模块作为GPS 传感器单元,串口输出数据经STM32F103 芯片进行解析;DW1000 模块作为UWB 传感器单元,STM32F103 芯片对UWB 采集数据由上位机软件解算速度和方位角并导出为Excel 格式.Matlab 构建基于无迹卡尔曼滤波的UWB和GPS 融合定位算法,对解算进行无迹卡尔曼滤波,最终用图像形式将滤波定位结果进行展示.

在10 m×10 m的正方形区域内,三基站BS.A、BS.B和BS.C 坐标分别为(0,10)、(10,0)和(10,10),小车分别沿y=x轴线和y=5 m轴线以进行匀速直线运动做两组实验,对两组实验的实验采集数据进行分析.每组实验分别包括基于EKF的UWB和GPS 融合定位系统、基于UKF的UWB和GPS 融合定位系统和GPS/IMU 组合定位系统,图5和图6分别为两组实验.

图5(a)表示第A 组实验,目标载体沿y=5 m进行0.2 m/s的直线运动,定位采样周期T=1 s,GPS/IMU组合定位系统采集的定位坐标振动明显,平均误差为84.46 cm;基于EKF的UWB和GPS 融合定位算法的定位坐标与真实值平均误差为13.89 cm;基于UKF的UWB和GPS 融合定位算法平均误差仅为10.11 cm,且无滤波发散现象.图5(b)为实验数据的局部细节图.

图5 A 组实验各种定位算法轨迹实验

为验证在更大范围的定位性能进行B 组实验,在100 m×100 m的区域内进行滤波定位实验,如图6所示,三基站BS.A、BS.B和BS.C 坐标分别为(0,100)、(100,0)和(100,100),目标载体沿y=x进行直线运动,定位采样周期T=10 s.

图6 实验场地及布点

图7(a)表示第B 组实验数据,GPS/IMU 组合定位系统定位坐标平均误差在79.29 cm;基于EKF的UWB和GPS 融合定位算法的定位坐标与真实值平均误差在21.86 cm;而基于UKF的UWB和GPS 融合定位算法平均误差仅为18.39 cm,其定位精度仍能达到cm 级.图7(b)为实验数据的局部细节图.

图7 B 组实验各种定位算法轨迹实验

表2列出了A、B 两组实验的实验数据和性能.

实验结果表明,基于无迹卡尔曼滤波的UWB和GPS 融合定位算法相比传统GPS/IMU 组合定位系统,精度提高83.54%,定位误差不随时间变化;相较于基于扩展卡尔曼滤波的UWB和GPS 融合定位算法优化效果明显,精度提高23.05%,定位坐标与真实值拟合度更高,且无滤波发散现象;本算法在100 m 范围平均误差有所增大,但仍能达到cm 级精度.在保持cm 级精度下,极限定位距离约为150 m.

5 结论

本文针对当今汽车广泛使用的GPS/IMU 组合定位系统存在时间积累误差问题,提出了一种基于无迹卡尔曼滤波的UWB和GPS 融合定位算法.为降低系统噪声的影响,使用加权最小二乘法对UWB 定位算法进行优化;滤波过程中使用无迹卡尔曼滤波器对数据融合算法进行优化.实验将基于UKF的UWB和GPS 融合算法写入到实验平台控制器,对滤波实时性和定位精度等性能指标进行分析,同时该算法与基于扩展卡尔曼滤波的UWB和GPS 融合定位算法以及传统GPS/IMU 组合定位系统做性能对比.得出以下结论:

1)基于无迹卡尔曼滤波的UWB和GPS 融合定位算法相较于传统的GPS/IMU 组合定位系统定位精度大幅提高,定位误差不随时间变化,系统在特定工况下容错性佳,可实现cm 级精度定位;

2)基于无迹卡尔曼滤波的UWB和GPS 融合定位算法避免雅可比矩阵和海塞矩阵参与运算,算法复杂度大幅降低,可在嵌入式平台进行实时性滤波,同时比EKF 算法定位精度提高23.05%,无滤波发散现象;

3)此外,加权最小二乘定位算法用于UWB 定位系统,优化定位系统的过程噪声进而提高定位精度,同时削弱粗差数据影响、提高定位方程抗差性.