一种基于超宽带实时定位的轨迹约束方法与流程

本发明属于实时定位领域，更具体的，涉及一种基于超宽带实时定位的轨迹约束方法。

背景技术：

近年来，随着城市的不断发展，商场、车站、工厂等室内建筑的不断扩大，室内环境的场景越来越复杂，人们对室内定位的需求越来越强烈，室内定位系统在越来越多的工厂和商场实施运行。随着工业4.0的到来，智能工厂的概念被普及开来，智能工厂中一个重要方面就是对工人的实时定位，以保证生产安全，并优化工作流程。

超宽带技术由于频率高，抗干扰能力强，容量大，被广泛应用在室内定位系统中。实现超宽带通信的首要任务是产生uwb(ultrawideband，超宽带)信号，从本质上看，uwb是发射和接收超短电磁脉冲的技术。与传统的无线收发信机结构相比，uwb收发信机的结构相对简单。一般而言，用于移动的uwb设备称作超宽带定位标签(也称为uwb定位标签)，用于辅助定位的固定uwb设备称为超宽带定位基站。参考文献：scholtzra.,winmz..ultra-widebandwidthtime-hoppingspread-spectrumimpulseradioforwirelessmultiple-accesscommunications[j].ieeetransactionsoncommunications,2000,48(4)。

uwb信号的发送和接收会以时间戳的方式记录下来，利用时间戳可生成距离相关的数据。tdoa(timedifferenceofarrival，到达时间差)为uwb发送设备发送信号之后到达两个uwb接收设备的到达时间的差值。tof(timeofflight，飞行时间)为两个uwb设备经过三次发送接收信号计算得到的单次飞行时间。参考文献：olebischoff,xinweiwang,nilsheidmann,etal.implementationofanultrasonicdistancemeasuringsystemwithkalmanfilteringinwirelesssensornetworksfortransportlogistics.2010,5：196-199。

在获取到tdoa和tof数据之后，超宽带解算模块可利用解算算法计算得到uwb定位标签所在的坐标，由于定位点携带uwb定位标签，因此也就获得了定位点的坐标。

当前室内定位系统虽然能对定位点做比较精确的定位，但在实际应用中仍然存在定位点的轨迹实时性较差、轨迹跳动较大，对异常点数据处理不足、对固定路线的轨迹数据处理显示效果不佳、平滑度较低以及在道路交点附近拉扯严重等问题。

技术实现要素：

发明目的：本发明所要解决的技术问题是针对现有技术的不足，提供一种基于超宽带实时定位的轨迹约束方法。

为了解决上述技术问题，本发明公开一种基于超宽带实时定位的轨迹约束方法，包括如下步骤：

步骤1，对道路路线进行采样，所述道路路线包括直线和曲线，采样后所有直线和曲线的集合记为r；

步骤2，对集合r中所有曲线分别进行分割，所有曲线分割点的坐标和直线道路两端的端点的坐标组成坐标集p；

步骤3，根据坐标集p生成定位区域，对定位区域进行分块，分块集合为b；

步骤4，以道路交点为圆心，创建半径为r的缓冲区，其中0.5米≤r≤1米；创建缓冲区的目的在于减少定位点在多条道路之间跳动，减轻轨迹的拉扯现象；

步骤5，超宽带解算模块解算出第i个周期的定位点坐标api；

步骤6，对定位点坐标api进行预处理；

步骤7，从定位区域的分块集合b中取离定位点api最近的分块；

步骤8，根据最近的分块计算道路上离定位点坐标api最近点的坐标npi；

步骤9，根据缓冲区策略生成新的定位点坐标；

步骤10，对新的定位点坐标进行卡尔曼滤波处理输出位置坐标。

在一种实现方式中，步骤1可采用两种方式采样道路直线和曲线。

方式1，基于贝塞尔曲线采样。贝塞尔曲线是在制作二维图形的应用程序中广泛应用的数学曲线，贝塞尔曲线的优点是绘制曲线简便易行，只需要定义曲线的起点、终点以及两个相互分离的中间点(控制点)即可获得符合实际道路轨迹的曲线。用贝塞尔曲线绘制复杂曲线可以先分段采样获得多段曲线，然后再将这些曲线段连接起来形成所需要的曲线或曲面图形。道路为直线时，采用一阶贝塞尔曲线采样；道路为u型形状的简单曲线时，采用多阶贝塞尔曲线采样；如果道路由h个u型形状组成的复杂曲线和直线构成，h大于等于2，直线采用一阶贝塞尔曲线采样，复杂曲线先分段采样，切分成多段简单曲线，然后对切分后的多段简单曲线分别采用多阶贝塞尔曲线采样。道路路线中所有直线和简单曲线的集合记为r。

方式2，基于实际道路等间距采样。对于简单曲线，等间距放置采样点，使用测量仪器测量出采样点坐标，得到坐标之后，通过最小二乘法拟合生成曲线的多次函数。对于道路曲线由h个u型形状组成的复杂曲线，对复杂曲线先分段采样，切分成多段简单曲线，然后对切分后的每段简单曲线等间距放置采样点，使用测量仪器测量出采样点坐标，得到坐标之后，通过最小二乘法拟合生成该段简单曲线的多次函数。道路路线中所有直线和简单曲线的集合记为r。

基于贝塞尔曲线采样的方法对取点数量的要求更少，可加快采样速度，但精度会降低；基于道路路线等间距采样的方法取点数量较多，精度更高。

在一种实现方式中，步骤2中，对道路集合r中的每段曲线进行分割取点，将每段曲线分割成长度为l的j条线段，0.5米≤l≤1米，j根据每段曲线的长度以及l进行确定；保存所有分割点的坐标，分割点的坐标和道路集合r中每段直线两端的端点坐标组成坐标集p；坐标的形式为(x,y)，单位为米，坐标集p中的所有坐标中的x最大值记为xmax、x最小值记为xmin、y最大值记为ymax、最小值记为ymin。

在一种实现方式中，步骤3中，根据坐标集p中的所有坐标中的x最大值、x最小值和y最大值、最小值确定定位区域的边界，由(xmin,ymin)、(xmin,ymax)、(xmax,ymin)和(xmax,ymax)四个坐标点连线组成的矩形区域即为定位区域。对定位区域进行分块，块的边长为a，2米≤a≤5米，分块集合记作b，其中一个分块用bn表示，即b＝{b1,b2,...,bn,...,bn}，1≤n≤n，n表示定位区域中分块的数量，分块bn中包含步骤2中的线段。将整个定位区域划分成规整的n块，以减少道路的查找时间。

在一种实现方式中，步骤5中，获取定位点携带的uwb定位标签的tdoa和tof数据，超宽带解算模块利用解算算法计算得到定位点携带的uwb定位标签的坐标，也即获得了定位点的坐标。uwb定位标签会周期性向外发送超宽带信号，超宽带解算模块周期性计算uwb标签的坐标，记第i个周期获得的定位点坐标为api，第i-1个周期获得的定位点坐标为api-1。

在一种实现方式中，步骤6中，对解算出的定位点坐标api做一定的预处理。如果定位点坐标api到步骤2中所述的所有线段的距离中最短的距离超过设定的阈值t，其中0.8米≤t≤2米，则直接输出定位点坐标api，不对其做后续的轨迹约束处理，继续执行步骤10，否则执行步骤7。

在一种实现方式中，步骤7中，从定位区域的分块集合b中取离定位点最近的m个分块。根据定位点坐标api，遍历查找分块集合b＝{b1,b2,...,bn,...,bn}中每个分块bn距离定位点坐标的最短距离dmin,n和最长距离dmax,n，其中最长距离dmax,n组成的集合的最小值为dmax,min。如果某个分块be的最短距离dmin,e>dmax,min，其中1≤e≤n，则判断分块be离定位点较远，将其排除，剩余的即为最近的分块，最近的分块个数记为m，1≤m≤n。

在一种实现方式中，步骤8中，在m个分块中，总的线段个数记为c，遍历计算定位点api到这m个分块中每个分块的每条线段的最短距离smin,c以及对应的线段上的坐标cpc，1≤c≤c，在smin,c组成的集合中获取的最小值即为定位点api到道路的最短距离，对应的线段上的坐标为离定位点api最近点的坐标，记为npi,；第i-1个周期离定位点坐标api-1最近点的坐标记为npi-1。

在一种实现方式中，步骤9包括以下步骤：

步骤9-1，如果没有第i-1个周期的最近点npi-1即没有历史最近点或者第i-1个周期的最近点npi-1的生成时间距离现在的时间差δt>15s，则更新保存第i个周期的最近点npi所在的道路信息，返回第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-2。

步骤9-2，如果第i个周期的最近点npi和第i-1个周期的最近点npi-1都不在缓冲区中但在同一条道路上，则更新保存第i个周期的最近点npi所在的道路信息，返回第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-3。

步骤9-3，如果第i个周期的最近点npi和第i-1个周期的最近点npi-1都不在缓冲区内且不在同一条道路上，则说明跨越了缓冲区，则更新保存第i个周期的最近点npi的道路信息，返回第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-4。

步骤9-4，如果第i个周期的最近点npi在缓冲区中但第i-1个周期的最近点npi-1不在缓冲区中，且第i-1个周期的最近点npi-1所在的道路是生成缓冲区的道路之一，检查在第i-1个周期的最近点npi-1和缓冲区圆心之间的道路上是否有插值，如果无插值，则沿第i-1个周期的最近点npi-1开始向缓冲区圆心方向的道路上插值第一个点ip1，返回第一个插值点ip1作为新的定位点，并记录插值相关信息，继续执行步骤10，否则执行步骤9-5。

步骤9-5，如果第i个周期的最近点npi在缓冲区中但第i-1个周期的最近点npi-1不在缓冲区中，检查在第i-1个周期的最近点npi-1和缓冲区圆心之间的道路上是否有插值，如果有插值，且插值点的个数f小于总插值点个数f，5≤f≤8，则继续沿上一个插值点向缓冲区圆心方向的道路上插值一个点，返回新的插值点ipf作为新的定位点，更新插值相关信息；若插值点的个数f等于总插值点个数f，则在缓冲区圆心附近生成随机点坐标rp，随机点坐标rp的形式为(xrp,yrp)，缓冲区圆心的坐标形式为(xcp,ycp)，随机点坐标rp的取值需要满足0≤|xrp-xcp|≤0.5米，0≤|yrp-ycp|≤0.5米；返回随机点坐标rp作为新的定位点，继续执行步骤10，否则执行步骤9-6。

步骤9-6，如果第i个周期的最近点npi不在缓冲区中但第i-1个周期的最近点npi-1在缓冲区中，且第i-1个周期的最近点npi-1所在的缓冲区是由第i个周期的最近点npi所在的道路和另一条道路相交产生的，若第i-1个周期的最近点npi-1是以缓冲区圆心为中心随机产生或者是在所述的另一条道路上插值的点且另一条道路上插值点个数小于总插值点个数f，则需要在第i个周期的最近点npi和缓冲区圆心之间插值g个点，3≤g≤8，输出g个插值点和第i个周期的最近点npi作为新的定位点，即新的定位点由多个点组成；继续执行步骤10，否则执行步骤9-7。

步骤9-7，如果第i个周期的最近点npi不在缓冲区中但第i-1个周期的最近点npi-1在缓冲区中，且第i-1个周期的最近点npi-1所在的缓冲区是由第i个周期的最近点npi所在的道路和另一条道路相交产生的，若第i-1个周期的最近点npi-1和第i个周期的最近点npi在同一条道路上，第i-1个周期的最近点npi-1是插值的点且其所在道路上的插值点个数小于总插值点个数f，若是从缓冲区折返的情况，则在第i个周期的最近点npi和第i-1个周期的最近点npi-1之间插值g个点，3≤g≤8，输出g个插值点和第i个周期的最近点npi作为新的定位点；若是从缓冲区穿过的情况，则需要在第i-1个周期的最近点npi-1和缓冲区圆心之间插值g个点，再在缓冲区圆心和第i个周期的最近点npi之前插值g个点，返回2*g个插值点和第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-8。

步骤9-8，返回第i个周期的最近点npi，继续执行步骤10。

在一种实现方式中，步骤10中，卡尔曼滤波器的操作包括两个阶段：预测与更新。将步骤9中根据缓冲区策略生成新的定位点的坐标作为输入，坐标形式为(xi,yi)，经过卡尔曼滤波器滤波后输出的定位点坐标形式为(xfi,yfi)；在预测阶段，滤波器使用第i-1个周期的坐标(xfi-1,yfi-1)的估计，做出对第i个周期的坐标(xfi,yfi)的估计。第i-1个周期的坐标(xfi-1,yfi-1)的初始值为xfi-1＝xi，yfi-1＝yi；计算卡尔曼增益xki,yki：

xki＝xpi/(xpi+r)

yki＝ypi/(ypi+r)

其中，r为固定的卡尔曼滤波参数，取值为1e-3；xpi和ypi为坐标取值xfi和yfi的协方差，初始值均设置为1。

结合测量值预测第i个周期的坐标(xfi,yfi)的最优值：

xfi＝xfi-1+xki*(xi–xfi-1)

yfi＝yfi-1+xki*(yi–yfi-1)

在更新阶段，更新下一周期的坐标取值xfi+1和yfi+1的协方差xpi+1和ypi+1以及卡尔曼增益xki+1和yki+1：

xpi+1＝xpi-xki*xpi+q

xki+1＝xpi+1/(xpi+1+r)

ypi+1＝ypi-yki*ypi+q

yki+1＝ypi+1/(ypi+1+r)

当下一周期输入定位点的坐标(xi+1,yi+1)时，卡尔曼滤波器滤波后输出的定位点坐标(xfi+1,yfi+1)的最优值：

xfi+1＝xfi+xki+1*(xi+1–xfi)

yfi+1＝yfi+yki+1*(yi+1–yfi)

其中，q为固定的卡尔曼滤波参数，取值为1e-4。

有益效果：

第一，本发明利用了缓冲区设计，有效减轻了定位点在道路交点附近的拉车现象，减少了定位点道路的误判，大大减轻了轨迹拉扯的现象；

第二，本发明在增加了对定位点坐标和道路数据的预处理，有效减少了计算量，降低系统的压力，做到输出坐标的实时性；

第三，本发明从实际应用的角度出发，对输出坐标进行了滤波处理，有效提升了轨迹的平滑性，提升了轨迹实时显示的效果。

附图说明

下面结合附图和具体实施方式对本发明做更进一步的具体说明，本发明的上述和/或其他方面的优点将会变得更加清楚。

图1为本发明系统的运行流程图。

图2为本发明实施例中所使用的三阶贝塞尔曲线采样方法图。

图3为本发明实施例中使用轨迹约束方法前后的对比图。

具体实施方式

图1是本发明一种基于超宽带实时定位的轨迹约束方法的运行流程图，包括10个步骤。

步骤1，对道路路线进行采样，所述道路路线包括直线和曲线，采样后所有直线和曲线的集合记为r；

步骤2，对集合r中所有曲线分别进行分割，所有曲线分割点的坐标和直线道路两端的端点的坐标组成坐标集p；

步骤3，根据坐标集p生成定位区域，对定位区域进行分块，分块集合为b；

步骤4，以道路交点为圆心，创建半径为r的缓冲区，其中0.5米≤r≤1米；创建缓冲区的目的在于减少定位点在多条道路之间跳动，减轻轨迹的拉扯现象；

步骤5，超宽带解算模块解算出第i个周期的定位点坐标api；

步骤6，对定位点坐标api进行预处理；

步骤7，从定位区域的分块集合b中取离定位点api最近的分块；

步骤8，根据最近的分块计算道路上离定位点坐标api最近点的坐标npi；

步骤9，根据缓冲区策略生成新的定位点坐标；

步骤10，对新的定位点坐标进行卡尔曼滤波处理输出位置坐标。

本实施例中，步骤1可采用两种方式采样道路直线和曲线。

方式1，基于贝塞尔曲线采样。基于道路的复杂程度，可采用分段采样和多阶贝塞尔曲线采样方法。道路为直线时，可采用一阶贝塞尔曲线采样。道路为u型形状的简单曲线时，可采用多阶贝塞尔曲线采样；本实施例中采用三阶贝塞尔曲线采样。如果道路由h个u型形状组成的复杂曲线和直线构成，h大于等于2，直线采用一阶贝塞尔曲线采样，复杂曲线先分段采样，切分成多段简单曲线，然后对切分后的多段简单曲线分别采用多阶贝塞尔曲线采样。道路路线中所有直线和简单曲线的集合记为r。具体的基于贝塞尔曲线采样的方法可以参见参考文献：abeziercurvebasedpathplanninginamulti-agentrobotsoccersystemwithoutviolatingtheaccelerationlimits[j].k.g.jolly,r.sreeramakumar,r.vijayakumar.roboticsandautonomoussystems.2008(1)。图2展示了本实施例中所使用的三阶贝塞尔曲线采样方法图。通过确定起点p0和终点p3，以及两个控制点p1和p2，即可获取一条道路曲线。

方式2，基于实际道路等间距采样。对于简单曲线，等间距放置采样点，本实施例中，间距范围优选0.5米～2米之间，使用测量仪器测量出采样点坐标，得到坐标之后，通过最小二乘法拟合生成曲线的多次函数。对于道路曲线由h个u型形状组成的复杂曲线，对复杂曲线先分段采样，切分成多段简单曲线，然后对切分后的每段简单曲线等间距放置采样点，使用测量仪器测量出采样点坐标，得到坐标之后，通过最小二乘法拟合生成该段简单曲线的多次函数。道路路线中所有直线和简单曲线的集合记为r。

本实施例中，步骤2中，对道路集合r中的每段曲线进行分割取点，将每段曲线分割成长度为l的j条线段，0.5米≤l≤1米，本实施例中l优选0.5米，j根据每段曲线的长度以及l进行确定；保存所有分割点的坐标，分割点的坐标和道路集合r中每段直线两端的端点坐标组成坐标集p；坐标的形式为(x,y)，单位为米，坐标集p中的所有坐标中的x最大值记为xmax、x最小值记为xmin、y最大值记为ymax、最小值记为ymin。

本实施例中，步骤3中，根据坐标集p中的所有坐标中的x最大值、x最小值和y最大值、最小值确定定位区域的边界，由(xmin,ymin)、(xmin,ymax)、(xmax,ymin)和(xmax,ymax)四个坐标点连线组成的矩形区域即为定位区域。对定位区域进行分块，块的边长为a，2米≤a≤5米，本实施例中a优选4米；分块集合记作b，其中一个分块用bn表示，即b＝{b1,b2,...,bn,...,bn}，1≤n≤n，n表示定位区域中分块的数量，分块bn中包含步骤2中的线段。

本实施例中，步骤5中，获取定位点携带的uwb定位标签的tdoa和tof数据，超宽带解算模块利用解算算法计算得到定位点携带的uwb定位标签的坐标，也即获得了定位点的坐标。uwb定位标签会周期性向外发送超宽带信号，超宽带解算模块周期性计算uwb标签的坐标，记第i个周期获得的定位点坐标为api，第i-1个周期获得的定位点坐标为api-1。具体的超宽带解算算法可以参考：徐志鑫.基于uwb的室内定位关键技术研究[d].南京邮电大学,2019。

本实施例中，步骤6中，对解算出的定位点坐标api做一定的预处理。如果定位点坐标api到步骤2中所述的所有线段的距离中最短的距离超过设定的阈值t，其中0.8米≤t≤2米，则直接输出定位点坐标api，不对其做后续的轨迹约束处理，继续执行步骤10，否则执行步骤7。。

本实施例中，步骤7中，从定位区域的分块集合b中取离定位点最近的m个分块。根据定位点坐标api，遍历查找分块集合b＝{b1,b2,...,bn,...,bn}中每个分块bn距离定位点坐标的最短距离dmin,n和最长距离dmax,n，其中最长距离dmax,n组成的集合的最小值为dmax,min。如果某个分块be的最短距离dmin,e>dmax,min，其中1≤e≤n，则判断分块be离定位点较远，将其排除，剩余的即为最近的分块，最近的分块个数记为m，1≤m≤n。

本实施例中，步骤8中，在m个分块中，总的线段个数记为c，遍历计算定位点api到这m个分块中每个分块的每条线段的最短距离smin,c以及对应的线段上的坐标cpc，1≤c≤c，在smin,c组成的集合中获取的最小值即为定位点api到道路的最短距离，对应的线段上的坐标为离定位点api最近点的坐标，记为npi,；第i-1个周期离定位点坐标api-1最近点的坐标记为npi-1。

本实施例中，步骤9包括以下步骤：

步骤9-1，如果没有第i-1个周期的最近点npi-1即没有历史最近点或者第i-1个周期的最近点npi-1的生成时间距离现在的时间差δt>15s，则更新保存第i个周期的最近点npi所在的道路信息，返回第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-2。

步骤9-2，如果第i个周期的最近点npi和第i-1个周期的最近点npi-1都不在缓冲区中但在同一条道路上，则更新保存第i个周期的最近点npi所在的道路信息，返回第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-3。

步骤9-3，如果第i个周期的最近点npi和第i-1个周期的最近点npi-1都不在缓冲区内且不在同一条道路上，则说明跨越了缓冲区，则更新保存第i个周期的最近点npi的道路信息，返回第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-4。

步骤9-4，如果第i个周期的最近点npi在缓冲区中但第i-1个周期的最近点npi-1不在缓冲区中，且第i-1个周期的最近点npi-1所在的道路是生成缓冲区的道路之一，检查在第i-1个周期的最近点npi-1和缓冲区圆心之间的道路上是否有插值，如果无插值，则沿第i-1个周期的最近点npi-1开始向缓冲区圆心方向的道路上插值第一个点ip1，返回第一个插值点ip1作为新的定位点，并记录插值相关信息，继续执行步骤10，否则执行步骤9-5。

步骤9-5，如果第i个周期的最近点npi在缓冲区中但第i-1个周期的最近点npi-1不在缓冲区中，检查在第i-1个周期的最近点npi-1和缓冲区圆心之间的道路上是否有插值，如果有插值，且插值点的个数f小于总插值点个数f，5≤f≤8，本实施例中f优选5，则继续沿上一个插值点向缓冲区圆心方向的道路上插值一个点，返回新的插值点ipf作为新的定位点，更新插值相关信息；若插值点的个数f等于总插值点个数f，则在缓冲区圆心附近生成随机点坐标rp，随机点坐标rp的形式为(xrp,yrp)，缓冲区圆心的坐标形式为(xcp,ycp)，随机点坐标rp的取值需要满足0≤|xrp-xcp|≤0.5米，0≤|yrp-ycp|≤0.5米；返回随机点坐标rp作为新的定位点，继续执行步骤10，否则执行步骤9-6。

步骤9-6，如果第i个周期的最近点npi不在缓冲区中但第i-1个周期的最近点npi-1在缓冲区中，且第i-1个周期的最近点npi-1所在的缓冲区是由第i个周期的最近点npi所在的道路和另一条道路相交产生的，若第i-1个周期的最近点npi-1是以缓冲区圆心为中心随机产生或者是在所述的另一条道路上插值的点且另一条道路上插值点个数小于总插值点个数f，则需要在第i个周期的最近点npi和缓冲区圆心之间插值g个点，3≤g≤8，本实施例中g优选5，输出g个插值点和第i个周期的最近点npi作为新的定位点，即新的定位点由多个点组成；继续执行步骤10，否则执行步骤9-7。

步骤9-7，如果第i个周期的最近点npi不在缓冲区中但第i-1个周期的最近点npi-1在缓冲区中，且第i-1个周期的最近点npi-1所在的缓冲区是由第i个周期的最近点npi所在的道路和另一条道路相交产生的，若第i-1个周期的最近点npi-1和第i个周期的最近点npi在同一条道路上，第i-1个周期的最近点npi-1是插值的点且其所在道路上的插值点个数小于总插值点个数f，若是从缓冲区折返的情况，则在第i个周期的最近点npi和第i-1个周期的最近点npi-1之间插值g个点，3≤g≤8，本实施例中g优选5，输出g个插值点和第i个周期的最近点npi作为新的定位点；若是从缓冲区穿过的情况，则需要在第i-1个周期的最近点npi-1和缓冲区圆心之间插值g个点，再在缓冲区圆心和第i个周期的最近点npi之前插值g个点，返回2*g个插值点和第i个周期的最近点npi作为新的定位点，继续执行步骤10，否则执行步骤9-8。

步骤9-8，返回第i个周期的最近点npi，继续执行步骤10。

本实施例中，步骤10中，卡尔曼滤波器的操作包括两个阶段：预测与更新。将步骤9中根据缓冲区策略生成新的定位点的坐标作为输入，坐标形式为(xi,yi)，经过卡尔曼滤波器滤波后输出的定位点坐标形式为(xfi,yfi)；在预测阶段，滤波器使用第i-1个周期的坐标(xfi-1,yfi-1)的估计，做出对第i个周期的坐标(xfi,yfi)的估计。第i-1个周期的坐标(xfi-1,yfi-1)的初始值为xfi-1＝xi，yfi-1＝yi；计算卡尔曼增益xki,yki：

xki＝xpi/(xpi+r)

yki＝ypi/(ypi+r)

其中，r为固定的卡尔曼滤波参数，取值为1e-3；xpi,ypi为坐标取值xfi和yfi的协方差，初始值均设置为1。

结合测量值预测第i个周期的坐标(xfi,yfi)的最优值：

xfi＝xfi-1+xki*(xi–xfi-1)

yfi＝yfi-1+xki*(yi–yfi-1)

在更新阶段，更新下一周期的坐标取值xfi+1和yfi+1的协方差xpi+1,ypi+1和卡尔曼增益xki+1,yki+1：

xpi+1＝xpi-xki*xpi+q

xki+1＝xpi+1/(xpi+1+r)

ypi+1＝ypi-yki*ypi+q

yki+1＝ypi+1/(ypi+1+r)

当下一周期输入定位点的坐标(xi+1,yi+1)时，卡尔曼滤波器滤波后输出的定位点坐标(xfi+1,yfi+1)的最优值：

xfi+1＝xfi+xki+1*(xi+1–xfi)

yfi+1＝yfi+yki+1*(yi+1–yfi)

其中，q为固定的卡尔曼滤波参数，取值为1e-4。

图3展示了本发明中使用基于超宽带实时定位的轨迹约束方法前后的对比图。1822、1810、1880和188f为uwb定位标签编号，其中188f标签的原始轨迹为动态，其余标签为静态。由图3可以直观的对比发现，未使用本发明的方法之前，标签动态轨迹波动较大，偏离实际道路较远，使用本发明的方法之后，定位点轨迹更为平滑，道路交点没有明显的跳点，视觉效果更佳。

本发明提供一种基于超宽带实时定位的轨迹约束方法，减轻了定位点在道路交点附近的拉车现象，减少了定位点道路的误判，有效减少了计算量，降低系统的压力，做到输出坐标的实时性，有效提升了轨迹的平滑性，提升了轨迹实时显示的效果。具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。