CN109946730B - 一种车路协同下基于超宽带的车辆高可靠融合定位方法 - Google Patents

Abstract

本发明公开了一种车路协同场景下的车辆高可靠融合定位方法，针对城市环境下，车载卫星导航易使用受限，定位准确性及可靠性不高等问题，在传统的车载卫星与惯性组合导航的基础上，引入了超宽带（Ultra‑Wideband，UWB）定位技术，通过合理布局UWB节点，结合非视距（Not Line of Sight，NLOS）误差处理算法，减小UWB观测值误差，在此基础之上，基于扩展卡尔曼滤波算法实现了车辆的融合定位。相比于传统的车载卫星与惯性组合导航，本发明中的方法，在城市环境，尤其是卫星信号受严重遮挡的复杂环境（如城市峡谷、交叉路口等）下，可靠性更高，有助于实现车辆的连续、完整、可靠、实时定位。

Description

一种车路协同下基于超宽带的车辆高可靠融合定位方法

技术领域

本发明涉及车辆导航定位领域，特别涉及一种面向城市车路协同环境下的车辆高可靠融合定位方法。

背景技术

随着经济社会的发展与进步，我国的机动车保有量快速增加，道路交通面临着巨大的挑战，为了解决日益严峻的城市交通问题，智能车路协同系统(Intelligent VehicleInfrastructure Cooperative Systems，IVICS)应运而生，并逐步成为智能交通系统(Intelligent Transport Systems，ITS)研究的最新发展方向。无论是车路协同的应用，还是智能交通的实现，都离不开高精度车辆定位技术：只有在实现准确、可靠车辆定位的前提下，全方位实施车车、车路动态实时信息交互，并在全时空动态交通信息采集与融合的基础上开展车辆主动安全控制和道路协同管理，充分实现人车路的有效协同，才能够有效地指挥调度车辆，改善城市交通，保证车辆安全行驶。因此，车辆定位技术是车路协同乃至智能交通等研究的基础与核心内容之一。

目前常见的车辆导航定位技术包括：航位推算(Dead Reckoning,DR)，惯性导航系统(Inertial Navigation System,INS)，卫星导航(Global Navigation SatelliteSystem,GNSS)等。由于单一定位技术的存在各自不足，为了实现城市环境下的准确、可靠、连续、完整定位，多采用两种及两种以上的定位技术相结合，其中GNSS/INS融合定位的应用最为广泛：GNSS/INS融合定位可以解决卫星信号短时中断情况下的定位问题，并在一定程度上补偿INS的累积误差，能够满足相对开阔环境下的车辆定位需求，但在卫星信号长时间中断环境(如城市峡谷、隧道等)中，若GNSS无法正常工作，仅依靠INS自主推算，依然会导致较大的定位误差。

近年来，基于超宽带(Ultra-Wideband，UWB)的无线定位技术的兴起和迅速发展为实现车辆在GNSS受限环境下的可靠定位提供了新思路，目前，UWB定位技术主要被用于室内定位领域如人员、智能小车、机器人定位等。由于UWB的带宽极宽，脉冲信号穿透力强，多径分辨能力良好等技术优点，基于UWB实现室外环境下的车辆定位亦具备了一定的可行性，在传感器层，若引入相对可靠的UWB信息源，则能够有效提高融合定位系统的鲁棒性，但在复杂城市环境下，UWB信号易受周围行驶的车辆、道路两旁的树木以及建筑物的干扰而出现多路径以及非视距误差，若将信号质量较差的UWB观测量用于融合定位，势必导致融合精度的下降，因此，如何针对相对复杂的交通场景(如十字交叉路口、环岛路口等)，结合道路交通设施，合理布局路侧UWB节点，在车路协同下实现人车路信息动态交互，进而选取合适的方法，辨识与补偿非视距误差，提高用于融合的UWB观测信息的可靠性，是实现基于UWB的车辆融合定位亟待解决的关键问题。

发明内容

技术问题：针对城市环境下，车载卫星导航易使用受限，定位准确性及可靠性不高等问题，在传统的车载卫星与惯性组合导航的基础上，引入了超宽带(Ultra-Wideband，UWB)定位技术，通过合理布局UWB节点，结合非视距误差处理算法，减小UWB观测值误差，并在此基础之上，基于扩展卡尔曼滤波算法实现了车辆的融合定位。

技术方案：为实现上述目的，本发明采用如下技术方案：首先，在道路两旁布置UWB固定节点，并通过高精度差分GNSS获取UWB固定节点的位置坐标；其次，在车辆顶部布置一个UWB移动节点，采用到达时间差法，获得该UWB移动节点到各个UWB固定节点的距离；然后，根据当前时刻之前的历史距离观测值建立ARIMA模型，利用ARIMA模型逐一对各个UWB固定节点进行NLOS辨识；最终，根据修正后的UWB距离观测值，基于扩展卡尔曼滤波实现车辆的融合定位获取车辆的精确位置。

下面结合附图，对本发明的思路作进一步的说明：

本发明的流程如图1所示。

一种车路协同下基于超宽带的车辆高可靠融合定位方法，其特征在于，在典型城市车路协同场景下，在车载卫星与惯性组合导航的基础上，引入超宽带(Ultra-Wideband，UWB)定位，通过合理布局UWB节点，结合非视距(Not Line of Sight，NLOS)误差处理算法，减小UWB观测值误差，在此基础之上，基于扩展卡尔曼滤波算法实现车辆高可靠融合定位，所述方法包括如下步骤：

步骤1)确定UWB布局方案，在道路两旁布置UWB固定节点，并测量UWB固定节点的位置坐标；

其中UWB固定节点的数目与安装位置要针对实际的交通场景，综合考虑成本与定位精度需求，确定合理的布局方案，请参考图2中，本发明为典型城市交叉路口区域提出的一种布局方案：在十字交叉路口的四个不同方向的道路两旁以及交叉路口环岛区域的中心，总共布设了九个UWB固定节点，这种布局方式，能够最大限度的覆盖整个交叉路口，实现车辆在路口区域的完整定位，并且在环岛中心布置的UWB节点，不易受遮挡干扰，有助于定位精度的提高；

步骤2)在车辆质心正上方的车顶上布置一个UWB移动节点，移动节点会跟随车辆移动，因此，移动节点的坐标即认为是车辆的坐标，UWB移动节点将周期性的发送并接收各个固定节点返回的信号，根据信号发送与返回的时间信息，采用到达时间差法(TimeDifference of Arrival，TDOA)计算出车载UWB移动节点到各个UWB固定节点间的距离；

若UWB节点间无遮挡，即视距(Line of sight，LOS)传播，则计算出的距离观测值近似于真实值，但在如图3中所示的动态交通场景下，UWB节点间的直线传播易受周围行驶的车辆，道路两旁的树木以及建筑物等遮挡，信号传播路径出现反射、衍射等现象，即非视距传播(Not Line of Sight，NLOS)，则距离观测值含有较大的误差，若引入定位解算，会严重影响定位精度；

步骤3)根据当前时刻之前的历史距离观测值建立差分整合移动平均自回归模型(Autoregressive Integrated Moving Average model，ARIMA)，利用ARIMA模型逐一对各个UWB固定节点进行NLOS辨识，并根据辨识结果对距离观测值进行补偿校正，具体过程如下：

31)检验由初始时刻到当前时刻的距离观测值组成的时间序列的平稳性，若为非平稳的时间序列，则对序列进行差分处理，差分处理的阶数为d，使之转化为平稳时间序列；

32)平稳化处理后，根据平稳时间序列的自相关函数和偏相关函数，确定ARIMA模型的阶数，ARIMA模型的阶数分别为p和q；

33)采用条件最小二乘法，估计ARIMA模型中的自回归部分以及滑动平均部分的参数；

34)对已建立的ARIMA(p,d,q)模型的残差序列进行单位根检验，判断其是否为白噪声序列，若通过检验，则可进行预测，否则，需重新进行模型参数估计直至通过检验为止；

35)利用通过检验的ARIMA模型对当前时刻UWB移动节点与UWB固定节点间的距离进行预测，得到当前时刻的距离预测值

当前时刻的距离观测值为d(t)，预测值与观测值差的绝对值为

36)根据交通场景的复杂程度以及定位精度的需求设置NLOS辨识的阈值θ_thr，若

则判定UWB移动节点与UWB固定节点之间为NLOS传播，其距离观测值d(t)存在较大的非视距误差，并根据ARIMA模型的预测值，修正移动节点与UWB固定节点间的距离观测值；

步骤4)结合车载GNSS、陀螺仪以及轮速传感器输出的观测信息，利用扩展卡尔曼滤波算法实现车辆的精确定位，具体步骤如下：

41)首先，建立车辆定位系统的扩展卡尔曼滤波状态模型

系统状态向量为

其中，

为车辆纬度信息，λ为车辆经度信息，h为车辆高度信息；v^e为车辆的东向速度，vⁿ为车辆的北向速度，v^u为车辆的垂向速度；p为车辆的俯仰角，r为车辆的侧倾角，A为车辆的航向角；sf^od为轮速精度因子误差，b^ω为陀螺仪随机漂移，b^GNSS为GNSS接收机钟差，d^GNSS为GNSS接收机钟差漂移率，b^UWB为UWB的偏移误差；

系统外部输入向量为U＝[v^od a^od f^x f^y ω^z]

其中，v^od为轮速传感器测得的车辆速度，a^od为车辆纵向加速度，f^x为车辆纵向加速度测量值，f^y为车辆横向加速度测量值，ω^z为陀螺仪输出的横摆角速度；

系统状态方程为：

其中，X(k)为离散时刻k的系统状态向量，X(k-1),U(k-1),W(k-1),T(k-1)分别为离散时刻(k-1)的系统状态向量、外部输入向量、系统噪声向量，外部输入噪声向量；R_M为地球子午线半径，R_N为地球卯酉圈半径，Δt为采样间隔；γ_w为轮速传感器与时间相关的误差，σ_w为其对应的噪声方差；β_z为陀螺仪随机漂移，σ_z为其对应的噪声方差；

42)然后，建立车辆定位系统的扩展卡尔曼滤波观测模型

系统观测向量为

其中，

分别为m个GNSS卫星的伪距观测量，

为n个UWB固定节点的经步骤3)修正后的距离观测值；

系统观测方程为：

Z(k)＝H(k)·X(k)+V(k)

其中，Z(k)，H(k)为离散时刻k的系统观测向量以及系统观测矩阵，V(k)为离散时刻k的观测噪声向量；

观测噪声向量V(k)对应的观测噪声方差阵为R(k)：

R(k)＝diag[R^GNSS R^UWB]

其中，R^GNSS为GNSS的观测噪声方差阵，R^UWB为UWB的观测噪声方差阵；

43)根据步骤41)所述的系统状态模型，步骤42)所述的观测模型，进行扩展卡尔曼滤波的时间更新和测量更新过程，得到车辆的精确的位置信息。

本发明的有益效果为：

1.本发明中利用ARIMA模型对UWB非视距节点进行辨识及误差补偿，可以有效减小动态环境下由非视距或多路径导致的UWB观测误差对系统融合精度的影响，有助于提高融合定位的准确性与可靠性。

2.本发明中的方法，解决了实际交通环境下，将UWB定位技术应用于车辆导航领域，并实现城市车辆的高可靠融合定位问题，相比于传统的车载卫星与惯性组合导航，本发明中的方法，在城市环境，尤其是卫星信号受严重遮挡的复杂环境(如城市峡谷、交叉路口等)下，可靠性更高，有助于实现车辆的连续、完整、可靠、实时定位。

附图说明

图1是本发明的流程图。

图2是典型城市十字交叉路口环境下的UWB节点布局示意图。

图3是动态交通场景下视距与非视距UWB节点示意图。

图4是引入了UWB观测信息前、后的车辆定位轨迹对比图。

图5是引入了UWB观测信息前、后融合定位误差图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅用于解释相关发明，而非对该发明的限定。

随着经济社会的发展与进步，我国的机动车保有量快速增加，道路交通面临着巨大的挑战，为了解决日益严峻的城市交通问题，智能车路协同系统(Intelligent VehicleInfrastructure Cooperative Systems，IVICS)应运而生，并逐步成为智能交通系统(Intelligent Transport Systems，ITS)研究的最新发展方向。无论是车路协同的应用，还是智能交通的实现，都离不开高精度车辆定位技术：只有在实现准确、可靠车辆定位的前提下，全方位实施车车、车路动态实时信息交互，并在全时空动态交通信息采集与融合的基础上开展车辆主动安全控制和道路协同管理，充分实现人车路的有效协同，才能够有效地指挥调度车辆，改善城市交通，保证车辆安全行驶。因此，车辆定位技术是车路协同乃至智能交通等研究的基础与核心内容之一。

目前常见的车辆导航定位技术包括：航位推算(Dead Reckoning,DR)，惯性导航系统(Inertial Navigation System,INS)，卫星导航(Global Navigation SatelliteSystem,GNSS)等。由于单一定位技术的存在各自不足，为了实现城市环境下的准确、可靠、连续、完整定位，多采用两种及两种以上的定位技术相结合，其中GNSS/INS融合定位的应用最为广泛：GNSS/INS融合定位可以解决卫星信号短时中断情况下的定位问题，并在一定程度上补偿INS的累积误差，能够满足相对开阔环境下的车辆定位需求，但在卫星信号长时间中断环境(如城市峡谷、隧道等)中，若GNSS无法正常工作，仅依靠INS自主推算，依然会导致较大的定位误差。

近年来，基于超宽带(Ultra-Wideband，UWB)的无线定位技术的兴起和迅速发展为实现车辆在GNSS受限环境下的可靠定位提供了新思路，目前，UWB定位技术主要被用于室内定位领域如人员、智能小车、机器人定位等。由于UWB的带宽极宽，脉冲信号穿透力强，多径分辨能力良好等技术优点，基于UWB实现室外环境下的车辆定位亦具备了一定的可行性，在传感器层，若引入相对可靠的UWB信息源，则能够有效提高融合定位系统的鲁棒性，但在复杂城市环境下，UWB信号易受周围行驶的车辆、道路两旁的树木以及建筑物的干扰而出现多路径以及非视距误差，若将信号质量较差的UWB观测量用于融合定位，势必导致融合精度的下降，因此，如何针对相对复杂的交通场景(如十字交叉路口、环岛路口等)，结合道路交通设施，合理布局路侧UWB节点，在车路协同下实现人车路信息动态交互，进而选取合适的方法，辨识与补偿非视距误差，提高用于融合的UWB观测信息的可靠性，是实现基于UWB的车辆融合定位亟待解决的关键问题。

针对城市环境下，车载卫星导航易使用受限，定位准确性及可靠性不高等问题，在传统的车载卫星与惯性组合导航的基础上，引入了超宽带(Ultra-Wideband，UWB)定位技术，通过合理布局UWB节点，结合非视距误差处理算法，减小UWB观测值误差，并在此基础之上，基于扩展卡尔曼滤波算法实现了车辆的融合定位。

请参考图1，其示出了根据本发明中的方法实现车辆高可靠融合定位方法的一个实施例的流程：

首先，在道路两旁布置UWB固定节点，并通过高精度差分GNSS获取UWB固定节点的位置坐标；其次，在车辆顶部布置一个UWB移动节点，采用到达时间差法，获得该UWB移动节点到各个UWB固定节点的距离；然后，根据当前时刻之前的历史距离观测值建立ARIMA模型，利用ARIMA模型逐一对各个UWB固定节点进行NLOS辨识；最终，根据修正后的UWB距离观测值，基于扩展卡尔曼滤波实现车辆的融合定位获取车辆的精确位置。

下面结合附图，对本发明的思路作进一步的说明：

一种车路协同下基于超宽带的车辆高可靠融合定位方法，其特征在于，在典型城市车路协同场景下，在车载卫星与惯性组合导航的基础上，引入超宽带(Ultra-Wideband，UWB)定位，通过合理布局UWB节点，结合非视距(Not Line of Sight，NLOS)误差处理算法，减小UWB观测值误差，在此基础之上，基于扩展卡尔曼滤波算法实现车辆高可靠融合定位，所述方法包括如下步骤：

步骤1)确定UWB布局方案，在道路两旁布置UWB固定节点，并测量UWB固定节点的位置坐标；

其中UWB固定节点的数目与安装位置要针对实际的交通场景，综合考虑成本与定位精度需求，确定合理的布局方案，请参考图2中，本发明为典型城市交叉路口区域提出的一种布局方案：在十字交叉路口的四个不同方向的道路两旁以及交叉路口环岛区域的中心，总共布设了九个UWB固定节点，这种布局方式，能够最大限度的覆盖整个交叉路口，实现车辆在路口区域的完整定位，并且在环岛中心布置的UWB节点，不易受遮挡干扰，有助于定位精度的提高；

步骤2)在车辆质心正上方的车顶上布置一个UWB移动节点，移动节点会跟随车辆移动，因此，移动节点的坐标即认为是车辆的坐标，UWB移动节点将周期性的发送并接收各个固定节点返回的信号，根据信号发送与返回的时间信息，采用到达时间差法(TimeDifference of Arrival，TDOA)计算出车载UWB移动节点到各个UWB固定节点间的距离；

若UWB节点间无遮挡，即视距(Line of sight，LOS)传播，则计算出的距离观测值近似于真实值，但在如图3中所示的动态交通场景下，UWB节点间的直线传播易受周围行驶的车辆，道路两旁的树木以及建筑物等遮挡，信号传播路径出现反射、衍射等现象，即非视距传播(Not Line of Sight，NLOS)，则距离观测值含有较大的误差，若引入定位解算，会严重影响定位精度；

步骤3)根据当前时刻之前的历史距离观测值建立差分整合移动平均自回归模型(Autoregressive Integrated Moving Average model，ARIMA)，ARIMA模型是由Box和Jenkins提出的著名的时间序列分析方法，又称为Box-Jenkins模型，具体模型可参考文献：“Pankratz A.Forecasting with univariate Box-Jenkins models:Concepts and cases[M].John Wiley&Sons,2009.”

本发明中针对NLOS的UWB节点距离观测值存在较大误差的问题，提出了一种基于ARIMA模型的NLOS辨识与补偿方法，利用ARIMA模型逐一对各个UWB固定节点进行NLOS辨识，并根据辨识结果对距离观测值进行补偿校正，具体过程如下：

31)检验由初始时刻到当前时刻的距离观测值组成的时间序列的平稳性，若为非平稳的时间序列，则对序列进行差分处理，差分处理的阶数为d，使之转化为平稳时间序列；

32)平稳化处理后，根据平稳时间序列的自相关函数和偏相关函数，确定ARIMA模型的阶数，ARIMA模型的阶数分别为p和q；

33)采用条件最小二乘法，估计ARIMA模型中的自回归部分以及滑动平均部分的参数；

34)对已建立的ARIMA(p,d,q)模型的残差序列进行单位根检验，判断其是否为白噪声序列，若通过检验，则可进行预测，否则，需重新进行模型参数估计直至通过检验为止；

35)利用通过检验的ARIMA模型对当前时刻UWB移动节点与UWB固定节点间的距离进行预测，得到当前时刻的距离预测值

当前时刻的距离观测值为d(t)，预测值与观测值差的绝对值为

36)根据交通场景的复杂程度以及定位精度的需求设置NLOS辨识的阈值θ_thr，若

则判定UWB移动节点与UWB固定节点之间为NLOS传播，其距离观测值d(t)存在较大的非视距误差，并根据ARIMA模型的预测值，修正移动节点与UWB固定节点间的距离观测值；

本实施例中，具体采用的是ARIMA(2,1,1)模型，NLOS辨识阈值θ_thr为0.5米。

步骤4)结合车载GNSS、陀螺仪以及轮速传感器输出的观测信息，利用扩展卡尔曼滤波算法实现车辆的精确定位，具体步骤如下：

41)首先，建立车辆定位系统的扩展卡尔曼滤波状态模型

系统状态向量为

其中，

为车辆纬度信息，λ为车辆经度信息，h为车辆高度信息；v^e为车辆的东向速度，vⁿ为车辆的北向速度，v^u为车辆的垂向速度；p为车辆的俯仰角，r为车辆的侧倾角，A为车辆的航向角；sf^od为轮速精度因子误差，b^ω为陀螺仪随机漂移，b^GNSS为GNSS接收机钟差，d^GNSS为GNSS接收机钟差漂移率，b^UWB为UWB的偏移误差；

系统外部输入向量为U＝[v^od a^od f^x f^y ω^z]

其中，v^od为轮速传感器测得的车辆速度，a^od为车辆纵向加速度，f^x为车辆纵向加速度测量值，f^y为车辆横向加速度测量值，ω^z为陀螺仪输出的横摆角速度；

系统状态方程为：

其中，X(k)为离散时刻k的系统状态向量，X(k-1),U(k-1),W(k-1),T(k-1)分别为离散时刻(k-1)的系统状态向量、外部输入向量、系统噪声向量，外部输入噪声向量；R_M为地球子午线半径，R_N为地球卯酉圈半径，Δt为采样间隔；γ_w为轮速传感器与时间相关的误差，σ_w为其对应的噪声方差；β_z为陀螺仪随机漂移，σ_z为其对应的噪声方差；

42)然后，建立车辆定位系统的扩展卡尔曼滤波观测模型

系统观测向量为

其中，

分别为m个GNSS卫星的伪距观测量，

为n个UWB固定节点经步骤3)修正后的距离观测值；

系统观测方程为：

Z(k)＝H(k)·X(k)+V(k)

其中，Z(k)，H(k)为离散时刻k的系统观测向量以及系统观测矩阵，V(k)为离散时刻k的观测噪声向量；

观测噪声向量V(k)对应的观测噪声方差阵为R(k)：

R(k)＝diag[R^GNSS R^UWB]

其中，R^GNSS为GNSS的观测噪声方差阵，R^UWB为UWB的观测噪声方差阵；

43)根据步骤41)所述的系统状态模型，步骤42)所述的观测模型，进行扩展卡尔曼滤波的时间更新和测量更新过程，得到车辆的精确的位置信息。

在本实施实例中，为了检验提出的算法对车辆定位精度提高的有益效果，开展了实车试验：对本发明中提出的GNSS/INS/UWB融合定位方法与传统的GNSS/INS融合定位方法进行了对比，图4为一组试验结果对比图，图5为两种方法的欧氏距离误差图，表1列出了两种方法的定位误差统计。

表1两种方法的欧氏距离定位误差对比表(单位：米)

由图4、图5及表1的结果可以明显看出，加入了经ARIMA模型处理后的UWB观测信息后，车辆定位的轨迹更加接近于真实值，误差更小，相比于传统的GNSS/INS融合定位方法，本实施例中的提出的GNSS/INS/UWB的融合定位算法精度更高。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (1)

1.一种车路协同下基于超宽带的车辆高可靠融合定位方法，其特征在于，所述方法包括如下步骤：

步骤1)确定UWB布局方案，在道路两旁布置UWB固定节点，并测量UWB固定节点的位置坐标；

步骤2)在车辆顶部布置一个UWB移动节点，采用到达时间差法获得该UWB移动节点到各个UWB固定节点的距离观测值；

步骤3)根据当前时刻之前的历史距离观测值建立差分整合移动平均自回归模型，利用ARIMA模型逐一对各个UWB固定节点进行NLOS辨识，并根据辨识结果对距离观测值进行补偿校正，具体过程如下：

1.检验由初始时刻到当前时刻的距离观测值组成的时间序列的平稳性，若为非平稳的时间序列，则对序列进行差分处理，使之转化为平稳时间序列；

2.平稳化处理后，根据平稳时间序列的自相关函数和偏相关函数，确定ARIMA模型的阶数；

3.采用条件最小二乘法，估计ARIMA模型中的自回归部分以及滑动平均部分的参数；

4.对已建立的ARIMA模型的残差序列进行单位根检验，判断其是否为白噪声序列，若通过检验，则可进行预测，否则，需重新进行模型参数估计直至通过检验为止；

5.利用通过检验的ARIMA模型对当前时刻UWB移动节点与UWB固定节点间的距离进行预测，得到当前时刻的距离预测值

当前时刻的距离观测值为d(t)，预测值与观测值差的绝对值为

6.根据交通场景的复杂程度以及定位精度的需求设置NLOS辨识的阈值θ_thr，若

则判定UWB移动节点与UWB固定节点之间为NLOS传播，其距离观测值d(t)存在大的非视距误差，并根据ARIMA模型的预测值，修正移动节点与UWB固定节点间的距离观测值；

步骤4)结合车载GNSS、陀螺仪以及轮速传感器输出的观测信息，利用扩展卡尔曼滤波算法实现车辆的精确定位，具体步骤如下：

41)首先，建立车辆定位系统的扩展卡尔曼滤波状态模型

系统状态向量为

其中，

为车辆纬度信息，λ为车辆经度信息，h为车辆高度信息；v^e为车辆的东向速度，vⁿ为车辆的北向速度，v^u为车辆的垂向速度；p为车辆的俯仰角，r为车辆的侧倾角，A为车辆的航向角；sf^od为轮速精度因子误差，b^ω为陀螺仪随机漂移，b^GNSS为GNSS接收机钟差，d^GNSS为GNSS接收机钟差漂移率，b^UWB为UWB的偏移误差；

系统外部输入向量为U＝[v^od a^od f^x f^y ω^z]

其中，v^od为轮速传感器测得的车辆速度，a^od为车辆纵向加速度，f^x为车辆纵向加速度测量值，f^y为车辆横向加速度测量值，ω^z为陀螺仪输出的横摆角速度；

系统状态方程为：

其中，X(k)为离散时刻k的系统状态向量，X(k-1),U(k-1),W(k-1),T(k-1)分别为离散时刻(k-1)的系统状态向量、外部输入向量、系统噪声向量，外部输入噪声向量；R_M为地球子午线半径，R_N为地球卯酉圈半径，Δt为采样间隔；γ_w为轮速传感器与时间相关的误差，σ_w为其对应的噪声方差；β_z为陀螺仪随机漂移，σ_z为其对应的噪声方差；

42)然后，建立车辆定位系统的扩展卡尔曼滤波观测模型

系统观测向量为

其中，

分别为m个GNSS卫星的伪距观测量，

为n个UWB固定节点的经步骤3)修正后的距离观测值；

系统观测方程为：

Z(k)＝H(k)·X(k)+V(k)

其中，Z(k)，H(k)为离散时刻k的系统观测向量以及系统观测矩阵，V(k)为离散时刻k的观测噪声向量；

观测噪声向量V(k)对应的观测噪声方差阵为R(k)：

R(k)＝diag[R^GNSS R^UWB]

其中，R^GNSS为GNSS的观测噪声方差阵，R^UWB为UWB的观测噪声方差阵；

43)根据步骤41)所述的系统状态模型，步骤42)所述的观测模型，进行扩展卡尔曼滤波的时间更新和测量更新过程，得到车辆的精确的位置信息。

Images