一种基于人工信标的多源协同位姿确定方法、装置及系统

技术领域

本发明涉及无线信号定位技术领域，具体而言，涉及一种基于人工信标的多源协同位姿确定方法、装置及系统。

背景技术

现代导航技术中，全球卫星导航系统(Global Navigation Satellite System，GNSS)是在军用和民用领域应用最为广泛的定位手段。随着计算机技术和移动通信网络技术的发展，人们对位置服务(Location Based Service，LBS)的关注度不断提升，对定位的需求也不断提高，GNSS的一些弊端也随之显现。在建筑物遮挡、树荫遮蔽等一些不开阔的环境中，GNSS接收机信号会受到干扰，GNSS测量误差会增大。在室内和地下等更为复杂的环境下，GNSS信号能够被阻断，难以提供高精度稳定的定位。

目前，在室内和地下等GNSS信号失效环境中常用以下几种定位方法：(1)射频识别(RFID)、蓝牙、超宽带(UWB)、Wi-Fi、无线传感器网络(WSN)、超声波等无线定位技术。无线定位技术主要根据基站发出的信息实现目标的导航解算，容易受到非视距传播、多径传播以及其它干扰等因素的影响，导致导航精度降低；(2)基于惯性导航的定位方法，该方法自主性强、可靠性高、动态性能好，能够提供丰富的导航信息，但其精度受传感器精度影响大，定位精度会随工作时间增长而不断累积；(3)基于地磁匹配的定位方法，该方法在一些地磁场特征不明显的地带，匹配精度较差，且容易受墙内的钢筋等导磁性材料和电子设备的影响，鲁棒性较差；(4)基于视觉的定位方法，该方法容易受室内光线的影响，对于快速运动的目标容易失真，且成本较高，具有一定的局限性；(5)基于磁信标的定位方法，也属于一种无线定位方式。

从以上分析可以看出，现有在室内和地下等GNSS信号失效环境中常用的定位方法在一些特殊的环境中或多或少存在一些局限性。

发明内容

针对上述现有方法存在的问题，本发明旨在提出一种基于人工信标的多源协同位姿确定方法、装置及系统，以解决上述背景方法难以实现在室内、地下等复杂环境下的高精度、高可靠性的定位、定姿问题。

为了达到上述目的，本发明采用了下列技术方案：

本发明提供了一种基于人工信标的多源协同位姿确定方法，包括以下步骤：

S1、获取多个磁信标在目标空间点处的磁场特征矢量，并获取超带宽(UWB)的状态输出和惯性导航系统(INS)的状态输出；

S2、根据所述磁场特征矢量确定所述目标的位置，根据所述超带宽的状态输出确定所述目标的位置，同时根据所述惯性导航系统的状态输出确定所述目标的位置；

S3、根据所述三种定位方式的位置信息确定目标的最优位置，并根据所述目标的最优位置确定所述目标的姿态。

进一步，所述步骤S1中获取多个磁信标在目标空间点处的磁场特征矢量的过程为：建立磁信标模型；根据所述磁信标模型确定磁感应强度矢量；根据所述磁感应强度矢量确定磁场的三轴分量；根据所述磁场的三轴分量确定目标的特征角；根据所述目标的特征角确定所述多个磁信标在目标处的磁场特征矢量。

更进一步，所述获取多个磁信标在目标空间点处的磁场特征矢量的具体过程为：

S11、基于双磁偶极子模型，在应用环境中设置至少三个工作频率互不相同的磁信标，从而建立所述磁信标模型，将多个磁信标的坐标统一到同一个坐标系下，以统一后的坐标系为基准，各个磁信标的位置为

S12、根据磁信标模型确定磁感应强度矢量，磁感应强度矢量表示为B＝[B

目标位置测量到的磁场模型可表示为：

其中，M为对应磁信标的磁矩，μ为磁场传播介质因子，r为目标与磁信标之间的距离，R

S13、根据测量得到的相对俯仰角

进一步，所述步骤S2中根据所述磁场特征矢量确定所述目标的位置的具体过程为：

S21、根据余弦定理确定磁场特征矢量与目标位置的关系，其公式为：

/>

其中，d

S22、根据S21所述关系确定目标的位置，公式为：

其中，P

进一步，所述步骤S2中根据所述超带宽的状态输出确定所述目标的位置的具体过程为：

采用TDOA定位方法，测量定位标签与各定位基站的距离差，并将定位基站作为焦点，距离差作为长轴作出双曲线模型，双曲线的交点即是未知定位标签的位置；设四个定位基站的坐标已知分别为BS

通过相关算法解以上方程组，即可得到基于TDOA定位方法下的目标位置。

进一步，所述步骤S2中根据所述惯性导航系统的状态输出确定所述目标的位置的具体过程为：

采集惯性测量单元中加速度计和陀螺仪的原始数据，通过惯性导航机械编排进行导航解算得到目标的速度、姿态和位置；其位置更新公式为：

进一步，所述步骤S3中根据所述三种定位方式的位置信息确定目标的最优位置的具体过程为：

通过基于分布式滤波的数据融合算法融合三种方式的定位信息，建立融合系统方程为：

x

z

将非线性方程一阶线性化后可以表示为：

x

z

预测状态向量：

预测误差协方差：

卡尔曼滤波增益：

状态更新：

协方差更新：P

通过以上滤波公式不断迭代得到最优的位置估计：

上式中，x

进一步，所述步骤S3中根据所述目标的最优位置确定所述目标的姿态的具体过程为：

根据解算得到的最优位置

其中

目标相对于磁信标的旋转矩阵

由上式即可求解得到目标的姿态角。

本发明还提供了一种基于人工信标的多源协同位姿确定装置，包括：

获取单元：所述获取单元用于获取多个磁信标在目标空间点处的磁场特征矢量，并获取超宽带与惯性导航系统的状态输出；

协同处理与通信单元：所述协同处理与通信单元用于根据所述磁场特征矢量确定所述目标的位置，以及根据所述超宽带的状态输出确定所述目标的位置，根据所述惯性导航系统的状态输出确定所述目标的位姿；

协同计算单元：所述协同计算单元用于根据所述三种定位方式的位置信息确定所述目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。

本发明第三方面提供了一种基于人工信标的多源协同位姿确定系统，包括：多个工作频率互不相同的磁信标以及上文所述的基于人工信标的多源协同位姿确定装置。

与现有技术相比本发明具有以下优点：

本发明通过融合三种定位方式的位置信息，获得了一个最优的目标位置和实时姿态，能够抑制INS定位误差随时间累积以及UWB受非视距传播、多径传播和其它干扰的影响，充分利用了每一种定位方法的优势，实现了在室内、地下等复杂环境下的长时间导航服务，具有定位定姿功能，有效地提升了本发明系统和方法的定位精度。

本发明在建立磁信标模型时，设置各个磁信标的工作频率各不相同，一方面能够减小地磁场的影响，另一方面能很好地区分测量到的磁场信号来自于哪个磁信标，进而可提高本发明系统和方法的定位精度。

本发明在建立磁信标模型时，基于双磁信标模型比基于单磁信标模型能够更加准确的描述螺线管的磁场分布，同时在应用环境中设置多个磁信标来建立磁信标模型，能实现更加精确的定位，从而有效地提高了本发明系统和方法的定位性能。

本发明通过磁信标模型确定磁感应强度矢量来得到磁信标在目标处的磁场特征矢量，受磁场衰减的影响较小，能够提升系统的鲁棒性，提高本发明系统和方法的定位精度。

本发明基于人工信标的多源协同位姿确定方法中，通过磁信标方法进行定位，信号能够较为深入地穿透地面或建筑物，从而可提高本发明系统和方法在复杂环境下的定位能力；采用TDOA定位方法，相比于到达时间(TOA)定位方法，不需要进行基站和移动终端之间的时间同步，减少了系统中时钟同步需求，降低了本发明系统和方法的复杂度；采用INS进行定位，自主性好，抗干扰能力强，能够提升本发明系统和方法在复杂环境中定位的适应能力。

附图说明

图1为本发明所述的基于人工信标的多源协同位姿确定方法的流程图；

图2为本发明所述的多磁信标模型示意图；

图3为多源协同位姿确定的示意图；

图4为本发明所述的基于人工信标的多源协同位姿确定装置的示意图。

具体实施方式

下面结合本发明实施例和附图，对本发明的技术方案进行具体、详细的说明。

如图1所示，本发明实施例提供一种基于人工信标的多源协同位姿确定方法，包括：获取多个磁信标在目标空间点处的磁场特征矢量，并获取超带宽(UWB)与惯性导航系统(INS)的状态输出；根据所述磁场特征矢量确定所述目标的位置，根据所述UWB的状态输出确定所述目标的位置，根据所述INS的状态输出确定所述目标的位姿；根据所述三种定位方式的位置信息确定目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。

具体地，在本实施例中，获取磁信标在目标空间点处的磁场特征矢量包括：建立磁信标模型；根据所述磁信标模型确定磁感应强度矢量；根据所述磁感应强度矢量确定磁场的三轴分量；根据所述磁场的三轴分量确定目标的特征角；根据所述目标的特征角确定所述多个磁信标在目标处的磁场特征矢量。

具体地，在本实施例中，磁信标模型的建立过程包括：基于双磁偶极子模型，在应用环境中设置至少三个磁信标(下文以设置三个为例进行描述)，同时将各个磁信标设置为不同的工作频率，一方面能够减小地磁场的影响，另一方面方便区分测量到的磁场信号来自于哪个磁信标，从而建立磁信标模型。

具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中获取多个磁信标在目标空间点处的磁场特征矢量的具体过程为：

(1)结合图2所示的磁信标模型图，基于双磁偶极子模型，在应用环境中设置三个工作频率(10Hz～1000Hz)互不相同的磁信标，并将三个磁信标的坐标统一到同一个坐标系下，以统一后的坐标系为基准，各个磁信标的位置为

三个磁信标的坐标位置和工作频率如下表1所示。

表1：磁信标坐标位置及工作频率

(2)根据磁信标模型确定磁感应强度矢量，磁感应强度矢量表示为B＝[B

图2中点P为待测目标，各个磁信标对应的磁感应强度矢量可表示为：

目标位置测量到的磁场模型可表示为：

其中M为对应磁信标的磁矩，μ为磁场传播介质因子，r为目标与磁信标之间的距离，R

(3)根据测量得到的相对俯仰角

其中

其中，采用矢量的方式求解，能减少磁场衰减的影响，增强系统的鲁棒性。

具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述磁场特征矢量确定所述目标的位置的具体过程为：

根据余弦定理确定磁场特征矢量与目标位置的关系，其公式为：

根据所述关系确定目标的位置，其所述公式为：

具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述超带宽的状态输出确定所述目标的位置的具体过程为：

采用TDOA定位方法，测量定位标签与各定位基站的距离差，并将定位基站作为焦点，距离差作为长轴作出双曲线模型，双曲线的交点即是未知定位标签的位置。设四个定位基站的坐标已知分别为BS

通过相关算法解以上方程组，即可得到基于TDOA定位方法下的目标位置。

具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述惯性导航系统的状态输出确定所述目标的位置的具体过程为：

采集惯性测量单元中加速度计和陀螺仪的原始数据，通过惯性导航机械编排进行导航解算得到目标的速度、姿态和位置。其位置更新公式为：

其中P

具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述三种定位方式的位置信息确定目标的最优位置的具体过程为：

如图3所示，通过基于分布式滤波的数据融合算法融合三种方式的定位信息，建立融合系统方程为：

x

z

将非线性方程一阶线性化后可以表示为：

x

z

预测状态向量：

预测误差协方差：

卡尔曼滤波增益：

状态更新：

协方差更新：P

通过以上滤波公式不断迭代得到最优的位置估计：

上式中，x

具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述目标的最优位置确定所述目标的姿态的具体过程为：

根据解算得到的最优位置

其中/>

目标相对于磁信标的旋转矩阵

由上式即可求解得到目标的姿态角。

本发明另一实施例提供一种基于人工信标的多源协同位姿确定装置，包括：

获取单元，所述获取单元用于获取多个磁信标在目标空间点处的磁场特征矢量，并获取超宽带与惯性导航系统的状态输出；

协同处理与通信单元，所述协同处理与计算单元用于根据所述磁场特征矢量确定所述目标的位置，以及根据所述UWB的状态输出确定所述目标的位置，根据所述INS的状态输出确定所述目标的位姿；

协同计算单元，所述协同计算单元用于根据所述三种定位方式的位置信息确定所述目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。

本发明另一实施例提供一种基于人工信标的多源协同位姿确定系统，包括：多个工作频率互不相同的磁信标以及上述基于人工信标的多源协同位姿确定装置。

虽然本发明公开披露如上，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。