一种实时定位防穿越优化方法与流程

本发明涉及定位领域，具体地，涉及一种实时定位防穿越优化方法。

背景技术：

随着科学技术的发展，基于定位的需求服务越来越多。在定位的系统中，由于系统所处环境的复杂(各种信号干扰)，导致定位结果出现一定的偏差(例如实际人员在一个房间中，却定位在一墙之隔的另一个房间中)，比如定位点出现在障碍物(墙或柱子等)中或定位轨迹穿越障碍物，与实际情况不符。在室外gps定位系统出现这样的情况对使用者来说可以接受，但在需要精确定位(室内定位等)的系统中，为了更好的定位效果，对定位穿越障碍物的情况需要做出处理。因此我们需要对定位的路径有一个好的处理方案。

在出现上述定位偏差问题时，可以调整定位系统(硬件调整，例如改变定位信号频率或信号强度等)来提高定位的准确性。这样的调整人力成本高，不能完全保证所有地方的信号，而且也不能保证效果。而且每张定位卡信号不同，定位效果不一，在多个定位卡的情况下并不能很好解决问题。定位路径的展示效果也不一定好。

技术实现要素：

本发明提供了一种实时定位防穿越优化方法，解决了实时定位轨迹穿障、跑偏、导致最终定位错误的问题，实现了实时定位轨迹显示规则，完整的技术效果。

为实现上述发明目的，本申请提供了一种实时定位防穿越优化方法，所述方法包括：

步骤1：初始化地图，在地图中设置障碍物区域与活动区域，记录障碍物区域多边形与活动区域多边形的顶点坐标，形成基础地图数据；

步骤2：基于基础地图数据，生成地图中活动区域与障碍物区域构成的可通行网络图；

步骤3：实时接收来自定位系统的定位数据；

步骤4：判断定位数据中的起点与终点时间t相差是否在设定时间范围，若在设定时间范围，起点有效，若超过时间范围起点无效；

步骤5：计算定位数据中的起点与终点是否为可达起点与可达终点；

步骤6：判断可达起点到可达终点可否直达，(可达起点与可达终点组成的线段与障碍物或活动区域的多边形的边没有交点，则为可直达)，若能直达则进入步骤9，若不可直达则进入步骤7；

步骤7：判断可达起点到可达终点是否存在可达路径，若存在可达路径，则返回通行路径点与可达终点到进入步骤9，否则进入步骤8；

步骤8：计算可达起点与可达终点的直线距离，若直线距离大于设定距离，则记录一个可跨越权重g，当权重g值大于设定权值，则返回可达终点坐标；若直线距离小于等于设定距离或者权重g值小于等于设定值，则计算距离可达起点最近的相交边上可达终点对应的边上的映射点，返回映射点到步骤9；

步骤9：将步骤6或步骤7或步骤8返回的一个或多个坐标点作为优化后的结果返回。

其中，本发明的原理为：在地图中设定多边形障碍物和活动区域，定位点不能进入或穿过障碍物，不能离开活动区域，定位点通过寻路的方式沿障碍物边移动，绕过障碍物或被障碍物挡住不动。本方案通过定位点计算出优化后的定位坐标与优化路径，最后将优化的路径及坐标展现出来。本方法在优化前输入：实际解算的定位坐标，坐标可能出现在地图任意位置；优化后输出：优化后的定位坐标，坐标只能在可行区域中。

进一步的，定位数据内容包括：定位源的唯一标识id、定位数据的时间t、定位坐标(x，y)，同一定位源上一次接收的定位坐标为起点，当次接收的定位坐标为终点。

进一步的，计算可达起点与可达终点，若点为可达点则返回自身；若点不是可达点则返回距离点最近的可达点；若起点无效，则只计算可达终点。

进一步的，计算可达点方法为：判断点是否在可行区域内，若在可行区域内，则该点为可达点；若不在可行区域内，则找到点距离最近一条边，返回点在边上的映射点。

进一步的，判断点与多边形关系算法包括：

(1)根据多边形的边遍历出多边形所有的线段组成，基于多边形所有的线段组成判断点是否在多边形内；

(2)计算线段与从坐标点向右射出平行于x轴的射线是否有交点，若有交点则则点在多边形内，若没有交点，则点在多边形外；

(3)判断交点个数的奇偶，奇数个交点则点在多边形内，偶数个交点则点在多边形外。

进一步的，线段与射线交点的计算方法为：

判断点在线段两侧的方法：

设线段端点为从a(x1,y1)到b(x2,y2),线外一点p(x0,y0),判断该点与有向线a—b位置关系为；

a＝(x2-x1,y2-y1)；

b＝(x0-x1,y0-y1)；

axb＝|a||b|sinα，α为两向量的夹角；

|a||b|！＝0时，axb决定点p的位置；

axb的z方向大小决定p位置；

(x2-x1)(y0-y1)-(y2-y1)(x0-x1)>0点位于有向线a—b的左侧；

(x2-x1)(y0-y1)-(y2-y1)(x0-x1)<0点位于有向线a—b的右侧；

(x2-x1)(y0-y1)-(y2-y1)(x0-x1)＝0点在有向线a—b线段上。

进一步的，判断可达起点到可达终点可否直达的方法为：可达起点与可达终点的线段与障碍物或活动区域是否相交，不相交则直达，相交则不可直达。

进一步的，可达路径算法为：

通过步骤6计算获得与可达起点a与可达终点b相交的线段，分别找到距离可达起点最近的线段与距离可达终点最近的线段；

枚举计算是否存在路径a到b可通行。

进一步的，可达起点终点线段ab与障碍物的线段cd、ef相交，df共点，枚举ab路径只有两条：一条是adb，一条路径是aceb，分别计算两条路径长度，取最短路径为寻路路径。

进一步的，将可达起点和可达终点加入可通行网络图，计算可达起点与通行网络图中所有的点是否连通；

计算可达起点到可达终点的最短寻路路径，寻路算法使用dijkstra寻路算法或a*寻路算法。

进一步的，步骤8具体为：可达起点a，可达终点g，路径ag分别与线段bc与de相交，距离a最近的相交线段为bc，g到线段bc的映射点为f，gf垂直bc，如果ag小于设定距离且跨越权重小于设定值，返回的映射点为f，否则，返回终点g。步骤8的特点是对于实时定位的轨迹进行纠正，能够消除定位时由于起点错误导致的本防穿越算法出现轨迹陷入死角等情况。

本申请提供的一个或多个技术方案，至少具有如下技术效果或优点：

第一，本发明最大的技术优点是在实时定位的过程中，显示规则(即不会穿越障碍物，或者穿越墙面)、准确(即不会因为错误穿越障碍物导致对人员定位出现错误)的行动轨迹。这对于实时对人员、物品等进行跟踪以及通过轨迹进行预测行动方向等有积极的意义。

第二，本发明方案还具有实时的定位修正机制，对于定位起点出现错误时，通过设置权重以及对后续几个定位点的区域判断来确定是否进行障碍物的合理穿越，从而避免由于初始起点定位偏差造成的轨迹错误以及最终定位点错误。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定；

图1是a点到b点的可达路径示意图；

图2是a点到b点的可达路径示意图；

图3是点在边上的映射点示意图；

图4是点在边上的映射点示意图；

图5是可通行网络图示意图；

图6是枚举路径示意图；

图7是寻路路径示意图；

图8是轨迹校正寻路示意图

图9是本申请中实时定位防穿越优化方法的流程示意图。

具体实施方式

为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在相互不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述范围内的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

下面对本申请中的相关概念与应用算法进行介绍：

障碍物：由用户设定，定位点不可达的区域(多边形表示)，如墙，柱子等。

活动区域：由用户设定，定位点可达的区域(多边形表示)，主要用于表示定位边界，例如我们定位范围只在一个房间中，定位物不会出现在房间外。活动区域主要控制定位坐标的可行范围，活动区域中可以画多个障碍物，定位物可以在一定条件下穿墙障碍物但是不会超出活动区域。

可行区域：由活动区域与障碍物区域组合而成，定位物在活动区域内且不在障碍物中，定位物始终不会超出可行区域。

可达点：定位点在可行区域内都是可达点(包含边界)。

可达路径：从起点到终点能够寻路到一条符合设定的路径(路径距离在设定范围内，且路径只能在可行区域中)。

如图1-图2，a点到b点的可达路径。

点在边上的映射点：点到线上的垂直投影点，如果投影点在线段上，为投影点，不在线段上为距离最近的线段端点。两种情况如下图3-图4：图3，点a在线段bc上的投影点为d(线段ad垂直于线段bc)；图4，点a在线段bc上的投影点为b(线段ad垂直于线段bc)。

可通行网络图：可通行网络图使用可视图法构建地图，其原理是将环境中的所有顶点(障碍物顶点和活动区域顶点)进行组合连接，要求各个顶点之间的连线不能穿越障碍物且在设定距离内(即连通点)。对地图所有顶点构建可达的连通关系，就构成了可通行网络图。可通行网络图的计算方法是计算每个点的连通点，记录每个点的连通点对应关系。如图5，g点的连通点有a，f，e，d，h，i，存储所有的连通点对应关系构成可通行网络图。

本方法优化定位坐标具体步骤如下：

步骤1：初始化地图，在地图中设置障碍物区域与活动区域，记录障碍物区域多边形与活动区域多边形的顶点坐标，构成基础地图数据。

步骤2：通过基础地图数据，计算地图中活动区域与障碍物区域构成的可通行网络图。

步骤3：实时接收来自定位系统的定位数据。定位系统定位方式例如gps，uwb，wifi，蓝牙，红外等等。定位数据内容包括：定位源的唯一标识id(例如卡号),定位数据的时间t，定位坐标(x，y)(定位系统人为设定的定位地图原点和2d坐标系x、y方向，x、y、分别为地图的x、y方向上的坐标)。同一定位源上一次接收的定位坐标为起点，当次接收的定位坐标为终点。以下步骤说明都是对同一定位源处理。

步骤4：判断起点与终点时间t相差是否在设定时间范围(默认5秒)，如果在设定时间范围，起点有效，如果超过时间范围起点无效。(如果定位源存在历史数据，可以使用历史数据为有效的起点，有效起点时间改为t-5)

步骤5：计算可达起点与可达终点(点为可达点返回自身，点不是可达点返回距离点最近的可达点，如果起点无效，只计算可达终点)，之后步骤中的起点与终点都是可达起点与可达终点。

计算可达点方法：

判断点是否在可行区域内(点在活动区域中，不在障碍物区域中)，在，说明点本身可达，不在，找到点距离最近一条边，返回点在边上的映射点。

判断点与多边形关系算法(射线法，点在多边形边上时判断为在可行区域内)：

1.根据多边形的边遍历出多边形所有的线段组成；

2.计算线段与射线(从坐标点向右射出平行于x轴的射线)是否有交点；

3.判断交点个数的奇偶(奇数个点在多边形内，偶数个点在多边形外)；

判断点在线段两侧的方法：

设线段端点为从a(x1,y1)到b(x2,y2),线外一点p(x0,y0),判断该点位于有向线a—b的那一侧(实际计算线段方向是根据y的大小确定的)。

a＝(x2-x1,y2-y1)

b＝(x0-x1,y0-y1)

axb＝|a||b|sinα(α为两向量的夹角)；

|a||b|！＝0时，axb决定点p的位置；

所以axb的z方向大小决定p位置；

(x2-x1)(y0-y1)-(y2-y1)(x0-x1)>0左侧；

(x2-x1)(y0-y1)-(y2-y1)(x0-x1)<0右侧；

(x2-x1)(y0-y1)-(y2-y1)(x0-x1)＝0在线段上；

实际判断有交点的方法：

1.点在线段左侧；

2.点的y坐标大于线段y的最小值，点的y坐标小于等于y的最大值；

步骤6：判断起点到终点可否直达，直达进入步骤9，不可直达进入步骤7。

判断直达方法：起点与终点的线段与障碍物或活动区域是否相交(判断线段与多边形每条边是否相交)，不相交直达，相交不可直达。

步骤7：判断起点到终点是否存在可达路径，存在可达路径，则返回通行路径点与可达终点到进入步骤9，否则进入步骤8。

可达路径算法(由于寻路算法计算量大，首先使用枚举路径算法，如果枚举不能找到寻路路径，再使用寻路算法计算)：

一.枚举路径算法介绍：

1.通过步骤6计算获得与起点a与终点b相交的线段，分别找到距离起点最近的线段与距离终点最近的线段(起点到最近线段的两个端点直线可达)

2.枚举计算是否存在路径a到b可通行，如图6。

起点终点线段ab与障碍物的线段cd，ef相交(df共点)，枚举ab路径只有两条：一条是adb，一条路径是aceb，分别计算两条路径长度，取最短路径为寻路路径。这里唯一需要计算df与ce是否可达，但是由于dfce为障碍物或活动区域的定点，通过地图的可通行网络图可以直接查询到是否可达与路径距离，这样计算量要小很多。

二.寻路路径算法

1.将起点和终点加入可通行网络图(计算起点与通行网络图中所有的点是否连通)。

2.计算起点到终点的最短寻路路径，寻路算法使用dijkstra寻路算法或a*寻路算法。

dijkstra寻路算法简介：

迪杰斯特拉算法是由荷兰计算机科学家狄克斯特拉于1959年提出的，因此又叫狄克斯特拉算法。是从一个顶点到其余各顶点的最短路径算法，解决的是有向图中最短路径问题。迪杰斯特拉算法主要特点是以起始点为中心向外层层扩展，直到扩展到终点为止。

a*寻路算法介绍：

a*算法是一种启发式搜索算法。本质上来讲，可以算作是广度优先搜索算法的改进。广度优先搜索总能找到路径最短的最优解，因为它每次新的一轮遍历永远是离起始点最近的位置，这样，当扫描到目标点时，可以保证目标点的距离是离起点距离最近的，也就是找到了寻路的最优解。a*算法的运行过程与广度优先搜索类似，不同的是，a*除了考虑当前点离起始点的距离外，还考虑了当前点离目标点的距离，分别用g和h来表示这两个距离，由此有代价函数f＝g+h。a*算法每次查找代价最低的点作为搜索点，并更新搜索节点列表。最终搜索到目标位置。

如图7，起点a，终点b，寻路路径acb，寻路点c：

步骤8：计算起点与终点的直线距离，如果直线距离大于设定距离(距离根据定位实际效果来定，默认3m)记录一个可跨越权重g(初值为0，每次进入该步骤加1，没有进入该步骤g修改为初值0)，当权重g值大于设定权值(根据定位频率设定权值，默认为5)，返回终点坐标。否则，计算距离起点最近的相交边上终点对应的边上的映射点，返回映射点到下一步。对于实时轨迹进行纠错。防止由于实时定位方案中的起点出错而导致的轨迹进入死角不能移动的情况，在多数数据点落到障碍物另一侧时，进行合理的障碍物穿越。即通过实时定位中的终点来对起点进行校正，得到符合实际情况的实时定位防穿墙轨迹。

如图8所示：起点a，终点g，路径ag分别与线段bc与de相交，距离a最近的相交线段为bc，g到线段bc的映射点为f(gf垂直bc)，如果ag小于设定距离且跨越权重小于设定值，返回的映射点为f，否则，返回终点g。

步骤9：将步骤6或步骤7或步骤8返回的一个或多个坐标点作为优化后的结果返回。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。