CN115326074A - 一种地上地下统一导航方法及装置 - Google Patents

Abstract

本发明实施例涉及一种地上地下统一导航方法及装置，该方法包括步骤：建立地上地下统一坐标系；基于统一坐标系进行地下路径规划；对所规划的路径进行可视化表达。本发明实施例的技术方案，通过将不同坐标系定位进行融合转换至2000国家大地坐标系而建立绝对坐标系，仅需一可用角向量便可进行较精确的定位，在特殊条件下可用度更高；同时减少了除开始定位和新系统信号参与定位时的运算量，降低了设备功耗；在新系统加入时，也更易从现有定位基础上将原系统相对坐标进一步转化为绝对坐标，从而实现定位系统间无缝切换。针对无法接收外界信号，无法配置基站的特殊区域可以通过图像信息，NFC标签等确定绝对坐标，从而进行校准和优化终端定位。

Description

一种地上地下统一导航方法及装置

技术领域

本发明实施例涉及导航定位和位置服务技术领域，尤其涉及一种地上地下统一导航方法及装置。

背景技术

随着移动物联网设备的日益普及完善,对于工业智能化的需求也逐渐由“能用够用”转变为“易用，高效，可拓展”。

其中，作为最早普及开来的导航系统稳定性，精度仍受限于卫星信号强度影响。在地形复杂区域，室内场所，特殊工作场合等信号不佳的场景下会存在难以定位无法定位的情况。现有解决该问题的方案多借助于不同定位方式，使用两套及以上的系统进行定位，这就导致终端收到多套定位坐标，随时需要转换才能确定定位位置。此问题造成了运算量的加大和多套系统定位运算量大幅增长的问题。同时，在终端设备在短暂失去信号情况下难以通过上一有效坐标大致计算出当下坐标。这些问题在特殊工作环境如矿山，地下交通等信号环境较差的场所中对终端电量，定位稳定性都是很大的挑战，在意外情况发生时定位系统不能正常工作的风险较高。

发明内容

基于现有技术的上述情况，本发明实施例的目的在于提供一种地上地下统一导航方法及装置，通过在建立统一坐标系的基础上实现地上地下无缝定位进而进行地上地下路径网络构建与连接，实现了定位系统间的无缝切换，提高了地上地下统一导航的精确度和稳定性。

为达到上述目的，根据本发明的一个方面，提供了一种地上地下统一导航方法，包括步骤：

建立地上地下统一坐标系；

基于统一坐标系进行地下路径规划；

对所规划的路径进行可视化表达。

进一步的，建立地上地下统一坐标系包括：

获取室外第一坐标系的三维坐标；

将室外第一坐标系转换为第二坐标系，该第二坐标系为绝对坐标系；

计算地上地下衔接区域，待定位点在第二坐标系下的绝对坐标；

计算待定位点在地下区域时在第二坐标系下的绝对坐标。

进一步的，所述计算地上地下衔接区域，待定位点在第二坐标系下的绝对坐标包括：

通过待定位点的加速度和角速度，计算待定位点的惯性位置变化量；

根据待定位点的先前惯性位置坐标和惯性位置变化量得到待定位点在第二坐标下的绝对坐标。

进一步的，所述计算待定位点在地下区域时在第二坐标系下的绝对坐标包括：

获取每个地上地下衔接点在第二坐标系下的绝对坐标；

计算待定位点到主基站的距离和距离主基站的角度；

根据所述距离和角度计算待定位点在第二坐标系下的坐标；

根据待定位点每次移动发出信号的时间戳，计算待定位点在时间维度的标记；

基于所述待定位点的坐标和待定位点在时间维度的标记获得待定位点的第二坐标系下的绝对坐标。

进一步的，所述建立地上地下统一坐标系还包括：

针对地上地下衔接区域待定位点在第二坐标系下的绝对坐标进行误差校准。

进一步的，所述基于统一坐标系进行地下路径规划包括：

基于统一坐标系构建地下路径网络；

在地下路径网络中，针对起点为地上或地下区域，终点在地下或地上区域的导航任务计算最短路径。

进一步的，所述构建地下路径网络包括：

将地下设施抽象为多面体，获取多面体的顶点在所述统一坐标系下的坐标；

在该多面体解决地下地面区域中选取若干点作为元素点，在可通行的区域选择若干点作为第一路径点，在地上地下衔接区域附近地上区域中选择若干点作为第二路径点；

连接所有元素点和路径点，形成路径网络拓扑图；

根据第一路径点和第二路径点的连接情况，将路径点分为连通路径点和非连通路径点。

进一步的，所述计算最短路径包括：

获取多个地下设施的多面体坐标集合；

判断终点是否位于任一多面体内，若是，则锁定终点所在的第一多面体；若否，则在第一路径点数组中寻找距离中点最近的路径点作为终点；

分别计算起点和终点距离连通路径点的最短距离、以及起点到终点的最短距离；

标记起点到终点的最短距离中所有经过的点并连线后得到起点至终点的最短路径。

根据本发明的另一个方面，提供了一种地上地下统一导航装置，包括：

统一坐标系建立模块，用于建立地上地下统一坐标系；

地下路径规划模块，用于基于统一坐标系进行地下路径规划；

可视化表达模块，用于对所规划的路径进行可视化表达。

综上所述，本发明实施例提供了一种地上地下统一导航方法及装置，该方法包括步骤：建立地上地下统一坐标系；基于统一坐标系进行地下路径规划；对所规划的路径进行可视化表达。本发明实施例的技术方案，通过将不同坐标系定位进行融合转换至2000国家大地坐标系而建立绝对坐标系，仅需一可用角向量便可进行较精确的定位，在特殊条件下可用度更高；同时减少了除开始定位和新系统信号参与定位时的运算量，降低了设备功耗；在新系统加入时，也更易从现有定位基础上将原系统相对坐标进一步转化为绝对坐标，从而实现定位系统间无缝切换。针对无法接收外界信号，无法配置基站的特殊区域可以通过图像信息，NFC标签等确定绝对坐标，从而进行进一步校准和优化终端定位。本发明具有如下有益的技术效果：弥补了现有技术地上地下导航无法一体化定位，以及地上地下交互区域同时存在两种定位数据，两种数据的精度由于受到环境影响而切换失误大的缺点。通过惯导技术与统一绝对坐标系的建立，保证地上地下定位精准的同时实现无缝切换与导航。

附图说明

图1是本发明实施例提供的地上地下统一导航方法的流程图；

图2是计算待定位点到主基站的距离和距离主基站的角度的示意图；

图3是本发明具体示例中地上地下一体化路径网络的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

需要说明的是，除非另外定义，本发明一个或多个实施例使用的技术术语或者科学术语应当为本公开所属领域内具有一般技能的人士所理解的通常意义。本发明一个或多个实施例中使用的“第一”、“第二”以及类似的词语并不表示任何顺序、数量或者重要性，而只是用来区分不同的组成部分。“包括”或者“包含”等类似的词语意指出现该词前面的元件或者物件涵盖出现在该词后面列举的元件或者物件及其等同，而不排除其他元件或者物件。“连接”或者“相连”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电性的连接，不管是直接的还是间接的。

下面对结合附图对本发明的技术方案进行详细说明。本发明的实施例，提供了一种地上地下统一导航方法，图1示出了该方法的流程图，如图1所示，包括如下步骤：

S102、建立地上地下统一坐标系。本发明该实施例中基于北斗CORS系统、惯性导航技术以及超宽带技术(UWB)实现地上地下统一坐标系的建立，具体来说，可以包括如下步骤：

S1022、获取室外第一坐标系的三维坐标。本发明该实施例中，该室外第一坐标系可以采用基于北斗CORS系统的坐标系。在北斗CORS系统建设完成的条件下，利用基于中国北斗卫星导航系统(BeiDou Navigation Satellite System，以下简称“BDS”)的网络载波相位差分技术RTK系统，获取室外用户全天候的三维坐标(X,Y,Z)和速度V_t以及时间信息。该步骤中还可以获取速度以及时间信息，其中速度为待定位点移动速度，时间信息可用于实时记录待定位点位置，以用于系统建成后的导航可视化。

S1024、将室外第一坐标系转换为第二坐标系，该第二坐标系为绝对坐标系。本发明该实施例中，该第二坐标系为2000国家大地坐标系，2000国家大地坐标系为精度较高、具有动态性和实用性的大地坐标系。该步骤中，将基于北斗CORS系统获取的相对坐标，利用布尔沙模型(B模型)转换至第二坐标系，即2000国家大地坐标系。该转换过程可以按照以下步骤进行：

设在第一坐标系中的坐标为(X₁,Y₁,Z₁),在第二坐标系(2000国家大地坐标系)中的坐标为(X₂,Y₂,Z₂)，第一坐标系和第二坐标系原点之间的差值，即三个平移参数为(Δ_X,Δ_Y,Δ_Z)，三个旋转参数(ε_X,ε_Y,ε_Z)，该旋转参数为通过按顺序旋转三个坐标轴指定角度，可以使两个空间直角坐标系的XYZ轴重合，和1个尺度变换参数m，即第一坐标系和第二坐标系内的同一段直线的长度比值，用于实现尺度的比例转换，通常m值几乎等于1；

根据布尔沙模型，变换公式为：

由此，可以获得地上户外各位置在第二坐标系(2000国家大地坐标系)中的绝对坐标(X₂,Y₂,Z₂)。

S1026、计算地上地下衔接区域，待定位点在第二坐标系下的绝对坐标。由于在地上地下衔接区域，地上与地下定位方式略有误差，即地上的北斗定位和地下的UWB定位方式在地上地下衔接区域的定位均不准确误差较大，当误差超过10％时，即将该部分认定为衔接区域，使用惯性导航进行定位。惯性即指加速度，是指利用惯性元件(加速度计)来测量运载体本身的加速度，经过积分和运算得到速度和位置，从而达到对运载体导航定位的目的。具体步骤如下：

S10261、通过待定位点的加速度和角速度随时间积分，计算待定位点的惯性位置变化量

S10262、根据待定位点的先前惯性位置坐标和惯性位置变化量得到待定位点在第二坐标下的绝对坐标。

上述步骤S10261和S10262中，可以先将当前绝对坐标系(导航坐标系)，即第二坐标系转换为此待定位点的惯性坐标系。惯性坐标系是为了简化世界坐标系到物体坐标系的转化而产生的，惯性坐标系的原点与物体坐标系的原点重合，惯性坐标系的轴平行于世界坐标系的轴。引入了惯性坐标系之后，物体坐标系转换到惯性坐标系只需旋转，从惯性坐标系转换到世界坐标系只需平移。测得绕X，Y，Z轴分别转动α，θ，

α，θ，

是指通过按顺序旋转三个坐标轴该指定角度，可以使两个空间直角坐标系的XYZ轴重合。则当前惯性坐标系下某点坐标(x′,y′,z′):

得到惯性坐标系下X，Y，Z轴方向的加速度分别a_X,a_Y,a_Z，该加速度可以通过惯性导航中的惯性元件测量，并根据以下公式：

得该点坐标系下新坐标(X′_t,Y′_t,Z′_t)：

再将该点转换至第二坐标系(2000国家大地坐标系)下的绝对坐标得到：

通过上述步骤，可以得到经惯性导航转换后的地下位置在第二坐标系(2000国家大地坐标系)下的绝对坐标(x_t,y_t,z_t)。

由于各地上位置可转换至第二坐标系(2000国家大地坐标系)中，惯性导航中的导航坐标系，即本发明该实施例中采用的中的2000国家大地坐标系，由惯性导航转换后获得的地上地下衔接区域坐标为在第二坐标系中的绝对坐标。

S1028、计算待定位点在地下区域时在第二坐标系下的绝对坐标。本发明该实施例中，对于地下位置的定位采用相对定位精准、功耗低等多优点的超宽带技术(UWB)进行室内定位坐标系的构建。本发明该实施例中，假设在所需的场景下已部署所需基站。该步骤具体可以包括：

S10281、获取每个地上地下衔接点在第二坐标系下的绝对坐标。本发明该实施例中，在所选择的地下环境周围设置至少四个固定基站来接收待定位点的脉冲无线电信号，在每个地上地下衔接点设置基站，获取每个地上地下衔接点在第二坐标系(2000国家大地坐标系)中对的绝对坐标(X_j,Y_j,Z_j),j∈(1,2…N)(N为基站数量)，假设坐标为(X₁,Y₁,Z₁)的基站为GIA地下区域的主基站。

S10282、计算待定位点到主基站的距离和距离主基站的角度。设各基站坐标为(X_j,Y_j,Z_j)，待定位点Q的坐标为(x,y,z)，计算待定位点到主基站的距离的公式为：

其中，N为基站数量，t₀代表需定位的目标位置发射信号的时间，t_j代表某基站参考点位置的信号到达时间。

根据上一步假设的待定位点Q与基站的坐标，需测出该UWB信号到达各基站(X_j,Y_j,Z_j)的垂直方向的俯仰角(β₁,β₂…β_N),UWB信号指待定位设备发送的脉冲信号，可用于计算距基站的距离进而进行定位，待定位点节点相对基站水平方向角度为(θ₁,θ₂…θ_N)。

S10283、根据所述距离和角度，通过以下公式计算待定位点在第二坐标系下的坐标：

图2中示出了该角度的示意图，图2中待定位点点即待定位点Q的坐标为(x,y,z)，基站的坐标为(X_j,Y_j,Z_j)，d为待定位点与基站的距离。根据以上步骤，即可计算出各个待定位点的坐标。

S10284、根据待定位点每次移动发出信号的时间戳，计算待定位点在时间维度的标记。随着待定位点的移动，每次获得待定位点发出信号的时间戳T_k(k＝1,2,3…)，从而获得待定位点在时间维度的标记。该时间维度的标记可用于实时更新待定位点在第二坐标系中的位置，例如在高德导航中可根据待定位点在时间维度的标记实时通过待定位点的移动计算预计到达目的地的时间。

S10285、基于所述待定位点的坐标和待定位点在时间维度的标记获得待定位点的第二坐标系下的绝对坐标。

根据上一步骤计算得到的待定位点的各个坐标，还可以进行误差校正，根据简单迭代的最小二乘法，排除各基站计算的误差，从而获得较高精度的待定位点位置坐标(x,y,z)。

本发明实施例通过以上步骤，将各基站位置坐标已由第一坐标系(北斗CORS站)获得的相对坐标转换至第二坐标系(2000国家大地坐标系)中，因此可以容易由待定位待定位点相对基站获得的地下坐标计算得出随时间变换的待定位点在第二坐标系(2000国家大地坐标系)下的绝对位置，从而建立了地上地下统一空间坐标系(X,Y,Z)，进而可实现地上地下定位无缝衔接。

根据某些可选的实施例，还可以针对地上地下衔接区域待定位点在第二坐标系下的绝对坐标进行误差校准。根据上述步骤可得分别由各个地上地下衔接点UWB基站位置通过惯性导航获得的位置坐标(X_j,Y_j,Z_j),j∈(1,2…N)(N为基站数量)，通过分析统一待定位点得到的各个坐标值，通过最小二乘法排除各基站计算的误差，从而获得较高精度的待定位点位置坐标(X,Y,Z)。

由获得的每个坐标距离各自基站的距离D＝[d₁,d₂…d_N]^T，设置N个基站分配的权重为W＝[w₁,w₂…w_N]。其中每个基站计算权重为w_j＝1/d_j，设

X方向上各个基站的坐标为X＝[x₁,x₂…x_N]^T，由此可以计算多基站下的惯性导航坐标：

Y,Z轴同理，如此可以惯导产生的误差，在基于多点的情况下，进行定位校准，得到较为准确可靠的坐标(X,Y,Z)。

S104、基于统一坐标系进行地下路径规划。在完成了坐标系的统一后，进行地下路径规划，可以包括如下步骤：

S1042、基于统一坐标系构建地下路径网络。

S10421、将地下设施抽象为多面体，获取多面体的顶点在所述统一坐标系下的坐标。通过观察地下结构，可将各类大型地下设施抽象为多面体，获取各多面体的顶点在已建设好的地上地下一体化坐标系下的坐标(x,y,z)，地下设施的位置信息通过多面体顶点坐标集合表示：

{(x₁,y₁,z₁),(x₂,y₂,z₂),…,(x_n,y_n,z_n)}

并为多面体添加各种地点相关信息。该相关信息包括但不限于：主键id、设备名name、所属拓扑地图map、设施类型type、坐标集合coordinate、大致描述description等，可以用于(1)可视化界面中提供用户点击查询信息(2)搜索最短路径过程中需要对坐标集合、设备id、所属地图进行访问。

S10422、在该多面体解决地下地面区域中选取若干点作为元素点，在可通行的区域选择若干点作为第一路径点，在地上地下衔接区域附近地上区域中选择若干点作为第二路径点。选取地下设施多面体接近地下地面区域中的若干点作为地下设施的元素点，多面体a的第i个元素点记为element[a][i]，存储数据(x,y,z)。在可通行的区域人工选择若干点作为第一路径点(x_i,y_i,z_i)，记作pathpoint[i]，应保证选点覆盖至所有可通行区域、每个选点的可达性、点与点之间距离在坐标系下的可度量性。在地下巷道入口的附近地上区域同样选取若干点作为第二路径点(x_i,y_i,z_i)，记作pathpoint_out[i]。

S10423、连接所有元素点和路径点，形成路径网络拓扑图。该步骤中，合理连接所有元素点和路径点，连接原则为两点之后间距离小于一定值即可连接，并且两点之间的连接线段不经过多面体，形成路网拓扑图。各点之间的线段即为可通行的路径，i点和j点连通可以表示为：

i点和j点未连通则表示为：

path[i][j]＝-1。

S10424、根据第一路径点和第二路径点的连接情况，将路径点分为连通路径点和非连通路径点。根据第一路径点pathpoint[i]与第二路径点pathpoint_out[i]的连接情况，将路径点分为连通路径点connect[i]和非连通路径点pathpoint[i]，通过“连通路径点”的用户可从地下拓扑到地上拓扑或反之，“连通路径点”必与地上区域上选取的路径点相连，地上区域上选取的路径点必与“连通路径点”相连，即对任意“连通路径点”connect[i],存在一个或一个以上地上路径点pathpoint_out[j]，使得：

path[connect[i],[pathpoint_out[j]]>＝0。

对“连通路径点”判断可通行方向，connect[i].in与connect[i].out，当connect[i].in＝true时表示可以通过该连通路径点从地上进入地下，false则不可以，当connect[i].out＝true时表示可以通过该连通路径点从地下进入地上，false则不可以。

从而，便可以通过有限接口“连通路径点”connect[i]，将限制较小的地上路网和存在有限约束的地下路网相连通，达到地上地下路网一体化。

S1044、在地下路径网络中，针对起点为地上或地下区域，终点在地下或地上区域的导航任务计算最短路径。设地上区域一点o点，其在地上地下统一坐标系下坐标为(x_o,y_o,z_o)；地下区域一点d点，其在地上地下统一坐标系下坐标为(x_d,y_d,z_d)；从地上起点o点到地下终点d点的路径搜索方法步骤如下：

S10441、获取多个地下设施的多面体坐标集合。

S10442、判断终点是否位于任一多面体内，若是，则锁定终点所在的第一多面体；若否，则在第一路径点数组中寻找距离中点最近的路径点作为终点。通过各多面体坐标集合coordinate判断d点(x_d,y_d,z_d)是否位于多面体内，若是，则锁定坐标所在多面体a；若否，则在非连通路径点pathpoint数组中寻找距离d点最近的路径点pathpoint[t]作为搜索终点：设路径点共有n个，第i个路径点到d点的距离为：

D(pathpoint[t],d)≤D(pathpoint[i],d)(1≤i≤n)。

S10443、分别计算起点和终点距离连通路径点的最短距离、以及起点到终点的最短距离。计算o点到“连通路径点”的最短距离。遍历connect数组，若第i个“连通路径点”connect[i]满足connect[i].in＝true，则使用A*算法，计算o点到connect[i]的最短距离，记为D(o,connect[i])。计算“连通路径点”到d点的最短距离。若d点位于多面体a中，则利用路径线段的权值,使用A*算法分别计算所有“连通路径点”connect[i]到多面体a各元素点element[a][j]的最短路径D(connect[i],element[a][j])，并计算多面体a各元素点element[a][j]到d点的距离D(element[a][j],d)，“连通路径点”到d点的最短距离为：

D(connect[i],d)＝min{D(connect[i],element[a][j])+

D(element[a][j],d)}；

否则，计算所有“连通路径点”connect[i]到距离d点最近的路径点pathpoint[t]的最短路径D(connect[i],pathpoint[t]),“连通路径点”到d点的最短距离为：

D(connect[i],d)＝D(connect[i],pathpoint[t])+D(pathpoint[t],d)。

计算o点到d点的最短距离，o点到d点最短距离为：

D(o,d)＝min{D(o,connect[i])+D(connect[i],element[a][j])}。

通过上述步骤S10443和S10444计算三阶段最短距离，最终得到起始点o到终点d的最短距离。

S10444、标记起点到终点的最短距离中所有经过的点并连线后得到起点至终点的最短路径。标记o点到d点最短路径D(o,d)中所有经过的点并连线，即得到o点到d点的最短路径。

而针对地下o点到地上d点的情况，同上述各步骤相同，其中，“连通路径点”需选取connect[i].out＝true的点。

S106、对所规划的路径进行可视化表达。对所规划的路径进行可视化表达包括对地上路径的可视化表达和对地下路径的可视化表达。

其中，对地上路径的可视化表达，当位于处于地上户外GNSS定位，进行地上导航部分时,系统会在现有2000国家大地坐标系中确认目的地绝对坐标,借助北斗卫星定位系统和CORS系统确认现有绝对坐标，利用现有地图数据和较为成熟的导航算法进行路径规划。可以通过调用高德地图等现有的导航方式的AmapNavi类实现对应的导航功能以及可视化展示。

对地下路径的可视化表达，可以根据以下步骤进行：

S1061、根据具体情况确认在绝对坐标系下地下可视化所采用的方法。在地下空间比较规则的情况下，将导航路径抽象为多个元素点的连接的边的集合。同时根据path[i][j]的相应取值确定出可通行路径。不可通行路径以及不存在路径点pathpoint[i]区域视为不可达区域。在处于一般地下条件下，通过判断用户定位坐标的z值判断其所处高度或楼层，转换为已有的二维导航。

S1062、在环境较为复杂的场景中，根据多个基站坐标确定出“连通路径点”的绝对坐标，采用实景和部分三维建模的形式来展示对应地图信息。利用激光雷达进行扫描，取得激光点云数据{(P_i,F_i)}。

S1063、为方便其进一步进行处理，在进行配准后将点云数据进行体素化。分别计算出XYZ三个方向点云数据坐标的最大值与最小值的差，然后根据三个差值来确定初始体素的长宽高，计算完成后，计算机自动建立出初始体素，所建立的初始体素中已包含所有的点云数据，之后用bresenham算法剔除初始体素中的无效体素，剩余的体素即可构成点云数据的三维模型，把点云数据投影到立方网格结构{V_u,v,w}。

S1064、根据实际需要调整优化立方网格结构{V_u,v,w}的r大小。在具有出入限制属性connect[i].in与connect[i].out的“连通路径点”connect[i]进行拼接匹配，同时在将导航路径和三维模型进行叠加，设定路径相对边界最小距离k，根据实际模型边界弧度设定路线修正参数，从而在现有最短路径算法得出的D(o,connect[i])基础上得到实际导航路径长度。

S1065、通过利用具有出入限制属性的路径点connect[i]的坐标信息，将分散的三维模型进行对接。根据地下位置绝对坐标(x_t,y_t,z_t)，将三维模型坐标和二维地图对应位置进行对接，直至UWB信号消失在三维模型范围内切换为三维地图导航。这样便可将每个扫描文件中独立的坐标和原来地图信息进行统一，再根据实际特征点如pathpoint[i]的大地坐标对地图进行坐标系转换，从而完成总体拼接，最后得到一张完整的同一坐标系的三维二维结合的大地图。

S1066、将该所有区域对应的path[i][j]，pathpoint[i]进行叠加组合，结合上文最短路径的计算方法实现其可视化导航功能。

以上举例说明了地下路径的可视化表达的过程，即将上述各步骤的结果进行组合之后实现可视化表达。本领域技术人员也可以基于上述各步骤中建立的坐标系采用现有的方法实现路径的可视化表达。

本发明的实施例，还提供了一种地上地下统一导航装置，包括：

统一坐标系建立模块，用于建立地上地下统一坐标系；

地下路径规划模块，用于基于统一坐标系进行地下路径规划；

可视化表达模块，用于对所规划的路径进行可视化表达。

本发明上述实施例提供的导航装置中各个模块实现其功能的具体过程与本发明上述实施例提供的导航方法的各步骤相同，在此不再一一赘述。

以下以一个具体的示例对本发明的技术方案进行说明。

该示例以矿山为场景，应用本发明实施例提供的地上地下统一导航方法，可以在手机端进行无缝导航，电脑端也可以实时监控巷道内人员位置，在矿山坍塌的紧急情况下便于及时定位受困人员位置。图3中示出了该示例中地上地下一体化路径网络的示意图，以下结合图3进行详细说明，具体步骤如下：

步骤一：假定矿山内人员均携带可发射定位信号的本导航系统应用，将工人携带的信号发射器或手机作为待定位点，将各个巷道口设为基站，基站数量少于四个的地下区将于U型巷道最里面或东南西北四个方向安装基站。

步骤二：建立成统一坐标系。

首先，本发明假设二北斗CORS站建设基本完成，利用BD的网络RTK系统，获取室外用户全天候的三维坐标(X₀,Y₀,Z₀)和速度V_t以及时间信息。

基于北斗CORS站系统获取各个巷道口的相对坐标。

利用布尔沙模型转换至2000国家大地坐标系中，变换公式为：

基于北斗CORS站系统获取各个巷道口的相对坐标。

利用布尔沙模型转换至2000国家大地坐标系中。由各个巷道口的相对坐标转换至2000国家大地坐标系中的绝对坐标系。

其次，利用惯导技术将坐标系延伸至地下巷道内。在地上地下衔接区域，地上卫星定位和地下UWB技术两种定位方式均略有误差，利用捷联惯性导航技术进行地上地下导航转换。通过对矿工携带信号器运动加速度分量和角速度随时间积分计算，得到惯性位置变化量，基于先前惯性位置以及惯性位置变化量得到当前惯性位置坐标。

由各个站口分别通过惯导技术得到某一位置的坐标，根据最小二乘法排除各基站计算的误差，从未获得较高精度的待定位点位置坐标(X₀,Y₀,Z₀)。

最后，巷道内地下定位采用相对定位精准、功耗低等多优点的超宽带技术(UWB)进行地下定位坐标系建立。本文假设本片矿区地下已部署所需基站。

在各个巷道口以及地下根据距离设置固定基站来接收矿下人员携带待定位点发出的脉冲无线电信号。

通过前文中通过北斗卫星信号结合CORS系统获得每个巷道口在2000国家大地坐标系中的绝对坐标。

计算矿下人员待定位点到基站的距离d。通过信号器发出信号的时间与巷道口基站收到信号的时间差计算人员距离各基站的距离长度。计算公式为：

计算地下人员距离基站角度。根据测得的UWB信号到达各基站垂直方向的俯仰角与相对基站水平方向的角度。

计算地下人员位置坐标。根据前两步骤获得的需定位点距离各基站的距离与角度，计算公式为：

进行误差分析。由各个基站得到的地下人员位置，根据简单迭代的最小二乘法，排除各基站计算的误差，从而获得较高精度的待定位点坐标。

随待定位点的移动，每次获得待定位点发出的时间戳，从而获得乘客在同一时空坐标系中的动态定位。

步骤三：地上地下衔接区域误差校准。

已知步骤二中通过惯导延伸坐标系，得到了由各个巷道口UWB基站位置通过惯性导航获得的位置坐标(X_j,Y_j,Z_j),j∈(1,2…N)(N为基站数量)，分析统一待定位点得到的各个坐标值，通过最小二乘法排除各基站计算的误差，从而获得较高精度的待定位点位置坐标(X,Y,Z)。

由获得的每个坐标距离各自基站的距离D＝[d₁,d₂…d_N]^T，设置N各基站分配的权重为W＝[w₁,w₂…w_N]。其中每个基站计算权重为w_j＝1/d_j，设

X方向上各个基站的坐标为X＝[x₁,x₂…x_N]^T，由此可以计算多基站下的惯性导航坐标：

Y,Z轴同理，如此可以惯导产生的误差，在基于多点的情况下，进行定位校准，得到较为准确可靠地下衔接区域的定位坐标(X,Y,Z)。

步骤四：基于统一坐标系的构建地下路径网路。

首先，由矿山内的三维平面图，将矿山内各设施设备抽象为多面体，基于步骤二中得到的地上地下一体化的绝对坐标，坐标点相连构成封闭多面体，即设备a、设备b，矿山下设备位置信息便通过集合表示，通过坐标即可在拓扑地图上将坐标点连接划定区域表示地下设施。

为多面体设备a、设备b添加各种地点信息，主键id、地点名name、所属拓扑地图map、设施类型type、门牌号number、坐标集合coordinate、大致描述description等，创建矿山内空间对象的地点信息列表。

将多面体显示效果覆盖在矿山内拓扑地图基础图层上，当操作人员到达或访问该处非暴露空间时，将地下矢量地图和地上导航系统地图连接显示。

选取设备a多面体接近地面区域中边界处的element[a][1]、element[a][2]作为和外界相连的节点(一般选择设备与道路连通的边界)，将设备a抽象为2个元素点进行表示，存储坐标数据，设备b同理，也选取了element[b][1]、element[b][2]。在可通行的区域人工选择多个路径点，将可通行的道路抽象为多节点连接的边，同时选点应保证路网模型的遍历性、可达性、可度量性。在地下巷道入口的附近地上区域同样人工选取若干路径点。

合理连接所有元素点和路径点，形成路网拓扑图，各点之间的线段即为可通行的路径。

根据路径点实际与地上的连通情况，将路径点分为连通路径点和非连通路径点，“连通路径点”2个，为connect[1]、connect[2]。记录connect[1]、connect[2]是否允许从地下到地上或从地上到地下。

如此，便可以通过有限接口“连通路径点”，将限制较小的矿山地上路网和存在有限约束的地下路网相连通，达到地上地下路网一体化。

步骤五：基于统一坐标的地下最短路径算法

假定矿山外一点o点，矿山内一点d点，获取二者在地上地下一体化坐标系下坐标。从o点到d点的路径搜索方法如下：

d点是多面体设备b内，锁定多面体设备b。

计算o点分别到connect[1]、connect[2]的最短距离。若connect[i]满足允许从地上到地下，则使用A*算法，计算o点到connect[i]的最短距离。

计算“连通路径点”到d点的最短距离。利用路径线段的权值,使用A*算法分别计算connect[1]、connect[2]到多面体设备b各元素点element[b][1]、element[b][2]的最短路径D(connect[1],element[b][1])、D(connect[1],element[b][2])、D(connect[2],element[b][1])、D(connect[2],element[b][2])，并计算element[b][1]、element[b][2]到d点的距离D(element[b][1],d)、D(element[b][2],d)，得到各“连通路径点”到d点的最短距离D(connect[1],d)、D(connect[2],d)。

计算o点到d点的最短距离。o点到d点最短路径为o点到“连通路径点”的距离和同一个“连通路径点”到d点的距离之的最小值。比较得出o点到d点的最短距离D(o,d).

标记o点到d点最短路径中所有经过的点，连线，显示在用户界面上，即得到o点到d点的最短路径。

从地下d点到地上o点同理，“连通路径点”需选取允许从地下到地上的点。

步骤六：路径可视化

首先，当位于处于室外GNSS定位，进行户外导航部分时,系统会在现有2000国家大地坐标系中确认目的地的绝对坐标,借助北斗卫星定位系统和CORS系统确认现有的绝对坐标，可以利用现有地图数据和较为成熟的导航算法进行路径规划。本文室外部分可通过调用高德地图的AmapNavi类实现对应的导航功能以及可视化展示。其次，矿井巷道内根据具体情况确认在绝对坐标系下可视化导航所采用的方法。

通过判断乘客定位坐标的z值即高度值判断其所处深度对应巷道，转换为已有的二维导航。

在环境较为复杂的场景中，如巷道分叉，上下交错，设备，地形较多较复杂的场景则根据已部署基站坐标确定出“连通路径点”的绝对坐标，采用实景和部分三维建模的形式来展示对应地图信息。三维建模部分采用激光扫描建模的方法。在取得相应激光点云数据后，通过网格化处理的方法对点云数据进行简化，初步取得三维模型。

根据实际需要调整优化立方网格结构的大小，例如场景路线较为复杂则降低立方网格大小，在重复度较高，较规则场景则适当放大立方网格大小，从而对模型精细程度和数据量进行进一步调整。

在根据各个路径点的相对位置信息来确认地图位置信息后，将地图数据进行拼接匹配，转化到同一坐标系中，从而得到统一的局部坐标系。

再根据特征点坐标信息和从北斗CORS系统确认的UWB基站的大地坐标信息对局部坐标系进行转换，完成和其它部分地图坐标系的对接，从而实现坐标系的一体化。

之后，考虑到区域设施的运作情况及地图信息的滞后性，将此前实际导航用户经过的路径，现有最短路径算法得出的导航路径，根据预设限定条件的实际可行路径这三种路径进行比较优化，从而得出最终导航路径。

最后，将所有地图信息进行组合对接。将该所有区域对应的具有出入限制属性的路径点进行叠加组合，结合本发明上述实施例提供的导航方法，最终实现其导航功能。

综上所述，本发明实施例涉及一种地上地下统一导航方法及装置，该方法包括步骤：建立地上地下统一坐标系；基于统一坐标系进行地下路径规划；对所规划的路径进行可视化表达。本发明实施例的技术方案，通过将不同坐标系定位进行融合转换至2000国家大地坐标系而建立绝对坐标系，仅需一可用角向量便可进行较精确的定位，在特殊条件下可用度更高；同时减少了除开始定位和新系统信号参与定位时的运算量，降低了设备功耗；在新系统加入时，也更易从现有定位基础上将原系统相对坐标进一步转化为绝对坐标，从而实现定位系统间无缝切换。针对无法接收外界信号，无法配置基站的特殊区域可以通过图像信息，NFC待定位点等确定绝对坐标，从而进行进一步校准和优化终端定位。

应当理解的是，以上任何实施例的讨论仅为示例性的，并非旨在暗示本发明的范围(包括权利要求)被限于这些例子；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明一个或多个实施例的不同方面的许多其它变化，为了简明它们没有在细节中提供。本发明的上述具体实施方式仅仅用于示例性说明或解释本发明的原理，而不构成对本发明的限制。因此，在不偏离本发明的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。此外，本发明所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。

Claims (9)

1.一种地上地下统一导航方法，其特征在于，包括步骤：

建立地上地下统一坐标系；

基于统一坐标系进行地下路径规划；

对所规划的路径进行可视化表达。

2.根据权利要求1所述的方法，其特征在于，建立地上地下统一坐标系包括：

获取室外第一坐标系的三维坐标；

将室外第一坐标系转换为第二坐标系，该第二坐标系为绝对坐标系；

计算地上地下衔接区域，待定位点在第二坐标系下的绝对坐标；

计算待定位点在地下区域时在第二坐标系下的绝对坐标。

3.根据权利要求2所述的方法，其特征在于，所述计算地上地下衔接区域，待定位点在第二坐标系下的绝对坐标包括：

通过待定位点的加速度和角速度，计算待定位点的惯性位置变化量；

根据待定位点的先前惯性位置坐标和惯性位置变化量得到待定位点在第二坐标下的绝对坐标。

4.根据权利要求3所述的方法，其特征在于，所述计算待定位点在地下区域时在第二坐标系下的绝对坐标包括：

获取每个地上地下衔接点在第二坐标系下的绝对坐标；

计算待定位点到主基站的距离和距离主基站的角度；

根据所述距离和角度计算待定位点在第二坐标系下的坐标；

根据待定位点每次移动发出信号的时间戳，计算待定位点在时间维度的标记；

基于所述待定位点的坐标和待定位点在时间维度的标记获得待定位点的第二坐标系下的绝对坐标。

5.根据权利要求2所述的方法，其特征在于，所述建立地上地下统一坐标系还包括：

针对地上地下衔接区域待定位点在第二坐标系下的绝对坐标进行误差校准。

6.根据权利要求1所述的方法，其特征在于，所述基于统一坐标系进行地下路径规划包括：

基于统一坐标系构建地下路径网络；

在地下路径网络中，针对起点为地上或地下区域，终点在地下或地上区域的导航任务计算最短路径。

7.根据权利要求6所述的方法，其特征在于，所述构建地下路径网络包括：

将地下设施抽象为多面体，获取多面体的顶点在所述统一坐标系下的坐标；

在该多面体解决地下地面区域中选取若干点作为元素点，在可通行的区域选择若干点作为第一路径点，在地上地下衔接区域附近地上区域中选择若干点作为第二路径点；

连接所有元素点和路径点，形成路径网络拓扑图；

根据第一路径点和第二路径点的连接情况，将路径点分为连通路径点和非连通路径点。

8.根据权利要求7所述的方法，其特征在于，所述计算最短路径包括：

获取多个地下设施的多面体坐标集合；

判断终点是否位于任一多面体内，若是，则锁定终点所在的第一多面体；若否，则在第一路径点数组中寻找距离中点最近的路径点作为终点；

分别计算起点和终点距离连通路径点的最短距离、以及起点到终点的最短距离；

标记起点到终点的最短距离中所有经过的点并连线后得到起点至终点的最短路径。

9.一种地上地下统一导航装置，其特征在于，包括：

统一坐标系建立模块，用于建立地上地下统一坐标系；

地下路径规划模块，用于基于统一坐标系进行地下路径规划；

可视化表达模块，用于对所规划的路径进行可视化表达。

Images