CN111954153B - 一种基于uwb定位的智能召车导航定位方法 - Google Patents

Abstract

本发明公开了一种基于UWB定位的智能召车导航定位方法，步骤如下：获取三个基站与标签之间的距离；建立空间直角坐标系；利用改进的三边测量法求出标签的坐标；分析峭度；若存在NLOS误差，利用无迹卡尔曼滤波算法预测状态方程，引入自适应因子修正增益矩阵，输出预测最优值；若不存在NLOS误差，利用标准无迹卡尔曼滤波算法输出预测最优值。本发明构建UWB定位模型，并对三边测量法进行加权优化，有效抑制了多径效应对定位的影响，提高了定位可信度，通过计算峭度判别UWB信道环境，有效实现了对定位结果的优化和对NLOS误差的修正，提高了UWB的定位精度。

Description

一种基于UWB定位的智能召车导航定位方法

技术领域

本发明属于定位技术领域，特别涉及一种基于UWB定位的智能召车导航定位方法。

背景技术

随着人工智能的兴起和车辆技术的不断发展，智能车的研究与应用越来越成为各国争相研究的热点。其中开发的各种功能，如自动泊车、避障预警等，大大方便了人们的出行，提高了安全系数。实现上述功能，关键是使车辆在非结构化环境中获取自身位置等信息。UWB定位(超宽带定位)作为一种新兴的高精度定位技术，以其定位精确、工作时间短、穿透能力强等优点，成为目前室内定位应用最佳物理层技术。

但是UWB也同样存在着时钟漂移误差、非视距(NLOS)误差、温度和气压环境造成的常值误差等，严重影响了定位精度。且在智能召车应用场景中，存在较为强烈的金属干扰，形成典型的NLOS环境，导致测距误差大幅上升。上述误差会使传统定位算法三边测量法无法交于一点，降低了定位可信度。

因此，需要一种新型的基于UWB的智能召车导航定位方法来解决定位过程中的上述问题。

发明内容

本发明针对现有技术中的不足，提供一种基于UWB定位的智能召车导航定位方法，目的是抑制多径效应和NLOS误差对定位的影响，提高定位的准确性。

为实现上述目的，本发明采用以下技术方案：一种基于UWB定位的智能召车导航定位方法，包括以下步骤：

S1、在车身上设置二个UWB定位基站A₀、A₁、A₂，车主手持标签M，分别获取三个基站A₀、A₁、A₂与标签M之间的距离d₀、d₁、d₂；

S2、以基站A₁、A₂的连线中点O为原点，建立空间直角坐标系，得到三个基站的坐标为A₀(x₀，y₀.z₀)，A₁(x₁，y₁.z₁)，A₂(x₂，y₂.z₂)；

S3、使用改进的三边定位算法求出标签的坐标M(x，y)(结合应用场景，只需解算出二维平面坐标)；

S4、计算峭度并进行分析，使用峭度来衡量信道状态；

S5、将峭度正常的情况判定为LOS环境，采用标准无迹卡尔曼滤波算法处理定位数据，输出预测最优值；将峭度异常的情况判定为NLOS环境，在标准无迹卡尔曼滤波算法的基础上引入自适应因子进行修正，减小误差，最后输出最优预测值。

为优化上述技术方案，采取的具体措施还包括：

进一步地，当标签反应时间等于基站反应时间时，求取基站与标签间的距离d公式如下：

式中，C为光速，TOF是UWB信号飞行时间，T_SP为标签向基站发送一条请求信号时刻，T_RP为基站开始接受信号时刻，T_SR为基站回复信息时刻，T_RR为标签开始接受信号时刻，T_SF为标签将数据包P₃(T_SP，T_RR，T_SF)发送给基站时刻；T_RF为基站接收到P₃数据包时刻。

进一步地，当标签反应时间不等于基站反应时间时，采用双边双向测距算法获得基站与标签间的距离d，公式如下：

式中，T_TRT＝T_RR-T_SP为标签往返时间，

为基站反应时间，T_ART＝T_RF-T_SR为基站信号往返时间，

为标签反应时间。

进一步地，步骤S3中，以三个基站为圆心，d₀、d₁、d₂为半径作圆，得到三个有效交点，以交点围成的三角形面积作为权值衡量定位可信程度，并以此计算出最终定位结果，所述改进的三边定位算法如下：

设传统三边测量法中用于定位的三个圆分别为O₁，O₂，O₃，其方程为：

式中，(x₀，y₀)、(x₁，y₁)、(x₂，y₂)分别为三个基站的坐标，d₀、d₁、d₂分别为标签到三个基站的距离；联立方程求解三圆两两相交的有效交点坐标分别为(x_a，y_a)，(x_b，y_b)，(x_c，y_c)，这里的有效交点定义为三圆相交区域中的交点(本发明中只取有效交点，剩余三个交点与实际位置距离过远，故舍去)；交点所围成的三角形的三条边l₀、l₁、l₂分别为：

三角形面积：

假设UWB每0.1秒获取n组有效数据，每组数据下所求取的交点围成的三角形面积为S，定义以下的权重函数，表示位置的可信程度

式中，(x_i，y_i)是根据第i组有效数据求解出的标签位置，S_i是根据第i组有效数据算出的所围成的三角形面积，(x_c，j，y_c，j)是经过加权后的标签位置，表示标签位置的可信度；

标准化处理，求得标签位置最优值：

进一步地，定义峭度为波形四阶矩和二阶矩平方的比值，即

其中，E{[|o(t)|-μ]⁴}是波形四阶矩的数学期望，E{[|o(t)|-μ]²}是波形二阶矩的数学期望，μ为t时刻O(t)的均值，o(t)为UWB信号在IEEE802.15.4a标准下的信道冲激响应，表示为

其中，l表示第l条多径，L表示多径总数，a_l、τ_l分别为第l条路径的幅度增益和时延，δ(x)是单位能量的单径脉冲函数。

进一步地，单位能量的单径脉冲函数δ(x)性质如下：

进一步地，步骤S4中使用峭度K(t)衡量信道状态，表示如下：

其中，thresh为设定的阈值。

进一步地，步骤S5中，在标准无迹卡尔曼滤波算法的基础上引入的自适应因子如下：

式中，

为UWB的真实测量值y_k与测量估计值

的差值，P_y，k为

的向量协方差矩阵，tr()为矩阵的迹，μ_k为所构建的自适应因子，用于判别滤波中是否存在状态模型误差，并且对误差有自适应调节的作用。

进一步地，步骤S5中，将自适应因子加入标准无迹卡尔曼滤波算法中，对其进行修正如下：

其中，W_i ^c为滤波过程中协方差权值，

为测量方程计算出的测量值，

为测量估计值，R_k为测量噪声，

为状态向量，

为变换后的Sigma样本点，

为预测协方差矩阵，K_k为增益矩阵。

本发明的有益效果是：本发明提出了一种基于UWB的智能召车导航定位方法，该方法基于双向测距方法的测距原理与精度，构建UWB定位模型，并对三边测量法进行加权优化，有效抑制了多径效应对定位的影响，提高了定位可信度。该定位方法通过计算峭度判别UWB信道环境，将参数正常的情况判定为LOS环境，采用标准无迹卡尔曼滤波算法进行滤波，将参数异常的情况判定为NLOS环境，在标准无迹卡尔曼滤波算法的基础上引入自适应因子进行修正，有效实现了对定位结果的优化和对NLOS误差的修正，提高了UWB的定位精度。

附图说明

图1为本发明的基于UWB的智能召车导航定位方法的流程图。

图2为本发明的基站和标签的位置示意图。

图3为本发明的改进的三边测量法的示意图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

如附图1-2所示，本发明公开了一种基于UWB定位的智能召车导航定位方法，包括以下步骤：

S1、在车身上设置二个UWB定位基站A₀、A₁、A₂，M为车主手持标签，分别测得三个基站A₀、A₁、A₂与标签M之间的距离为d₀、d₁、d₂。

S2、以基站A₁、A₂的连线中点O为原点，建立空间直角坐标系，得到三个基站的坐标为A₀(x₀，y₀.z₀)，A₁(x₁，y₁.z₁)，A₂(x₂，y₂.z₂)；

S3、使用改进的三边定位算法求出标签的坐标M(x，y)(结合应用场景，只需解算出二维平面坐标)；

S4、计算峭度，并进行分析，使用峭度来衡量UWB信道状态。

S5、将峭度正常的情况判定为LOS环境，采用标准无迹卡尔曼滤波算法进行滤波，处理定位数据，输出预测最优值；将峭度异常的情况判定为NLOS环境，在标准无迹卡尔曼滤波算法的基础上引入自适应因子进行修正，减小误差，最后输出最优预测值。

由于设备存在响应时间，并且现实中不可避免地存在因时钟振荡器不同引起的时钟漂移问题，为提高测距精度，本发明在步骤一采用以下测距方法：

当基站反应时间

等于标签反应时间

时，基站与标签间的距离

式中，C为光速，TOF是UWB信号飞行时间，T_SP为标签向基站发送一条请求信号时刻，T_RP为基站开始接受信号时刻，T_SR为基站回复信息时刻，T_RR为标签开始接受信号时刻，T_SF为标签将数据包P₃(T_SP，T_RR，T_SF)发送给基站时刻；T_RF为基站接收到P₃数据包时刻。

当基站反应时间

不等于标签反应时间

时，采用双边双向测距算法获得基站与标签间的距离

式中，T_TRT＝T_RR-T_SP为标签往返时间，

为基站反应时间，T_ART＝T_RF-T_SR为基站信号往返时间，

为标签反应时间。

由于以基站为圆心作的三个圆大多数情况下不可能理想化交于一点，且多径效应的存在大大提高了定位的不准确性，故在步骤S3中，本发明对三边测量法做出了改进，如图3所示，具体如下：

设传统三边测量法中用于定位的三个圆分别为O₁，O₂，O₃，其方程为：

式中，(x₀，y₀)、(x₁，y₁)、(x₂，y₂)分别为三个基站的坐标，d₀、d₁、d₂分别为标签到三个基站的距离。

以三个基站为圆心，d₀、d₁、d₂为半径作圆，得到三个有效交点，以交点围成的三角形面积作为权值衡量定位可信程度，并以此计算出最终定位结果，联立方程求解三圆两两相交的有效交点坐标分别为(x_a，y_a)，(x_b，y_b)，(x_c，y_c)，这里的有效交点定义为三圆相交区域中的交点(本发明中只取有效交点，剩余三个交点与实际位置距离过远，故舍去)；交点所围成的三角形的三条边l₀、l₁、l₂分别为：

所围成的三角形面积：

假设UWB每0.1秒获取n组有效数据，每组数据下所求取的交点围成的三角形面积为S，定义以下的权重函数，表示位置的可信程度

式中，(x_i，y_i)是根据第i组有效数据求解出的标签位置，S_i是根据第i组有效数据算出的所围成的三角形面积，(x_c，j，y_c，j)是经过加权后的标签位置，表示标签位置的可信度；

标准化处理，求得标签位置最优值：

由于在NLOS环境下，标签的信号到达基站需经过障碍物，障碍物的介电系数大于1，会对测距模型带来附加延迟，从而产生NLOS误差，对定位造成极大的干扰，故在步骤三，本发明使用峭度作为参数鉴别UWB信道环境。具体如下：

定义峭度为波形四阶矩和二阶矩平方的比值，即

其中，E{[|o(t)|-μ]⁴}是波形四阶矩的数学期望，E{[|O(t)|-μ]²}是波形二阶矩的数学期望，μ为t时刻o(t)的均值，O(t)为UWB信号在IEEE802.15.4a标准下的信道冲激响应(CIR)，可表示为：

其中，l表示第l条多径，L表示多径总数，a_l、τ_l分别为第l条路径的幅度增益和时延，δ(x)是单位能量的单径脉冲函数，其性质如下：

峭度作为信号的归一化四阶矩，对信号中的冲击特征尤其敏感。

一般LOS环境中因为没有阻挡，数据变化更为剧烈，倾向于有更高的峭度；NLOS环境中数据变化较为缓慢，倾向于有更低的峭度。故峭度可作为衡量NLOS误差的参数，表示如下，

其中，thresh为设定的阈值，因为K等于3时，波形具有正常峰值(零峭度)，故可取thresh为3。

由于标准无迹卡尔曼滤波在系统噪声不精确的NLOS环境下会出现滤波估值的精度下降、收敛速度慢等问题，故本发明在标准无迹卡尔曼滤波算法的基础上引入了自适应因子，其特征在于构建的自适应因子如下：

式中，

为UWB的真实测量值y_k与测量估计值

的差值，P_y，k为

的向量协方差矩阵，tr()为矩阵的迹，μ_k为所构建的自适应因子(0＜μ_k≤1)，可判别滤波中是否存在状态模型误差，并且对误差有自适应调节的作用。

将构建的自适应子加入标准无迹卡尔曼滤波算法中，在量测更新环节中对向量协方差矩阵P_y，k、理论残差向量矩阵P_xy，k、误差协方差矩阵P_x，k进行修正：

其中，W_i ^c为滤波过程中协方差权值，

为测量方程计算出的测量值，

为测量估计值，R_k为测量噪声，

为状态向量，

为变换后的Sigma样本点，

为预测协方差矩阵，K_k为增益矩阵。

本发明的定位方法通过计算峭度判别UWB信道环境，将参数正常的情况判定为LOS环境，采用标准无迹卡尔曼滤波算法进行滤波，将参数异常的情况判定为NLOS环境，在标准无迹卡尔曼滤波算法的基础上引入自适应因子进行修正，有效实现了对定位结果的优化和对NLOS误差的修正，提高了UWB的定位精度。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (1)

1.一种基于UWB定位的智能召车导航定位方法，其特征在于，包括以下步骤：

S1、在车身上设置三个UWB定位基站A₀、A₁、A₂，车主手持标签M，分别获取三个基站A₀、A₁、A₂与标签M之间的距离d₀、d₁、d₂；

S2、以基站A₁、A₂的连线中点O为原点，建立空间直角坐标系，得到三个基站的坐标为A₀(x₀,y₀.z₀)，A₁(x₁,y₁.z₁)，A₂(x₂,y₂.z₂)；

S3、使用改进的三边定位算法求出标签的坐标M(x,y)，结合应用场景，只需解算出二维平面坐标；

S4、计算峭度并进行分析，使用峭度来衡量信道状态；

S5、将峭度正常的情况判定为LOS环境，采用标准无迹卡尔曼滤波算法处理定位数据，输出预测最优值；将峭度异常的情况判定为NLOS环境，在标准无迹卡尔曼滤波算法的基础上引入自适应因子进行修正，减小误差，最后输出最优预测值；

当标签反应时间等于基站反应时间时，求取基站与标签间的距离d公式如下：

式中，C为光速，TOF是UWB信号飞行时间，T_SP为标签向基站发送一条请求信号时刻，T_RP为基站开始接收信号时刻，T_SR为基站回复信息时刻，T_RR为标签开始接收信号时刻，T_SF为标签将数据包P₃(T_SP,T_RR,T_SF)发送给基站时刻；T_RF为基站接收到P₃数据包时刻；

当标签反应时间不等于基站反应时间时，采用双边双向测距算法获得基站与标签间的距离d，公式如下：

式中，T_TRT＝T_RR-T_SP为标签往返时间，

为基站反应时间，T_ART＝T_RF-T_SR为基站信号往返时间，

为标签反应时间；

所述改进的三边定位算法如下：

设传统三边测量法中用于定位的三个圆分别为O₁，O₂，O₃，其方程为：

式中，(x₀,y₀)、(x₁,y₁)、(x₂,y₂)分别为三个基站的坐标，d₀、d₁、d₂分别为标签到三个基站的距离；

联立方程求解三个圆两两相交的有效交点分别为(x_a,y_a)，(x_b,y_b)，(x_c,y_c)，这里的有效交点定义为三圆相交区域中的交点；交点所围成的三角形的三条边l₀、l₁、l₂分别为：

三角形面积：

假设UWB每0.1秒获取n组有效数据，每组数据下所求取的交点围成的三角形面积为S，定义以下的权重函数，表示位置的可信程度

式中，(x_i,y_i)是根据第i组有效数据求解出的标签位置，S_i是根据第i组有效数据算出的所围成的三角形面积，(x_c,j,y_c,j)是经过加权后的标签位置，表示标签位置的可信度；

标准化处理，求得标签位置最优值：

定义峭度为波形四阶矩和二阶矩平方的比值，即

其中，E{[|O(t)|-μ]⁴}是波形四阶矩的数学期望，E{[|O(t)|- μ]²}是波形二阶矩的数学期望，μ为t时刻O(t)的均值，O(t)为UWB信号在IEEE802.15.4a标准下的信道冲激响应，表示为

其中，l表示第l条多径，L表示多径总数，a_l、τ_l分别为第l条路径的幅度增益和时延，δ(x)是单位能量的单径脉冲函数；

单位能量的单径脉冲函数δ(x)性质如下：

步骤S4中使用峭度K(t)衡量信道状态，表示如下：

其中，thresh为设定的阈值；

步骤S5中，在标准无迹卡尔曼滤波算法的基础上引入的自适应因子如下：

式中，

为UWB的真实测量值y_k与测量估计值

的差值，P_y,k为

的向量协方差矩阵，tr()为矩阵的迹，μ_k为所构建的自适应因子，用于判别滤波中是否存在状态模型误差，并且对误差有自适应调节的作用；

步骤S5中，将自适应因子加入标准无迹卡尔曼滤波算法中，对其进行修正如下：

其中，W_i ^c为滤波过程中协方差权值，

为测量方程计算出的测量值，

为测量估计值，R_k为测量噪声，

为状态向量，

为变换后的Sigma样本点，

为预测协方差矩阵，K_k为增益矩阵。

Images