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

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

　　技术领域

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

　　背景技术

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

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

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

　　发明内容

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

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

　　S1、在车身上设置二个UWB定位基站A0、A1、A2，车主手持标签M，分别获取三个基站A0、A1、A2与标签M之间的距离d0、d1、d2；

　　S2、以基站A1、A2的连线中点O为原点，建立空间直角坐标系，得到三个基站的坐标为A0(x0，y0.z0)，A1(x1，y1.z1)，A2(x2，y2.z2)；

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

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

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

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

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

　　

　　式中，C为光速，TOF是UWB信号飞行时间，TSP为标签向基站发送一条请求信号时刻，TRP为基站开始接受信号时刻，TSR为基站回复信息时刻，TRR为标签开始接受信号时刻，TSF为标签将数据包P3(TSP，TRR，TSF)发送给基站时刻；TRF为基站接收到P3数据包时刻。

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

　　

　　式中，TTRT＝TRR-TSP为标签往返时间，为基站反应时间，TART＝TRF-TSR为基站信号往返时间，为标签反应时间。

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

　　设传统三边测量法中用于定位的三个圆分别为O1，O2，O3，其方程为：

　　

　　式中，(x0，y0)、(x1，y1)、(x2，y2)分别为三个基站的坐标，d0、d1、d2分别为标签到三个基站的距离；联立方程求解三圆两两相交的有效交点坐标分别为(xa，ya)，(xb，yb)，(xc，yc)，这里的有效交点定义为三圆相交区域中的交点(本发明中只取有效交点，剩余三个交点与实际位置距离过远，故舍去)；交点所围成的三角形的三条边l0、l1、l2分别为：

　　

　　三角形面积：

　　

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

　　

　　

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

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

　　

　　

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

　　

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

　　

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

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

　　

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

　　

　　其中，thresh为设定的阈值。

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

　　

　　式中，为UWB的真实测量值yk与测量估计值的差值，Py，k为的向量协方差矩阵，tr()为矩阵的迹，μk为所构建的自适应因子，用于判别滤波中是否存在状态模型误差，并且对误差有自适应调节的作用。

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

　　

　　

　　

　　其中，Wic为滤波过程中协方差权值，为测量方程计算出的测量值，为测量估计值，Rk为测量噪声，为状态向量，为变换后的Sigma样本点，为预测协方差矩阵，Kk为增益矩阵。

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

　　附图说明

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

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

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

　　具体实施方式

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

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

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

　　S1、在车身上设置二个UWB定位基站A0、A1、A2，M为车主手持标签，分别测得三个基站A0、A1、A2与标签M之间的距离为d0、d1、d2。

　　S2、以基站A1、A2的连线中点O为原点，建立空间直角坐标系，得到三个基站的坐标为A0(x0，y0.z0)，A1(x1，y1.z1)，A2(x2，y2.z2)；

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

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

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

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

　　当基站反应时间等于标签反应时间时，基站与标签间的距离

　　

　　式中，C为光速，TOF是UWB信号飞行时间，TSP为标签向基站发送一条请求信号时刻，TRP为基站开始接受信号时刻，TSR为基站回复信息时刻，TRR为标签开始接受信号时刻，TSF为标签将数据包P3(TSP，TRR，TSF)发送给基站时刻；TRF为基站接收到P3数据包时刻。

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

　　

　　式中，TTRT＝TRR-TSP为标签往返时间，为基站反应时间，TART＝TRF-TSR为基站信号往返时间，为标签反应时间。

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

　　设传统三边测量法中用于定位的三个圆分别为O1，O2，O3，其方程为：

　　

　　式中，(x0，y0)、(x1，y1)、(x2，y2)分别为三个基站的坐标，d0、d1、d2分别为标签到三个基站的距离。

　　以三个基站为圆心，d0、d1、d2为半径作圆，得到三个有效交点，以交点围成的三角形面积作为权值衡量定位可信程度，并以此计算出最终定位结果，联立方程求解三圆两两相交的有效交点坐标分别为(xa，ya)，(xb，yb)，(xc，yc)，这里的有效交点定义为三圆相交区域中的交点(本发明中只取有效交点，剩余三个交点与实际位置距离过远，故舍去)；交点所围成的三角形的三条边l0、l1、l2分别为：

　　

　　所围成的三角形面积：

　　

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

　　

　　

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

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

　　

　　

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

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

　　

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

　　

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

　　

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

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

　　

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

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

　　

　　式中，为UWB的真实测量值yk与测量估计值的差值，Py，k为的向量协方差矩阵，tr()为矩阵的迹，μk为所构建的自适应因子(0＜μk≤1)，可判别滤波中是否存在状态模型误差，并且对误差有自适应调节的作用。

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

　　

　　

　　

　　其中，Wic为滤波过程中协方差权值，为测量方程计算出的测量值，为测量估计值，Rk为测量噪声，为状态向量，为变换后的Sigma样本点，为预测协方差矩阵，Kk为增益矩阵。

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

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