CN113108791B

CN113108791B - 一种导航定位方法及导航定位设备

Info

Publication number: CN113108791B
Application number: CN202110244879.4A
Authority: CN
Inventors: 冯大权; 彭俊杰; 周小安
Original assignee: Shenzhen University
Current assignee: Shenzhen University
Priority date: 2021-03-05
Filing date: 2021-03-05
Publication date: 2023-08-04
Anticipated expiration: 2041-03-05
Also published as: CN113108791A

Abstract

本发明提供了一种导航定位方法及导航定位设备，该方法包括获取目标环境中当前时刻通过UWB测量各基站与定位节点间的第一测量距离以及各基站与通过IMU预测定位节点在下一时刻的位置间的第二测量距离；基于目标环境的预设运动距离阈值与第一测量距离和第二测量距离间的关系，对下一时刻各基站进行工作状态识别，确定下一时刻目标环境中处于视距状态下的基站；根据下一时刻目标环境中处于视距状态下的基站数目，确定相应的定位算法对定位节点的下一时刻进行导航定位。从而通过UWB和IMU融合的方式，实时识别非视距信号，确定室内环境中处于视距状态下的基站数目选择适应当前环境的定位算法，实现了不同室内环境条件下的实时精准导航定位。

Description

一种导航定位方法及导航定位设备

技术领域

本发明涉及室内定位导航技术领域，具体涉及一种导航定位方法及导航定位设备。

背景技术

随着科学技术的发展，定位系统的应用日益广泛。目前最为常用的两种定位技术为超宽带(Ultra-wide band，简称UWB)技术和惯性测量单元(Inertial MeasurementUnit，简称IMU)。其中，是UWB一种基于无线电定位的方法，可获得较高的定位精度，可用于室内复杂环境下的定位，但是UWB定位过程中所产生的非视距(Non Line of Sight，简称NLOS)误差会严重影响系统的位置估计，导致测距精度降低，而IMU是一种自主导航技术，其不受非视距和其它信号的干扰，可以从已知的初始点计算出系统的当前姿态(或方向)、速度和位置。但是，由于陀螺/加速度计偏差和标度因子误差等长期漂移，计算出的导航状态(位置、速度和姿态)误差将随时间增长而增大，无法长时间提供精准定位。

为了保障在建筑结构复杂环境动态多变的复杂室内环境实现精准定位，人们开始尝试采用UWB定位技术和IMU定位技术相融合的定位策略。但是，由于UWB定位技术的实现依赖于部署的基站，而在特定的室内环境通常是结合定位成本，部署相应数量的基站，如：高精度的室内环境部署三到四个基站，定位区域小且定位精度需求不高的环境部署一到二个基站，并且即使是在同一室内环境中，由于障碍物的移动，也会造成不同数量的基站受障碍物影响，在UWB定位过程中产生NLOS误差。而由于现有的融合定位策略仅能实现特定环境条件如：需要有三个没有非视距影响的基站实现融合定位，其难以适应不同室内环境条件如：仅有两基站或单基站没有非视距影响情况下的精准导航定位。

发明内容

有鉴于此，本发明实施例提供了一种导航定位方法及导航定位设备，以克服现有技术中在复杂室内环境中难以实现精准导航定位的问题。

本发明实施例提供了一种导航定位方法，应用于导航定位系统，所述导航定位系统包括设置于目标环境中的若干基站和定位目标上的定位节点，所述定位节点固定有UWB标签和IMU传感器，所述方法包括：

获取目标环境中当前时刻通过UWB测量各基站与定位节点间的第一测量距离以及各基站与通过IMU预测所述定位节点在下一时刻的位置间的第二测量距离；

基于所述目标环境的预设运动距离阈值与所述第一测量距离和所述第二测量距离间的关系，对下一时刻各基站进行工作状态识别，确定下一时刻所述目标环境中处于视距状态下的基站；

根据下一时刻所述目标环境中处于视距状态下的基站数目，确定相应的定位算法；

基于所述定位算法对所述定位节点的下一时刻进行导航定位。

可选地，所述预设运动距离阈值的确定过程如下：

获取所述目标环境中各基站在视距状态下的第一样本数据以及在非视距状态下的第二样本数据；

利用所述第一样本数据和第二样本数据输入预设运动距离阈值模型进行训练，得到所述预设运动距离阈值。

可选地，所述基于所述目标环境的预设运动距离阈值与所述第一测量距离和所述第二测量距离间的关系，对下一时刻各基站进行非视距识别，包括：

获取当前基站当前时刻的工作状态，所述工作状态包括：视距状态和非视距状态；

判断所述第二测量距离和所述第一测量距离的差值的绝对值是否小于所述预设运动距离阈值；

当所述第二测量距离和所述第一测量距离的差值的绝对值大于所述预设运动距离阈值时，确定所述当前基站下一时刻的工作状态与当前时刻的工作状态不同。

可选地，所述根据下一时刻所述目标环境中处于视距状态下的基站数目，确定相应的定位算法，包括：

当下一时刻所述目标环境中处于视距状态下的基站数目小于两个时，确定采用基于两步卡尔曼滤波定位算法；

当所述目标环境中有两个基站处于视距状态时，确定采用基于双基站的扩展卡尔曼滤波定位算法；

当所述目标环境中有三个基站处于视距状态时，确定采用基于三基站的扩展卡尔曼滤波定位算法；

当所述目标环境中有三个以上基站处于视距状态时，确定采用基于到达时间差的定位算法或基于到达时间差与卡尔曼滤波结合的定位算法。

可选地，当所述定位算法为基于两步卡尔曼滤波定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

基于IMU获取当前时刻的加速度和速度；

对所述加速度和速度进行卡尔曼滤波，得到更新后的加速度和速度；

基于更新后的加速度和速度，确定所述定位节点的位移；

基于更新后的加速度和速度结合所述定位节点的位移进行卡尔曼滤波，得到下一时刻所述定位节点的第一预测位置。

可选地，当所述定位算法为基于双基站的扩展卡尔曼滤波定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

获取当前时刻所述定位节点的当前位置；

通过IMU计算得到所述定位节点在下一时刻的初始预测位置；

基于所述当前位置和所述初始预测位置，确定所述定位节点的第一位移；

通过UWB测量得到处于视距状态下的两个基站与所述定位节点间的距离和角度；

基于所述第一位移和两个基站与所述定位节点间的距离和角度进行扩展卡尔曼滤波，得到下一时刻所述定位节点的第二预测位置。

可选地，当所述定位算法为基于三基站的扩展卡尔曼滤波定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

获取基于两步卡尔曼滤波定位算法对所述定位节点的下一时刻进行导航定位的第一预测位置、通过UWB测量得到处于视距状态下的三个基站与所述定位节点间的距离以及当前时刻所述定位节点的当前位置；

基于所述当前位置和所述第一预测位置，确定所述定位节点的第二位移；

基于所述第二位移和三个基站与所述定位节点间的距离进行扩展卡尔曼滤波，得到下一时刻所述定位节点的第三预测位置。

可选地，当所述定位算法为基于到达时间差与卡尔曼滤波结合的定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

获取利用时间差算法计算所述定位节点在下一时刻的第四预测位置、通过IMU计算所述定位节点在下一时刻的第五预测位置、通过IMU计算当前时刻的第一速度以及当前时刻所述定位节点的当前位置；

基于所述当前位置与所述第四预测位置及所述第五预测位置的位置关系，分别确定所述定位节点的第三位移和第四位移；

基于所述第三位移确定当前时刻的第二速度；

基于所述第三位移、第四位移、第三位移与第四位移的位移差值以及第一速度与第二速度的速度差值进行卡尔曼滤波，得到下一时刻所述定位节点的第六预测位置。

本发明实施例还提供了一种导航定位设备，应用于导航定位系统，所述导航定位系统包括设置于目标环境中的若干基站和定位目标上的定位节点，所述定位节点固定有UWB标签和IMU传感器，所述导航定位设备包括：存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器中存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行本发明实施例提供的方法。

本发明实施例还提供了一种计算机可读存储介质，所述计算机可读存储介质存储计算机指令，所述计算机指令用于使所述计算机执行本发明实施例提供的方法。

本发明技术方案，具有如下优点：

本发明实施例提供了一种导航定位方法及导航定位设备，通过获取目标环境中当前时刻通过UWB测量各基站与定位节点间的第一测量距离以及各基站与通过IMU预测定位节点在下一时刻的位置间的第二测量距离；基于目标环境的预设运动距离阈值与第一测量距离和第二测量距离间的关系，对下一时刻各基站进行工作状态识别，确定下一时刻目标环境中处于视距状态下的基站；根据下一时刻目标环境中处于视距状态下的基站数目，确定相应的定位算法；基于定位算法对定位节点的下一时刻进行导航定位。从而通过基于UWB和IMU融合的方式，首先实时识别非视距信号，确定室内环境中处于视距状态下的基站数目，然后以此来灵活选择适应当前环境的定位算法，实现了不同室内环境条件下的实时精准导航定位。

附图说明

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

图1为本发明实施例中导航定位系统的架构示意图；

图2为本发明实施例中的导航定位方法的流程图；

图3为本发明实施例中三角几何阈值检测非视距示意图；

图4为本发明实施例中三角几何阈值检测非视距流程示意图；

图5为本发明实施例中支持向量机阈值检测非视距示意图；

图6为本发明实施例中基于两步卡尔曼滤波定位算法的示意图；

图7为本发明实施例中基于双基站的扩展卡尔曼滤波定位算法的示意图；

图8为本发明实施例中基于三基站的扩展卡尔曼滤波定位算法的示意图；

图9为本发明实施例中基于到达时间差与卡尔曼滤波结合的定位算法的示意图；

图10为本发明实施例中基于到达时间节点的智能定位策略选择示意图；

图11是到达时间计算示意图；

图12为本发明实施例中的导航定位设备的结构示意图。

具体实施方式

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

下面所描述的本发明不同实施方式中所涉及的技术特征只要彼此之间未构成冲突就可以相互结合。

为了构建高精度、低时延和高有效性的实时定位系统，解决目前室内定位所面临非视距对定位精度影响严重的问题。本发明实施例提供了一种导航定位方法，应用于导航定位系统，如图1所示，该导航定位系统包括设置于目标环境中的若干基站和定位目标上的定位节点(图1中未示出)，定位节点固定有UWB标签和IMU传感器，其中，该导航定位系统基于特定的室内环境结合定位成本，部署相应数量的基站。如在高精度的室内环境部署三到四个基站，定位区域小且定位精度需求不高的环境部署一到两个基站等。

通过将UWB标签简称标签固定在待定位的未知节点，基站和标签之间进行通信。基站向标签发送轮询信号，基站接收到轮询信号后向标签发送一个回应信号，标签接收到回应信号后发送一个结束信号完成一次测距的时间测量，可以测得标签到各个基站的距离信息以及基站之间的到达时间差信息。

通过在未知节点上固定一个IMU传感器，可以通过三轴加速度计和三轴陀螺仪测得未知节点的加速度和角度信息。通过方向矩阵将载体坐标系下的速度和角度信息转化为导航坐标系下进行位置的解算。

如图2所示，本发明实施例提供的导航定位方法主要包括如下步骤：

步骤S101：获取目标环境中当前时刻通过UWB测量各基站与定位节点间的第一测量距离以及各基站与通过IMU预测定位节点在下一时刻的位置间的第二测量距离。

其中，第一测量距离为目标环境中部署的当前基站和标签之间进行通信，所测得标签到当前基站的距离信息，利用IMU传感器测量的速度和角度信息可以解算得到定位节点下一时刻的预测位置，第二测量距离即为当前基站到该预测位置间的距离。

步骤S102：基于目标环境的预设运动距离阈值与第一测量距离和第二测量距离间的关系，对下一时刻各基站进行工作状态识别，确定下一时刻目标环境中处于视距状态下的基站。

其中，基站的工作状态包括：视距状态和非视距状态。当基站与定位标签间存在障碍物遮挡时，基站的工作状态为非视距状态，反之，基站的工作状态即为视距状态。该预设运动距离阈值为提前设置的经验阈值。

步骤S103：根据下一时刻目标环境中处于视距状态下的基站数目，确定相应的定位算法。

其中，通过利用处于视距状态下基站的UWB信号与IMU传感器获取的定位信息进行融合定位，不同的基站数目条件下，对应不同的定位算法，以实现实时精准定位。

步骤S104：基于定位算法对定位节点的下一时刻进行导航定位。

通过上述步骤S101至步骤S104，本发明实施例提供的导航定位方法，通过基于UWB和IMU融合的方式，首先实时识别非视距信号，确定室内环境中处于视距状态下的基站数目，然后以此来灵活选择适应当前环境的定位算法，实现了不同室内环境条件下的实时精准导航定位。

具体地，在一实施例中，鉴于UWB信号受非视距影响严重，信号在传播途径中遇到非视距的阻挡会大大降低测距的精度，因此在定位之前需要进行非视距的识别。分别识别信号从视距情况转变到非视距情况和从非视距情况转变到视距情况。在UWB的测距算法过程中，脉冲信号一共传递了三次。当出现障碍物遮挡时，测得的时间会多次受到NLOS的影响，相比于前一时刻的信号传播时间会出现较大的跳变。在实际情况中，同一环境下(LOS或是NLOS)，基于被测标签在一定的速度范围内，基站和标签之间下一时刻的测量距离与当前测量距离和标签下一时刻的运动距离保持一定的阈值关系。基于三角形的几何阈值关系可知，当前时刻UWB测得的未知节点的距离，以及根据当前时刻IMU传感器测得的未知标签的速度、加速度和已知的初始位置可以计算出下一时刻所运行的路径大小。上述的步骤S102，具体包括如下步骤：

步骤S201：获取当前基站当前时刻的工作状态。示例性地，当前基站的工作状态为视距状态。

步骤S202：判断第二测量距离和第一测量距离的差值的绝对值是否小于预设运动距离阈值。

其中，该预设运动距离阈值用于确定当前基站的工作状态是否发生了变化。当第二测量距离和第一测量距离的差值的绝对值大于预设运动距离阈值时，确定当前基站下一时刻的工作状态与当前时刻的工作状态不同，即当前基站下一时刻的工作状态为非视距状态；当第二测量距离和第一测量距离的差值的绝对值不大于预设运动距离阈值时，确定当前基站下一时刻的工作状态与当前时刻的工作状态相同，即当前基站下一时刻的工作状态为依然为视距状态。

具体地，在本发明实施例中采用基于三角几何阈值检测的非视距识别方法，如图3所示，在当前时刻通过UWB测距半径所在的圆，与通过IMU的当前位置、速度和加速度的先验信息预测未知标签下一时刻所在的运动半径所在的圆和位置构成了图3所示的三角形。根据三角形的几何特点可以得到三边之间的关系。如果是从视距情况转向非视距情况或是非视距情况转向视距情况时无法构成三角形，从而可以进行非视距的阈值检测。ΔS相对于基站与标签之间的距离测量小很多。假设当前时刻的测量距离为d_i,t，下一时刻的运动距离和测量距离分别为ΔS和d_i,t+1。当标签的所处环境没有发生变化时应满足：

d_i,t-ΔS<d_i,t+1<d_i,t+ΔS

当标签所处环境发生改变时则有：

因此，可以先判断d_i,t+1-d_i,t的值。大于0的时候可能存在LOS(视距)转变为NLOS(非视距)的情况，小于0的时候可能存在NLOS转变为LOS。然后再和ΔS进一步判断，以得到当前基站是否发生工作状态的变化，具体如流程图4所示，从而可结合当前基站的工作状态，确定下一时刻当前基站的工作状态。采用上述过程可以确定目标环境中所有基站在下一时刻的工作状态，即确定下一时刻不受NLOS影响的基站的数目，从而根据不受NLOS影响的基站的数量来灵活选择合适的定位算法，提高导航系统定位的精确性。

具体地，由于上述预设运动距离阈值为人为设置的经验阈值，以此判断NLOS时存在一定的误差，不能较为准确的进行阈值的判断。为了提高NLOS识别的准确性，在本发明实施例中，通过如下步骤来确定该预设运动距离阈值：

步骤S301：获取目标环境中各基站在视距状态下的第一样本数据以及在非视距状态下的第二样本数据。

示例性地，如图5所示，在目标环境中设置两个基站和一个标签。障碍物从基站2往基站1方向等距离运动，总共实现N次距离的测量。在基站之间设置障碍物观测非视距的测量距离d₁和实际距离l₁。设置NLOS情况下的样本点的取值为+1，LOS情况下的样本点的取值为-1。将第i次的测量值d₁ ⁱ和实际距离l₁相除得到非视距环境下的样本点移动站的位置也从基站1开始跟随障碍物的位置移动测量，记录视距下的测量距离d₂以及实际距离l₂。将第i次的测量值和实际距离相除得到视距环境下的样本点/>

步骤S302：利用第一样本数据和第二样本数据输入预设运动距离阈值模型进行训练，得到预设运动距离阈值。

具体地，该预设运动距离阈值模型为一种线性划分的模型将NLOS情况下测量的数据和LOS情况下测得数据中最优的预设运动距离阈值解算出来，其中，在本发明实施例中采用支持向量机模型进行预设运动距离阈值的训练。由于移动障碍物会对UWB系统的NLOS测距结果产生规律性的影响，从而使用机器学习的方式，利用支持向量机通过对实际场景中的视距和非视距情况下的距离测量值进行训练，从而能够得到符合实际场景的有效阈值，进而提高NLOS识别结果的准确性。此外，在实际应用中，还可以通过支持向量机进行数据的训练，将训练的结果作为样本点再次放入训练集中作为新的样本，这样根据具体的定位环境可以更新样本点的数据库，使得训练结果更加准确。

具体地，在一实施例中，上述的步骤S103，具体包括如下步骤：

步骤S401：当下一时刻目标环境中处于视距状态下的基站数目小于两个时，确定采用基于两步卡尔曼滤波定位算法。

步骤S402：当目标环境中有两个基站处于视距状态时，确定采用基于双基站的扩展卡尔曼滤波定位算法。

步骤S403：当目标环境中有三个基站处于视距状态时，确定采用基于三基站的扩展卡尔曼滤波定位算法。

步骤S404：当目标环境中有三个以上基站处于视距状态时，确定采用基于到达时间差的定位算法或基于到达时间差与卡尔曼滤波结合的定位算法。

本发明实施例通过提出几种适用于在目标环境中不同数量基站在视距状态的定位算法，从而能够针对环境变化实时灵活调整不同的定位算法，以确保实时定位结果的准确性，并且在各个定位算法中通过融合卡尔曼滤波的方式以进一步提高定位结果的精确性。首先对卡尔曼滤波的原理进行介绍：

卡尔曼滤波(Kalman filtering)一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。对于LOS＝0的情况下，通过位于水平的x,y轴方向的速度信息和加速度信息，进行两次卡尔曼滤波。首先通过对速度和加速度的更新优化，再次通过滤波操作使得位置得到更优解。

卡尔曼滤波的具体实现如下所示，考虑状态空间模型下的状态方程和观测方程分别为：

X(k+1)＝ΦX(k)+ΓW(k)

Y(k)＝HX(k)+V(k)

式中，k为离散时间，Φ为状态转移矩阵，Γ为噪声驱动矩阵，H为观测矩阵。系统在k时刻的状态为X(k)∈R^r，观测信号为Y(k)∈R^m。输入的白噪声为W(k)∈R^r，观测噪声为V(k)∈R^m，两个噪声都是均值为0，且方差分别为Q和R的互不相关的高斯白噪声。

卡尔曼滤波器实现步骤如下，系统的状态均值为U(0)＝E[X(0)]，状态协方差矩阵为P(0)＝var[X(0)]，I_n为n×n的单位矩阵。

1.状态的一步预测：

2.协方差阵的一步预测：

P(k+1|k)＝ΦP(k|k)Φ^T+ΓQΓ^T

3.增益矩阵：

K(k+1)＝P(k+1|k)H^T[HP(k+1|k)H^T+R]^-1

4.状态更新

5.协方差阵更新：

P(k+1|k+1)＝[I_n-K(k+1)H]P(k+1|k)

以上内容为卡尔曼滤波的原理，其他详细内容参见现有技术的相关描述在此不再进行赘述。具体各个定位算法进行定位的工作原理及过程将在下文进行详细的介绍，在此不再进行赘述。

具体地，在一实施例中，由于卡尔曼滤波作为一种线性的滤波算法，短时间内可以有效地减少加速度和陀螺仪的测量误差，而无法保证长见时间的准确定位，主要原因在于加速度计的累计偏移误差。因此，本发明实施例提出一种基于两步卡尔曼滤波定位算法，首先利用基于IMU传感器定位的策略，获得速度和加速度信息，对加速度和速度进行一次卡尔曼滤波，以减少加速度所带来的偏移。然后将更新后的速度和加速度值结合水平和垂直方向的位移大小做一次整体的卡尔曼滤波，从而减少定位误差。当定位算法为基于两步卡尔曼滤波定位算法时，上述的步骤S104具体包括如下步骤：

基于IMU获取当前时刻的加速度和速度；对加速度和速度进行卡尔曼滤波，得到更新后的加速度和速度；步骤S503：基于更新后的加速度和速度，确定定位节点的位移；步骤S504：基于更新后的加速度和速度结合定位节点的位移进行卡尔曼滤波，得到下一时刻定位节点的第一预测位置。

本发明实施例中基于两步卡尔曼滤波定位算法实现定位的原理是通过位于水平的x,y轴方向的速度信息和加速度信息，进行两次卡尔曼滤波的系统框架如图6所示。详细过程如下：

第一步，对速度和加速度进行第一步滤波操作，对测量的速度和加速度去噪。优化降低速度和加速度的观测噪声，使得优化结果更加接近真实值。

首先根据水平和垂直方向的速度与加速度，设置状态向量X₁(k)为：

X₁(k)＝[v_x(k) v_y(k) a_x(k) a_y(k)]

v_x(k)，v_y(k)分别是当前时刻水平和垂直方向的速度，a_x(k)，a_y(k)分别是当前时刻水平和垂直方向的加速度。

根据速度与加速度的关系，可以得到状态方程式为：

v_x(k+1)和v_y(k+1)分别是预测的下一时刻的水平和垂直方向速度值，a_x(k+1)和a_y(k+1)分别是预测的下一时刻的水平和垂直方向加速度值，w_x(k)和w_y(k)分别是水平方向和垂直方向的噪声。T是信号的采样周期。

X₁(k+1)＝F₁X₁(k)+G₁W₁(k)

X₁(k+1)是第一步卡尔曼滤波预测的下一时刻的状态向量，W₁(k)是对应的观测噪声。F₁和G₁分别是对应第一步卡尔曼滤波的的状态转换矩阵和噪声驱动矩阵：

设置观测方程为：

Z₁(k)＝H₁X₁(k)+V₁(k)

H₁和V₁分别是对应第一步卡尔曼滤波的的观测矩阵和观测噪声：

第二步，更新加速度和速度值的大小，再次通过卡尔曼滤波更新下一时刻的位置。这一步卡尔曼滤波的主要作用是优化缓解观测距离的误差值，在计算得到的速度和加速度更优解的基础上得到的定位优化结果。设置状态向量为：

X₂(k)＝[x_x(k) x_y(k) v_x(k) v_y(k) a_x(k) a_y(k)]

根据位移，速度与加速度的关系，可以得到状态方程式为：

x_x(k+1)和x_y(k+1)分别是预测的下一时刻水平和垂直方向位移值，其它参数同上。由于在第一步卡尔曼滤波已经考虑过了加速度和速度的噪声，所以在第二步卡尔曼滤波不再考虑速度和加速度的噪声。

X₂(k+1)＝F₂X₂(k)+G₂W₂(k)

X₂(k+1)是预测的下一时刻的状态向量，F₂和G₂分别是对应第二步卡尔曼滤波的的状态转换矩阵和噪声驱动矩阵：

设置观测方程为：

Z₂(k)＝H₂X₂(k)+V₂(k)

H₂和V₂分别是对应第二步卡尔曼滤波的观测矩阵和观测噪声：

最后通过上述过程可以解算出定位节点在下一时刻的预测位置，并且能够保障该预测位置与实际位置的匹配度，保障目标环境中没有基站或者仅有一个基站处于视距状态时，导航定位系统定位结果的精确性。

具体地，在另一实施例中，当定位算法为基于双基站的扩展卡尔曼滤波定位算法时，上述的步骤S104具体包括如下步骤：

获取当前时刻定位节点的当前位置；通过IMU计算得到定位节点在下一时刻的初始预测位置；基于当前位置和初始预测位置，确定定位节点的第一位移；通过UWB测量得到处于视距状态下的两个基站与定位节点间的距离和角度；基于第一位移和两个基站与定位节点间的距离和角度进行扩展卡尔曼滤波，得到下一时刻定位节点的第二预测位置。

本发明实施例中基于双基站的扩展卡尔曼滤波定位算法实现定位的原理是基于IMU的定位的先验信息，以及UWB的测量信息解算出未知标签到基站的距离和角度信息。设置水平和垂直方向的位移值作为状态向量，将两个基站的距离和角度信息作为观测向量，同时通过雅可比矩阵求得观测矩阵，基于扩展卡尔曼滤波实现更高精度的定位。详细过程如下：

在非线性系统的参数估计中，扩展卡尔曼滤波是一种简单有效的估计算法。同线性卡尔曼滤波基本方程相比，状态转移Φ(k+1|k)和观测矩阵H(k+1)由f[*]和h[*]的雅可比矩阵代替。

选取水平和垂直方向的位移作为观测向量：X(k)＝[x_x(k) x_y(k)]^T。根据标签到两个基站的距离和角度可以解算出标签的位置，采用扩展卡尔曼滤波的方式进行滤波处理。观测向量的观测噪声为：观测噪声的均值为0，/>分别是标签对应两个基站观测角度的噪声，/>分别是标签对应两个基站观测距离的噪声；协方差矩阵为：/>分别是标签对应两个基站观测角度的噪声协方差，/>分别是标签对应两个基站观测距离的噪声协方差。其算法流程如图7所示。Station1和Station2分别表示两个基站的坐标。基于IMU解算的结果即预测位置与两个基站的距离和角度信息作为观测向量：

Arct(X_k+1,station1)，Arct(X_k+1,station1)表示预测的k+1时刻所处状态位置与基站1和基站2之间的角度值。Dist(X_k+1,station1)，Dist(X_k+1,station2)表示预测的k+1时刻所处状态位置与基站1和基站2之间的距离值。V(:,t)是对应时刻t的观测噪声。

通过雅可比矩阵求得对应的观测矩阵为：

式中D1，D2表示预测的标签到基站1和基站2的距离，Z_k+1(1)，Z_k+1(2)表示观测的标签到基站1和基站2的距离。观测矩阵的第一列的分子表示的是水平方向位移的相减，第二列的分子表示的是垂直方向的位移的相减。

最后通过上述过程可以解算出定位节点在下一时刻的预测位置，并且能够保障该预测位置与实际位置的匹配度，提高系统定位的精确性。

最后通过上述过程可以解算出定位节点在下一时刻的预测位置，并且能够保障该预测位置与实际位置的匹配度，保障目标环境中有且仅有两个基站处于视距状态时，导航定位系统定位结果的精确性。

具体地，在另一实施例中，当定位算法为基于三基站的扩展卡尔曼滤波定位算法时，上述的步骤S104具体包括如下步骤：

获取基于两步卡尔曼滤波定位算法对定位节点的下一时刻进行导航定位的第一预测位置、通过UWB测量得到处于视距状态下的三个基站与定位节点间的距离以及当前时刻定位节点的当前位置；基于当前位置和第一预测位置，确定定位节点的第二位移；基于第二位移和三个基站与定位节点间的距离进行扩展卡尔曼滤波，得到下一时刻定位节点的第三预测位置。

本发明实施例中基于三基站的扩展卡尔曼滤波定位算法实现定位的原理是利用IMU经过上述基于两步卡尔曼滤波后的定位信息作为先验信息，设置水平和垂直方向的位移值作为观测向量，将未知标签到各个基站的距离值作为观测向量，同时通过雅可比矩阵求得观测矩阵，利用扩展卡尔曼滤波器进行滤波定位。详细过程如下：

通过上述基于两步卡尔曼滤波定位算法和基于两基站的扩展卡尔曼滤波定位算法，运用到当处于视距状态的基站数量为三的情况下进行定位。IMU经过两次卡尔曼滤波过后的定位结果作为扩展卡尔曼滤波的先验信息。选取水平和垂直方向的位移作为状态向量：X(k)＝[x_x(k) x_y(k)]^T。观测向量的观测噪声为：观测噪声的均值为0，/>分别是标签对应三个基站观测距离的噪声；协方差矩阵为：分别是标签对应三个基站观测距离的噪声协方差；Station1，Station2和Station3分别表示三个基站的坐标。其算法流程如图8所示。基于IMU解算的结果和三个基站之间的距离信息作为观测向量：

Dist(X_k，Station 1)，Dist(X_k，Station 2)，Dist(X_k，Station 3)分别表示预测的k时刻所处状态位置与基站1、基站2及基站3之间的距离值。

通过求解雅可比矩阵得到观测矩阵：

观测向量与观测矩阵与上类似。

最后通过上述过程可以解算出定位节点在下一时刻的预测位置，并且能够保障该预测位置与实际位置的匹配度，保障目标环境中有三个基站处于视距状态时，导航定位系统定位结果的精确性。

具体地，在另一实施例中，当定位算法为基于到达时间差与卡尔曼滤波结合的定位算法时，上述的步骤S104具体包括如下步骤：

获取利用时间差算法计算定位节点在下一时刻的第四预测位置、通过IMU计算定位节点在下一时刻的第五预测位置、通过IMU计算当前时刻的第一速度以及当前时刻定位节点的当前位置；基于当前位置与第四预测位置及第五预测位置的位置关系，分别确定定位节点的第三位移和第四位移；基于第三位移确定当前时刻的第二速度；基于第三位移、第四位移、第三位移与第四位移的位移差值以及第一速度与第二速度的速度差值进行卡尔曼滤波，得到下一时刻定位节点的第六预测位置。

本发明实施例中基于到达时间差与卡尔曼滤波结合的定位算法实现定位的原理是UWB可以通过基于到达时间差算法(简称TDOA)解算出未知标签的位置。将IMU解算出的位置作为先验信息，将UWB解算的水平和垂直方向的位移和速度同IMU解算的水平和垂直方向的位移和速度信息的差值信息作为状态向量，将水平和垂直方向的位移作为观测向量，通过卡尔曼滤波器进行定位解算。详细过程如下：

如图9所示的基于到达时间差算法和卡尔曼滤波的融合定位算法。在视距范围内基站信号充足时，UWB可以通过到达时间差算法得到目标的解算位置。与此同时IMU也能够解算出目标的位置，因此可以充分利用UWB和IMU的测量信息进行卡尔曼滤波。将水平和垂直方向的位移作为状态向量：X(k)＝[x_x(k) x_y(k)]^T。测量方程如下所示：

Z(k)＝HX(k)+V(k)

式中H表示对应的观测矩阵，状态向量中的x_x(k)，x_y(k)分别表示在k时刻标签水平和垂直方向的位移值。观测噪声：观测矩阵的均值为0，其中/>和/>表示k时刻水平和垂直方向位移的观测噪声，/>和/>表示k时刻水平和垂直方向速度的观测噪声，协方差矩阵为：/>σ_x，σ_y分别对应着水平和垂直方向位移的噪声协方差，/> 分别对应着水平和垂直方向速度的噪声协方差。

将两者的解算的水平和垂直方向的位移和速度信息的差值作为卡尔曼滤波的观测向量：

/>

式中分别表示通过UWB在k时刻得到的水平和垂直方向的位移大小，以及水平和垂直方向的速度大小。/>分别表示通过IMU在k时刻得到的水平和垂直方向的位移大小，以及水平和垂直方向的速度大小。

设置观测矩阵：

最后通过上述过程可以解算出定位节点在下一时刻的预测位置，并且能够保障该预测位置与实际位置的匹配度，保障目标环境中有三个以上基站处于视距状态时，即在视距范围内基站信号充足时，导航定位系统定位结果的精确性。

1、具体地，在实际应用中，上述几种定位算法的定位精度存在一定差异，处于视距状态下的基站数量越多，定位的精度越高，但是实际应用场景下并不能保证室内的所有环境下都有足够多满足视距要求的基站进行定位。因此，在本发明实施例中，基于不同算法的定位精度，提出了一种适应环境变化的智能选择算法。当没有UWB基站信息时可以实现短时间的精准定位，在一定的时间节点内位置标签接受三基站或是两基站的视距信息时，可以认定为具有准确的初始位置而可以进行有效的连续定位，超过这段时间进入两基站的环境，由于初始位置的解算不够准确，所以无法依赖IMU的先验信息进行两基站的扩展卡尔曼滤波的高精度定位。当超过这段时间进入三基站时首先根据到达时间差的定位算法解算目标位置，因为到达时间差与卡尔曼相结合的定位算法相较于到达时间差有更高的定位精度，经过一段时间的初始位置校准后改用到达时间差与卡尔曼相结合的滤波定位算法。从而实现适应环境变化的智能定位算法。

在没有基站信号的情况下，利用IMU进行定位。从初始位置开始计算，当运行到实际位置和计算位置之间的误差距离达到所规定的定位精度时，所运行的时间就是适应环境变化的智能定位算法所对应的时间节点。上述实施方式提出了适应于环境的基于到达时间节点的智能定位策略的选择，如图10所示，提出节点时间T是为了解决在LOS信号基站较少的情况下，可能存在定位精度不高的问题而提出的一种定位策略。根据不同的定位基站数量，设定三种不同的状态。当没有视距信号时设定为C状态，由于受加速度计和陀螺仪的偏移其在短时间内能实现高精度的定位。将这个时间节点设置为T，超过这个时间节点则认为不准确的定位。在两基站定位的环境视为B状态，当视距信号充足又不少于三个基站的情况下视为A状态。且在A，B状态下上述实施例中所对应的定位算法都能实现精准定位，由C状态进入其它状态时，可依据时间节点T采取不同的定位算法。

情况一：时间节点T内进入A，B状态，进入A或B状态的时间节点的初始位置准确，可以采用A或B状态相应的定位算法；

情况二：时间节点T外进入B状态，因为初始位置不够准确，采用B状态对应的算法进行定位时，将存在一定的定位偏差；

情况三：时间节点外进入A状态，由于UWB和IMU融合算法都是基于IMU解算的先验位置进行定位，IMU定位依赖初始位置的准确性。因此在A状态下分A1和A2算法。A1算法直接是到达时间差的定位算法，不依赖初始位置就可以进行定位。A2算法是基于到达时间差和卡尔曼滤波定位算法，需要初始位置准确，但是定位精度相较于到达时间差的定位更加准确。因此先用A1算法经过Δt的时间校正初始位置后，采用A2算法进行更准确的定位。

根据具体环境的定位需求而设定时间节点T的大小，如图11所示的计算到达时间的示意图。在C状态下的初始位置的时间开始计算，当达到实际位置和计算的位置之间的误差距离达到规定的定位精度所用的时间即相应定位精度对应的时间节点T。

在本发明实施例中，如图1所示的导航定位系统，包括UWB传感器1、IMU传感器2、非视距识别3、非视距缓解4。其中：

UWB传感器1分别布置基站和标签，基站固定分布在室内环境下，一个室内空间布置3到4个基站，标签固定于定位目标上。通过基站与标签间双边双向的信号传输方式测得基站和标签之间的距离，也可以获得到达时间差的大小。当然，结合特定环境的定位要求和定位成本，也可以在部分室内环境下布置1到2个基站。

IMU传感器2和UWB标签固定在一起，通过测得三轴加速度和三轴角速度获得IMU解算的位置。惯性导航在姿态更新和速度更新的基础上，利用惯性器件输出的角速度和重力加速度信息对载体的位置进行解算。导航坐标系选取地理坐标系，由惯性导航基本公式，可知速度和位置在导航坐标系中的方程分别为：

式中：vⁿ为载体相对于地球的速度在导航坐标系上的投影；为载体坐标系相对于导航坐标系的姿态矩阵；f^b由加速度计在载体坐标系上获取的比力加速度；/>为地球坐标系相对惯性坐标系得旋转角速度在导航坐标系上的投影；/>导航坐标系相对于地理坐标系上的投影；gⁿ为重力加速度在导航系上的投影；P为载体位置，P＝[λ,φ,h]^T，它由距离地球表面的高度h，导航坐标系相对地球坐标系的角速度获得，通常只包含经度和纬度；R_c为当地曲率矩阵。

考虑到导航坐标系旋转的影响，进而可以得到速度的积分公式：

vⁿ(t_k+1)考虑表示计算得到的t_k+1时刻导航坐标系下速度大小，表示导航坐标系下t_k时刻相较于t_k+1时刻的姿态矩阵，/>表示载体坐标系下t_k时刻相较于导航坐标系下t_k时刻的姿态矩阵，/>表示载体坐标系下t时刻相较于t_k时刻的姿态矩阵，/>表示导航坐标系下t时刻相较于t_k时刻的姿态矩阵。

考虑到当地水平坐标系中曲率的影响，进而可以得位置积分公式：

/>

式中r(t_k+1)表示t_k+1时刻的位移大小，H表示测量周期，根据上述公式可以实现基于IMU的定位。

非视距识别3由三角几何阈值检测和支持向量机检测实现。当为了降低系统的复杂度，减轻室内环境部署的复杂度时可以采取三角几何阈值检测的方法。当为了取得更准确的非视距识别时，可以采取支持向量机的阈值检测方法。需要在室内环境中采集在视距和非视距情况下的样本点，通过支持向量机训练得到非视距检测的阈值。

非视距缓解4主要采取针对不同的视距环境而采取的定位方式。UWB通过卡尔曼滤波和IMU结合，提出了基于两步卡尔曼滤波定位算法；基于双基站的扩展卡尔曼滤波定位；基于三基站的扩展卡尔曼滤波定位；基于到达时间差与卡尔曼滤波结合的定位算法。通过到达时间节点的智能定位方式构成高精度的室内定位系统，能够实现在降低定位成本的同时，缓解非视距带来的定位误差，满足室内定位需求。

根据本发明实施例还提供了一种导航定位设备，如图12所示，该导航定位设备可以包括处理器901和存储器902，其中处理器901和存储器902可以通过总线或者其他方式连接，图12中以通过总线连接为例。

处理器901可以为中央处理器(Central Processing Unit，CPU)。处理器901还可以为其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等芯片，或者上述各类芯片的组合。

存储器902作为一种非暂态计算机可读存储介质，可用于存储非暂态软件程序、非暂态计算机可执行程序以及模块，如本发明方法实施例中的方法所对应的程序指令/模块。处理器901通过运行存储在存储器902中的非暂态软件程序、指令以及模块，从而执行处理器的各种功能应用以及数据处理，即实现上述方法实施例中的方法。

存储器902可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储处理器901所创建的数据等。此外，存储器902可以包括高速随机存取存储器，还可以包括非暂态存储器，例如至少一个磁盘存储器件、闪存器件、或其他非暂态固态存储器件。在一些实施例中，存储器902可选包括相对于处理器901远程设置的存储器，这些远程存储器可以通过网络连接至处理器901。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

一个或者多个模块存储在存储器902中，当被处理器901执行时，执行上述方法实施例中的方法。

上述导航定位设备具体细节可以对应参阅上述方法实施例中对应的相关描述和效果进行理解，此处不再赘述。

本领域技术人员可以理解，实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)、随机存储记忆体(Random Access Memory，RAM)、快闪存储器(Flash Memory)、硬盘(Hard Disk Drive，缩写：HDD)或固态硬盘(Solid-StateDrive，SSD)等；存储介质还可以包括上述种类的存储器的组合。

虽然结合附图描述了本发明的实施例，但是本领域技术人员可以在不脱离本发明的精神和范围的情况下作出各种修改和变型，这样的修改和变型均落入由所附权利要求所限定的范围之内。

Claims

1.一种导航定位方法，应用于导航定位系统，所述导航定位系统包括设置于目标环境中的若干基站和定位目标上的定位节点，所述定位节点固定有UWB标签和IMU传感器，其特征在于，所述方法包括：

基于所述定位算法对所述定位节点的下一时刻进行导航定位；

所述预设运动距离阈值的确定过程如下：

利用所述第一样本数据和第二样本数据输入预设运动距离阈值模型进行训练，得到所述预设运动距离阈值；

所述基于所述目标环境的预设运动距离阈值与所述第一测量距离和所述第二测量距离间的关系，对下一时刻各基站进行非视距识别，包括：

当所述第二测量距离和所述第一测量距离的差值的绝对值大于所述预设运动距离阈值时，确定所述当前基站下一时刻的工作状态与当前时刻的工作状态不同；

所述根据下一时刻所述目标环境中处于视距状态下的基站数目，确定相应的定位算法，包括：

2.根据权利要求1所述的方法，其特征在于，当所述定位算法为基于两步卡尔曼滤波定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

基于IMU获取当前时刻的加速度和速度；

基于更新后的加速度和速度，确定所述定位节点的位移；

3.根据权利要求1所述的方法，其特征在于，当所述定位算法为基于双基站的扩展卡尔曼滤波定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

获取当前时刻所述定位节点的当前位置；

通过IMU计算得到所述定位节点在下一时刻的初始预测位置；

4.根据权利要求3所述的方法，其特征在于，当所述定位算法为基于三基站的扩展卡尔曼滤波定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

5.根据权利要求1所述的方法，其特征在于，当所述定位算法为基于到达时间差与卡尔曼滤波结合的定位算法时，所述基于所述定位算法对所述定位节点的下一时刻进行导航定位，包括：

基于所述第三位移确定当前时刻的第二速度；

6.一种导航定位设备，应用于导航定位系统，所述导航定位系统包括设置于目标环境中的若干基站和定位目标上的定位节点，所述定位节点固定有UWB标签和IMU传感器，其特征在于，所述导航定位设备包括：

存储器和处理器，所述存储器和所述处理器之间互相通信连接，所述存储器中存储有计算机指令，所述处理器通过执行所述计算机指令，从而执行权利要求1-5任一项所述的方法。

7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机指令，所述计算机指令用于使所述计算机从而执行权利要求1-5任一项所述的方法。