CN112492523B - 一种基于超宽带实时定位的轨迹约束方法 - Google Patents

一种基于超宽带实时定位的轨迹约束方法 Download PDF

Info

Publication number
CN112492523B
CN112492523B CN202011222875.8A CN202011222875A CN112492523B CN 112492523 B CN112492523 B CN 112492523B CN 202011222875 A CN202011222875 A CN 202011222875A CN 112492523 B CN112492523 B CN 112492523B
Authority
CN
China
Prior art keywords
point
positioning
coordinates
road
cycle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011222875.8A
Other languages
English (en)
Other versions
CN112492523A (zh
Inventor
周壮壮
李俊
申富饶
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Heguang Intelligent Manufacturing Research Institute Co ltd
Nanjing University
Original Assignee
Nanjing Heguang Intelligent Manufacturing Research Institute Co ltd
Nanjing University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Heguang Intelligent Manufacturing Research Institute Co ltd, Nanjing University filed Critical Nanjing Heguang Intelligent Manufacturing Research Institute Co ltd
Priority to CN202011222875.8A priority Critical patent/CN112492523B/zh
Publication of CN112492523A publication Critical patent/CN112492523A/zh
Application granted granted Critical
Publication of CN112492523B publication Critical patent/CN112492523B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/021—Services related to particular areas, e.g. point of interest [POI] services, venue services or geofences
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings

Landscapes

  • Engineering & Computer Science (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Navigation (AREA)

Abstract

本发明提供了一种基于超宽带实时定位的轨迹约束方法，包括：步骤1，采样道路直线或曲线；步骤2，曲线分割取点；步骤3，区域分块；步骤4，根据道路交点创建缓冲区；步骤5，超宽带解算模块解算出定位点坐标等相关信息；步骤6，定位坐标的预处理；步骤7，选取定位坐标最近的M块区域；步骤8，取道路上离定位坐标最近的点；步骤9，根据缓冲区策略生成新的定位点；步骤10，对新的定位点进行卡尔曼滤波处理输出位置。本发明有效地对定位点进行了约束，并且提高了轨迹的平滑度，添加了对异常数据的处理，特别地对道路交叉点进行了特殊处理，有效降低了定位点道路的误判，大大减轻了轨迹拉扯的现象。

Description

一种基于超宽带实时定位的轨迹约束方法
技术领域
本发明属于实时定位领域，更具体的，涉及一种基于超宽带实时定位的轨迹约束方法。
背景技术
近年来，随着城市的不断发展，商场、车站、工厂等室内建筑的不断扩大，室内环境的场景越来越复杂，人们对室内定位的需求越来越强烈，室内定位系统在越来越多的工厂和商场实施运行。随着工业4.0的到来，智能工厂的概念被普及开来，智能工厂中一个重要方面就是对工人的实时定位，以保证生产安全，并优化工作流程。
超宽带技术由于频率高，抗干扰能力强，容量大，被广泛应用在室内定位系统中。实现超宽带通信的首要任务是产生UWB(Ultra Wide Band，超宽带)信号，从本质上看，UWB是发射和接收超短电磁脉冲的技术。与传统的无线收发信机结构相比，UWB收发信机的结构相对简单。一般而言，用于移动的UWB设备称作超宽带定位标签(也称为UWB定位标签)，用于辅助定位的固定UWB设备称为超宽带定位基站。参考文献：Scholtz RA.,Win MZ..Ultra-wide bandwidth time-hopping spread-spectrum impulse radio for wirelessmultiple-access communications[J].IEEE Transactions on Communications,2000,48(4)。
UWB信号的发送和接收会以时间戳的方式记录下来，利用时间戳可生成距离相关的数据。TDOA(Time Difference of Arrival，到达时间差)为UWB发送设备发送信号之后到达两个UWB接收设备的到达时间的差值。TOF(Time of flight，飞行时间)为两个UWB设备经过三次发送接收信号计算得到的单次飞行时间。参考文献：Ole Bischoff,Xinwei Wang,Nils Heidmann,et al.Implementation of an ultrasonic distance measuring systemwith kalman filtering in wireless sensor networks for transportlogistics.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。具体的基于贝塞尔曲线采样的方法可以参见参考文献：A Bezier curve based path planning in a multi-agent robotsoccer system without violating the acceleration limits[J].K.G.Jolly,R.Sreerama Kumar,R.Vijayakumar.Robotics and Autonomous Systems.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可以直观的对比发现，未使用本发明的方法之前，标签动态轨迹波动较大，偏离实际道路较远，使用本发明的方法之后，定位点轨迹更为平滑，道路交点没有明显的跳点，视觉效果更佳。
本发明提供一种基于超宽带实时定位的轨迹约束方法，减轻了定位点在道路交点附近的拉车现象，减少了定位点道路的误判，有效减少了计算量，降低系统的压力，做到输出坐标的实时性，有效提升了轨迹的平滑性，提升了轨迹实时显示的效果。具体实现该技术方案的方法和途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。本实施例中未明确的各组成部分均可用现有技术加以实现。

Claims (5)

1.一种基于超宽带实时定位的轨迹约束方法，其特征在于，包括如下步骤：
步骤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，对新的定位点坐标进行卡尔曼滤波处理输出位置坐标；
步骤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中的线段；
步骤6中，对解算出的定位点坐标APi做预处理；如果定位点坐标APi到步骤2中所述的所有线段的距离中最短的距离超过设定的阈值T，其中0.8米≤T≤2米，则直接输出定位点坐标APi，不对其做后续的轨迹约束处理，继续执行步骤10，否则执行步骤7；
步骤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和上一周期的最近点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。
2.根据权利要求1所述的一种基于超宽带实时定位的轨迹约束方法，其特征在于，步骤1中，可采用两种方式采样道路直线和曲线；
方式1，基于贝塞尔曲线采样；基于道路的复杂程度，采用分段采样和多阶贝塞尔曲线采样方法；道路为直线时，采用一阶贝塞尔曲线采样；道路为U型形状的简单曲线时，采用多阶贝塞尔曲线采样；如果道路由H个U型形状组成的复杂曲线和直线构成，H大于等于2，直线采用一阶贝塞尔曲线采样，复杂曲线先分段采样，切分成多段简单曲线，然后对切分后的多段简单曲线分别采用多阶贝塞尔曲线采样；道路路线中所有直线和简单曲线的集合记为R；
方式2，基于实际道路等间距采样；对于道路为U型形状的简单曲线，等间距放置采样点，使用测量仪器测量出采样点坐标，得到坐标后通过最小二乘法拟合生成曲线的多次函数；对于道路曲线由H个U型形状组成的复杂曲线，对复杂曲线先分段采样，切分成多段简单曲线，然后对切分后的每段简单曲线等间距放置采样点，使用测量仪器测量出采样点坐标，得到坐标之后，通过最小二乘法拟合生成该段简单曲线的多次函数；道路路线中所有直线和简单曲线的集合记为R。
3.根据权利要求1所述的一种基于超宽带实时定位的轨迹约束方法，其特征在于，步骤5中，获取定位点携带的UWB定位标签的到达时间差TDOA和飞行时间TOF数据，超宽带解算模块利用解算算法解算出定位点携带的UWB定位标签的坐标，也即获得了定位点的坐标；UWB定位标签会周期性向外发送超宽带信号，超宽带解算模块周期性计算UWB标签的坐标，记第i个周期获得的定位点坐标为APi，第i-1个周期获得的定位点坐标为APi-1。
4.根据权利要求1所述的一种基于超宽带实时定位的轨迹约束方法，其特征在于，步骤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。
5.根据权利要求4所述的一种基于超宽带实时定位的轨迹约束方法，其特征在于，步骤8中，在M个分块中，总的线段个数记为C，遍历计算定位点APi到这M个分块中每个分块的每条线段的最短距离smin,c以及对应的线段上的坐标CPc，1≤c≤C，在smin,c组成的集合中获取的最小值即为定位点APi到道路的最短距离，对应的线段上的坐标为离定位点APi最近点的坐标，记为NPi；第i-1个周期离定位点坐标APi-1最近点的坐标记为NPi-1。
CN202011222875.8A 2020-11-05 2020-11-05 一种基于超宽带实时定位的轨迹约束方法 Active CN112492523B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011222875.8A CN112492523B (zh) 2020-11-05 2020-11-05 一种基于超宽带实时定位的轨迹约束方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011222875.8A CN112492523B (zh) 2020-11-05 2020-11-05 一种基于超宽带实时定位的轨迹约束方法

Publications (2)

Publication Number Publication Date
CN112492523A CN112492523A (zh) 2021-03-12
CN112492523B true CN112492523B (zh) 2021-09-14

Family

ID=74928450

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011222875.8A Active CN112492523B (zh) 2020-11-05 2020-11-05 一种基于超宽带实时定位的轨迹约束方法

Country Status (1)

Country Link
CN (1) CN112492523B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113759824B (zh) * 2021-09-29 2023-03-14 苏州谷夫道自动化科技有限公司 基于误差约束的轨迹优化方法及系统

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104898675A (zh) * 2015-06-05 2015-09-09 东华大学 一种机器人智能导航控制方法
CN110262495A (zh) * 2019-06-26 2019-09-20 山东大学 可实现移动机器人自主导航与精确定位的控制系统及方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8344948B2 (en) * 2010-03-29 2013-01-01 Xsens Holding B.V. Positioning system calibration
WO2013188598A2 (en) * 2012-06-12 2013-12-19 Trx Systems, Inc. Fusion of sensor and map data using constraint based optimization
CN106162867A (zh) * 2015-04-27 2016-11-23 张大强 一种基于等分距离的自适应室内定位方法
CN110031876A (zh) * 2018-01-11 2019-07-19 中南大学 一种基于卡尔曼滤波的车载导航轨迹点偏移矫正方法
CN110456820B (zh) * 2019-08-22 2021-07-27 江苏农林职业技术学院 基于超带宽无线定位的农药喷施系统及控制方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104898675A (zh) * 2015-06-05 2015-09-09 东华大学 一种机器人智能导航控制方法
CN110262495A (zh) * 2019-06-26 2019-09-20 山东大学 可实现移动机器人自主导航与精确定位的控制系统及方法

Also Published As

Publication number Publication date
CN112492523A (zh) 2021-03-12

Similar Documents

Publication Publication Date Title
FI113409B (fi) Sekvenssiperusteinen paikannustekniikka
CN112533149B (zh) 一种基于uwb移动节点的移动目标定位算法
CN110121150B (zh) 一种基于uwb技术的室内跨区域无缝定位系统
CN112492523B (zh) 一种基于超宽带实时定位的轨迹约束方法
CN108615070A (zh) 一种基于混沌粒子群算法的tdoa和aoa混合定位方法
JP2010187359A (ja) 無線チャネルの状態を推定する方法
US6272350B1 (en) Method for improved line of sight signal detection using time/frequency analysis
CN102879774B (zh) 高频地波超视距雷达短航迹合成方法和装置
CN110850363B (zh) 一种基于实时定位轨迹数据进行动态滤波优化的方法
CN113671481B (zh) 一种基于毫米波雷达的3d多目标跟踪处理方法
WO2022242075A1 (zh) 一种机器人定位方法、装置、机器人和可读存储介质
CN111829525A (zh) Uwb室内外一体智能导航定位方法和系统
Teng et al. Binary variational filtering for target tracking in sensor networks
Monica et al. Optimized anchors placement: An analytical approach in UWB-based TDOA localization
Zhang et al. Review on UWB-based and multi-sensor fusion positioning algorithms in indoor environment
CN106353724B (zh) 一种基于无线传感器网络的精确室内定位方法
US6259894B1 (en) Method for improved line-of-sight signal detection using RF model parameters
CN109613477A (zh) 一种复杂环境下的tdoa定位追踪方法
CN114034298B (zh) 一种基于可重构智能表面的车辆定位方法
Ibrahim et al. Enhanced localization for indoor construction
KR101101051B1 (ko) 울트라 와이드밴드를 이용한 실내 환경에서의 위치인식방법
CN114527481A (zh) 一种面向遮挡测评环境的多传感高精度融合测量方法
Zhen et al. Adaptive AR model based robust mobile location estimation approach in NLOS environment
CN111190139A (zh) 一种用于复杂环境中超宽带非视线传输的高精度定位方法
Inoue et al. LSTM-based high precision pedestrian positioning

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant