CN107167130A - 地图匹配定位方法及系统 - Google Patents

Abstract

本发明提供了一种地图匹配定位方法及系统，包括：步骤1：由惯性导航系统提供检测到的运动轨迹，以及由数字地图匹配导航系统提供数字地图中的路网信息；步骤2：经过地图匹配算法进行轨迹匹配，最终输出融合估计后的导航位置，以及误差修正信息。其中，所述误差修正信息用于修正惯性导航系统，形成闭环反馈。本发明将惯性导航数据与地图数据结合起来，大大提高了惯性导航系统的定位精度，解决了惯性导航系统在随时间累积的位置漂移问题，增强了惯性导航系统长时间定位的可靠性。

Description

地图匹配定位方法及系统

技术领域

本发明涉及综合定位技术领域，具体地，涉及地图匹配定位方法及系统。

背景技术

现代的导航方法和导航系统多种多样，但每一种导航方法都有各自的优点和缺点，各自擅长应用的领域也不尽相同。随着现代科学技术的不断发展，导航技术也逐渐向着综合化，智能化的方向发展。

惯性导航系统是以三轴陀螺仪和三轴加速度计为敏感元件的导航解算系统。根据陀螺仪的输出可以构建出导航系统的导航坐标系，而根据加速度计输出的比力可以通过积分来解算出载体在导航坐标系中的速度以及位置。惯性导航系统以牛顿力学为理论基础，可以对载体自主地进行方位解算，因此它完全不依赖于外界的声、光、电以及磁等传播信号，也使得它不受地域限制，以及自然环境和人为因素的影响，隐蔽性好。这是其他包括天文导航，地球物理导航，无线电导航以及卫星导航等均无法实现的。因此惯性导航系统作为一种完全自主的，全信息输出(包括位置、姿态、速度信息)的导航系统，在陆、海、空、宇各个领域均有广泛的应用。但由于惯性导航系统是靠积分原理来对载体的速度、位置进行解算的，因此惯导系统的输出误差会随时间而积累，长时间使用会出现很大的偏差。

因此，如何提供一种辅助定位的技术，修正惯性导航系统随时间累积的偏差，实现综合定位及智能定位，是本发明需要解决的问题。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种地图匹配定位方法及系统。

根据本发明提供的地图匹配定位方法，包括如下步骤：

将惯性导航系统提供检测到的运动轨迹和数字地图匹配导航系统提供的数字地图中的路网信息进行轨迹匹配，输出融合估计后的导航位置，以及误差修正信息，其中，所述误差修正信息用于修正惯性导航系统，从而形成闭环反馈。

优选地，所述惯性导航系统包括：陀螺仪、加速度计、磁力计、气压计、温度计，用于输出载体的水平位置信息和高度位置信息，并形成载体运动轨迹，其中所述载体包括：行人、车辆、智能机器人、以及其他室内外移动物体。

优选地，数字地图匹配导航系统提供的数字地图是指：将二维或三维的实物地图以数字化的方式进行存储，所述数字地图包括：室内地图、室外地图，数字地图中的信息包括：路径信息、障碍物信息、高度信息、地形信息。

优选地，通过地图匹配算法进行轨迹匹配，所述地图匹配算法包括：寻找载体当前移动的路径，将当前定位点投影到载体移动的路径上。

优选地，还包括惯性导航系统的运动轨迹解算步骤，具体地，所述惯性导航系统的运动轨迹解算步骤包括如下步骤：

姿态解算步骤：由陀螺仪检测到的姿态初值及姿态角速率获得姿态角信息；

速度解算步骤：由加速度计输出的比力，经过哥式加速度和重力加速度的修正获得载体相对运动的加速度，通过积分运算求得地理坐标下的速度；

位置解算步骤：通过已知的载体在地理坐标系下的速度、加速度信息，以及初始位置获得载体参考位置信息。

优选地，还包括数字地图匹配导航系统的路网信息获取步骤，具体地，所述数字地图匹配导航系统的路网信息获取步骤包括以下步骤：

区域提取步骤：根据惯性导航系统提供的参考位置及位置误差范围提取数字地图数据库中导航区域内的数据，形成基准地图，将所述基准地图作为匹配算法的参考源；

数字地图匹配步骤：基于基准地图，根据搜索、比较、寻优的过程确定载体信息，所述载体信息包括：载体在基准地图中的位置、前进方向信息；

导航解算步骤：根据坐标变换关系，确定载体参数，所述载体的参数包括：载体的地理位置、航向角，进而对惯性导航系统的误差进行校正。

优选地，所述区域提取步骤包括：根据载体运动的连续特性，以一定量度的步长，将整个路径网络分成若干个网格，为每个网格设置唯一的索引号，且各自都包括相应的内部的路径信息。

优选地，所述数字地图匹配步骤包括：利用匹配规则对候选路径进行匹配，其中，所述匹配规则包括：定位点到候选路径的投影距离、载体当前移动方向与候选路径方向的夹角、以及候选路径与上一匹配路径的投影距离。

根据本发明提供的地图匹配定位系统，包括：惯性导航系统、数字地图匹配导航系统，以及轨迹匹配模块，其中：

所述惯性导航系统包括：陀螺仪、加速度计、磁力计、气压计、温度计，用于输出载体的水平位置信息和高度位置信息，并形成载体运动轨迹；其中，所述载体包括：行人、车辆、智能机器人、以及其他室内外移动物体；

所述数字地图匹配导航系统，用于提供的数字地图中的路网信息；

所述轨迹匹配模块，用于将惯性导航系统提供检测到的运动轨迹和数字地图匹配导航系统提供的数字地图中的路网信息进行轨迹匹配，输出融合估计后的导航位置，以及误差修正信息，其中，所述误差修正信息用于修正惯性导航系统，从而形成闭环反馈。

优选地，通过地图匹配算法进行轨迹匹配，所述地图匹配算法包括：寻找载体当前移动的路径，将当前定位点投影到载体移动的路径上。

与现有技术相比，本发明具有如下的有益效果：

1、本发明将惯性导航数据与地图数据结合起来，给惯性导航系统装载了地理环境信息，极大扩充了惯性导航的实际应用范围。

2、本发明大大提高了惯性导航系统的定位精度，解决了惯性导航系统在随时间累积的位置漂移问题，增强了惯性导航系统长时间定位的可靠性。

3、本发明的方法中采用了多种形式的算法，优化了地图匹配的准确性，即使在匹配过程中出现错误，随着载体轨迹的继续生成，依然可以快速纠正，得到正确的位置。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明中提供的地图匹配定位方法的原理图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

根据本发明提供的地图匹配定位方法，包括：将惯性导航系统提供检测到的运动轨迹和数字地图匹配导航系统提供的数字地图中的路网信息经由地图匹配算法进行轨迹匹配，最终输出融合估计后的导航位置，以及误差修正信息，其中，所述误差修正信息用于修正惯性导航系统，从而形成闭环反馈。

具体地，惯性导航系统包括：陀螺仪、加速度计、磁力计、气压计、温度计，用于输出载体的水平位置信息和高度位置信息，并形成载体运动轨迹。数字地图匹配导航系统提供的数字地图包括：室内地图、室外地图。具体地，数字地图是指：将二维或三维的实物地图以数字化的方式进行存储。数字地图中的信息包括：路径信息、障碍物信息、高度信息、地形信息；并且在数字地图上划分出可连通区域和非连通区域作为规则参与计算。

所述地图匹配算法包括：寻找载体当前移动的路径，将当前定位点投影到载体移动的路径上。

具体地，惯性导航系统的运动轨迹解算包括以下步骤：

姿态解算步骤：由陀螺仪检测到的姿态初值及姿态角速率获得姿态角信息；

速度解算步骤：由加速度计输出的比力，经过哥式加速度和重力加速度的修正获得载体相对运动的加速度，通过积分运算求得地理坐标下的速度；

位置解算步骤：通过已知的载体在地理坐标系下的速度、加速度信息，以及初始位置获得载体参考位置信息。

所述惯性导航系统中的载体包括：行人、车辆、智能机器人、以及其他室内外移动物体。

具体地，数字地图匹配导航系统的路网信息获取包括以下步骤：

区域提取步骤：根据惯性导航系统提供的参考位置及位置误差范围提取数字地图数据库中导航区域内的数据，形成基准地图，将所述基准地图作为匹配算法的参考源；

数字地图匹配步骤：基于基准地图，根据搜索、比较、寻优的过程确定载体在基准地图中的位置、前进方向等信息；

导航解算步骤：根据坐标变换关系，确定载体的地理位置、航向角等导航参数，进而对惯性导航系统的误差进行校正。

更进一步地，所述区域提取步骤包括：根据载体运动的连续特性，以一定量度的步长，将整个路径网络分成若干个网格，为每个网格设置唯一的索引号，且各自都包括其内部的路径信息。

所述数字地图匹配步骤包括：利用匹配规则对候选路径进行匹配，其中，所述匹配规则主要包括：定位点到候选路径的投影距离、载体当前移动方向与候选路径方向的夹角、以及候选路径与上一匹配路径的投影距离。

下面结合说明书附图和具体实施例对本发明中的技术方案做更加详细的说明。

本发明提供的地图匹配定位方法的基本原理图如图1所示，包括：惯性导航系统、导航电子地图、地图匹配算法、融合估计，导航位置输出。其中，数字地图匹配导航系统包括导航电子地图、地图匹配算法、融合估计、导航位置输出。

本发明优选的行人室内定位实施例包括以下步骤：

步骤S1：通过惯性导航系统获取载体的水平位置信息、高度位置信息，并通过姿态解算、速度解算、位置解算，形成载体运动轨迹信息；具体地，所述惯性导航系统中的传感器包括：陀螺仪、加速度计、磁力计、气压计、温度计。

其中，所述姿态解算的微分方程为式中，为载体坐标系相对于导航坐标系的转动角速率，为导航坐标系和类机体坐标系的转换矩阵。速度解算的微分方程为式中，fⁿ为比力在导航坐标系下的投影，gⁿ为地球重力加速度矢量，为导航坐标系相对于地球坐标系的转动角速率，为地球坐标系相对于地心惯性坐标系的转动角速率在导航坐标系上的投影，Vⁿ表示载体在导航坐标系下的速度值。

位置解算通过具体的经纬度信息和高度信息计算，公式如下：

式中，L表示载体所在位置的纬度值，V_y表示载体在y轴的速度分量，V_x表示载体在x轴的速度分量，R表示地球半径，h表示载体所在位置的高度值，cos表示余弦运算，λ表示载体所在位置的经度值。

步骤S2：通过地图匹配导航系统进行区域提取、地图匹配算法以及导航解算；

其中，区域提取是指：根据惯性导航系统提供的参考位置及位置误差范围，提取地图数据库中导航区域内的数据，形成基准地图，作为匹配算法的参考源。所述基准地图中的每一条二维线段为一条路径，路径之间的交叉点为节点，系统后台根据传感器位置为所述路径和节点指定唯一ID、权重值、以及载体是否在当前路径。所述基准地图的网络结构同时包含地图比例、坐标方向、路径角度信息。所述地图匹配算法包括匹配初始化、在路径上匹配、在节点处匹配。

具体地，匹配初始化时，主要考虑行进方向和距离两方面因素，来求出总权重值最大的路径。其中，行进方向代表的是载体的移动方向，通常由智能移动终端传感器提供，行走方向与路径方向越接近，该路径越有可能是正确路径。所述距离指当前匹配的估计位置点到路径的垂直距离，同样地，这一距离值越小，该路径越有可能是正确路径。

可以用匹配初始化公式表示以上关系：

式中，W₀表示t＝0时的总权重值，β表示传感器方向与路径的夹角。dis表示待匹配点到路径的距离，dis_max表示待匹配点到路径的最大距离，c_h、c_d分别表示在方向、距离上的加权系数。

在路径上匹配时，主要参考两点间方向和路径的上一次权重值，来判断匹配结果的正确性。所述两点间方向主要是指上一估计位置点与当前待匹配估计位置点连成一线段，该线段与路径之间的夹角，夹角越小，该路径越有可能是正确路径。所述路径的上一次权重值是指在计算t时刻时的总权重指W_t时，同时也考虑路径上一时刻t-1时刻的权重值W_t-1，如果两者差值较大，则可能出现匹配错误，需重新选择当前路径。

可以用在路径上匹配计算公式表示以上关系：

式中，c_l、c_n分别表示上一时刻和当前计算值的系数，β₁表示行走方向与路径方向的夹角，β₂表示连续两个估计位置点方向与路径方向的夹角，dis表示待匹配点到路径的距离，而c_h、c_a、c_d依次代表行走方向权重、两点间方向权重、距离权重的加权系数。

其中，在节点处匹配是指：当载体行至节点附近时，载体可能将要更换路径，应该要重新考虑所有因素，加入新的路径信息，重新选择正确的当前路径。

所述导航解算是指：利用Kalman滤波技术实现INS/MM(惯性导航和地图匹配)组合导航系统的过程。在滤波算法中，选取惯性导航指示误差作为状态变量：

式中，δL_INS,δλ_INS,δh_INS表示惯性导航的经度误差、纬度误差、以及高度误差，φ_x,φ_y,φ_z表示对应坐标轴X、坐标轴Y、坐标轴Z的转动角，δV_x,δV_y,δV_z表示载体在对应坐标轴X、坐标轴Y、坐标轴Z的运动速度误差，δφ_x,δφ_y,δφ_z表示导航坐标系与计算坐标系之间关于坐标轴X、坐标轴Y、坐标轴Z的姿态失准角，ε_x,ε_y,ε_z表示陀螺仪在载体坐标系上关于坐标轴X、坐标轴Y、坐标轴Z的分量，加速度计零偏等效在导航坐标系上坐标轴X、坐标轴Y、坐标轴Z的分量。

选取惯性导航输出位置和地图匹配算法得到的匹配位置之差作为观测量z，其计算公式为：z＝(△L,△λ)^T＝(L_INS-L_MM,λ_INS-λ_MM)^T，式中，L,λ分别表示纬度和经度，下标INS,MM分别表示惯性导航算法和地图匹配算法。

所述Kalman滤波技术适用的系统模型如下：

式中，X_k是系统的n维状态量，Z_k是系统的m维观测量，W_k-1是系统p维过程噪声向量，V_k是系统m维观测噪声向量，Φ_k,k-1是系统n×n维状态转移矩阵，Γ_k,k-1是系统的n×p维噪声输入矩阵，H_k是m×n维系统观测矩阵。

步骤S3：反馈误差校正，如图1所示，步骤二所得出的误差修正信息将返回惯性导航系统，对该系统进行校正，形成闭环控制。

本发明地图匹配定位方法，与现有技术相比，本发明将惯性导航数据与地图数据结合起来，给惯性导航系统装载了地理环境信息，极大扩充了惯性导航的实际应用意义；本发明大大提高了惯性导航系统的定位精度，解决了惯性导航系统在随时间累积的位置漂移问题，增强了惯性导航系统长时间定位的可靠性；本发明采用多种形式的算法，优化了地图匹配的准确性，即使在匹配过程中出现错误，随着载体轨迹的继续生成，依然可以快速纠正，得到正确的位置。

需要说明的是，本发明提供的所述地图匹配定位方法中的步骤，可以利用所述地图匹配定位系统中对应的模块、装置、单元等予以实现，本领域技术人员可以参照所述系统的技术方案实现所述方法的步骤流程，即，所述系统中的实施例可理解为实现所述方法的优选例，在此不予赘述。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种地图匹配定位方法，其特征在于，包括如下步骤：

将惯性导航系统提供检测到的运动轨迹和数字地图匹配导航系统提供的数字地图中的路网信息进行轨迹匹配，输出融合估计后的导航位置，以及误差修正信息，其中，所述误差修正信息用于修正惯性导航系统，从而形成闭环反馈。

2.根据权利要求1所述的地图匹配定位方法，其特征在于，所述惯性导航系统包括：陀螺仪、加速度计、磁力计、气压计、温度计，用于输出载体的水平位置信息和高度位置信息，并形成载体运动轨迹，其中所述载体包括：行人、车辆、智能机器人、以及其他室内外移动物体。

3.根据权利要求1所述的地图匹配定位方法，其特征在于，数字地图匹配导航系统提供的数字地图是指：将二维或三维的实物地图以数字化的方式进行存储，所述数字地图包括：室内地图、室外地图，数字地图中的信息包括：路径信息、障碍物信息、高度信息、地形信息。

4.根据权利要求1至3中任一项所述的地图匹配定位方法，其特征在于，通过地图匹配算法进行轨迹匹配，所述地图匹配算法包括：寻找载体当前移动的路径，将当前定位点投影到载体移动的路径上。

5.根据权利要求1所述的地图匹配定位方法，其特征在于，还包括惯性导航系统的运动轨迹解算步骤，具体地，所述惯性导航系统的运动轨迹解算步骤包括如下步骤：

姿态解算步骤：由陀螺仪检测到的姿态初值及姿态角速率获得姿态角信息；

速度解算步骤：由加速度计输出的比力，经过哥式加速度和重力加速度的修正获得载体相对运动的加速度，通过积分运算求得地理坐标下的速度；

位置解算步骤：通过已知的载体在地理坐标系下的速度、加速度信息，以及初始位置获得载体参考位置信息。

6.根据权利要求1所述的地图匹配定位方法，其特征在于，还包括数字地图匹配导航系统的路网信息获取步骤，具体地，所述数字地图匹配导航系统的路网信息获取步骤包括以下步骤：

区域提取步骤：根据惯性导航系统提供的参考位置及位置误差范围提取数字地图数据库中导航区域内的数据，形成基准地图，将所述基准地图作为匹配算法的参考源；

数字地图匹配步骤：基于基准地图，根据搜索、比较、寻优的过程确定载体信息，所述载体信息包括：载体在基准地图中的位置、前进方向信息；

导航解算步骤：根据坐标变换关系，确定载体参数，所述载体的参数包括：载体的地理位置、航向角，进而对惯性导航系统的误差进行校正。

7.根据权利要求6所述的地图匹配定位方法，其特征在于，所述区域提取步骤包括：根据载体运动的连续特性，以一定量度的步长，将整个路径网络分成若干个网格，为每个网格设置唯一的索引号，且各自都包括相应的内部的路径信息。

8.根据权利要求6所述的地图匹配定位方法，其特征在于，所述数字地图匹配步骤包括：利用匹配规则对候选路径进行匹配，其中，所述匹配规则包括：定位点到候选路径的投影距离、载体当前移动方向与候选路径方向的夹角、以及候选路径与上一匹配路径的投影距离。

9.一种地图匹配定位系统，其特征在于，包括：惯性导航系统、数字地图匹配导航系统，以及轨迹匹配模块，其中：

所述惯性导航系统包括：陀螺仪、加速度计、磁力计、气压计、温度计，用于输出载体的水平位置信息和高度位置信息，并形成载体运动轨迹；其中，所述载体包括：行人、车辆、智能机器人、以及其他室内外移动物体；

所述数字地图匹配导航系统，用于提供的数字地图中的路网信息；

所述轨迹匹配模块，用于将惯性导航系统提供检测到的运动轨迹和数字地图匹配导航系统提供的数字地图中的路网信息进行轨迹匹配，输出融合估计后的导航位置，以及误差修正信息，其中，所述误差修正信息用于修正惯性导航系统，从而形成闭环反馈。

10.根据权利要求9所述的地图匹配定位系统，其特征在于，通过地图匹配算法进行轨迹匹配，所述地图匹配算法包括：寻找载体当前移动的路径，将当前定位点投影到载体移动的路径上。