CN110763236A - 基于最小二乘的地图匹配定位优化方法 - Google Patents

Abstract

本发明公开了一种基于最小二乘的地图匹配定位优化方法，包括：获取目标原始定位数据并基于历史定位数据使用最小二乘法修正定位结果，根据目标运动状态对上一时刻粒子集进行更新并重新计算其粒子权重。基于地图信息使用地图匹配算法，评估粒子集可靠性并进行相应修正，更新粒子权重以及粒子可靠性评估。计算粒子综合权重，并对其归一化处理，根据当前时刻粒子集运动状态计算目标坐标值。本发明修正了因多径效应等室内因素引起的定位误差，在地图信息的辅助下修正了定位轨迹，提高了室内定位的精度以及定位轨迹的可靠性。

Description

基于最小二乘的地图匹配定位优化方法

技术领域

本发明涉及通信技术、室内定位技术和计算机技术领域，尤其涉及一种基于最小二乘和地图匹配的定位方法。

背景技术

近年来，基于位置的服务受到人们越来越多的关注，并在各种场景中得到了广泛的应用。现已有大量基于GPS/AGPS的室外定位解决方案，但是在室内环境下，其所能接收到的GPS信号太弱，无法使用这些技术提供准确的位置信息。然而，室内定位在人们的生产生活中扮演着越来越重要的角色，其在消防救援、大型商场、展厅、物联网等具有重要的应用场景。因而，高精度的室内定位解决方案的研究成为了热点。

粒子滤波是在机器人领域逐渐兴起的滤波跟踪算法，适用于在Kalman滤波的状态模型的高斯、线性假设条件不满足的情况，它通过使用蒙特卡洛法抽样最大后验估计，从而逼近贝叶斯最优估计。目前各种室内定位如WLAN室内定位、超宽带室内定位、基于视觉室内定位等都采用粒子滤波来改善定位精度以及解决多目标跟踪问题，并且使用粒子滤波来融合不同定位系统的数据，从而相互辅助提高定位结果。经常采用粒子滤波来将地图信息加入到位置估计中，根据地图数据来调整粒子滤波中的权值参数，从而不仅提高精度，也防止了跟踪目标出现穿墙等特殊情况。

发明内容

本发明为了解决当前室内定位精度较低、定位轨迹穿墙的问题，提出了一种高精度的基于最小二乘的地图匹配定位优化方法。

为实现上述目的，本发明的技术方案如下：

一种基于最小二乘的地图匹配定位优化方法，包括以下步骤：

步骤S1：获取目标原始定位数据并基于历史定位数据使用最小二乘法正定位结果；

步骤S2：根据目标运动状态对上一时刻粒子集进行更新并重新计算其粒子权重；

步骤S3：基于地图信息使用地图匹配算法，评估粒子集可靠性并进行相应修正，更新粒子权重以及粒子可靠性评估；

步骤S4：计算粒子综合权重，并对其归一化处理，根据当前时刻粒子集运动状态计算目标坐标值，此值即为最小二乘的地图匹配定位优化结果；

其中，所述步骤S1进一步包括：

获取目标原始定位数据；

选取m个连续的历史定位数据进行一阶线性拟合；

计算过目标点与拟合直线垂直的投影线，其交点为投影点；

根据历史定位数据分析并计算当前点投影点坐标值，此点为所求修正点，修正结果为目标点在拟合线上的投影点；

所述步骤S3进一步包括：

根据蒙特卡洛方法采集得目标点粒子集；

计算目标点粒子集位于不可达区域粒子数占粒子总数比例，若大于所定阈值，则对粒子集进行整体修正，并更新粒子集的所有粒子权重；

根据地图信息分析粒子是否处于不可达区域，计算其可靠性；

根据粒子可靠性、粒子权重计算粒子综合权重，并将其进行归一化处理；

根据粒子运动状态计算目标修正坐标值；

其中，对粒子集中不可达区域粒子占比进行了分析，当其占比大于所定阈值，则对其进行整体修正；粒子集整体进行优化时，以粒子集中心点为参考将所有粒子转移到最近的不可达边界；当其处于不可达区域，则说明该点不合理，将该点可靠性设为0.01；反正，若其位于可达区域，则说明该点可靠，将该点可靠性设为1。

在一种优选实施方式中，包括以下步骤：

步骤(1)：实时采集目标定位数据，对每个定位点根据历史定位数据使用最小二乘法定位算法进行轨迹修正。计算每个目标点的平均运动速度，其与目标修正坐标组成目标当前运动状态。

步骤(2)：使用粒子滤波，若该点为初始点，无粒子集，则基于蒙特卡洛方法从初始点(x₀,y₀)附近选取合适的粒子集(粒子集服从均值x₀,y₀，方差μ₁,μ₂的二维正态分布，μ₁,μ₂为常数，根据实际情况选取)，而后计算各粒子权重。否则，则根据步骤(1)获得的目标运动状态更新上一时刻的粒子集，计算更新后各粒子权重。

步骤(3)：使用地图匹配算法分析修正粒子集。计算粒子处于不可达区域占粒子总数的比例是否大于阈值，若大于，则对粒子集进行整体修正，以粒子集中心为基准将其修正到最近的不可达区域边缘，并重复步骤(2)更新粒子权重。而后根据粒子是否处于不可达区域(如隔壁房间、障碍物内部等)，计算粒子可靠性评估。若其位于不可达区域，则其可靠性定为0.01；反正，则为1。

步骤(4)：计算粒子综合权重(粒子权重与其可靠性评估乘积)，并将其归一化处理，最后根据粒子运动状态计算目标修正坐标值。

综上所述，本发明提供的基于最小二乘的地图匹配定位优化方法，其根据历史定位数据对实时采集的原始数据使用最小二乘定位算法修正，对定位轨迹平滑处理，提高了定位精度。而后根据地图匹配算法使用粒子滤波分析粒子集，加入了粒子可靠性评估，并且对不可达区域粒子占比过大的粒子集进行整体修正，进一步优化不合理定位轨迹(穿墙等)，提高了定位精度。

附图说明

图1所示为本发明实施例提供的基于最小二乘的地图匹配定位优化方法的流程图

图2所示为本发明实施例提供的最小二乘定位算法的定位点修正流程图。

图3所示为本发明实施例提供的地图匹配算法修正粒子集流程图。

具体实施方式

实际的室内定位环境是复杂多变的，故为更好的削弱多径效应的影响，提高定位精度，增强轨迹合理性，需要使用最小二乘法和粒子滤波利用地图信息来优化定位轨迹。鉴于此，本实施例提供一种基于最小二乘的地图匹配定位优化方法，主要由最小二乘定位算法和地图匹配定位算法融合而成。最小二乘定位算法主要是为了提高定位精度，平滑定位轨迹。其具体步骤包括：历史定位数据的选取、历史轨迹一阶线性拟合直线的计算、投影线的选取、投影点的计算。地图匹配算法主要功能是基于地图信息修正不合理轨迹，提高定位精度。其具体步骤包括：目标点粒子集的选取、粒子集修正、粒子可靠性评估、粒子综合权重的计算以及归一化、目标运动状态的计算。上述两种算法融合形成了地图匹配定位优化方法，具体流程包括：获取目标原始定位数据并基于历史定位数据使用最小二乘法修正定位结果，根据目标运动状态对上一时刻粒子集进行更新并重新计算其粒子权重。基于地图信息使用地图匹配算法，评估粒子集可靠性并进行相应修正，更新粒子权重以及粒子可靠性评估。计算粒子综合权重，并对其归一化处理，根据当前时刻粒子集运动状态计算目标坐标值。本发明修正了因多径效应等室内因素引起的定位误差，在地图信息的辅助下修正了定位轨迹，提高了室内定位的精度以及定位轨迹的可靠性。

下面结合附图和实施例对本发明作进一步说明。

如图1所示，本实施例提供的一种最小二乘的地图匹配定位优化方法包括：获取目标原始定位数据并基于历史定位数据使用最小二乘法修正定位结果。根据目标运动状态对上一时刻粒子集进行更新并重新计算其粒子权重。基于地图信息使用地图匹配算法，评估粒子集可靠性并进行相应修正，更新粒子权重以及粒子可靠性评估。计算粒子综合权重，并对其归一化处理，根据当前时刻粒子集运动状态计算目标坐标值。

如图2所示，本实施例提供的一种最小二乘定位算法优化过程包括：获取目标原始定位数据。选取m个连续的历史定位数据进行一阶线性拟合(m根据实际环境进行设定)。计算过目标点与拟合直线垂直的投影线，其交点为投影点。最后计算投影点坐标值，此点为所求修正点。

最小二乘定位算法的定位点修正具体原理如下：

假设原始定位数据为(x_k,y_k)，k表示采样时刻。根据定位环境，选取m个连续的历史定位数据进行定位轨迹一阶线性拟合。设轨迹样本回归模型为

其中a和b为所求回归方程参数，e_i为样本误差，则平方损失函数为

为了使样本误差平方和最小，则应使Q最小。将式(2)看作a和b关于Q的函数，则可将问题转化为求式(2)极值。根据费马定理可知，函数的极值点为偏导为零的点。则可得：

解的回归方程参数a,b的值为

如图2所示，若想将目标点修正到一阶线性拟合直线y＝ax+b的投影点(r_k,s_k)上，需求过目标点的拟合直线的垂线。若设过点(x_k,y_k)、(r_k,s_k)的直线为y＝a’x+b’，根据两直线垂直的斜率性质可知

进而可得(r_k,s_k)的值为

经上述历史定位数据的选取、历史轨迹一阶线性拟合直线的计算、投影线的选取、投影点的计算，形成最小二乘定位算法。

如图3所示，本实施例提供的一种地图匹配算法优化过程包括：获得目标点粒子集。计算目标点粒子集位于不可达区域粒子数占粒子总数比例，若大于所定阈值，则对粒子集进行整体修正，并更新粒子集的所有粒子权重。然后根据地图信息分析粒子是否处于不可达区域，计算其可靠性。根据粒子可靠性、粒子权重计算粒子综合权重，并将其进行归一化处理。根据粒子运动状态计算目标修正坐标值。

地图匹配算法修正粒子集原理如下：

假设目标k时刻第j个粒子状态为X^j _k＝[r^j _k,v_x(k),s^j _k,v_y(k)](M表示采样粒子个数，j＝1,…,M)，v_x(k)和v_y(k)则分别表示k时刻各粒子在x轴和y轴方向上的速度分量(由当前时刻采集数据估计得到)，r^j _k和s^j _k为粒子在x和y轴方向上的坐标值。由于人室内移动时倾向于保持匀速运动，因而可以近似认为k时刻和k-1时刻速度相等，将k-1时刻速度作为k时刻目标速度。

计算位于不可达区域粒子数N_b与粒子总数N_s的比值N_b/N_s，若其小于所设阈值时(阈值根据实验环境选取，一般设为0.5)，则以粒子集中心为参考将所有粒子转移到最近的不可达边界。

首先计算粒子集的中心点(m_r(k),m_s(k))，m_r(k)表示k时刻粒子集x轴数据r^j _k的均值，m_s(k)表示k时刻粒子集y轴数据s^j _k的均值。

可表示为

由于室内环境下障碍物、墙壁等不可达区域边界多为规则几何多边形(如三角形、四边形、圆形等)，它们都可以由多条线段相连接近似表示。因而，中心点(m_r(k),m_s(k))最近的不可达区域边界可设为y＝cx+e(x∈[u₁,u₂])，其中c和e为已知参数，u₁和u₂为边界线段范围。根据式(7)所求中心点，以及式(6)可以在边界线上找到中心点的投影，并以此将所有粒子约束到边界附近，粒子更新为

此时，计算k时刻各粒子与观测值(观测值每次选择离定位点最近的基站(x_l,y_l)，l根据基站与目标距离选取)的距离d^j _k。若考虑粒子权重分布为均值为μ、方差为σ的高斯分布，将d^j _k作为输入，则各粒子权重为

加入各粒子可靠性评估p(k,j)，其表示k时刻第j个粒子的可靠性权重。

计算k时刻第j个粒子的综合权重W^j _k为

将综合权重W^j _k归一化

根据各粒子运动状态X^j _k可得目标运动状态为

经上述目标点粒子集的选取、粒子集修正、粒子可靠性评估、粒子综合权重的计算以及归一化、目标运动状态的计算，形成地图匹配算法。

以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (1)

1.基于最小二乘的地图匹配定位优化方法，其特征在于，包括以下步骤：

步骤S1：获取目标原始定位数据并基于历史定位数据使用最小二乘法正定位结果；

步骤S2：根据目标运动状态对上一时刻粒子集进行更新并重新计算其粒子权重；

步骤S3：基于地图信息使用地图匹配算法，评估粒子集可靠性并进行相应修正，更新粒子权重以及粒子可靠性评估；

步骤S4：计算粒子综合权重，并对其归一化处理，根据当前时刻粒子集运动状态计算目标坐标值，此值即为最小二乘的地图匹配定位优化结果；

其中，所述步骤S1进一步包括：

获取目标原始定位数据；

选取m个连续的历史定位数据进行一阶线性拟合；

计算过目标点与拟合直线垂直的投影线，其交点为投影点；

根据历史定位数据分析并计算当前点投影点坐标值，此点为所求修正点，修正结果为目标点在拟合线上的投影点；

所述步骤S3进一步包括：

根据蒙特卡洛方法采集得目标点粒子集；

计算目标点粒子集位于不可达区域粒子数占粒子总数比例，若大于所定阈值，则对粒子集进行整体修正，并更新粒子集的所有粒子权重；

根据地图信息分析粒子是否处于不可达区域，计算其可靠性；

根据粒子可靠性、粒子权重计算粒子综合权重，并将其进行归一化处理；

根据粒子运动状态计算目标修正坐标值；

其中，对粒子集中不可达区域粒子占比进行了分析，当其占比大于所定阈值，则对其进行整体修正；粒子集整体进行优化时，以粒子集中心点为参考将所有粒子转移到最近的不可达边界；当其处于不可达区域，则说明该点不合理，将该点可靠性设为0.01；反正，若其位于可达区域，则说明该点可靠，将该点可靠性设为1。

Images