一种基于超宽带室内定位方法和系统、导航方法和系统与流程

本发明属于定位导航领域，特别涉及一种基于超宽带室内定位方法和系统、导航方法和系统。

背景技术：

随着城市的不断发展，楼宇、商场、大型车站等室内建筑规模的扩大，室内环境的场景复杂度不断增加，容易使人在室内的方向感缺失，使身处室内的人迷失方向，无法及时到达室内目的地。

目前针对室内定位导航的技术主要是采用gps方法进行定位，该方法在多层建筑或建筑群中的室内定位精度很低，使其无法满足室内导航实际要求。

目前主要是采用gps结合室内构建出的建筑结构图以及电磁波被动定位实现定位，以上方法在室内环境中定位不够准确，无法达到0。5m的精度。而且在构建室内建筑结构图时，其工作量将会非常巨大。同时结合了建筑结构图后的系统运算量过大，影响了系统时效性和可实施性，而且在室内环境越复杂的时候其缺陷越发突出。

精确定位的关键在于，接收节点能正确接收到发射节点所发送信号的直射路径，采用gps定位，电磁波会受到例如墙面，金属等物体的反射，产生多径信号，导致接收节点不仅能接收到直射路径传播的首达信号，还能接收到反射路径传播的多径信号，并且首达信号和多径信号是叠加的关系。如果接收节点不能分离出首达信号，则接收节点定位依据的是首达信号和多径信号叠加的结果，从而影响时间信息的获取，对定位精度造成影响。参照图1，如果使用连续波进行定位，其多径信号叠加后仍是正弦波，接收机无法对首达信号进行分离。

技术实现要素：

针对上述问题，本发明提出一种基于超宽带室内定位方法，

建立局部坐标系；

将室内三维模型放入局部坐标系中；

建立以多个超宽带定位基站为顶点的虚拟线框；

基于tdoa算法或tof算法获取定位标签在局部坐标系中的第一坐标，判断第一坐标位于虚拟线框内外，并基于判断结果执行以下步骤：

第一坐标位于虚拟线框内，选用tdoa算法获取定位标签在局部坐标系中的第二坐标；

第一坐标位于虚拟线框外，选用tof算法获取定位标签在局部坐标系中的第二坐标。

优选地，间隔多个定位周期，判断第二坐标位于虚拟线框内外，并基于判断结果执行以下步骤：

第二坐标位于虚拟线框内，判断当前是否采用的是tdoa算法，判断结果为是，沿用tdoa算法，判断结果为否，改用tdoa算法；

第二坐标位于虚拟线框外，判断当前是否采用的是tof算法，判断结果为是，沿用tof算法，判断结果为否，改用tof算法。

优选地，判断连续多个定位周期的第二坐标位于虚拟线框外的比例，并基于判断结果执行以下步骤：

所述比例小于等于预设阈值k，判断当前是否采用的是tdoa算法，判断结果为是，沿用tdoa算法，判断结果为否，改用tdoa算法；

所述比例大于预设阈值k，判断当前是否采用的是tof算法，判断结果为是，沿用tof算法，判断结果为否，改用tof算法。

优选地，10％≤k≤30％。

本发明还提出了一种基于超宽带室内定位系统，包括：

坐标建立模块，用于建立局部坐标系；

建模模块，用于将室内三维模型放入局部坐标系中；

线框建立模块，用于建立以多个超宽带定位基站为顶点的虚拟线框；

坐标计算模块，用于基于tdoa算法或tof算法获取定位标签在局部坐标系中的第一坐标；

判断模块，用于判断第一坐标位于虚拟线框内外；

算法调用模块，用于基于判断结果执行以下步骤：

第一坐标位于虚拟线框内，选用tdoa算法获取定位标签在局部坐标系中的第二坐标；

第一坐标位于虚拟线框外，选用tof算法获取定位标签在局部坐标系中的第二坐标。

优选地，判断模块，用于间隔多个定位周期，判断第二坐标位于虚拟线框内外；

算法调用模块，用于基于判断结果执行以下步骤：

第二坐标位于虚拟线框内，判断当前是否采用的是tdoa算法，判断结果为是，沿用tdoa算法，判断结果为否，改用tdoa算法；

第二坐标位于虚拟线框外，判断当前是否采用的是tof算法，判断结果为是，沿用tof算法，判断结果为否，改用tof算法。

优选地，判断模块，用于判断连续多个定位周期的第二坐标位于虚拟线框外的比例；

算法调用模块，用于基于判断结果执行以下步骤：

所述比例小于等于预设阈值k，判断当前是否采用的是tdoa算法，判断结果为是，沿用tdoa算法，判断结果为否，改用tdoa算法；

所述比例大于预设阈值k，判断当前是否采用的是tof算法，判断结果为是，沿用tof算法，判断结果为否，改用tof算法。

优选地，10％≤k≤30％。

本发明还提出了一种基于超宽带的导航方法，

基于上述基于超宽带室内定位方法获取定位标签的第二坐标；

获取终点在局部坐标系中的第三坐标；

基于路径规划算法获取第二坐标至第三坐标的最优路径；

基于上述基于超宽带室内定位方法周期性获取第二坐标；

基于最优路径和周期性获取的第二坐标实时导航。

优选地，所述基于路径规划算法获取第二坐标至第三坐标的最优路径具体包括：

将室内三维模型的每一楼层划分为至少一个区域，任意相邻区域建立通道，任意相邻楼层建立通道，所有区域映射至同一平面上；

基于路径规划算法获取第二坐标至第三坐标的最优路径。

优选地，路径规划算法为a*算法、蚁群算法、遗传算法、d*算法中的任一种。

优选地，将周期性获取的第二坐标转换为地理坐标系下的坐标，将所述地理坐标系下的坐标转换为摄像机坐标系的坐标，将所述摄像机坐标系的坐标转换为投影平面坐标系的坐标，将所述投影平面坐标系转换为屏幕坐标系的坐标。

本发明还提出了一种基于超宽带的导航系统，包括：

坐标获取模块，用于获取终点在局部坐标系中的第三坐标，用于基于上述基于超宽带室内定位方法获取定位标签的第二坐标，基于权利要求上述基于超宽带室内定位方法周期性获取第二坐标；

路径规划模块，用于基于路径规划算法获取第二坐标至第三坐标的最优路径；

导航模块，用于基于最优路径和周期性获取的第二坐标实时导航。

优选地，所述路径规划模块，用于基于路径规划算法获取第二坐标至第三坐标的最优路径具体包括：

区域划分单元，用于将室内三维模型的每一楼层划分为至少一个区域，任意相邻区域建立通道，任意相邻楼层建立通道，所有区域映射至同一平面上；

路径规划单元，用于基于路径规划算法获取第二坐标至第三坐标的最优路径。

优选地，路径规划算法为a*算法、蚁群算法、遗传算法、d*算法中的任一种。

优选地，还包括坐标转换模块，用于将周期性获取的第二坐标转换为地理坐标系下的坐标，将所述地理坐标系下的坐标转换为摄像机坐标系的坐标，将所述摄像机坐标系的坐标转换为投影平面坐标系的坐标，将所述投影平面坐标系转换为屏幕坐标系的坐标。

本发明的基于超宽带室内定位方法和系统、导航方法和系统，采用uwb技术进行室内定位，相对于目前gps定位，uwb具有很好的直线性，在复杂的室内环境中可以准确且有效的得到被定位目标的信息，具有更好的室内精确度，而且可以在任何大型室内环境中使用，且不影响精度；另外采用tdoa算法获取框内数据，tof算法获取框外数据，确保了每次定位数据的准确性，相对于单一方法具有更高的精确度、鲁棒性和纠错力；基于uwb技术获取的定位数据进行导航，具有更准确的导航指引、更好的实时性、更强的环境容纳力。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所指出的结构来实现和获得。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1示出了现有技术中gps定位的实际信号；

图2示出了本发明实施例中超宽带定位的实际信号；

图3示出了本发明实施例中tdoa算法进行定位的双曲线图；

图4示出了本发明实施例中超宽带室内定位方法的流程图；

图5示出了现有技术的最优路径与本发明实施例中的最优路径对比示意图；

图6示出了本发明实施例中坐标转换示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例采用超宽带(uwb)技术进行室内定位，相对于目前gps定位，uwb具有很好的直线性、鲁棒性，在复杂的室内环境中可以准确且有效的得到被定位目标的信息，具有更好的室内精确度，而且可以在任何大型室内环境中使用，且不影响精度；在使用超宽带技术时，不需要产生连续的高频载波，仅仅需要产生一个时间间隔极短(小于ns)的脉冲，因此也称作脉冲无线电，便可通过天线进行发送。需要传送信息可以通过改变脉冲的幅度、时间、相位进行加载，进而实现信息传输。

窄脉冲uwb可以在定位时通过接受器收到返回信号的幅值，区分出直接路径和反射衍生路径，相对于非窄脉通信方式，有效避免了多径干扰，提高定位识别精度。

参照图2，由于每个脉冲持续时间很短，经墙面，金属等物体的反射，反射脉冲信号在到达时间上都落后于首达信号，因此接收节点可以准确的分离出首达信号，并利用首达信号进行定位，有效解决了定位信号的多径效应。

uwb技术主要分为两种定位方法：tof和tdoa。

飞行时间法(timeofflight，tof)通过定位基站向所有定位标签广播定位请求，定位标签收到定位请求并返回定位信息，定位基站通过两次信号之间的时间差值计算出定位标签到定位基站的距离。结合多个定位基站的信息算出定位标签的位置。根据数学关系，一点到已知点的距离为常数，那么这点一定在以已知点为圆心，以该常数为半径的圆上，有两个已知点，就有两个交点。以三个已知点和距离作三个圆，他们交于同一个点，该点就是定位标签的位置。

但tof测距方法的时间取决于时钟精度，时钟偏移会带来误差。为了减少时钟偏移量造成的测距误差，通常采用正反两个方向的测量方法，即远端基站发送测距信息，定位标签接收测距信息并回复，然后再由定位标签发起测距信息，远端定位基站回复，通过求取飞行时间平均值，减少两者之间的时间偏移，从而提高测距精度。但是正因如此就造成了tof功耗大大提高，续航时间行对较短。就像一台长时间保持高强度作业的电脑，工作时间长，耗损大，相对的寿命也会缩短；tof测距方法对于定位基站连线框外和框内定位标签的定位还是精准的。

到达时间差(timedifferenceofarrival，tdoa)是一种利用到达时间差进行定位的方法，又称为双曲线定位。通过定位标签定时广播定位信号，定位基站在接受到信号后加上自己的时间戳，并将此信息传给服务器，通过tdoa定位引擎对信息进行整理计算，得到定位标签到达各个定位基站间的时间差，同时结合各个定位基站间的固有距离算出定位标签的位置。定位标签对外发送一次uwb信号，在定位标签定位距离内的所有定位基站都会收到无线信号，如果有两个已知坐标点的定位基站收到信号，由于定位标签和不同定位基站的距离间隔不同，因此这两个收到信号的时间节点是不一样的，根据数学关系，到已知两点为常数的点，一定处于以这两点为焦点的双曲线上。那么有四个已知点(四个定位基站)就会有四条双曲线，四条双曲线交于一点就是定位标签的位置。

假设空间布置有n个定位基站，同时利用n个tdoa测量值可以构成关于定位标签位置的双曲线方程组，求解此方程组即可得到定位标签坐标。

使用tdoa算法不需要定位标签与定位基站之间进行往复通信，只需要定位标签发射一次uwb信号，工作时长缩短了，功耗也就大大降低了，故能做到更高的定位动态和定位容量。

参照图3，三个圆点分别是三个定位基站的位置，双曲线以等间隔组成坐标网格，对于三角形边沿区域，相同的网格围成的区域越大，测量的误差也越大，因而当定位标签位于定位基站所组成的三角形内部时，定位的准确度最高，越靠三角形边沿和三角形外部的位置，误差越大。因而采用tdoa算法对定位基站连线框内定位标签的定位精准的，但对于定位基站连线框外定位标签的定位不精准的。

因此本实施例中采用tdoa算法和tof算法相结合的定位方法，tof算法相对于tdoa算法来说能耗更高，但tof算法相对于tdoa算法来说框外定位精度更高，因此当检测到定位标签位于定位基站连线框内时使用tdoa算法进行定位，当检测到定位标签位于定位基站连线框外时使用tof算法进行定位，从而即确保了框内框外都能达到较高的定位精度，又有效提高定位标签的电池使用时间，相对于单一方法具有更高的精确度、鲁棒性和纠错力，可以使其定位精度不变的情况下增大定位范围半径10米以上。

参照图4，本实施例提出了一种基于超宽带的室内定位方法，首先需要建立一个局部坐标系，将建筑物三维模型放在局部坐标系中，在建筑物上设置多个定位基站，一般每一楼层至少设置四个定位基站，待定位的物体或人上设置有定位标签，通过定位基站与定位标签之间的uwb信号传输来获取定位标签在局部坐标系中的坐标，进而实现对物体或人的定位。

建立以多个超宽带定位基站为顶点的虚拟线框，虚拟线框内的所有坐标为一个坐标集。

先进行第一次定位得到定位标签的第一坐标，第一次定位可以采用tdoa算法或tof算法，示例性地采用tdoa算法，得到第一坐标后判断第一坐标是否属于所述坐标集，如果第一坐标属于所述坐标集，则说明定位标签此时位于定位基站连线框内，如果第一坐标不属于所述坐标集，则说明定位标签此时位于定位基站连线框外。

如果定位标签位于虚拟线框内，则将通过tdoa算法获取的第一坐标作为定位标签实际坐标，也即第二坐标；

如果定位标签位于虚拟线框外，则使用tof算法重新获取定位标签在局部坐标系中的第二坐标，将该第二坐标作为定位标签当前实际坐标。

在导航的过程中需要周期性获取定位标签的坐标，以获取定位为标签的实时位置，由于定位标签实时移动，定位标签可能一段时间在虚拟线框内，一段时间在虚拟线框外，因此也就需要间隔时间判断定位标签位于虚拟线框内外，基于每次判断结果重新确定定位算法。

第一种方式上，间隔数个定位周期，取一个第二坐标进行判断，判断该第二坐标是否属于所述坐标集：

如果属于所述坐标集，则说明定位标签位于虚拟线框内，再判断当前采用是tdoa算法还是tof算法，如果是tdoa算法，则到下一次判断坐标是否属于所述坐标集的几个定位周期，沿用tdoa算法进行定位，如果是tof算法，则到下一次判断坐标是否属于所述坐标集的几个定位周期，改用tdoa算法进行定位。

如果不属于所述坐标集，则说明定位标签位于虚拟线框外，再判断当前采用是tdoa算法还是tof算法，如果是tdoa算法，则到下一次判断坐标是否属于所述坐标集的几个定位周期，改用tof算法进行定位，如果是tof算法，则到下一次判断坐标是否属于所述坐标集的几个定位周期，沿用tof算法进行定位。

示例性地，导航过程中，以10ms为一周期进行周期性定位，间隔10个周期判断第二坐标是否属于所述坐标集，当前采用的tdoa算法，取一个第二坐标进行判断，判断该第二坐标是否属于所述坐标集，经判断该第二坐标不属于所述坐标集，则后续10个周期改用tof算法进行定位，经过10个周期后，再将第十个周期获取的第二坐标进行判断，重复上述步骤，确定后续10个周期的定位算法，如此重复，每间隔10个周期，确定定位算法，以确保定位数据精准。

第二种方式上，每个定位周期获取到第二坐标后，计算该周期和前面连续几个周期获取的第二坐标不属于所述坐标集的比例，并判断该比例是否超过阈值k，以此判断定位标签位于虚拟线框内外。

如果计算得到的比例小于等于阈值k，则说明定位标签位于虚拟线框内，再判断当前采用是tdoa算法还是tof算法，如果是tdoa算法，则到下一周期依旧沿用tdoa算法进行定位，如果是tof算法，则到下一周期改用tdoa算法进行定位。

如果计算得到的比例大于阈值k，则说明定位标签位于虚拟线框外，再判断当前采用是tdoa算法还是tof算法，如果是tdoa算法，则到下一周期改用tof算法进行定位，如果是tof算法，则到下一周期沿用tof算法进行定位。

其中阈值k可以为10％、20％、30％中的任意一个，根据实际需要进行配置。

示例性地，导航过程中，以10ms为一周期进行周期性定位，每个周期获取到定位坐标后，计算该周期和前面9个连续周期获取的第二坐标不属于所述坐标集的比例，并判断该比例是否超过阈值k，这里阈值k＝30％，计算得到该周期和前面9个连续周期获取的第二坐标不属于所述坐标集的比例为40％，比例大于阈值k，可知定位标签位于虚拟线框外，当前周期采用的tdoa算法，则下一周期改用tof算法进行定位，下一周期获取到定位坐标后再次计算阈值并判断，确定下一周期的定位算法，如此重复，确保定位坐标精准。

上述是计算该周期和前面几个连续周期获取的第二坐标不属于所述坐标集的比例，也可以是计算该周期和前面几个连续周期获取的第二坐标属于所述坐标集的比例：

如果计算得到的比例小于等于阈值k，则说明定位标签位于虚拟线框外，再判断当前采用是tdoa算法还是tof算法，如果是tdoa算法，则到下一周期改用tof算法进行定位，如果是tof算法，则到下一周期沿用tof算法进行定位。

如果计算得到的比例大于阈值k，则说明定位标签位于虚拟线框内，再判断当前采用是tdoa算法还是tof算法，如果是tdoa算法，则到下一周期沿用tdoa算法进行定位，如果是tof算法，则到下一周期改用tdoa算法进行定位。

本实施例还提出了一种基于超宽带室内定位系统，其特征在于，包括坐标建立模块，用于建立局部坐标系，将建筑物放入局部坐标系中；

建模模块，用于将室内三维模型放入所述基于超宽带室内定位方法的局部坐标系中；

线框建立模块，用于建立以多个超宽带定位基站为顶点的虚拟线框；

坐标计算模块，用于基于tdoa算法或tof算法获取定位标签在局部坐标系中的第一坐标；

判断模块，用于判断第一坐标位于虚拟线框内外；

算法调用模块，用于基于判断结果执行以下步骤：

第一坐标位于虚拟线框内，选用tdoa算法获取定位标签在局部坐标系中的第二坐标；

第一坐标位于虚拟线框外，选用tof算法获取定位标签在局部坐标系中的第二坐标。

进一步地，判断模块，用于间隔定位周期，判断第二坐标位于虚拟线框内外；

算法调用模块，用于基于判断结果执行以下步骤：

第二坐标位于虚拟线框内，判断当前是否采用的是tdoa算法，判断结果为是，沿用tdoa算法，判断结果为否，改用tdoa算法；

第二坐标位于虚拟线框外，判断当前是否采用的是tof算法，判断结果为是，沿用tof算法，判断结果为否，改用tdoa算法。

进一步地，判断模块，用于判断连续多个定位周期的第二坐标位于虚拟线框外的比例；

算法调用模块，用于基于判断结果执行以下步骤：

所述比例小于等于预设阈值k，判断当前是否采用的是tdoa算法，判断结果为是，沿用tdoa算法，判断结果为否，改用tdoa算法；

所述比例大于预设阈值k，判断当前是否采用的是tof算法，判断结果为是，沿用tof算法，判断结果为否，改用tof算法。

进一步地，其中阈值k可以为10％、20％、30％中的任意一个，根据实际需要进行配置。

应用上述基于超宽带室内定位方法和系统，可以为室内导航服务，基于超宽带的室内定位导航系统可以使身处大规模复杂室内环境的人可以通过基于超宽带室内定位系统准确的知道自己的在室内环境中的位置，若是想去室内环境中的某个地方，通过规划合理路径，并能使用此路径为其进行准确的室内导航。

基于超宽带的室内定位导航系统可以将大规模复杂室内环境中的关键建筑设施(如店铺、超市、电影院、办公室、会议室、进站口等)以及公共设施(如洗手间、自助购/取票机、atm等)的具体位置显示在三维场景中。系统采用超宽带技术对室内人员进行定位，使其可以清晰明了的知道自己在室内环境中的位置。当室内人员需要到达某一设施时，只需要在客户端中搜索设施的名字，系统就会在三维模型地图中为其规划合理的路径，同时系统还将为室内人员提供实时导航。

本实施例提出了一种基于超宽带的导航方法，

首先需要建立建筑物的室内三维模型，将室内三维模型放入上述基于超宽带室内定位方法的局部坐标系中，从而室内的每一点在局部坐标系中都具有坐标；

定位标签安装在移动端，基于所述基于超宽带室内定位方法获取定位标签的第二坐标；

获取终点在局部坐标系中的第三坐标；

基于路径规划算法获取第二坐标至第三坐标的最优路径；

定位标签沿着所述最优路径移动过程中，基于所述基于超宽带室内定位方法周期性获取第二坐标，获取定位标签的实时位置；

基于最优路径和定位标签的实时位置实时导航，准确的提示转向的位置、直行的距离、到达电梯/楼道的距离。

进一步地，需要对室内进行区域划分，每个区域内设置多个定位基站，具体地：1、有多楼梯连接的楼层，需按照楼层进行区域划分；2、跨度较大的单层场景，需要按照弱关联性进行划分，其中弱关联性是此区域与其他区域只有一个或者较少单通道相连接；3、设备数量限制，当单个场景中定位基站数量超过50时，若此时不划分区域就会在被定位人数较多时出现数据拥堵，进而影响定位的时效性。

进行区域划分后，常用路径规划方法在定位时采用的都是单一区域计算的形式，但这种方式最终获得的路径往往只是局部最优路径，而不是全局最优路径。

本实施例中，将室内三维模型的每一楼层划分为至少一个区域，任意相邻区域建立通道，任意相邻楼层建立通道，对每个区域建立建筑物碰撞属性，再对各个连接通道进行编号并予以连接属性。这些连接属性对应了每个区域之间的连接关系，使其可以在服务其中将各个不同场景的区域地图连接为一个整体的全局地图，并且消除了楼层之间的立体关系，将整个分离且立体的空间映射到一个平面化的地图中，减少了对高级算法的依耐性，减少了程序的计算量，减小了对服务器的压力；在进行路径规划时，考虑所有区域，得到全局最优路径。

参照图5，将室内单层划分为4个区域，图5中的多边形为室内物体，由于起点和终点分别位于区域1和区域2内，常用路径规划方法针对单个区域计算，路径规划方法只考虑的区域1和区域2，而没有考虑区域3和区域4，获得的最优路径为路径1，路径1只是区域1和区域2内的最优路径，即局部最优路径。

而本实施例中，综合考虑区域1、区域2、区域3和区域4，获得的最优路径为路径2，路径2是综合考虑4个区域的最优路径，经计算路径2的长度小于路径1的长度，路径2即全局最优路径。

具体地，路径规划算法为a*算法、蚁群算法、遗传算法、d*算法的任一种，以a*算法为例，a*算法引入了一个估价函数，使用估价函数对可用节点进评估，最后得到消耗资源最少的路径，又考虑所有区域，避免了规划的路径陷入局部最优路径的情况；a*算法为在选择下一个被检查的节点时，使用了已知的全局信息作为参考，即将所有区域整体考虑，对于目标节点到当前节点的距离做出估计，当作评价当前节点处于最优路线的可能性的度量，使得搜索算法考虑最有希望的节点，以此提高搜索效率。

得到的最优路径最终是要展现在移动端的屏幕上，定位标签的实时位置也需要展现在屏幕上，由于室内三维模型需要转换为屏幕坐标系，涉及到多种坐标系之间的转换与关联，其中包括局部坐标系、地理坐标系、摄像机坐标系、投影平面坐标系和屏幕坐标系，涉及上述五种坐标系间四种转换关系：局部坐标系到地理坐标系的关系w，地理坐标系到摄像机坐标系的关系c，摄像机坐标系到投影平面坐标系的关系p，投影平面坐标系到屏幕坐标系的转换关系u。

参照图6，局部坐标系下任意点(xv，yv，zv)，最终映射到屏幕坐标系的坐标为(up，vp)。利用齐次坐标系描述该过程为公式(1)。

其中w4×4与增强要素在地理坐标系中的位姿有关，c4×4与摄像机在地理坐标系中的位姿有关，两者均为旋转平移矩阵，用r3×3和t3×1分别表示旋转矩阵和平移矩阵有：

p3×4与u3×3描述了摄像机内部成像方式，与摄像机模型有关，摄像机坐标系下点(xc，yc，zc)在投影平面上投影点为(uc，yc)，并对应到屏幕坐标系下的点(up，vp)，在齐次坐标系下有如下关系：

其中f表示摄像机模型中摄像机焦距，du和dv分别表示投影平面坐标系中uc和vc方向的物理尺寸，(u0，v0)为投影平面坐标系原点在平面坐标系中的坐标。

综上，局部坐标系中点(xv，yv，zv)最终对应到屏幕上的像素点(up，vp)，存在如下对应关系：

导航过程中，依靠移动端的速度传感器、加速度传感器、方向传感器等获取移动端的位姿，依据位姿发送导航提示。

本实施例还提出了一种基于超宽带的导航系统，包括：

坐标获取模块，用于获取终点在局部坐标系中的第三坐标，用于基于所述基于超宽带室内定位方法获取定位标签的第二坐标，基于超宽带室内定位方法周期性获取第二坐标；

路径规划模块，用于基于路径规划算法获取第二坐标至第三坐标的最优路径；

导航模块，用于基于最优路径和周期性获取的第二坐标实时导航。

进一步地，所述，路径规划模块，用于基于路径规划算法获取第二坐标至第三坐标的最优路径具体包括：

区域划分单元，用于将室内三维模型的每一楼层划分为至少一个区域，任意相邻区域建立通道，任意相邻楼层建立通道，所有区域映射至同一平面上；

路径规划单元，用于基于路径规划算法获取第二坐标至第三坐标的最优路径。

进一步地，路径规划算法为a*算法、蚁群算法、遗传算法、d*算法的任一种。

进一步地，还包括坐标转换模块，用于将周期性获取的第二坐标转换为地理坐标系下的坐标，将所述地理坐标系下的坐标转换为摄像机坐标系的坐标，将所述摄像机坐标系的坐标转换为投影平面坐标系的坐标，将所述投影平面坐标系转换为屏幕坐标系的坐标，从而将第二坐标显示在移动端的屏幕上。

尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。