CN110645980A - 一种室内外一体化定位导航方法及其系统 - Google Patents

Abstract

发明公开了一种室内外一体化定位导航方法及其系统，涉及定位融合领域；其方法包括步骤1：获取GPS、蓝牙和惯性的定位数据；步骤2：定位数据通过自适应加权、卡尔曼滤波和基于距离修正完成定位解算，获取最终定位点；步骤3：将导航线进行点集划分得到投影计算的参考点，利用滑动窗口和弯道几何投影将所述最终定位点投影至导航线上的参考点，获取投影点；步骤4：利用所述投影点进行导航与地图显示。本发明采用自适应加权和卡尔曼滤波模型获得平滑的定位结果，对结果进行基于距离的修正，获得稳定且准确的定位，克服将惯性定位、蓝牙定位和GPS定位进行融合带来的计算量、实时性和稳定性的难点，实现平滑的室内外一体化定位导航。

Description

一种室内外一体化定位导航方法及其系统

技术领域

本发明涉及定位融合领域，尤其是一种室内外一体化定位导航方法及其系统。

背景技术

随着GPS定位的快速发展，一系列基于LBS的服务日益增多，也日益成熟，例如交通导航，位置呈现等方面。但是在室内时，GPS信号受到严重干扰，无法做到精确的室内定位，而室内定位的需求在日益增大，例如室内商场导购，室内停车引导，医院导诊等方面。

室内定位多依靠若干传感器进行定位，例如WIFI，蓝牙，红外线，地磁等。但是由于室内的环境复杂，遮挡物较多，此类基于无线传感器的定位精度受环境影响较大，定位精度不稳定。

基于蓝牙的定位，根据蓝牙信号接收端的信号特征或者与蓝牙信号发射端的衰减距离判断蓝牙信号接收端的定位，具体定位方式有基于指纹的定位与基于距离的定位。基于指纹的定位，首先在分布有蓝牙信号发射端的场景下，使用蓝牙信号接收端按照一定的布局采集蓝牙信号强度值，将在某一位置上搜索到的所有蓝牙信号作为在该位置的指纹，定位时根据搜索到的蓝牙信号与指纹进行匹配，找到最佳匹配点作为定位点；基于距离的定位，根据搜索到的某蓝牙发射端发射的信号，以信号衰减公式计算当前蓝牙信号接收端到该蓝牙发射端的距离，以此类推，计算到多个蓝牙信号发射端的距离，根据三角定位等方法计算定位点。但在实际室内环境中，蓝牙信号受到干扰导致定位精度较差以及定位不稳定，所以需要与惯性定位进行融合，以得到更高的定位精度与更稳定的定位结果。

惯性定位导航基于移动终端中的加速度传感器实现，首先获取设备的三轴加速度，根据加速度的特征判断行走状态即是否走了一步，再根据步长计算公式得到此时的步长，最后根据设备罗盘方向获得此时相对于上一点的定位。但是此技术由于传感器有误差，会造成定位过程中的累计误差，定位误差随着时间的增长而增大，所以需要与蓝牙定位融合以消除累计误差。

因此，本申请将惯性定位、蓝牙定位和GPS定位进行融合，克服计算量、数据融合、实时性、稳定性等难点，提出一种基于惯性定位、蓝牙定位和GPS定位的室内外一体化定位导航方法及其系统。

发明内容

本发明的目的在于：本发明提供了一种室内外一体化定位导航方法及其系统，克服将惯性定位、蓝牙定位和GPS定位进行融合带来的计算量、实时性和稳定性的难点，实现平滑的室内外一体化定位导航。

本发明采用的技术方案如下：

一种室内外一体化定位导航方法，包括如下步骤：

步骤1：获取GPS、蓝牙和惯性的定位数据后，利用所述定位数据进行初始定位；

步骤2：初始定位后的定位数据进行自适应加权、卡尔曼滤波和基于距离修正完成定位解算，获取最终定位点；

步骤3：将导航线进行点集划分得到投影计算的参考点，利用滑动窗口和弯道几何投影将所述最终定位点投影至导航线上的参考点，获取投影点；

步骤4：利用所述投影点进行导航与地图显示。

优选地，所述步骤1包括如下步骤：

步骤1.1：获取GPS、蓝牙和惯性的定位数据，所述GPS定位数据包括经纬度坐标数据P_gps与GPS定位精度a；所述蓝牙定位数据包括经纬度坐标数据P_bluetooth、定位计算数据d₀、与前置定位距离d与角度β；所述惯性定位数据包括包含经纬度坐标数据P_pedestrain；所述定位计算数据d₀为计算蓝牙定位使用KNN算法的过程中获得的前K个欧式距离的均值，与前置定位的距离d为当前蓝牙定位点与上一定位结果的距离，角度β为当前蓝牙定位点与当前轨迹角度差；

步骤1.2：判断前三秒内是否存在蓝牙定位，若存在，则将蓝牙定位数据的均值作为初始定位；否则，将GPS定位作为初始定位。

优选地，所述步骤2包括如下步骤：

步骤2.1：根据GPS、蓝牙的定位数据定义GPS、蓝牙和惯性的权重；

步骤2.2：根据所述GPS、蓝牙、惯性的权重和初始定位后GPS、蓝牙、惯性的定位数据计算定位测量值；

步骤2.3：卡尔曼滤波器初始化，将所述初始定位输入卡尔曼滤波器进行预测获取预测值，将所述预测值与定位测量值进行更新；

步骤2.4：根据距离更新步骤2.3获取的最终定位数据，并修正静止状态下的定位数据，完成定位解算获取最终的定位点。

优选地，所述步骤2.1中权重定义公式如下：

GPS权重WG：

蓝牙权重WB：

惯性定位权重WP：WP＝1-WB。

优选地，所述步骤2.2中定位测量值计算如下：

当GPS信号存在且满足条件，蓝牙定位和惯性定位同时存在时，测量值计算公式为：WG*P_gps+(1-WG)*(WB*P_blutooth+WP*P_pedestrain)；

当GPS信号存在且满足条件，存在惯性定位时，测量值的计算方式为：WG*P_gps+(1-WG)*P_pedestrain；

当无GPS信号或GPS信号不满足条件，有蓝牙定位和惯性定位时，测量值计算公式为：WB*P_bluetooth+WP*P_pedestrain；

当只有惯性定位时，测量值为P_pedestrain；

当无惯性定位时，判定其处于静止状态，定位不变，测量值为空。

优选地，所述步骤2.3中包括如下步骤：

步骤2.3.1：根据下列方程进行滤波预测：：

P′_k＝AP_k-1A^T+Q

其中，

为k-1时刻的状态向量，状态转移矩阵

P_k-1为k-1时刻的协方差矩阵，是元素值小于1的4维对角线矩阵，Q为预测误差方差矩阵，

为经预测方程得到的一个k时刻的预测状态向量，P′_k为P_k-1经预测方程得到的k时刻的预测协方差矩阵，T为滤波器执行的时间间隔；

的初始值为前述初始定位，P_k-1初始值为对角线元素小于1的对角阵；

步骤2.3.2：基于步骤2.3.1获取的预测值与所述定位测量值进行更新，更新获得的协方差矩阵将用于下一次的卡尔曼滤波，更新的公式如下：

κ＝P′_kH(HP′_kH^T+R)^-1

p_k＝(I-κHP′_k)

其中，测量估计矩阵

z_k为k时刻的测量值即步骤2.2中获得的定位测量值，κ为增益系数，R为测量误差方差矩阵，I为单位矩阵，为计算的k时刻的滤波结果。

优选地，所述步骤2.4包括如下步骤：

步骤2.4.1：判断当前定位滤波点与上一定位点的距离是否超过X米，X取值范围为1.5-2，若超过，则将当前定位更新为(1-1.2/dl)*last_location+(1.2/dl)*current_location，其中，dl是当前滤波结果与上一定位点的距离，last_location为上一定位点，current_location为当前卡尔曼滤波结果；若未超过，则不更新；

步骤2.4.2：以蓝牙和GPS定位数据为空间信息，并结合时间信息修正静止状态下的定位。

优选地，所述步骤3中将导航线进行点集划分得到投影计算的参考点，利用滑动窗口将所述最终定位点投影至导航线上距离最终定位点最近的参考点包括如下步骤：

步骤3.1.1：将导航线进行划点得到投影计算的参考点，参考点按照起点到终点的顺序进行自增编号，从1开始，形成参考点集合；

步骤3.1.2：设置滑动窗口的大小为m，判断2*m是否大于参考点集合中最大编号，若大于，则不进行滑动窗口计算，否则跳至步骤3.1.3进行滑动窗口计算；

步骤3.1.3：以上一定位投影点为基准，计算在该点前后各m个范围内的投影参考点与定位点的距离；

步骤3.1.4：将所述最终定位点投影至导航线上距离最终定位点最近的参考点。

优选地，所述步骤3中根据最终定位点位置对上述投影的参考点进行弯道几何投影，获取投影点包括如下步骤：

步骤3.2.1：判断最终定位点是否在弯道判定三角形范围内，若在，则跳至步骤3.2.2；否则，不进行弯道几何投影，只进行滑动窗口投影；其中，弯道判定三角形，其以弯道两端点连线为底边，高长度为该连线长度的等腰三角形；两端点为弯道的两条边上距弯道拐点X米的两个点，X取值范围为5-10；

步骤3.2.2：计算定位点关于弯道判定三角形底边的对称点，根据对称点计算最近的投影参考点即投影点。

一种系统，包括

定位模块，用于获取蓝牙定位模块、惯性定位模块和GPS定位模块的定位数据，所述定位数据经过初始定位模块、自适应加权模块、卡尔曼滤波模块和修正模块进行定位解算，获取最终定位点；

导航模块，用于通过路径规划模块将导航线进行点集划分得到投影计算的参考点，通过导航处理模块利用滑动窗口和弯道几何投影将定位模块获取的最终定位点投影至导航线上的参考点，获取投影点；

地图显示模块，用于利用所述导航模块生成的投影点进行导航和地图显示。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

1.本发明采用自适应加权获得稳定的测量值，并采用卡尔曼滤波模型获得平滑的定位结果，同时进行基于距离的修正，获得稳定且准确的定位，避免了单信号定位精度较差、稳定性差且室内外无法平滑过渡的缺陷，兼具定位与导航功能，实现了平滑的室内外一体化定位导航；

2.本发明将各种信号根据该信号的相关特征进行赋权，避免了使用固定权重时，蓝牙定位误差较大或者GPS定位误差较大时影响定位的问题，实现多种定位信号的融合；

3.本发明的融合滤波模型在加入GPS定位数据时可做到室内外的一体化的平滑定位导航，避免了使用信号判断法造成的乒乓效应；

4.本发明在弯道使用的弯道几何投影方法，减少了弯道投影中切角现象的发生；利用滑动窗口计算投影点减少计算量。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本发明的方法流程图；

图2为本发明的方法中滑动窗口投影计算流程图；

图3为本发明的弯道投影示意图；

图4为本发明的定位点投影实例示意图；

图5为本发明的系统框图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明，即所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。

因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，术语“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以下结合实施例对本发明的特征和性能作进一步的详细描述。

实施例1

一种室内外一体化定位导航方法，包括如下步骤：

步骤1：获取GPS、蓝牙和惯性的定位数据后，利用所述定位数据进行初始定位；

步骤2：初始定位后的定位数据进行自适应加权、卡尔曼滤波和基于距离修正完成定位解算，获取最终定位点；

步骤3：将导航线进行点集划分得到投影计算的参考点，利用滑动窗口和弯道几何投影将所述最终定位点投影至导航线上的参考点，获取投影点；

步骤4：利用所述投影点进行导航与地图显示。

所述步骤1包括如下步骤：

步骤1.1：获取GPS、蓝牙和惯性的定位数据，所述GPS定位数据包括经纬度坐标数据P_gps与GPS定位精度a；所述蓝牙定位数据包括经纬度坐标数据P_bluetooth、定位计算数据d₀、与前置定位距离d与角度β；所述惯性定位数据包括包含经纬度坐标数据P_pedestrain；所述定位计算数据d₀为计算蓝牙定位使用KNN算法的过程中获得的前K个欧式距离的均值，与前置定位的距离d为当前蓝牙定位点与上一定位结果的距离，角度β为当前蓝牙定位点与当前轨迹角度差；

步骤1.2：判断前三秒内是否存在蓝牙定位，若存在，则将蓝牙定位数据的均值作为初始定位；否则，将GPS定位作为初始定位。

所述步骤2包括如下步骤：

步骤2.1：根据GPS、蓝牙的定位数据定义GPS、蓝牙和惯性的权重；所述步骤2.1中权重定义公式如下：

GPS权重WG：

蓝牙权重WB：

惯性定位权重WP：WP＝1-WB。

步骤2.2：根据所述GPS、蓝牙、惯性的权重和初始定位后GPS、蓝牙、惯性的定位数据计算定位测量值；

所述步骤2.2中定位测量值计算如下：

当GPS信号存在且满足条件，蓝牙定位和惯性定位同时存在时，测量值计算公式为：WG*P_gps+(1-WG)*(WB*P_blutooth+WP*P_pedestrain)；

当GPS信号存在且满足条件，存在惯性定位时，测量值的计算方式为：WG*P_gps+(1-WG)*P_pedestrain；

当无GPS信号或GPS信号不满足条件，有蓝牙定位和惯性定位时，测量值计算公式为：WB*P_bluetooth+WP*P_pedestrain；

当只有惯性定位时，测量值为P_pedestrain；

当无惯性定位时，判定其处于静止状态，定位不变，测量值为空。

步骤2.3：卡尔曼滤波器初始化，将所述初始定位输入卡尔曼滤波器进行预测获取预测值，将所述预测值与定位测量值进行更新；

所述步骤2.3中包括如下步骤：

步骤2.3.1：根据下列方程进行滤波预测：

P′_k＝AP_k-1A^T+Q

其中，为k-1时刻的状态向量，状态转移矩阵

P_k-1为k-1时刻的协方差矩阵，是元素值小于1的4维对角线矩阵，Q为预测误差方差矩阵，为

经预测方程得到的一个k时刻的预测状态向量，P′_k为P_k-1经预测方程得到的k时刻的预测协方差矩阵，T为滤波器执行的时间间隔；

的初始值为前述初始定位，P_k-1初始值为对角线元素小于1的对角阵；

步骤2.3.2：基于步骤2.3.1获取的预测值与所述定位测量值进行更新，协方差矩阵将替换上一时刻的协方差矩阵用于下一次的卡尔曼滤波，更新的公式如下：

κ＝P′_kH(HP′_kH^T+R)^-1

p_k＝(I-κHP′_k)

其中，测量估计矩阵

z_k为k时刻的测量值即步骤2.2中获得的定位测量值，κ为增益系数，R为测量误差方差矩阵，I为单位矩阵，

为计算的k时刻的滤波结果。

卡尔曼滤波为迭代过程，后一时刻需要用到前一时刻计算的值；这5个方程需顺序执行，首次滤波的预测阶段的2个方程组中，需给定

与P_k-1的初始值(状态向量

的初始值

为步骤1.2中所述初始定位坐标，在两个方向上的速度的初始值为0.000001，协方差矩阵P的初始值P₀是一个对角线上元素都为0.1的4维矩阵)，因为首次滤波时，

与P₀还未通过滤波模型生成，其后进行滤波时，

就是上一时刻的最终定位，P_k-1就是上一次滤波方程计算的结果。因此步骤2.3设置卡尔曼滤波器初始协方差矩阵，将之与步骤1.2中所述初始定位输入卡尔曼滤波器进行预测，再基于预测值与定位测量值进行更新，此后的协方差矩阵都将替换上一时刻的协方差矩阵用于下一次的卡尔曼滤波。

步骤2.4根据距离更新步骤2.3获取的最终定位数据，并修正静止状态下的定位数据，完成定位解算获取最终的定位点。此后的最终定位点都将成为下一次卡尔曼滤波器中预测阶段的输入定位。

步骤2.4：根据距离更新步骤2.3获取的最终定位数据，并修正静止状态下的定位数据，完成定位解算获取最终的定位点。

所述步骤2.4包括如下步骤：

步骤2.4.1：判断当前定位滤波点与上一定位点的距离是否超过X米，X范围为1.5-2，若超过，则将当前定位更新为(1-1.2/dl)*last_location+(1.2/dl)*current_location，其中，dl是当前滤波结果与上一定位点的距离，last_location为上一定位点，current_location为当前卡尔曼滤波结果；若未超过，则不更新；

步骤2.4.2：以蓝牙和GPS定位数据为空间信息，并结合时间信息修正静止状态下的定位。

静止状态下的定位纠正处理以蓝牙定位与GPS定位作为依据。当无惯性定位时，判断其处于静止状态，设置一个计时器，当静止时间大于1秒时，使用以下方法判断是否需要纠正当前定位：

1.设置一个计数器count_gps＝0，当GPS精度小于10且GPS定位点与当前点位距离大于等于10米时，count_gps＝count_gps+1；

2.设置一个计数器count_bluetooth＝0，当有3个蓝牙定位信号时，计算其均值定位，当均值定位与当前点距离大于等于10米时，count_bluetooth＝count_bluetooth+1；

3.当count_gps大于3且count_bluetooth＝0时，将此时的定位定位纠正到GPS定位处，清零count_gps；

4.当count_bluetooth大于3时，将此时的定位纠正到蓝牙定位处，清零count_bluetooth；

5.其余情况下，不纠正当前定位；

6.当由静止状态变为运动状态时，清零所有计数器。

所述步骤3中将导航线进行点集划分得到投影计算的参考点，利用滑动窗口将所述最终定位点投影至导航线上距离最终定位点最近的参考点包括如下步骤：

步骤3.1.1：将导航线进行划点得到投影计算的参考点，参考点按照起点到终点的顺序进行自增编号，从1开始，形成参考点集合；

步骤3.1.2：设置滑动窗口的大小为m，判断2*m是否大于参考点集合中最大编号，若大于，则不进行滑动窗口计算，否则跳至步骤3.1.3进行滑动窗口计算；

步骤3.1.3：以上一定位投影点为基准，计算在该点前后各m个范围内的投影参考点与定位点的距离；当计算第一个投影点时，只计算其后m个参考点。具体的，当最后投影参考点不足m个时，以编号最大值为边界；

步骤3.1.4：将所述最终定位点投影至导航线上距离最终定位点最近的参考点。

所述步骤3中根据最终定位点位置对上述投影的参考点进行弯道几何投影，获取投影点包括如下步骤：

步骤3.2.1：判断最终定位点是否在弯道判定三角形范围内，若在，则跳至步骤3.2.2；否则，不进行弯道几何投影，只进行滑动窗口投影；其中，弯道判定三角形，其以弯道两端点连线为底边，高长度为该连线长度的等腰三角形；两端点为弯道的两条边上距弯道拐点X米的两个点，X取值范围为5-10；

步骤3.2.2：计算定位点关于弯道判定三角形底边的对称点，根据对称点计算最近的投影参考点即投影点。

其中，以滑动窗口的方法计算投影点来减少计算量，首先判断是否需要进行滑动窗口计算，即当2*m(m为滑动窗口大小)的值比超出投影点集合范围时，不进行滑动窗口的处理，反之则进行滑动窗口处理。在进行滑动窗口处理时，以上一定位点为基准，计算其前后范围内的点，如图4所示。当计算第一个投影点时，计算定位点与前m个点的距离，以此类推，其后的点计算上一投影点前后各m个点的距离。当上一投影点的前范围或者后范围不足m个点时，以边界点作为计算范围。m取值与如何划分点集有关，需保证计算范围在投影点前后50米范围内，即m＝50*i，i为正整数。

为减少弯道投影的切角现象，使用以下几何方法投影，如图4所示，弯道IAJ处，当定位点G在三角形BCF内时，求其关于BC的对称点G’，此时u最小，H为最终投影点。

IAJ为某一弯道，其中B，C点为弯道弧线的起始点，弧线c为B，C，A点经贝塞尔曲线处理后获得，E为BC中点，其中EF＝BC且EF⊥BC，获得三角形BCF。

当定位点在三角形BCF范围内时，首先寻找其关于BC的对称点，以该对称点求相关投影点，若不在三角形BCF面内，则不进行弯道几何投影和进行滑动窗口投影。

导航包括获取路径数据，规划路径和导航处理；路径数据为地图中的点与线的数据，路径规划模块寻找最短的点与线的集合；导航处理模块将路径规划模块中规划的线路进行点集划分处理，将线路按照规则进行均匀划分点集，形成关于当前线路的投影点集合；划分点集的规则为：若为直线，1米划分成i个点，若为弯道，1米划分成2*i个点；i取值范围应在3～10之间；以滑动窗口的方法计算投影点来减少计算量；为减少弯道投影的切角现象，将获取的定位点按照规则投影到路线，具体规则为投影到距离定位点最近的点。

显示室内和室外电子地图，室内地图包括楼层信息和房间位置信息。

本申请在现有技术中惯性定位、蓝牙定位和GPS定位单独使用时存在的缺点，提出将惯性定位、蓝牙定位和GPS定位进行融合定位；融合定位有多种方法可以实现，例如卡尔曼，扩展卡尔曼，无迹卡尔曼，粒子滤波，神经网络等方法，虽然无迹卡尔曼，粒子滤波的融合效果较好，但是在实际应用中，由于算法较为复杂，计算量较大，在移动端特别是网页端中无法使用，达不到实时性的要求；而神经网络更为复杂，无法达到实时定位导航的效果。本申请需适用于小程序，小程序为网页形式，需要考虑网页性能。同时为了减少计算量，减少代码的复杂度，选择了最简单的卡尔曼滤波模型，使用卡尔曼滤波的定位所使用的是根据惯性定位计算方程建立状态转移方程，其他定位对其进行修正。面临的难题是蓝牙定位与WiFi定位不同，蓝牙定位因蓝牙信号广频率的原因获取的时间是不固定和不规律的，所以无法与惯性定位的时间进行对应，因此一般的惯性定位方程无法使用。本申请按一定时间间隔获取数据，在此时间间隔内有什么定位信号就用什么定位信号；具体为在状态转移方程中，滤波时按照一定时间间隔来计算的，每T秒计算一次，在T秒钟的所有的最新定位信号都加入加权计算；通过卡尔曼滤波模型和利用滑动窗口计算投影点减少计算量，满足实时性要求；通过弯道几何投影，减少了弯道投影中切角现象，提高定位精度。

综上，通过自适应加权的方式解决了各种信号定位的时间不匹配造成的难以设立卡尔曼滤波器方程的问题，使用卡尔曼滤波器获得一个较为稳定的定位，相比于单信号定位精度较差，稳定性差，且室内外无法平滑过渡的缺陷；本申请方法提高精度、稳定性的同时，进行后处理修正结果，精度相对于原始滤波结果精度得到进一步的提升，满足定位准确性要求与稳定性要求；在三者信号融合后，可实现室内外的平滑一体化定位导航，相比于使用信号判断的方式，本申请方法可避免在室内外入口的乒乓效应，满足稳定性要求。

本实施例中移动终端为一智能手机，用户于智能手机中使用本系统时，首先根据信号判断其处于室内还是室外，本实施例中，在前3秒有3个蓝牙定位信息，如下表所示：

序号	经度	纬度
			1	104.0607751564789	30.5972242069494
2	104.0607751873325	30.5972231284009
			3	104.0607752832883	30.59722278993765

那么根据以上三个定位计算经度和纬度的均值获得一个初始定位P1＝[104.060775209033，30.597223375096]，在本实施例中，对导航规划的路线进行点集划分和计算相应的投影点，以本实施例中一段具有弯道的路线为例，该路线划分如图4所示：

P1计算所得投影点为图4中P1’，而在T时间间隔后，此处T＝0.5秒。

在本实施例中，此时获得的蓝牙定位为[104.0607658320978，30.597223506549]，其KNN欧氏距离均值为4.5，与P1的距离为1.04米，此时与初始点无夹角，不进行夹角计算。那么此时的权重WB＝1/4.5+0.2＝0.42；此时惯性定位为[104.0607746764903，30.597224209448]，其权重为WP＝1-0.42＝0.58；此时的GPS定位为[104.061144，30.595201]，其精度为1070米，大于40米，则丢弃此数据。

根据自适应加权得到一个测量值

[104.0607709618455,30.59722391423042]＝[104.0607746764903,30.597224209448]*0.58+[104.0607658320978,30.597223506549]*0.42，将其代入卡尔曼滤波公式中，在本实施例中，令初始P＝0.1*I，I为单位矩阵，令初始速度(vx₀,vy₀)＝(10^-6，10^-6)，估计误差方差和测量误差方差矩阵分别为：

首先根据以下方程式

P′_k＝AP_k-1A^T+Q

计算获得

再根据以下方程计算定位值与更新协方差矩阵P：

κ＝P′_kH(HP′_kH^T+R)^-1

p_k＝(I-κHP′_k)

其中，测量值z₂＝[104.0607709618455,30.59722391423042]，

计算出z₂＝[-0.00000211007353811965,0.0000000173954255444642]；

求得增益系数

那么滤波结果

最后协方差矩阵

计算

与初始定位P1的距离为0.48米，判断其无需进行修正和后处理，那么此滤波值即为定位结果P2，投影点为图4中点P2’。注意此示意图中的点不依据真实坐标显示，只作为投影处理的示意图用。

本实施例中，存在用户在导航过程中静止的情况，当系统检测到用户无惯性定位输出时间超过1秒，此时时间戳为1541578051072，即判定用户处于静止状态，此时触发所述定位纠正流程，此时在室内GPS定位精度均超过10米，忽略GPS定位，从1541578051072时刻开始记录蓝牙定位，并计算其均值，注意此时蓝牙均值定位以滑动窗口的形式计算，即每次只计算最新的3个定位的均值。本实施例中，停留时间约为5秒，计算此5秒时间内的均值定位与当前定位的距离，其都小于10米，所以本实施例没有触发定位纠正，定位在电子地图的表现形式为处于同一位置静止。

实施例中，场景为从室内到室外，在室内到室外的定位导航过程中，存在GPS，蓝牙定位与惯性定位三种定位信号，依据所述加权方法得到测量值，进行卡尔曼滤波，实现室内到室外的平滑导航。

本实施例中，在室内到室外的过程中，时刻t的定位为[104.061090469665,30.59747875903]，从t+T时刻开始，同时存在蓝牙定位，GPS与惯性定位。蓝牙定位B＝[104.0610165120785,30.59749098622245]以及与当前轨迹的夹角β＝165度，所以此刻WB＝0.1；惯性定位P＝[104.061093469,30.5974776108]，此刻WP＝1-0.1＝0.9；符合条件(精度小于40米)的GPS定位G＝[104.061192147985,30.5974076812]及其精度a＝5，此时WG＝0.145；

那么此刻获得的加权测量值[104.0611075210097,30.59746756918166]＝WG*G+(1-WG)*(WB*B+WP*P)，按上述方法计算得到一个卡尔曼滤波结果[104.061100560065,30.59747383115984]，其与上一定位点的距离为1.29米，小于1.5米，不进行后处理，并以此定位值进行投影计算，获得在室内外导航线上的一个导航点。

本实施例中，将导航线与导航点绘制于包含有室内与室外地图信息的电子地图上，并且根据定位实时更新导航点。

实施例2

基于实施例1的方法，提供一种系统，所述系统包括处理器、存储器和存储在所述存储器中并可在处理器上运行的计算机程序“步骤1：获取GPS、蓝牙和惯性的定位数据后，利用所述定位数据进行初始定位；步骤2：初始定位后的定位数据进行自适应加权、卡尔曼滤波和基于距离修正完成定位解算，获取最终定位点；步骤3：将导航线进行点集划分得到投影计算的参考点，利用滑动窗口和弯道几何投影将所述最终定位点投影至导航线上的参考点，获取投影点；步骤4：利用所述投影点进行导航与地图显示。”，计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器中，并由所述处理器执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在系统中的执行过程。例如，如图5所示，本实施例的系统包括定位模块、导航模块和地图显示模块；定位模块包括蓝牙定位模块、惯性定位模块、GPS定位模块、初始定位模块、自适应加权模块、卡尔曼滤波模块和修正模块；定位模块，用于获取蓝牙定位模块、惯性定位模块和GPS定位模块的定位数据，所述定位数据经过初始定位模块、自适应加权模块、卡尔曼滤波模块和修正模块进行定位解算，获取最终定位点；导航模块，用于通过路径规划模块将导航线进行点集划分得到投影计算的参考点，通过导航处理模块利用滑动窗口和弯道几何投影将定位模块获取的最终定位点投影至导航线上的参考点，获取投影点；地图显示模块，用于利用所述导航模块生成的投影点进行导航和地图显示。

所述系统可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述系统可包括，但不仅限于，处理器、存储器。本领域技术人员可以理解，所述示意图仅仅是所述系统的示例，并不构成对所述系统的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述系统设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述分辨率提升系统的控制中心，利用各种接口和线路连接整个分辨率提升系统的各个部分。

所述存储器可用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述分辨率提升系统的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

综上，本系统的定位模块获取蓝牙定位模块、惯性定位模块和GPS定位模块的定位数据，所述定位数据经过初始定位模块、自适应加权模块、卡尔曼滤波模块和修正模块进行定位解算，获取最终定位点；导航模块将导航线进行点集划分，通过导航处理模块利用滑动窗口和弯道几何投影将定位模块获取的最终定位点投影至导航线上的参考点，获取投影点，地图显示模块利用投影点进行导航和地图显示，实现平滑的室内外一体化定位导航；通过自适应加权的方式解决了各种信号定位的时间不匹配造成的难以设立卡尔曼滤波器方程的问题，使用卡尔曼滤波器获得一个较为稳定的定位，相比于单信号定位精度较差，稳定性差，且室内外无法平滑过渡的缺陷；本申请方法提高精度、稳定性的同时，进行后处理修正结果，精度相对于原始滤波结果精度得到进一步的提升，满足定位准确性要求与稳定性要求；在三者信号融合后，可实现室内外的平滑一体化定位导航，相比于使用信号判断的方式，本申请方法可避免在室内外入口的乒乓效应，满足稳定性要求。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种室内外一体化定位导航方法，其特征在于：包括如下步骤：

步骤1：获取GPS、蓝牙和惯性的定位数据后，利用所述定位数据进行初始定位；

步骤2：初始定位后的定位数据进行自适应加权、卡尔曼滤波和基于距离修正完成定位解算，获取最终定位点；

步骤3：将导航线进行点集划分得到投影计算的参考点，利用滑动窗口和弯道几何投影将所述最终定位点投影至导航线上的参考点，获取投影点；

步骤4：利用所述投影点进行导航与地图显示。

2.根据权利要求1所述的一种室内外一体化定位导航方法，其特征在于：所述步骤1包括如下步骤：

步骤1.1：获取GPS、蓝牙和惯性的定位数据，所述GPS定位数据包括经纬度坐标数据P_gps与GPS定位精度a；所述蓝牙定位数据包括经纬度坐标数据P_bluetooth、定位计算数据d₀、与前置定位距离d与角度β；所述惯性定位数据包括包含经纬度坐标数据P_pedestrain；所述定位计算数据d₀为计算蓝牙定位使用KNN算法的过程中获得的前K个欧式距离的均值，与前置定位的距离d为当前蓝牙定位点与上一定位结果的距离，角度β为当前蓝牙定位点与当前轨迹角度差；

步骤1.2：判断前三秒内是否存在蓝牙定位，若存在，则将蓝牙定位数据的均值作为初始定位；否则，将GPS定位作为初始定位。

3.根据权利要求1所述的一种室内外一体化定位导航方法，其特征在于：所述步骤2包括如下步骤：

步骤2.1：根据GPS、蓝牙的定位数据定义GPS、蓝牙和惯性的权重；

步骤2.2：根据所述GPS、蓝牙、惯性的权重和初始定位后GPS、蓝牙、惯性的定位数据计算定位测量值；

步骤2.3：卡尔曼滤波器初始化，将所述初始定位输入卡尔曼滤波器进行预测获取预测值，将所述预测值与定位测量值进行更新；

步骤2.4：根据距离更新步骤2.3获取的最终定位数据，并修正静止状态下的定位数据，完成定位解算获取最终的定位点。

4.根据权利要求3所述的一种室内外一体化定位导航方法，其特征在于：所述步骤2.1中权重定义公式如下：

GPS权重WG：

蓝牙权重WB：

惯性定位权重WP：WP＝1-WB。

5.根据权利要求3所述的一种室内外一体化定位导航方法，其特征在于：所述步骤2.2中定位测量值计算如下：

当GPS信号存在且满足条件，蓝牙定位和惯性定位同时存在时，测量值计算公式为：WG*P_gps+(1-WG)*(WB*P_blutooth+WP*P_pedestrain)；

当GPS信号存在且满足条件，存在惯性定位时，测量值的计算方式为：WG*P_gps+(1-WG)*P_pedestrain；

当无GPS信号或GPS信号不满足条件，有蓝牙定位和惯性定位时，测量值计算公式为：WB*P_bluetooth+WP*P_pedestrain；

当只有惯性定位时，测量值为P_pedestrain；

当无惯性定位时，判定其处于静止状态，定位不变，测量值为空。

6.根据权利要求3所述的一种室内外一体化定位导航方法，其特征在于：所述步骤2.3中包括如下步骤：

步骤2.3.1：根据下列方程进行滤波预测：

P′_k＝AP_k-1A^T+Q

其中，

为k-1时刻的状态向量，状态转移矩阵

P_k-1为k-1时刻的协方差矩阵，是元素值小于1的4维对角线矩阵，Q为预测误差方差矩阵，

为

经预测方程得到的一个k时刻的预测状态向量，P′_k为P_k-1经预测方程得到的k时刻的预测协方差矩阵，T为滤波器执行的时间间隔，

的初始值为前述初始定位，P_k-1初始值为对角线元素小于1的对角阵；

步骤2.3.2：基于步骤2.3.1获取的预测值与所述定位测量值进行更新，更新获得的协方差矩阵将用于下一次的卡尔曼滤波，更新的公式如下：

κ＝P′_kH(HP′_kH^T+R)^-1

p_k＝(I-κHP_k′)

其中，测量估计矩阵

z_k为k时刻的测量值即步骤2.2中获得的定位测量值，κ为增益系数，R为测量误差方差矩阵，I为单位矩阵，

为计算的k时刻的滤波结果。

7.根据权利要求3所述的一种室内外一体化定位导航方法，其特征在于：所述步骤2.4包括如下步骤：

步骤2.4.1：判断当前定位滤波点与上一定位点的距离是否超过X米，X取值范围为1.5-2，若超过，则将当前定位更新为(1-1.2/dl)*last_location+(1.2/dl)*current_location，其中，dl是当前滤波结果与上一定位点的距离，last_location为上一定位点，current_location为当前卡尔曼滤波结果；若未超过，则不更新；

步骤2.4.2：以蓝牙和GPS定位数据为空间信息，并结合时间信息修正静止状态下的定位。

8.根据权利要求1所述的一种室内外一体化定位导航方法，其特征在于：所述步骤3中将导航线进行点集划分得到投影计算的参考点，利用滑动窗口将所述最终定位点投影至导航线上距离最终定位点最近的参考点包括如下步骤：

步骤3.1.1：将导航线进行划点得到投影计算的参考点，参考点按照起点到终点的顺序进行自增编号，从1开始，形成参考点集合；

步骤3.1.2：设置滑动窗口的大小为m，判断2*m是否大于参考点集合中最大编号，若大于，则不进行滑动窗口计算，否则跳至步骤3.1.3进行滑动窗口计算；

步骤3.1.3：以上一定位投影点为基准，计算在该点前后各m个范围内的投影参考点与定位点的距离；

步骤3.1.4：将所述最终定位点投影至导航线上距离最终定位点最近的参考点。

9.根据权利要求8所述的一种室内外一体化定位导航方法，其特征在于：所述步骤3中根据最终定位点位置对上述投影的参考点进行弯道几何投影，获取投影点包括如下步骤：

步骤3.2.1：判断最终定位点是否在弯道判定三角形范围内，若在，则跳至步骤3.2.2；否则，不进行弯道几何投影，只进行滑动窗口投影；其中，弯道判定三角形，其是以弯道两端点连线为底边，高长度为该连线长度的等腰三角形；两端点为弯道的两条边上距弯道拐点X米的两个点，X取值范围为5-10；

步骤3.2.2：计算定位点关于弯道判定三角形底边的对称点，根据对称点计算最近的投影参考点即投影点。

10.一种基于权利要求1所述方法的系统，其特征在于：包括

定位模块，用于获取蓝牙定位模块、惯性定位模块和GPS定位模块的定位数据，所述定位数据进行初始定位模块、自适应加权模块、卡尔曼滤波模块和修正模块进行定位解算，获取最终定位点；

导航模块，用于通过路径规划模块将导航线进行点集划分得到投影计算的参考点，通过导航处理模块利用滑动窗口和弯道几何投影将定位模块获取的最终定位点投影至导航线上的参考点，获取投影点；

地图显示模块，用于利用所述导航模块生成的投影点进行导航和地图显示。

Images