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

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

Info

Publication number
CN115979256A
CN115979256A CN202211652321.0A CN202211652321A CN115979256A CN 115979256 A CN115979256 A CN 115979256A CN 202211652321 A CN202211652321 A CN 202211652321A CN 115979256 A CN115979256 A CN 115979256A
Authority
CN
China
Prior art keywords
magnetic
target
beacon
determining
vector
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211652321.0A
Other languages
English (en)
Inventor
刘鹏
张慧云
徐秋灵
张鹏
李孟委
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
North University of China
Original Assignee
North University of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by North University of China filed Critical North University of China
Priority to CN202211652321.0A priority Critical patent/CN115979256A/zh
Publication of CN115979256A publication Critical patent/CN115979256A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Navigation (AREA)

Abstract

本发明涉及无线信号定位技术领域，公开了一种基于人工信标的多源协同位姿确定方法、装置及系统。本发明通过融合磁信标、超宽带(UWB)与惯性导航系统(INS)三种定位方式的位置信息，获得一个最优的目标位置和实时姿态，该方法能够抑制INS定位误差随时间累积以及UWB受非视距传播、多径传播和其它干扰的影响，充分利用每一种定位方法的优势，实现在室内、地下等复杂环境下的长时间导航服务，具有定位定姿功能，有效地提升了本发明系统和方法的定位精度。

Description

一种基于人工信标的多源协同位姿确定方法、装置及系统
技术领域
本发明涉及无线信号定位技术领域，具体而言，涉及一种基于人工信标的多源协同位姿确定方法、装置及系统。
背景技术
现代导航技术中，全球卫星导航系统(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、基于双磁偶极子模型，在应用环境中设置至少三个工作频率互不相同的磁信标，从而建立所述磁信标模型，将多个磁信标的坐标统一到同一个坐标系下，以统一后的坐标系为基准，各个磁信标的位置为
Figure BDA0004011104090000031
Figure BDA0004011104090000032
S12、根据磁信标模型确定磁感应强度矢量，磁感应强度矢量表示为B＝[Bx,By,Bz]T，Bx,By和Bz对应为三轴分量，各个磁信标对应的磁感应强度矢量可表示为：
Figure BDA0004011104090000033
Figure BDA0004011104090000034
目标位置测量到的磁场模型可表示为：
Figure BDA0004011104090000035
其中，M为对应磁信标的磁矩，μ为磁场传播介质因子，r为目标与磁信标之间的距离，Rd、hd为磁信标参数相关的等效尺寸，
Figure BDA0004011104090000036
为相对俯仰角，θ为相对方位角；
S13、根据测量得到的相对俯仰角
Figure BDA0004011104090000037
和相对方位角θ，计算磁信标在目标处的磁场特征矢量
Figure BDA0004011104090000038
其中
Figure BDA0004011104090000039
和θ可由目标处的磁场的三轴分量计算得到：
Figure BDA0004011104090000041
Figure BDA0004011104090000042
进一步，所述步骤S2中根据所述磁场特征矢量确定所述目标的位置的具体过程为：
S21、根据余弦定理确定磁场特征矢量与目标位置的关系，其公式为：
Figure BDA0004011104090000043
其中，dij表示磁信标i与磁信标j之间的距离，di表示磁信标i与目标之间的距离，vi和vj分别表示磁信标i和磁信标j在目标空间点处的特征矢量，αij为vi和vj之间的夹角；
S22、根据S21所述关系确定目标的位置，公式为：
Figure BDA0004011104090000044
其中，P1＝(p1x,p1y,p1z,......)表示由磁信标解算得到的位置，
Figure BDA0004011104090000045
Figure BDA0004011104090000046
表示各个所述磁信标的位置。
进一步，所述步骤S2中根据所述超带宽的状态输出确定所述目标的位置的具体过程为：
采用TDOA定位方法，测量定位标签与各定位基站的距离差，并将定位基站作为焦点，距离差作为长轴作出双曲线模型，双曲线的交点即是未知定位标签的位置；设四个定位基站的坐标已知分别为BSi(xi,yi),i＝1,2,3,4，待测定位标签为MS(x0,y0)，定位标签MS的超带宽信号到定位基站BSi的时间为ti，ti,1为定位标签到两基站的时间差，c为光速；将基站1作为参考基站，则TDOA的算法表达式为：
Figure BDA0004011104090000051
通过相关算法解以上方程组，即可得到基于TDOA定位方法下的目标位置。
进一步，所述步骤S2中根据所述惯性导航系统的状态输出确定所述目标的位置的具体过程为：
采集惯性测量单元中加速度计和陀螺仪的原始数据，通过惯性导航机械编排进行导航解算得到目标的速度、姿态和位置；其位置更新公式为：
Figure BDA0004011104090000052
Figure BDA0004011104090000053
其中Pm为m时刻的位置，Pm-1为m-1时刻的位置，T为采样时间，
Figure BDA0004011104090000054
L表示当地纬度，RNh＝RN+h，RMh＝RM+h，h为当地高度，RN为卯酉圈半径，RM为子午圈半径。
进一步，所述步骤S3中根据所述三种定位方式的位置信息确定目标的最优位置的具体过程为：
通过基于分布式滤波的数据融合算法融合三种方式的定位信息，建立融合系统方程为：
xk＝f(xk-1)+wk-1
zk＝h(xk)+vk
将非线性方程一阶线性化后可以表示为：
xk＝Φk/k-1xk-1+wk-1
zk＝Hkxk+vk
预测状态向量：
Figure BDA0004011104090000055
预测误差协方差：
Figure BDA0004011104090000056
卡尔曼滤波增益：
Figure BDA0004011104090000057
状态更新：
Figure BDA0004011104090000058
协方差更新：Pk＝(I-KkHk)Pk/k-1
通过以上滤波公式不断迭代得到最优的位置估计：
Figure BDA0004011104090000061
上式中，xk是k时刻的n维状态向量，f(xk-1)是n维非线性向量函数，wk-1是系统噪声向量，zk是k时刻的m维量测向量，h(xk)是m维非线性向量函数，vk是量测噪声向量，E(wk)＝0，E(vk)＝0，
Figure BDA0004011104090000062
Figure BDA0004011104090000063
Φk/k-1为非线性函数f(xk-1)线性化后的状态转移矩阵，Hk为非线性函数h(xk)线性化后的量测矩阵。
进一步，所述步骤S3中根据所述目标的最优位置确定所述目标的姿态的具体过程为：
根据解算得到的最优位置
Figure BDA0004011104090000064
可计算出估计目标特征矢量
Figure BDA0004011104090000065
Figure BDA0004011104090000066
测量特征矢量
Figure BDA0004011104090000067
与估计目标特征矢量
Figure BDA0004011104090000068
存在如下关系：
Figure BDA0004011104090000069
其中
Figure BDA00040111040900000610
可以根据测量值
Figure BDA00040111040900000611
得到，
Figure BDA00040111040900000612
Mi为磁信标i的坐标，
Figure BDA00040111040900000613
表示磁性标i与目标之间的距离，
Figure BDA00040111040900000614
为目标相对于磁信标的旋转矩阵；
目标相对于磁信标的旋转矩阵
Figure BDA00040111040900000615
其中，
Figure BDA00040111040900000616
Figure BDA00040111040900000617
令航向角为θ，俯仰角为
Figure BDA00040111040900000618
横滚角为φ，c代表cos函数，s代表sin函数，可得
Figure BDA00040111040900000619
Figure BDA00040111040900000620
由上式即可求解得到目标的姿态角。
本发明还提供了一种基于人工信标的多源协同位姿确定装置，包括：
获取单元：所述获取单元用于获取多个磁信标在目标空间点处的磁场特征矢量，并获取超宽带与惯性导航系统的状态输出；
协同处理与通信单元：所述协同处理与通信单元用于根据所述磁场特征矢量确定所述目标的位置，以及根据所述超宽带的状态输出确定所述目标的位置，根据所述惯性导航系统的状态输出确定所述目标的位姿；
协同计算单元：所述协同计算单元用于根据所述三种定位方式的位置信息确定所述目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。
本发明第三方面提供了一种基于人工信标的多源协同位姿确定系统，包括：多个工作频率互不相同的磁信标以及上文所述的基于人工信标的多源协同位姿确定装置。
与现有技术相比本发明具有以下优点：
本发明通过融合三种定位方式的位置信息，获得了一个最优的目标位置和实时姿态，能够抑制INS定位误差随时间累积以及UWB受非视距传播、多径传播和其它干扰的影响，充分利用了每一种定位方法的优势，实现了在室内、地下等复杂环境下的长时间导航服务，具有定位定姿功能，有效地提升了本发明系统和方法的定位精度。
本发明在建立磁信标模型时，设置各个磁信标的工作频率各不相同，一方面能够减小地磁场的影响，另一方面能很好地区分测量到的磁场信号来自于哪个磁信标，进而可提高本发明系统和方法的定位精度。
本发明在建立磁信标模型时，基于双磁信标模型比基于单磁信标模型能够更加准确的描述螺线管的磁场分布，同时在应用环境中设置多个磁信标来建立磁信标模型，能实现更加精确的定位，从而有效地提高了本发明系统和方法的定位性能。
本发明通过磁信标模型确定磁感应强度矢量来得到磁信标在目标处的磁场特征矢量，受磁场衰减的影响较小，能够提升系统的鲁棒性，提高本发明系统和方法的定位精度。
本发明基于人工信标的多源协同位姿确定方法中，通过磁信标方法进行定位，信号能够较为深入地穿透地面或建筑物，从而可提高本发明系统和方法在复杂环境下的定位能力；采用TDOA定位方法，相比于到达时间(TOA)定位方法，不需要进行基站和移动终端之间的时间同步，减少了系统中时钟同步需求，降低了本发明系统和方法的复杂度；采用INS进行定位，自主性好，抗干扰能力强，能够提升本发明系统和方法在复杂环境中定位的适应能力。
附图说明
图1为本发明所述的基于人工信标的多源协同位姿确定方法的流程图；
图2为本发明所述的多磁信标模型示意图；
图3为多源协同位姿确定的示意图；
图4为本发明所述的基于人工信标的多源协同位姿确定装置的示意图。
具体实施方式
下面结合本发明实施例和附图，对本发明的技术方案进行具体、详细的说明。
如图1所示，本发明实施例提供一种基于人工信标的多源协同位姿确定方法，包括：获取多个磁信标在目标空间点处的磁场特征矢量，并获取超带宽(UWB)与惯性导航系统(INS)的状态输出；根据所述磁场特征矢量确定所述目标的位置，根据所述UWB的状态输出确定所述目标的位置，根据所述INS的状态输出确定所述目标的位姿；根据所述三种定位方式的位置信息确定目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。
具体地，在本实施例中，获取磁信标在目标空间点处的磁场特征矢量包括：建立磁信标模型；根据所述磁信标模型确定磁感应强度矢量；根据所述磁感应强度矢量确定磁场的三轴分量；根据所述磁场的三轴分量确定目标的特征角；根据所述目标的特征角确定所述多个磁信标在目标处的磁场特征矢量。
具体地，在本实施例中，磁信标模型的建立过程包括：基于双磁偶极子模型，在应用环境中设置至少三个磁信标(下文以设置三个为例进行描述)，同时将各个磁信标设置为不同的工作频率，一方面能够减小地磁场的影响，另一方面方便区分测量到的磁场信号来自于哪个磁信标，从而建立磁信标模型。
具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中获取多个磁信标在目标空间点处的磁场特征矢量的具体过程为：
(1)结合图2所示的磁信标模型图，基于双磁偶极子模型，在应用环境中设置三个工作频率(10Hz～1000Hz)互不相同的磁信标，并将三个磁信标的坐标统一到同一个坐标系下，以统一后的坐标系为基准，各个磁信标的位置为
Figure BDA0004011104090000091
Figure BDA0004011104090000092
磁信标信号的频率越低，磁场的穿透能力越强，将磁信标频率设置在合适的范围内，能够很好地实现在室内和地下等复杂环境中的定位。同时，将三个磁信标设置为不同的频率，能够很好地区分测量到的磁场信号来自于哪个磁信标，减小地磁场的干扰。
三个磁信标的坐标位置和工作频率如下表1所示。
表1：磁信标坐标位置及工作频率
Figure BDA0004011104090000093
(2)根据磁信标模型确定磁感应强度矢量，磁感应强度矢量表示为B＝[Bx,By,Bz]T，Bx,By和Bz对应为三轴分量；
图2中点P为待测目标，各个磁信标对应的磁感应强度矢量可表示为：
Figure BDA0004011104090000101
Figure BDA0004011104090000102
目标位置测量到的磁场模型可表示为：
Figure BDA0004011104090000103
其中M为对应磁信标的磁矩，μ为磁场传播介质因子，r为目标与磁信标之间的距离，Rd、hd为磁信标参数相关的等效尺寸，
Figure BDA0004011104090000104
为相对俯仰角，θ为相对方位角。
(3)根据测量得到的相对俯仰角
Figure BDA0004011104090000105
和相对方位角θ，计算磁信标在目标处的磁场特征矢量
Figure BDA0004011104090000106
其中
Figure BDA0004011104090000107
和θ的意义如图2所示，可由目标处的磁场的三轴分量计算得到：
Figure BDA0004011104090000108
Figure BDA0004011104090000109
其中，采用矢量的方式求解，能减少磁场衰减的影响，增强系统的鲁棒性。
具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述磁场特征矢量确定所述目标的位置的具体过程为：
根据余弦定理确定磁场特征矢量与目标位置的关系，其公式为：
Figure BDA0004011104090000111
Figure BDA0004011104090000112
其中，dij表示磁信标i与磁信标j之间的距离，di表示磁信标i与目标之间的距离，vi和vj分别表示磁信标i和磁信标j在目标空间点处的特征矢量，αij为vi和vj之间的夹角；
根据所述关系确定目标的位置，其所述公式为：
Figure BDA0004011104090000113
Figure BDA0004011104090000114
其中，P1＝(p1x,p1y,p1z)表示由磁信标解算得到的位置，
Figure BDA0004011104090000115
表示各个所述磁信标的位置。
具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述超带宽的状态输出确定所述目标的位置的具体过程为：
采用TDOA定位方法，测量定位标签与各定位基站的距离差，并将定位基站作为焦点，距离差作为长轴作出双曲线模型，双曲线的交点即是未知定位标签的位置。设四个定位基站的坐标已知分别为BSi(xi,yi),i＝1,2,3,4，待测定位标签为MS(x0,y0)，定位标签MS的UWB信号到定位基站BSi的时间为ti，ti,1为定位标签到两基站的时间差，c为光速。将基站1作为参考基站，则TDOA的算法表达式为：
Figure BDA0004011104090000116
通过相关算法解以上方程组，即可得到基于TDOA定位方法下的目标位置。
具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述惯性导航系统的状态输出确定所述目标的位置的具体过程为：
采集惯性测量单元中加速度计和陀螺仪的原始数据，通过惯性导航机械编排进行导航解算得到目标的速度、姿态和位置。其位置更新公式为：
Figure BDA0004011104090000121
其中Pm为m时刻的位置，Pm-1为m-1时刻的位置，T为采样时间，
Figure BDA0004011104090000122
L表示当地纬度，RNh＝RN+h，RMh＝RM+h，h为当地高度，RN为卯酉圈半径，RM为子午圈半径。
具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述三种定位方式的位置信息确定目标的最优位置的具体过程为：
如图3所示，通过基于分布式滤波的数据融合算法融合三种方式的定位信息，建立融合系统方程为：
xk＝f(xk-1)+wk-1
zk＝h(xk)+vk
将非线性方程一阶线性化后可以表示为：
xk＝Φk/k-1xk-1+wk-1
zk＝Hkxk+vk
预测状态向量：
Figure BDA0004011104090000123
预测误差协方差：
Figure BDA0004011104090000124
卡尔曼滤波增益：
Figure BDA0004011104090000125
状态更新：
Figure BDA0004011104090000126
协方差更新：Pk＝(I-KkHk)Pk/k-1
通过以上滤波公式不断迭代得到最优的位置估计：
Figure BDA0004011104090000127
上式中，xk是k时刻的n维状态向量，f(xk-1)是n维非线性向量函数，wk-1是系统噪声向量，zk是k时刻的m维量测向量，h(xk)是m维非线性向量函数，vk是量测噪声向量，E(wk)＝0，E(vk)＝0，
Figure BDA0004011104090000128
Figure BDA0004011104090000131
Φk/k-1为非线性函数f(xk-1)线性化后的状态转移矩阵，Hk为非线性函数h(xk)线性化后的量测矩阵。
具体地，在本实施例中，基于人工信标的多源协同位姿确定方法中根据所述目标的最优位置确定所述目标的姿态的具体过程为：
根据解算得到的最优位置
Figure BDA0004011104090000132
可计算出估计目标特征矢量
Figure BDA0004011104090000133
Figure BDA0004011104090000134
测量特征矢量
Figure BDA0004011104090000135
与估计目标特征矢量
Figure BDA0004011104090000136
存在如下关系：
Figure BDA0004011104090000137
其中
Figure BDA0004011104090000138
可以根据测量值
Figure BDA0004011104090000139
得到，
Figure BDA00040111040900001310
Mi为磁信标i的坐标，
Figure BDA00040111040900001311
表示磁性标i与目标之间的距离，
Figure BDA00040111040900001312
为目标相对于磁信标的旋转矩阵。
目标相对于磁信标的旋转矩阵
Figure BDA00040111040900001313
其中，
Figure BDA00040111040900001314
Figure BDA00040111040900001315
令航向角为θ，俯仰角为
Figure BDA00040111040900001316
横滚角为φ，c代表cos函数，s代表sin函数，可得
Figure BDA00040111040900001317
Figure BDA00040111040900001318
由上式即可求解得到目标的姿态角。
本发明另一实施例提供一种基于人工信标的多源协同位姿确定装置，包括：
获取单元，所述获取单元用于获取多个磁信标在目标空间点处的磁场特征矢量，并获取超宽带与惯性导航系统的状态输出；
协同处理与通信单元，所述协同处理与计算单元用于根据所述磁场特征矢量确定所述目标的位置，以及根据所述UWB的状态输出确定所述目标的位置，根据所述INS的状态输出确定所述目标的位姿；
协同计算单元，所述协同计算单元用于根据所述三种定位方式的位置信息确定所述目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。
本发明另一实施例提供一种基于人工信标的多源协同位姿确定系统，包括：多个工作频率互不相同的磁信标以及上述基于人工信标的多源协同位姿确定装置。
虽然本发明公开披露如上，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (10)

1.一种基于人工信标的多源协同位姿确定方法，其特征在于，包括以下步骤：
S1、获取多个磁信标在目标空间点处的磁场特征矢量，并获取超带宽的状态输出和惯性导航系统的状态输出；
S2、根据所述磁场特征矢量确定所述目标的位置，根据所述超带宽的状态输出确定所述目标的位置，同时根据所述惯性导航系统的状态输出确定所述目标的位置；
S3、根据所述三种定位方式的位置信息确定目标的最优位置，并根据所述目标的最优位置确定所述目标的姿态。
2.根据权利要求1所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述步骤S1中获取多个磁信标在目标空间点处的磁场特征矢量的过程为：建立磁信标模型；根据所述磁信标模型确定磁感应强度矢量；根据所述磁感应强度矢量确定磁场的三轴分量；根据所述磁场的三轴分量确定目标的特征角；根据所述目标的特征角确定所述多个磁信标在目标处的磁场特征矢量。
3.根据权利要求2所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述获取多个磁信标在目标空间点处的磁场特征矢量的具体过程为：
S11、基于双磁偶极子模型，在应用环境中设置至少三个工作频率互不相同的磁信标，从而建立所述磁信标模型，将多个磁信标的坐标统一到同一个坐标系下，以统一后的坐标系为基准，各个磁信标的位置为
Figure FDA0004011104080000011
Figure FDA0004011104080000012
S12、根据磁信标模型确定磁感应强度矢量，磁感应强度矢量表示为B＝[Bx,By,Bz]T，Bx,By和Bz对应为三轴分量，各个磁信标对应的磁感应强度矢量可表示为：
Figure FDA0004011104080000021
Figure FDA0004011104080000022
目标位置测量到的磁场模型可表示为：
Figure FDA0004011104080000023
其中，M为对应磁信标的磁矩，μ为磁场传播介质因子，r为目标与磁信标之间的距离，Rd、hd为磁信标参数相关的等效尺寸，
Figure FDA0004011104080000024
为相对俯仰角，θ为相对方位角；
S13、根据测量得到的相对俯仰角
Figure FDA0004011104080000025
和相对方位角θ，计算磁信标在目标处的磁场特征矢量
Figure FDA0004011104080000026
其中
Figure FDA0004011104080000027
和θ可由目标处的磁场的三轴分量计算得到：
Figure FDA0004011104080000028
Figure FDA0004011104080000029
4.根据权利要求1所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述步骤S2中根据所述磁场特征矢量确定所述目标的位置的具体过程为：
S21、根据余弦定理确定磁场特征矢量与目标位置的关系，其公式为：
Figure FDA00040111040800000210
其中，dij表示磁信标i与磁信标j之间的距离，di表示磁信标i与目标之间的距离，vi和vj分别表示磁信标i和磁信标j在目标空间点处的特征矢量，αij为vi和vj之间的夹角；
S22、根据S21所述关系确定目标的位置，公式为：
Figure FDA0004011104080000031
其中，P1＝(p1x,p1y,p1z,......)表示由磁信标解算得到的位置，
Figure FDA0004011104080000032
Figure FDA0004011104080000033
表示各个所述磁信标的位置。
5.根据权利要求1所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述步骤S2中根据所述超带宽的状态输出确定所述目标的位置的具体过程为：
采用TDOA定位方法，测量定位标签与各定位基站的距离差，并将定位基站作为焦点，距离差作为长轴作出双曲线模型，双曲线的交点即是未知定位标签的位置；设四个定位基站的坐标已知分别为BSi(xi,yi),i＝1,2,3,4，待测定位标签为MS(x0,y0)，定位标签MS的超带宽信号到定位基站BSi的时间为ti，ti,1为定位标签到两基站的时间差，c为光速；将基站1作为参考基站，则TDOA的算法表达式为：
Figure FDA0004011104080000034
通过相关算法解以上方程组，即可得到基于TDOA定位方法下的目标位置。
6.根据权利要求1所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述步骤S2中根据所述惯性导航系统的状态输出确定所述目标的位置的具体过程为：
采集惯性测量单元中加速度计和陀螺仪的原始数据，通过惯性导航机械编排进行导航解算得到目标的速度、姿态和位置；其位置更新公式为：
Figure FDA0004011104080000035
Figure FDA0004011104080000041
其中Pm为m时刻的位置，Pm-1为m-1时刻的位置，T为采样时间，
Figure FDA0004011104080000042
L表示当地纬度，RNh＝RN+h，RMh＝RM+h，h为当地高度，RN为卯酉圈半径，RM为子午圈半径。
7.根据权利要求1所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述步骤S3中根据所述三种定位方式的位置信息确定目标的最优位置的具体过程为：
通过基于分布式滤波的数据融合算法融合三种方式的定位信息，建立融合系统方程为：
xk＝f(xk-1)+wk-1
zk＝h(xk)+vk
将非线性方程一阶线性化后可以表示为：
xk＝Φk/k-1xk-1+wk-1
zk＝Hkxk+vk
预测状态向量：
Figure FDA0004011104080000043
预测误差协方差：
Figure FDA0004011104080000044
卡尔曼滤波增益：
Figure FDA0004011104080000045
状态更新：
Figure FDA0004011104080000046
协方差更新：Pk＝(I-KkHk)Pk/k-1
通过以上滤波公式不断迭代得到最优的位置估计：
Figure FDA0004011104080000047
上式中，xk是k时刻的n维状态向量，f(xk-1)是n维非线性向量函数，wk-1是系统噪声向量，zk是k时刻的m维量测向量，h(xk)是m维非线性向量函数，vk是量测噪声向量，E(wk)＝0，E(vk)＝0，
Figure FDA0004011104080000048
Figure FDA0004011104080000051
Φk/k-1为非线性函数f(xk-1)线性化后的状态转移矩阵，Hk为非线性函数h(xk)线性化后的量测矩阵。
8.根据权利要求1所述的一种基于人工信标的多源协同位姿确定方法，其特征在于，所述步骤S3中根据所述目标的最优位置确定所述目标的姿态的具体过程为：
根据解算得到的最优位置
Figure FDA0004011104080000052
可计算出估计目标特征矢量
Figure FDA0004011104080000053
Figure FDA0004011104080000054
测量特征矢量
Figure FDA0004011104080000055
与估计目标特征矢量
Figure FDA0004011104080000056
存在如下关系：
Figure FDA0004011104080000057
其中
Figure FDA0004011104080000059
可以根据测量值
Figure FDA00040111040800000510
得到，
Figure FDA00040111040800000511
Mi为磁信标i的坐标，
Figure FDA00040111040800000512
表示磁性标i与目标之间的距离，
Figure FDA00040111040800000513
为目标相对于磁信标的旋转矩阵；
目标相对于磁信标的旋转矩阵
Figure FDA00040111040800000514
其中，
Figure FDA00040111040800000515
Figure FDA00040111040800000516
令航向角为θ，俯仰角为
Figure FDA00040111040800000517
横滚角为φ，c代表cos函数，s代表sin函数，可得
Figure FDA00040111040800000518
Figure FDA00040111040800000519
由上式即可求解得到目标的姿态角。
9.一种基于人工信标的多源协同位姿确定装置，其特征在于，包括：
获取单元：所述获取单元用于获取多个磁信标在目标空间点处的磁场特征矢量，并获取超宽带与惯性导航系统的状态输出；
协同处理与通信单元：所述协同处理与通信单元用于根据所述磁场特征矢量确定所述目标的位置，以及根据所述超宽带的状态输出确定所述目标的位置，根据所述惯性导航系统的状态输出确定所述目标的位姿；
协同计算单元：所述协同计算单元用于根据所述三种定位方式的位置信息确定所述目标的最优位置，根据所述目标的最优位置确定所述目标的姿态。
10.一种基于人工信标的多源协同位姿确定系统，其特征在于，包括：多个工作频率互不相同的磁信标以及权利要求9所述的基于人工信标的多源协同位姿确定装置。
CN202211652321.0A 2022-12-21 2022-12-21 一种基于人工信标的多源协同位姿确定方法、装置及系统 Pending CN115979256A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211652321.0A CN115979256A (zh) 2022-12-21 2022-12-21 一种基于人工信标的多源协同位姿确定方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211652321.0A CN115979256A (zh) 2022-12-21 2022-12-21 一种基于人工信标的多源协同位姿确定方法、装置及系统

Publications (1)

Publication Number Publication Date
CN115979256A true CN115979256A (zh) 2023-04-18

Family

ID=85962134

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211652321.0A Pending CN115979256A (zh) 2022-12-21 2022-12-21 一种基于人工信标的多源协同位姿确定方法、装置及系统

Country Status (1)

Country Link
CN (1) CN115979256A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117092665A (zh) * 2023-08-03 2023-11-21 广州海格晶维信息产业有限公司 一种组合导航设备抗多径干扰的方法及系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117092665A (zh) * 2023-08-03 2023-11-21 广州海格晶维信息产业有限公司 一种组合导航设备抗多径干扰的方法及系统
CN117092665B (zh) * 2023-08-03 2024-04-19 广州海格晶维信息产业有限公司 一种组合导航设备抗多径干扰的方法及系统

Similar Documents

Publication Publication Date Title
CN103363988B (zh) 一种利用智能手机传感器实现地磁室内定位导航的方法
KR101660002B1 (ko) 모바일 장치의 방향을 결정하기 위한 장치 및 방법
Sun et al. Indoor positioning tightly coupled Wi-Fi FTM ranging and PDR based on the extended Kalman filter for smartphones
CN103561462B (zh) 一种完全基于智能移动终端平台的室内定位系统和方法
US7570213B2 (en) Method and apparatus for detecting and locating intrusion in a wireless network
CN102932742A (zh) 基于惯性传感器与无线信号特征的室内定位方法及系统
CN113706612B (zh) 融合uwb和单目视觉slam的煤矿井下车辆定位方法
CN104181500A (zh) 一种基于惯性信息和机会无线信号特征的实时定位方法
CN107655474A (zh) 一种基于智能终端的导航方法及导航系统
Lategahn et al. Tdoa and rss based extended kalman filter for indoor person localization
Long et al. Single UWB anchor aided PDR heading and step length correcting indoor localization system
CN115979256A (zh) 一种基于人工信标的多源协同位姿确定方法、装置及系统
Guo et al. Large-scale indoor localization solution for pervasive smartphones using corrected acoustic signals and data-driven PDR
Guo et al. Virtual wireless device-constrained robust extended Kalman filters for smartphone positioning in indoor corridor environment
Kohlbacher et al. A low cost omnidirectional relative localization sensor for swarm applications
Kuusniemi et al. Multi-sensor multi-network seamless positioning with visual aiding
Brown et al. Classification schemes of positioning technologies for indoor navigation
CN112556689B (zh) 一种融合加速度计和超宽带测距的定位方法
Zhuang et al. Autonomous WLAN heading and position for smartphones
Liu et al. A calibrated-RSSI/PDR/Map intergrated system based on a novel particle filter for indoor navigation
CN206369546U (zh) 一种基于智能手机的混合定位系统
CN110944293B (zh) 基于路径衰减和卡尔曼滤波融合的无线电搜索定位方法
CN111272168B (zh) 一种基于磁场特征矢量的定位方法、装置及系统
CN109286977B (zh) 基于距离信息的无锚点定位方法
CN104833995A (zh) 基于Android平台的无源区域地理信息采集系统及方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination