[0001] 本发明涉及导航定位技术领域，具体涉及一种基于UWB/PDR的无基础设施行人协同导航的方法。

[0002] 个人定位的应用领域非常广泛，包括商业和公共安全。在商业领域，高精度的室内外无缝定位帮助顾客更快的在商场中找到商铺；在公共安全领域，可以用于地震、火灾救援中消防员的定位指挥和路径规划。

[0003] 公开号为CN106574838B的现有专利文献，公开了一种行人导航装置及方法，这种导航方法存在需要提前建设设备基站或采集数据库，设备和数据建设、维护和更新成本高，消防救援、紧急抢险等场景无法即时建立导航定位基础设施的问题。

[0019] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0020] 如图1所示，本具体实施方式中的基于UWB/PDR的无基础设施行人协同导航的方法，包括以下步骤：

[0021] (1)将UWB传感器佩戴在多个行人上；

[0022] (2)通过UWB进行编队组网，网内各节点之间相互测距和位置信息共享；

[0023] (3)采用基于EKF的集中式协同导航算法，以所有节点的坐标、步长和航向角作为状态建立集中式协同导航状态模型，以单个行人为节点，设置k时刻节点i的状态向量为即 其中， 表示东向坐标， 表示北向坐标， 表示航向角，表示步长距离，节点i在k时刻的时间更新方程表述为
其中 表示k‑1时刻节点i的后验状态估计值，
表示k时刻节点i的先验状态估计值， 表示状态转移矩阵，将单个节点扩展到多个节点，集中式协同导航系统状态模型表示为

[0024] (4)以网内UWB的测距信息为观测建立集中式协同导航观测模型，使用UWB节点间相对距 离 ，设 置k时 刻节点 i对节 点j的 相对 距离为 观测 方程为 ：其中 表示为量测噪声，将观测
方程通过泰勒展开得到线性化观测方程为 其中 与
分别为函数 关于 与 的雅可比矩阵，根据网内的多个节点，将k时刻系
统的状态扩展为Xk，并对线性化观测方程进行拓展，得到集中式协同导航观测模型的方程为：Zk＝HkΔXk+R， 其中Zk表示为
观测量，Hk为观测矩阵，R为观测噪声；

[0025] (5)利用集中式协同导航系统状态模型和集中式协同导航观测模型同时对网内所有节点的航位进行推算，状态方程对节点当前的位置进行估计，观测方程用于修正当前的位置，以提高精度。

[0026] 在测试中，测试人员佩戴的测试装置包含MIMU模块、UWB模块、GNSS接收机和天线，MIMU模块固定于测试人员的腰部，MIMU模块包含IMU模块和磁力计模块，测量测试人员行走时的加速度、姿态角和磁场强度，采样频率为100HZ；UWB模块一般放置于测试人员头顶，持续接收UWB测距信号,采样频率为1HZ；测试人员携带的GNSS接收机在测试过程中，持续接收GNSS数据，通过与基站做差分GNSS得到测试人员真实轨迹，差分GNSS后的轨迹精度可达厘米级，可作为参考轨迹。

[0027] 协同导航测试选取空旷的室外操场进行测试，操场最外圈周长为300m。图2为测试人员在测试起点位的站位图，测试人员分别为图2中的1、2和3节点。在测试中，测试人员按照站位以固定编队按各自跑道沿着操场跑道行走一周，测试时间大概约5分钟，场景中无明显遮挡物，UWB信号接收良好，卫星观测条件较好，整个测试过程中可观测卫星数据保持在10颗以上。同时测试过程中，MIMU模块负责持续测量惯性数据，UWB模块负责人员间的相互测距，图3为UWB测距示意图，其中1、2、3节点互为测距关系，因此测试中一共有三对测距关系。图4为PDR与GNSS基准轨迹图，图5为协同导航与GNSS基准轨迹图，由图4和图5可看出，协同导航相对PDR算法定位轨迹更优，但是仍有一定的误差。

[0028] 以差分GNSS轨迹为基准，分析PDR与协同导航误差。表1为各节点的最大误差和平均误差。节点1PDR算法的最大误差和平均误差分别为18.49m和11.38m，协同导航的最大误差和平均误差分别为10.28m和6.40m，节点3PDR算法的最大误差和平均误差分别为15.9m和11.38m，协同导航的最大误差和平均误差分别为10.60m和6.45m；节点1和节点3的协同导航精度较PDR算法分别提高43.8％和43.9％，但节点2PDR算法最大误差和平均误差分别为
10.35m和5.38m，协同导航的最大误差和平均误差分别为11.50m和6.68m，协同导航误差增大至PDR误差的119.7％，略大于PDR误差，这是由于在无锚点条件下，协同导航牺牲了部分节点精度，换取总体精度的提高。

[0029]

[0030] 表1

[0031] 协同导航中一方面需确定编队中节点的绝对位置，另一方面获取更优的节点间相对位置关系也是协同导航的关键问题，有利于节点获取全面的环境态势信息，协同完成任务。图6为协同导航以及PDR的节点间位置约束关系图，从图6中可以看出在UWB技术的辅助下，节点间形成了稳定的相对位置关系。

[0032] 针对于GNSS受限以及基站设备难以建立等无基础设施的场所，利用其任务特点小规模编队行动，将UWB组网测距与航位推算技术相结合，形成基于信息共享和测距约束的协同导航，根据测试表明，无导航参考基准条件下，编队中各人员的定位性能可提高约30％。协同导航提高了编队的整体定位精度，相对单人导航在时空覆盖、时间效率、鲁棒性、故障容错与系统重构等方面具有独特优势，同时系统配置灵活，不需要布设其它基础设施，非常适合应急救援等编队任务。

[0033] 最后需要说明的是，以上实施例仅用以说明本发明的技术方案而非限制技术方案，本领域的普通技术人员应当理解，那些对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，均应涵盖在本发明的权利要求范围当中。