随着智能交通和智能车辆的发展, 汽车驾驶辅助系统、车辆安全等领域对高精度定位的需求日益加大. 由高精度定位传感器与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的融合定位系统架构包括: 硬件物理层、数据解析层和滤波算法层. 其中数据解析层是软硬件中间层, 为算法层提供量测向量的数据来源; 算法层将计算采样点经过非线性系统方程进行无迹变换, 利用卡尔曼滤波对量测预测和均方差预测进行更新.

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

2 数据解析

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

$ {d_i} = \sqrt {{{\left( {x - {x_i}} \right)}^2} + {{\left( {y - {y_i}} \right)}^2}} $

(1)

测量距离和基站与标签间的真实距离差值记为 $ {\rho _i} = {d'}_i - {d_i} $ , 利用最小二乘法最小化 $\displaystyle\sum\limits_{i = 1}^n {{\rho _i}^2}$ 的值解算标签的坐标 $ \left( {x, y} \right) $ , 利用式(2)解待定坐标 $ X $ .

$ HX = a $

(2)

其中,

$\begin{split}& H = \left[ {\begin{array}{*{20}{c}} {{x_2} - {x_1}}&{{y_2} - {y_1}} \\ {{x_3} - {x_1}}&{{y_3} - {y_1}} \\ \vdots & \vdots \\ {{x_n} - {x_1}}&{{y_n} - {y_1}} \end{array}} \right], \; X = {\left[ {x{{ \; y}}} \right]^{\text{T}}}, \\ &a = \frac{1}{2}\left[ {\begin{array}{*{20}{c}} {x_2^2 + y_2^2 - d_2^2 - x_1^2 - y_1^2 + d_1^2} \\ {x_3^2 + y_3^2 - d_3^2 - x_1^2 - y_1^2 + d_1^2} \\ \vdots \\ {x_n^2 + y_n^2 - d_n^2 - x_1^2 - y_1^2 + d_1^2} \end{array}} \right] \end{split}$

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

$ X = {\left( {{H^{\text{T}}}H} \right)^{ - 1}}{H^{\text{T}}}a $

(3)

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

$ X = {\left( {{H^{\text{T}}}\omega H} \right)^{ - 1}}{H^{\text{T}}}\omega a $

(4)

其中, $ \omega $ 是加权矩阵, 其中距离观测噪声是均值为0, 方差为 $ {\sigma ^2} $ 的高斯白噪声, 其中加权矩阵 $ \omega $ 如式(5)所示:

$ \omega = \left[ {\begin{array}{*{20}{c}} {\dfrac{{\sigma _2^2}}{{d_2^2}}}&0&0&0 \\ 0&{\dfrac{{\sigma _3^2}}{{d_3^2}}}&0&0 \\ \vdots & \vdots & \vdots & \vdots \\ 0&0&0&{\dfrac{{\sigma _n^2}}{{d_n^2}}} \end{array}} \right] $

(5)

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

由仿真数据可知, 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 = {\left[ x \; y \;{x'} \; {y'} \right]^{\text{T}}}$ , 其中, $ x $ 、 $ y $ 分别是目标载体的分坐标; $ x' $ 、 $ y' $ 分别是目标载体的分速度. 对采集到的数据进行离散化建立状态方程如式(6)所示:

$ X\left( {k + 1} \right) = FX\left( k \right) + \Gamma W\left( k \right) $

(6)

其中, $ F $ 为状态转移矩阵, ${\Gamma}$ 为噪声驱动矩阵, $ W\left( k \right) $ 为 $ k $ 时刻的过程噪声, 其相应的协方差矩阵为 $ Q = $ $ diag\left[ {{\sigma ^2}_{{w_{x'}}}, {\sigma ^2}_{{w_{y'}}}} \right] $ . $ X\left( k \right) $ 和 $ X\left( {k + 1} \right) $ 分别为 $ k $ 和 $ k + 1 $ 时刻的状态向量, $ F $ 、 $ \Gamma $ 、 $ W\left( k \right) $ 具体值如式(7)所示:

$ \left\{ {\begin{split}& {F = \left[ {\begin{array}{*{20}{l}} 1&0&T&0 \\ 0&1&0&T \\ 0&0&1&0 \\ 0&0&0&1 \end{array}} \right], \Gamma = \left[ {\begin{array}{*{20}{l}} {\frac{{{T^2}}}{2}}&0 \\ 0&{\frac{{{T^2}}}{2}} \\ T&0 \\ 0&T \end{array}} \right]} \\ & {W = \left[ {\begin{array}{*{20}{c}} {{w_x}\left( k \right)} \\ {{w_y}\left( k \right)} \end{array}} \right]} \end{split}} \right. $

(7)

其中, $ T $ 为采样周期, $ {w_x}\left( k \right) $ 和 $ {w_y}\left( k \right) $ 分别为 $ x $ 、 $ y $ 轴的噪声分量.

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

$ Z\left( k \right) = f\left[ {X\left( k \right)} \right] + V\left( k \right) $

(8)

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

$ \left\{ {\begin{array}{*{20}{c}} {Z\left( k \right) = \left[ {\begin{array}{*{20}{c}} {x\left( k \right)} \\ {y\left( k \right)} \\ {v\left( k \right)} \\ {\theta \left( k \right)} \end{array}} \right]{\text{ }}, {\text{ }}V\left( k \right) = \left[ {\begin{array}{*{20}{c}} {{v_x}\left( k \right)} \\ {{v_y}\left( k \right)} \\ {{v_v}\left( k \right)} \\ {{v_\theta }\left( k \right)} \end{array}} \right]} \\ {f\left( {\left[ {X\left( k \right)} \right]} \right) = \left[ {\begin{array}{*{20}{c}} {x\left( k \right)} \\ {y\left( k \right)} \\ {\sqrt {x{'^2}\left( k \right) + y{'^2}\left( k \right)} } \\ {\arctan \dfrac{{x'\left( k \right)}}{{y'\left( k \right)}}} \end{array}} \right]} \end{array}} \right. $

(9)

其中, $ Z\left( k \right) $ 为 $ k $ 时刻的观测向量, $ f\left[ {X\left( k \right)} \right] $ 为状态向量与观测向量的关系函数, $ V\left( k \right) $ 为观测噪声, 其相应的协方差矩阵为 $ R $ . 观测向量 $ V\left( k \right) $ 中, 坐标量测噪声 $ {v_x} $ 和 $ {v_y} $ 分别服从均值为0, 方差为 $ \sigma _x^2 $ 和 $ \sigma _y^2 $ 的高斯白噪声; 速度和方位角量测噪声 $ {v_v} $ 和 $ {v_\theta } $ 分别服从均值为0, 方差为 $ \sigma _v^2 $ 和 $ \sigma _\theta ^2 $ 的高斯白噪声, $ V\left( k \right) $ 对应的协方差矩阵 $ R = $ $ diag\left[ {\sigma _x^2, \sigma _y^2, \sigma _v^2, \sigma _\theta ^2} \right] $ .

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

$ \begin{split} Z\left( k \right) =& f\left[ {\hat X\left( {k|k - 1} \right)} \right] \hfill \\ & +\frac{{\partial f}}{{\partial \hat X\left( k \right)}}{|_{\hat X\left( {k|k - 1} \right)}}\left[ {X\left( k \right) - \hat X\left( {k - 1} \right)} \right] + V\left( k \right) \hfill \end{split} $

(10)

令 $H\left( k \right) = \dfrac{{\partial f}}{{\partial \hat X\left( k \right)}}{|_{\hat X\left( {k|k - 1} \right)}}$ , $y\left( k \right) = f\left[ {\hat X\left( {k|k - 1} \right)} \right] - $ $ \dfrac{{\partial f}}{{\partial \hat X\left( k \right)}}{|_{\hat X\left( {k|k - 1} \right)}}\hat X\left( {k|k - 1} \right)$ , 则线性化量测方程为 $Z\left( k \right) = $ $ H\left( k \right)X\left( k \right) + V\left( k \right) + y\left( k \right)$ , 式中 $ H(k) $ 具体值为: $H(k) = \left[ {\begin{array}{*{20}{c}} 1&0&0&0 \\ 0&1&0&0 \\ 0&0&{\dfrac{{\hat x'\left( k \right)}}{{\sqrt {\hat x'^{2}\left( k \right) + \hat y'^{2}\left( k \right)} }}}&{\dfrac{{\hat y'\left( k \right)}}{{\sqrt {{{\hat {\dot x}}^2}\left( k \right) + \hat y'^{2}\left( k \right)} }}} \\ 0&0&{\dfrac{{\hat y'\left( k \right)}}{{\hat x'^{2}\left( k \right) + \hat y'^{2}\left( k \right)}}}&{\dfrac{{ - \hat x'\left( k \right)}}{{\hat x'^{2}\left( k \right) + \hat y'^{2}\left( k \right)}}} \end{array}} \right]$ , 其中, $ \hat x{'^{2}}\left( k \right) + \hat y{'^{2}}\left( k \right) \ne 0. $

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

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

$ \left\{ {\begin{split}& {\hat X\left( {k + 1|k} \right) = F\hat X\left( {k|k} \right)} \\ &{P(k + 1|k) = FP(k|k){F^{\text{T}}} + \Gamma Q{\Gamma ^{\text{T}}}} \end{split}} \right. $

(11)

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

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

$ \begin{split} K\left( {k + 1} \right) = &P\left( {k + 1|k} \right){H^{\text{T}}}\left( {k + 1} \right) \hfill \\ & \cdot {\left[ {H\left( {k + 1} \right)P\left( {k + 1|k} \right){H^{\text{T}}}\left( {k + 1} \right) + R} \right]^{ - 1}} \hfill \end{split} $

(12)

状态向量更新为:

$ \begin{split} \hat X\left( {k + 1|k + 1} \right) = &\hat X\left( {k + 1|k} \right) + K\left( {k + 1} \right) \hfill \\ & \cdot \left[ {Z\left( {k + 1} \right) - f\left( {\hat X\left( {k + 1|k} \right)} \right)} \right] \hfill \end{split} $

(13)

均方差矩阵更新为:

$ \begin{split}& P\left( {k + 1|k + 1} \right) = \left[ {{I_4} - K\left( {k + 1} \right)H\left( {k + 1} \right)} \right] \cdot P\left( {k + 1|k} \right) \hfill \end{split} $

(14)

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

3.2 优化滤波器

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

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

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

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

$ \left\{ {\begin{array}{*{20}{l}} {x\left( {k + 1} \right) = x\left( k \right) + Tx'\left( k \right) + \dfrac{{{T^2}}}{2}x''\left( k \right) + \dfrac{{{T^3}}}{6}{w_x}\left( k \right)} \\ {y\left( {k + 1} \right) = y\left( k \right) + Ty'\left( k \right) + \dfrac{{{T^2}}}{2}y''\left( k \right) + \dfrac{{{T^3}}}{6}{w_y}\left( k \right)} \\ {x'\left( {k + 1} \right) = x'\left( k \right) + Tx''\left( k \right) + \dfrac{{{T^2}}}{2}{w_x}\left( k \right)} \\ {y'\left( {k + 1} \right) = y'\left( k \right) + Tx''\left( k \right) + \dfrac{{{T^2}}}{2}{w_y}\left( k \right)} \\ {x''\left( {k + 1} \right) = x''\left( k \right) + T{w_x}\left( k \right)} \\ {y''\left( {k + 1} \right) = y''\left( k \right) + T{w_y}\left( k \right)} \end{array}} \right. $

(15)

设定位系统状态方程为 $ X\left( {k + 1} \right) = FX\left( k \right) + \Gamma W\left( k \right) $ , 则状态转移矩阵 $ F $ 和噪声驱动矩阵 $ \Gamma $ 如式(16)所示, $ W\left( k \right) = {\left[ {{w_x}\left( k \right){\text{ }}{{\text{w}}_y}\left( k \right)} \right]^{\text{T}}} $ 表示均值为0、协方差为 $ Q = diag\left( {\sigma _{x''}^2, \sigma _{y''}^2} \right) $ 的过程噪声.

$ F = \left[ {\begin{array}{*{20}{c}} 1&0&T&0&{\dfrac{{{T^2}}}{2}}&0 \\ 0&1&0&{{T}}&0&{\dfrac{{{T^2}}}{2}} \\ 0&0&1&0&T&0 \\ 0&0&0&1&0&T \\ 0&0&0&0&1&0 \\ 0&0&0&0&0&1 \end{array}} \right], \Gamma = \left[ {\begin{array}{*{20}{c}} {\dfrac{{{T^3}}}{6}}&0 \\ 0&{\dfrac{{{T^3}}}{6}} \\ {\dfrac{{{T^2}}}{2}}&0 \\ 0&{\dfrac{{{T^2}}}{2}} \\ T&0 \\ 0&T \end{array}} \right] $

(16)

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

$ \begin{split} Z\left( k \right) =& f\left[ {X\left( k \right)} \right] + V\left( k \right) \hfill \\ = & \left[ {\begin{array}{*{20}{c}} {x\left( k \right)} \\ {y\left( k \right)} \\ {\sqrt {x{'^2}\left( k \right) + y{'^2}\left( k \right)} } \\ {\arctan \dfrac{{x'\left( k \right)}}{{y'\left( k \right)}}} \end{array}} \right] + V\left( k \right) \hfill \\ \end{split} $

(17)

其中, $ Z\left( k \right) $ 为 $ k $ 时刻状态向量的量测值, $Z\left( k \right) = [ {x\left( k \right)}\;\; $ $ {y\left( k \right)}\;\;{v\left( k \right)}\;\;{\theta \left( k \right)} ]^{\text{T}}$ , $ V\left( k \right) $ 为 $ k $ 时刻状态向量的量测值, $V\left( k \right) = {\left[ {{v_x}\left( k \right)}\;\;\;{{v_y}\left( k \right)}\;\;\;{{v_v}\left( k \right)}\;\;\;{{v_\theta }\left( k \right)} \right]^{\text{T}}}$ , 坐标量测噪声 $ {v_x} $ 和 $ {v_y} $ 分别服从 $ \left( {0, \sigma _x^2} \right) $ 和 $ \left( {0, \sigma _y^2} \right) $ 的高斯白噪声; 速度和方位角向量测噪声 $ {v_v} $ 和 $ {v_\theta } $ 分别服从 $ \left( {0, \sigma _v^2} \right) $ 和 $ \left( {0, \sigma _\theta ^2} \right) $ 的高斯白噪声, 量测噪声 $ V\left( k \right) $ 对应的协方差矩阵 $ R = $ $ diag\left[ {\sigma _x^2, \sigma _y^2, \sigma _v^2, \sigma _\theta ^2} \right] $ .

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

$ \left\{ {\begin{array}{*{20}{l}} {\hat X\left( 0 \right) = E\left[ {X\left( 0 \right)} \right]} \\ {P\left( 0 \right) = E\left[ {\left( {X\left( 0 \right) - \hat X\left( 0 \right)} \right){{\left( {X\left( 0 \right) - \hat X\left( 0 \right)} \right)}^{\text{T}}}} \right]} \end{array}} \right. $

(18)

$ \left\{ {\begin{array}{*{20}{l}} {K\left( k \right) = {P_{XZ}}\left( {k/k - 1} \right)P_Z^{ - 1}\left( {k/k - 1} \right)} \\ {\hat X\left( k \right) = \hat X\left( {k/k - 1} \right) + {K(k)}\left( {Z\left( k \right) - \hat Z\left( {k/k - 1} \right)} \right)} \\ {{P_X}\left( k \right) = {P_X}\left( {k/k - 1} \right) - K\left( k \right){P_Z}\left( {k/k - 1} \right){K^{\text{T}}}\left( k \right)} \end{array}} \right. $

(19)

计算增益 $ K\left( k \right) $ 进行 $ {l^3} + \left( {2n{l^2} - nl} \right) $ 次flops, 计算状态向量估计 $ \hat X\left( k \right) $ 进行 $ 2nl + l $ 次flops, 计算协方差矩阵 $ {P_X}\left( k \right) $ 进行 $ 2n{l^2} - nl + 2{n^2}l $ 次flops.

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

计算采样点 $ \chi \left( {k - 1} \right) $ 进行 $ \frac{1}{3}{n^3} + 3{n^2} $ 次flops, 计算状态一步预测 $ \hat X\left( {k/k - 1} \right) $ 和均方差矩阵一步预测 $ {P_X}\left( {k/k - 1} \right) $ 分别进行 $ 2{n^2} + 2n $ 次flops和 $ 4{n^3} $ $ + 5{n^2} + 2n $ 次flops.

$ \chi \left( {k - 1} \right) = \left[ {\begin{array}{*{20}{c}} {\hat X\left( {k - 1} \right)}&{{{[\hat X\left( {k - 1} \right)]}_n} + \gamma \sqrt {{P_X}\left( {k - 1} \right)} }&{{{[\hat X\left( {k - 1} \right)]}_n} - \gamma \sqrt {{P_X}\left( {k - 1} \right)} } \end{array}} \right] $

(20)

$ \left\{ {\begin{array}{*{20}{l}} {\hat X\left( {k/k - 1} \right) = F\left( {{\chi _i}\left( {k - 1} \right)} \right)} \\ {{P_X}\left( {k/k - 1} \right) = F{P_X}\left( {k - 1} \right){F^T} + Q} \end{array}} \right. $

(21)

量测预测过程中, 采样点更新如式(22)所示, 量测均值更新、量测均方差矩阵更新和状态量测协方差矩阵更新如式(23)所示. $ {\eta _i} $ 为非线性变换后的状态向量, $ W_i^m $ 和 $ W_i^c $ 分别为采样后均值和协方差的加权系数.

计算量测样本点 $ \chi \left( {k/k - 1} \right) $ 进行 $\frac{1}{3}{n^3} + 3{n^2}$ 次flops, 采样点预测 $ {\eta _i}\left( {k/k - 1} \right) $ 进行 $ 4{n^2}l - l $ 次flops, 量测均值更新 $ \hat Z\left( {k/k - 1} \right) $ 进行 $ 2nl + 2l $ 次flops、量测均方差矩阵更新 $ {P_Z}\left( {k/k - 1} \right) $ 进行 $ 4n{l^2} + 2nl + 3{l^2} + 2l $ 次flops, 状态量测协方差矩阵更新 $ {P_{XZ}}\left( {k/k - 1} \right) $ 进行 $ 4{n^2}l + 2{n^2} + 2nl + 2n $ 次flops.

$ \chi \left( {k/k - 1} \right) = \left[ {\begin{array}{*{20}{c}} {\hat X\left( {k/k - 1} \right)}&{{{[\hat X\left( {k/k - 1} \right)]}_n} + \gamma \sqrt {{P_X}\left( {k/k - 1} \right)} }& {{{[\hat X\left( {k/k - 1} \right)]}_n} - \gamma \sqrt {{P_X}\left( {k/k - 1} \right)} } \end{array}} \right] $

(22)

$ \left\{ {\begin{array}{*{20}{l}} {{\eta _i}\left( {k/k - 1} \right) = f\left( {{\chi _i}\left( {k/k - 1} \right)} \right)} \\ {\hat Z\left( {k/k - 1} \right) = \displaystyle\sum\nolimits_{i = 0}^{2n} {W_i^m{\eta _i}\left( {k/k - 1} \right)} } \\ {{P_Z}\left( {k/k - 1} \right) = \displaystyle\sum\nolimits_{i = 0}^{2n} {W_i^c\left( {{\eta _i}\left( {k/k - 1} \right) - \hat Z\left( {k/k - 1} \right)} \right){{\left( {{\eta _i}\left( {k/k - 1} \right) - \hat Z\left( {k/k - 1} \right)} \right)}^{\text{T}}}} + R\left( k \right)} \\ {{P_{XZ}}\left( {k/k - 1} \right) = \displaystyle\sum\nolimits_{i = 0}^{2n} {W_i^c\left( {{\chi _i}\left( {k/k - 1} \right) - \hat Z\left( {k/k - 1} \right)} \right){{\left( {{\eta _i}\left( {k/k - 1} \right) - \hat Z\left( {k/k - 1} \right)} \right)}^{\text{T}}}} } \end{array}} \right. $

(23)

综上, 基于UKF的定位算法复杂度总计为 $ \dfrac{{14}}{3}{n^3} + $ $ 15{n^2} + 10{n^2}l + 6n + 8n{l^2} + 6nl + {l^3} + 3{l^2} + 4l $ , 同理基于一阶EKF的定位算法复杂度为 $4{n^3} + 6{n^2}l + 6{n}{l^2} + {n^2} - n + $ $ {l^3}$ . 二阶EKF算法复杂度远大于一阶EKF, 故UKF算法复杂度低于EKF.

4 实验分析

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

为评估提出的基于无迹卡尔曼滤波的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坐标分别为 $ \left( {0, 10} \right) $ 、 $ \left( {10, 0} \right) $ 和 $ \left( {10, 10} \right) $ , 小车分别沿 $ y = x $ 轴线和 $ y = 5 {\text{ m}}\begin{array}{*{20}{l}}\end{array} $ 轴线以进行匀速直线运动做两组实验, 对两组实验的实验采集数据进行分析. 每组实验分别包括基于EKF的UWB和GPS融合定位系统、基于UKF的UWB和GPS融合定位系统和GPS/IMU组合定位系统, 图5和图6分别为两组实验.

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

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

图7(a)表示第B组实验数据, GPS/IMU组合定位系统定位坐标平均误差在79.29 cm; 基于EKF的UWB和GPS融合定位算法的定位坐标与真实值平均误差在21.86 cm; 而基于UKF的UWB和GPS融合定位算法平均误差仅为18.39 cm, 其定位精度仍能达到cm级. 图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定位系统, 优化定位系统的过程噪声进而提高定位精度, 同时削弱粗差数据影响、提高定位方程抗差性.