CN117308925B - 一种频谱地图惯导组合的导航方法、装置、设备和介质 - Google Patents

一种频谱地图惯导组合的导航方法、装置、设备和介质 Download PDF

Info

Publication number
CN117308925B
CN117308925B CN202311610655.6A CN202311610655A CN117308925B CN 117308925 B CN117308925 B CN 117308925B CN 202311610655 A CN202311610655 A CN 202311610655A CN 117308925 B CN117308925 B CN 117308925B
Authority
CN
China
Prior art keywords
particle
value
inertial navigation
mobile carrier
speed
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.)
Active
Application number
CN202311610655.6A
Other languages
English (en)
Other versions
CN117308925A (zh
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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202311610655.6A priority Critical patent/CN117308925B/zh
Publication of CN117308925A publication Critical patent/CN117308925A/zh
Application granted granted Critical
Publication of CN117308925B publication Critical patent/CN117308925B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0252—Radio frequency fingerprinting
    • G01S5/02521—Radio frequency fingerprinting using a radio-map
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0268—Hybrid positioning by deriving positions from different combinations of signals or of estimated positions in a single positioning system
    • G01S5/02685—Hybrid positioning by deriving positions from different combinations of signals or of estimated positions in a single positioning system involving dead reckoning based on radio wave measurements

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

本申请涉及一种频谱地图惯导组合的导航方法、装置、设备和介质。所述方法包括：通过频谱地图构建无线电频谱场景，在其中布撒粒子，结合直接获取的辐射场强度测量值，通过粒子滤波算法得到移动载***置估计值和速度估计值，再根据惯导测量数据，通过卡尔曼滤波算法计算误差状态，对惯导位置信息和速度信息的误差进行闭环校正，并根据移动载***置估计值和辐射场强度测量值对频谱地图进行闭环修正。本发明通过融合辐射场强度测量值、惯导测量值和频谱地图来实现位置和速度的估计，解决了由辐射场强度测量值带来的空间模糊性问题和频谱地图误差问题，并提高周围场景中的频谱地图精度。

Description

一种频谱地图惯导组合的导航方法、装置、设备和介质
技术领域
本申请涉及未知环境下的导航领域、Radio-SLAM导航领域和惯导（IMU）领域，特别是涉及一种频谱地图惯导组合的导航方法、装置、计算机设备和存储介质。
背景技术
在以往的无线电导航方式中，基于直接测量的无线电信号强度RSS（Receivedsignal strength）值和位置标签的导航方法可基于频谱地图（Radio map）进行导航，如逼近匹配方法和指纹匹配等。若将频谱地图应用到无线电即时定位与地图构建（Radio-SLAM）中，用户可根据自身的实时位置和频谱地图获取周围频谱场景，通过自身测量的RSS信息和频谱场景中包含的RSS信息及位置标签进行导航和运动状态的估计。其优点是可以减少辐射源位置难以估计的影响。频谱地图可通过一个典型的架构生成，该架构由一个具备数据融合单元及接收器的移动用户和多个配置接收器的监测站组成，数据融合单元可利用监测站所收集的RSS数据生成无线电频谱地图（Radio map）。但是，受计算方式和环境影响，该框架生成的频谱地图仍然存在误差较大及空间构建不充分的问题，且由于RSS值受环境和多径影响，基于RSS的导航方法又会带来空间模糊性问题。空间模糊是指两个空间上不同的点之间的RSS差异可能不明显，从而导致匹配困难。因此，现有技术存在效果不佳的问题。
发明内容
基于此，有必要针对上述技术问题，提供一种能够有效实现未知环境下基于机会无线电信号的Radio-SLAM导航的频谱地图/惯导组合的导航方法、装置、计算机设备和存储介质。
一种频谱地图惯导组合的导航方法，所述方法包括：
获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；所述惯导测量数据包括惯导位置信息、速度信息及姿态信息；
在所述无线电频谱场景中生成粒子群，并得到粒子场强预测值；所述粒子群中粒子状态还包括位置状态和观测噪声方差；
根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值；
根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正；
根据所述移动载***置估计值和所述辐射场强度测量值对所述频谱地图进行闭环修正。
在其中一个实施例中，还包括：根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量为：
；
其中，为所述辐射场强度测量值，/>为所述粒子场强预测值，/>为在所述无线电频谱场景中生成的粒子数，/>表示当前时刻；
根据所述观测量更新粒子权值为：
；
其中，为粒子群半径，/>为测量噪声的方差阵，/>表示指数函数，/>表示矩阵转置。
在其中一个实施例中，还包括：根据所述粒子权值进行粒子筛选，若筛选后有效粒子数小于预设阈值则通过改变粒子分布空间进行重采样，直到有效粒子数大于所述阈值，确定粒子滤波后粒子位置状态向量及其权重。
在其中一个实施例中，还包括：根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值为：
；
其中，为粒子滤波后的粒子数，/>为滤波后粒子的位置信息。
在其中一个实施例中，还包括：通过时间累积，以10次1s间隔连续有效滤波为一个速度估计阶段，确定所述速度估计阶段内移动载体有效滤波的位移距离为；其中向量包含/>三个方向的分量，下标/>表示在北东地坐标系中只取平面北东坐标系的值，忽略地向，下标/>表示有效滤波时刻，/>表示实际时刻；
在所述速度估计阶段，获取由移动载体的实际速度所得到的位移增量；其中/>，/>表示惯导解算的速度；
根据所述移动载体有效滤波的位移距离和所述由移动载体的实际速度所得到的位移增量估计移动载体速度大小，用偏航角约束移动载体速度方向：
；
；
其中，下标表示粒子滤波前的估计值，下标/>表示粒子滤波后的新估计值，表示偏航角。
在其中一个实施例中，还包括：选取误差状态矢量；所述误差状态矢量中的误差状态元素包括：对地位置误差矢量、对地速度误差矢量、失准角矢量、陀螺零偏和加表零偏；所述对地位置误差矢量由所述惯导测量数据中惯导位置信息和所述移动载***置估计值作差得到；所述对地速度误差矢量由所述惯导测量数据中惯导速度信息和所述移动载体速度估计值作差得到；
构建所述误差状态矢量的微分方程；
构建卡尔曼滤波观测方程；
根据所述误差状态矢量的微分方程、所述卡尔曼滤波观测方程进行卡尔曼滤波，得到所述误差状态矢量的最优估计值；
根据所述误差状态矢量的最优估计值对所述惯导位置信息和速度信息的误差进行闭环校正。
在其中一个实施例中，还包括：选取卡尔曼滤波观测量为：
其中，下标IMU代表惯导输出的状态值，下标PF代表粒子滤波输出的状态值，建立观测方程：
；
其中，为所述误差状态矢量，/>为观测噪声矩阵，/>为观测矩阵，。
一种频谱地图惯导组合的导航装置，所述装置包括：
无线电频谱场景构建模块，用于获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；所述惯导测量数据包括惯导位置信息、速度信息及姿态信息；
粒子群生成模块，用于在所述无线电频谱场景中生成粒子群，并得到粒子场强预测值；所述粒子群中粒子状态还包括位置状态和观测噪声方差；
粒子滤波模块，用于根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
导航信息估计模块，用于根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值；
惯导误差闭环校正模块，用于根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正；
频谱地图闭环修正模块，用于根据所述移动载***置估计值和所述辐射场强度测量值对所述频谱地图进行闭环修正。
一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；所述惯导测量数据包括惯导位置信息、速度信息及姿态信息；
在所述无线电频谱场景中生成粒子群，并得到粒子场强预测值；所述粒子群中粒子状态还包括位置状态和观测噪声方差；
根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值；
根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正；
根据所述移动载***置估计值和所述辐射场强度测量值对所述频谱地图进行闭环修正。
一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现以下步骤：
获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；所述惯导测量数据包括惯导位置信息、速度信息及姿态信息；
在所述无线电频谱场景中生成粒子群，并得到粒子场强预测值；所述粒子群中粒子状态还包括位置状态和观测噪声方差；
根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值；
根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正；
根据所述移动载***置估计值和所述辐射场强度测量值对所述频谱地图进行闭环修正。
上述频谱地图/惯导组合的导航方法、装置、计算机设备和存储介质，通过频谱地图构建无线电频谱场景，在其中布撒粒子，结合直接获取的辐射场强度测量值，通过粒子滤波算法得到移动载***置估计值和速度估计值，再根据惯导测量数据，通过卡尔曼滤波算法计算误差状态，对惯导位置信息和速度信息的误差进行闭环校正，并根据移动载***置估计值和辐射场强度测量值对频谱地图进行闭环修正。本发明通过融合辐射场强度测量值、惯导测量值和频谱地图来实现位置和速度的估计，解决了由辐射场强度测量值带来的空间模糊性问题和频谱地图误差问题，并提高周围场景中的频谱地图精度，为未知环境下的导航问题提供一种新的解决途径。
附图说明
图1为一个实施例中频谱地图惯导组合的导航方法的流程示意图；
图2为一个实施例中粒子滤波示意图；
图3为另一个实施例中频谱地图惯导组合的导航方法的流程示意图；
图4为一个实施例中频谱地图惯导组合的导航装置的结构框图；
图5为一个实施例中计算机设备的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种频谱地图惯导组合的导航方法，包括以下步骤：
步骤102，获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景。
本发明提供一种频谱地图和惯导（IMU）***组合的导航方法。
惯导***是一种不依赖于外部信息、也不向外部辐射能量的自主式导航***，通过测量载体在惯性参考系的加速度，将它对时间进行积分，且把它变换到导航坐标系中，得到在导航坐标系中的速度、偏航角和位置等信息。本实施例中惯导测量数据包括惯导位置信息、速度信息及姿态信息。
辐射场强度测量值即无线电信号强度RSS值，可以直接测量得到。
频谱地图可通过一个典型的架构生成，该架构由一个具备数据融合单元及接收器的移动用户和多个配置接收器的监测站组成，数据融合单元可利用监测站所收集的RSS数据生成无线电频谱地图。若将频谱地图应用到Radio-SLAM中，可通过现有技术根据导航用户自身的实时位置和频谱地图获取周围频谱场景。
步骤104，在无线电频谱场景中生成粒子群，并得到粒子场强预测值。
在无线电频谱场景中生成粒子群，得到粒子状态是现有技术。本步骤在无线电频谱场景中布撒粒子得到粒子的三个状态值，，分别为粒子场强预测值，粒子位置信息，以及粒子对应噪声方差。
步骤106，根据辐射场强度测量值和粒子场强预测值的差值得到观测量，根据观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重。
粒子滤波算法通过在地图中分布撒播粒子（采样）作为移动载***姿的一种猜测，然后将辐射场强度测量值与每个粒子周围的环境比较，相似程度越高，粒子接近移动载体真实位姿的置信度（权重）就越高，计算粒子权重这一过程会造成样本退化，即粒子群的权重方差较大。解决方法是根据权重筛选这些粒子，这样就得到了最接近移动载体真实位姿的粒子，经过筛选，使得粒子群始终收敛到移动载体真实位姿附近，它们的平均位姿（也就是滤波得到的结果）将作为移动载体定位的一种估计。
如图2为本发明所采用的粒子滤波算法流程示意图。
首先进行状态初始化，然后获取惯导状态估计值；生成随机粒子群，每个粒子包含位置状态和速度状态，根据频谱地图，匹配地图生成、/>，/>表示粒子场强预测值，/>表示粒子对应噪声方差；结合辐射场强度测量数据/>，计算/>、/>、/>，/>表示观测量，/>表示粒子权值，/>表示测量噪声的方差阵；若权值退化，有效粒子数小于设定的阈值，即，粒子滤波算法要进行重采样，通过改变粒子分布空间，直到有效粒子数大于设定的阈值，即/>，给出新粒子位置状态向量及其权重/>，用于对移动载体进行状态估计，状态估计包括位置估计和速度估计。
步骤108，根据滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据移动载***置估计值通过时间累积得到移动载体速度估计值。
移动载***置估计值和速度估计值为组合导航需要输出的结果，另外，组合导航输出的姿态信息仍为惯导***输出的姿态值。
步骤110，根据惯导测量数据、移动载***置估计值和移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据误差状态对惯导位置信息和速度信息的误差进行闭环校正。
本发明创造性地基于场图/惯性/磁力计组合导航***选取15维误差状态矢量如下：
；
各误差状态元素的含义如下所示：
——对地位置误差矢量，即位置矢量预报值与真值之差在/>系中的投影，分别为北向位置误差（m），东向位置误差（m），地向位置误差（m）；本发明以得到移动载***置估计值为预测值，惯导位置信息作为真值，由两者之差得到对地位置误差矢量。
——对地速度误差矢量，即对地速度矢量预报值与真值之差在/>系（北东地）中的投影（m/s）；本发明以移动载体速度估计值为预测值，惯导速度信息作为真值，由两者之差得到对地速度误差矢量。
——失准角矢量（真实的当地北东地坐标系/>，旋转/>角度后，变为失准的当地北东地坐标系/>），其元素分别为/>、/>、/>（rad）；
——陀螺零偏（rad/s），在体坐标系b系中的值；
——加表零偏（/>），在体坐标系b系中的值；
上述所有误差状态（真实值）都对应IMU采样时刻。
卡尔曼滤波算法通常分为两个阶段：预测和更新。在预测阶段，使用***模型和上一时刻的状态估计值，预测当前时刻的状态值；在更新阶段，将预测值与观测值进行比较，得到一个残差，根据残差大小调整预测值，以得到更精确的状态估计值。本发明在构建观测方程时，选取卡尔曼滤波观测量为：
；
其中，下标IMU代表惯导输出的状态值，下标PF代表粒子滤波输出的状态值。
本方法基于卡尔曼滤波校正惯导误差，估计陀螺及加表零偏误差。
步骤112，根据移动载***置估计值和辐射场强度测量值对频谱地图进行闭环修正。
本方法使用位置估计值和所测辐射场强度RSS值对频谱地图进行SLAM闭环修正，提高频谱地图精度。
上述频谱地图/惯导组合的导航方法中，通过频谱地图构建无线电频谱场景，在其中布撒粒子，结合直接获取的辐射场强度测量值，通过粒子滤波算法得到移动载***置估计值和速度估计值，再根据惯导测量数据，通过卡尔曼滤波算法计算误差状态，对惯导位置信息和速度信息的误差进行闭环校正，并根据移动载***置估计值和辐射场强度测量值对频谱地图进行闭环修正。本发明通过融合辐射场强度测量值、惯导测量值和频谱地图来实现位置和速度的估计，解决了由辐射场强度测量值带来的空间模糊性问题和频谱地图误差问题，并提高周围场景中的频谱地图精度，为未知环境下的导航问题提供一种新的解决途径。
在另一个实施例中，如图3所示，提供一种频谱地图惯导组合的导航方法，包括：
步骤1、提取移动载体自身***读取的机会无线电信号强度测量值，即RSS测量值，并假设由监测站构建的频谱地图以及惯导初始位置、速度及姿态已知，记为，另外惯导的陀螺和加表测量值分别为/>。
步骤2、根据用户的位置、RSS测量值和频谱地图构建周围的无线电频谱场景，在该场景中布撒粒子得到。
步骤3、利用步骤2得到粒子场强预测值和真实测量值/>之间的差值构造观测量/>，更新粒子权值/>；
；
步骤4、判断有效粒子数。
若权值退化，有效粒子数小于设定的阈值，粒子滤波算法要进行重采样，通过改变粒子分布空间，直到有效粒子数大于设定的阈值/>，给出新粒子位置误差状态向量及其权重/>。
步骤5、估计***位置。
步骤6、由步骤5估计的***位置通过时间累积，估计***速度/>。
步骤7、将与代入卡尔曼滤波公式，计算误差状态并闭环校正惯导位置、速度误差。
步骤8、使用估计值和辐射场强度测量值完成SLAM闭环频谱场景地图的修正。
其中，步骤6具体实现过程如下：
通过时间累积，以10次1s间隔连续有效滤波为一个速度估计阶段，其位移增量为。考虑到由于滤波失效的情况发生，10次有效滤波往往不连续，其移动载体的位移估计值应为/>。其中向量包含/>三个方向的分量，下标/>表示在北东地坐标系中只取平面北东坐标系的值，忽略地向，下标/>表示有效滤波时刻，/>表示实际时刻。
在速度估计阶段，获取由移动载体的实际速度所得到的位移增量；其中/>，/>表示惯导解算的速度；
根据有效滤波的位移距离和由速度计算的位移距离估计移动载体速度大小，用偏航角约束移动载体速度方向：
；/>；
其中，下标表示粒子滤波前的估计值，下标/>表示粒子滤波后的新估计值，表示偏航角。
步骤7具体实现过程如下：
步骤7.1、基于场图/惯性/磁力计松组合导航***选取15维误差状态矢量如下：
；
步骤7.2、误差状态微分方程，包括系下的位置、速度和姿态误差微分方程，和表述陀螺和加表零偏误差微分的一阶高斯马尔科夫模型，合并写成矩阵形式，可以记为：
；
上式中，代表15维***的误差状态转移矩阵，/>代表15维***的噪声转移矩阵，/>为15维***的白噪声连续随机过程在/>时刻到/>时刻的平均值。
步骤7.3、建立EKF观测方程如下：
选取观测量
；
其中IMU代表惯导输出的状态值，下标PF代表粒子滤波输出的状态值。那么有如下观测方程：
；
其中为观测噪声阵
；
步骤7.4、进行EKF滤波
离散形式的Kalman滤波过程，分为预测（时间更新）和更新（测量更新）两个过程，其中预测过程又包括状态一步预测和方差一步预测。误差状态预报值及其方差阵为
；/>；
式中，为上一时刻误差状态的最优估计，/>是对应的方差阵。/>为***噪声/>的方差阵。
更新过程要计算滤波增益系数矩阵、最优估计值/>和最优估计值方差阵，具体公式如下所示；
；
；
；
给定初值和初始方差阵/>，根据/>时刻的测量值/>，就可以递推得到k时刻的状态估计。在本文的滤波器设计中，由于初始是对准的，/>赋零，/>值根据经验赋予。
为实现步骤4，该方法使用惯导速度估计值辅助粒子状态转移，避免了粒子衰退问题。为实现步骤7，本方法基于卡尔曼滤波校正惯导误差，估计陀螺及加表零偏误差。为使频谱地图精度更高，本方法使用位置估计值和所测辐射场强度RSS值对频谱地图进行SLAM闭环修正，提高频谱地图精度。
应该理解的是，虽然图1、图2的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1、图2中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图4所示，提供了一种频谱地图/惯导组合的导航装置，包括：无线电频谱场景构建模块402、粒子群生成模块404、粒子滤波模块406、导航信息估计模块408、惯导误差闭环校正模块410和频谱地图闭环修正模块412，其中：
无线电频谱场景构建模块402，用于获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；惯导测量数据包括惯导位置信息、速度信息及姿态信息；
粒子群生成模块404，用于在无线电频谱场景中生成粒子群，并得到粒子场强预测值；粒子群中粒子状态还包括位置状态和观测噪声方差；
粒子滤波模块406，用于根据辐射场强度测量值和粒子场强预测值的差值得到观测量，根据观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
导航信息估计模块408，用于根据滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据移动载***置估计值通过时间累积得到移动载体速度估计值；
惯导误差闭环校正模块410，用于根据惯导测量数据、移动载***置估计值和移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据误差状态对惯导位置信息和速度信息的误差进行闭环校正；
频谱地图闭环修正模块412，用于根据移动载***置估计值和辐射场强度测量值对频谱地图进行闭环修正。
粒子滤波模块406还用于根据辐射场强度测量值和粒子场强预测值的差值得到观测量为：
；
其中，为辐射场强度测量值，/>为粒子场强预测值，/>为在无线电频谱场景中生成的粒子数，/>表示当前时刻；
根据观测量更新粒子权值为：
；
其中，为粒子群半径，/>为测量噪声的方差阵，/>表示指数函数，/>表示矩阵转置。
粒子滤波模块406还用于根据粒子权值进行粒子筛选，若筛选后有效粒子数小于预设阈值则通过改变粒子分布空间进行重采样，直到有效粒子数大于阈值，确定粒子滤波后粒子位置状态向量及其权重。
导航信息估计模块408还用于根据滤波后粒子位置状态向量及其权重得到移动载***置估计值为：
；
其中，为粒子滤波后的粒子数，/>为滤波后粒子的位置信息。
导航信息估计模块408还用于通过时间累积，以10次1s间隔连续有效滤波为一个速度估计阶段，确定速度估计阶段内移动载体有效滤波的位移距离为；其中向量包含/>三个方向的分量，下标/>表示在北东地坐标系中只取平面北东坐标系的值，忽略地向，下标/>表示有效滤波时刻，/>表示实际时刻；
在速度估计阶段，获取由移动载体的实际速度所得到的位移增量；其中/>，/>表示惯导解算的速度；
根据移动载体有效滤波的位移距离和由移动载体的实际速度所得到的位移增量估计移动载体速度大小，用偏航角约束移动载体速度方向：
；
；
其中，下标表示粒子滤波前的估计值，下标/>表示粒子滤波后的新估计值，表示偏航角。
惯导误差闭环校正模块410还用于选取误差状态矢量；误差状态矢量中的误差状态元素包括：对地位置误差矢量、对地速度误差矢量、失准角矢量、陀螺零偏和加表零偏；对地位置误差矢量由惯导测量数据中惯导位置信息和移动载***置估计值作差得到；对地速度误差矢量由惯导测量数据中惯导速度信息和移动载体速度估计值作差得到；构建误差状态矢量的微分方程；构建卡尔曼滤波观测方程；根据误差状态矢量的微分方程、卡尔曼滤波观测方程进行卡尔曼滤波，得到误差状态矢量的最优估计值；根据误差状态矢量的最优估计值对惯导位置信息和速度信息的误差进行闭环校正。
惯导误差闭环校正模块410还用于选取卡尔曼滤波观测量为：
；
其中，下标IMU代表惯导输出的状态值，下标PF代表粒子滤波输出的状态值，建立观测方程：
；
其中，为误差状态矢量，/>为观测噪声矩阵，/>为观测矩阵，。
关于频谱地图/惯导组合的导航装置的具体限定可以参见上文中对于频谱地图/惯导组合的导航方法的限定，在此不再赘述。上述频谱地图/惯导组合的导航装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端，其内部结构图可以如图5所示。该计算机设备包括通过***总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作***和计算机程序。该内存储器为非易失性存储介质中的操作***和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种频谱地图惯导组合的导航方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
本领域技术人员可以理解，图5中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，提供了一种计算机设备，包括存储器和处理器，该存储器存储有计算机程序，该处理器执行计算机程序时实现上述方法实施例中的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述方法实施例中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种频谱地图惯导组合的导航方法，其特征在于，所述方法包括：
获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；所述惯导测量数据包括惯导位置信息、速度信息及姿态信息；
在所述无线电频谱场景中生成粒子群，并得到粒子场强预测值；所述粒子群中粒子状态还包括位置状态和观测噪声方差；
根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值；
根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正；
根据所述移动载***置估计值和所述辐射场强度测量值对所述频谱地图进行闭环修正。
2.根据权利要求1所述的方法，其特征在于，根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，包括：
根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量为：
；
其中，为所述辐射场强度测量值，/>为所述粒子场强预测值，/>为在所述无线电频谱场景中生成的粒子数，/>表示当前时刻；
根据所述观测量更新粒子权值为：
；
其中，为粒子群半径，/>为测量噪声的方差阵，/>表示指数函数，/>表示矩阵转置。
3.根据权利要求1所述的方法，其特征在于，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重，包括：
根据所述粒子权值进行粒子筛选，若筛选后有效粒子数小于预设阈值则通过改变粒子分布空间进行重采样，直到有效粒子数大于所述阈值，确定粒子滤波后粒子位置状态向量及其权重。
4.根据权利要求2所述的方法，其特征在于，根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，包括：
根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值为：
；
其中，为粒子滤波后的粒子数，/>为滤波后粒子的位置信息。
5.根据权利要求1所述的方法，其特征在于，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值，包括：
通过时间累积，以10次1s间隔连续有效滤波为一个速度估计阶段，确定所述速度估计阶段内移动载体有效滤波的位移距离为；其中向量包含/>三个方向的分量，下标/>表示在北东地坐标系中只取平面北东坐标系的值，忽略地向，下标/>表示有效滤波时刻，/>表示实际时刻；
在所述速度估计阶段，获取由移动载体的实际速度所得到的位移增量；其中/>，/>表示惯导解算的速度；
根据所述移动载体有效滤波的位移距离和所述由移动载体的实际速度所得到的位移增量估计移动载体速度大小，用偏航角约束移动载体速度方向：
；
；
其中，下标表示粒子滤波前的估计值，下标/>表示粒子滤波后的新估计值，/>表示偏航角。
6.根据权利要求1所述的方法，其特征在于，根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正，包括：
选取误差状态矢量；所述误差状态矢量中的误差状态元素包括：对地位置误差矢量、对地速度误差矢量、失准角矢量、陀螺零偏和加表零偏；所述对地位置误差矢量由所述惯导测量数据中惯导位置信息和所述移动载***置估计值作差得到；所述对地速度误差矢量由所述惯导测量数据中惯导速度信息和所述移动载体速度估计值作差得到；
构建所述误差状态矢量的微分方程；
构建卡尔曼滤波观测方程；
根据所述误差状态矢量的微分方程、所述卡尔曼滤波观测方程进行卡尔曼滤波，得到所述误差状态矢量的最优估计值；
根据所述误差状态矢量的最优估计值对所述惯导位置信息和速度信息的误差进行闭环校正。
7.根据权利要求6所述的方法，其特征在于，构建卡尔曼滤波观测方程，包括：
选取卡尔曼滤波观测量为：
；
其中，下标IMU代表惯导输出的状态值，下标PF代表粒子滤波输出的状态值，建立观测方程：
；
其中，为所述误差状态矢量，/>为观测噪声矩阵，/>为观测矩阵，。
8.一种频谱地图惯导组合的导航装置，其特征在于，所述装置包括：
无线电频谱场景构建模块，用于获取移动载体的惯导测量数据、辐射场强度测量值，以及由监测站构建的频谱地图，并构建移动载体周围的无线电频谱场景；所述惯导测量数据包括惯导位置信息、速度信息及姿态信息；
粒子群生成模块，用于在所述无线电频谱场景中生成粒子群，并得到粒子场强预测值；所述粒子群中粒子状态还包括位置状态和观测噪声方差；
粒子滤波模块，用于根据所述辐射场强度测量值和所述粒子场强预测值的差值得到观测量，根据所述观测量更新粒子权值，通过粒子滤波算法得到滤波后粒子位置状态向量及其权重；
导航信息估计模块，用于根据所述滤波后粒子位置状态向量及其权重得到移动载***置估计值，根据所述移动载***置估计值通过时间累积得到移动载体速度估计值；
惯导误差闭环校正模块，用于根据所述惯导测量数据、所述移动载***置估计值和所述移动载体速度估计值，通过卡尔曼滤波算法计算误差状态，根据所述误差状态对所述惯导位置信息和速度信息的误差进行闭环校正；
频谱地图闭环修正模块，用于根据所述移动载***置估计值和所述辐射场强度测量值对所述频谱地图进行闭环修正。
9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至7中任一项所述的方法的步骤。
CN202311610655.6A 2023-11-29 2023-11-29 一种频谱地图惯导组合的导航方法、装置、设备和介质 Active CN117308925B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311610655.6A CN117308925B (zh) 2023-11-29 2023-11-29 一种频谱地图惯导组合的导航方法、装置、设备和介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311610655.6A CN117308925B (zh) 2023-11-29 2023-11-29 一种频谱地图惯导组合的导航方法、装置、设备和介质

Publications (2)

Publication Number Publication Date
CN117308925A CN117308925A (zh) 2023-12-29
CN117308925B true CN117308925B (zh) 2024-02-09

Family

ID=89281535

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311610655.6A Active CN117308925B (zh) 2023-11-29 2023-11-29 一种频谱地图惯导组合的导航方法、装置、设备和介质

Country Status (1)

Country Link
CN (1) CN117308925B (zh)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110333479A (zh) * 2019-07-09 2019-10-15 东华大学 复杂室内环境下一种基于改进粒子滤波的无线定位方法
CN111044050A (zh) * 2019-12-30 2020-04-21 中电海康集团有限公司 一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法
EP3764058A1 (en) * 2019-07-10 2021-01-13 HERE Global B.V. Indoor optimized offline radio map
CN112729301A (zh) * 2020-12-10 2021-04-30 深圳大学 一种基于多源数据融合的室内定位方法
CN115406435A (zh) * 2022-08-24 2022-11-29 同济大学 基于wlan和mems的室内电子地图构建方法、装置及存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103228040A (zh) * 2012-01-31 2013-07-31 国际商业机器公司 室内电子地图生成方法与***及室内目标定位方法与***
US10190881B2 (en) * 2015-01-08 2019-01-29 Profound Positioning Inc. Method and apparatus for enhanced pedestrian navigation based on WLAN and MEMS sensors
WO2018139773A1 (ko) * 2017-01-25 2018-08-02 한국과학기술연구원 무선 환경 변화에 강인한 slam 방법 및 장치

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110333479A (zh) * 2019-07-09 2019-10-15 东华大学 复杂室内环境下一种基于改进粒子滤波的无线定位方法
EP3764058A1 (en) * 2019-07-10 2021-01-13 HERE Global B.V. Indoor optimized offline radio map
CN111044050A (zh) * 2019-12-30 2020-04-21 中电海康集团有限公司 一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法
CN112729301A (zh) * 2020-12-10 2021-04-30 深圳大学 一种基于多源数据融合的室内定位方法
CN115406435A (zh) * 2022-08-24 2022-11-29 同济大学 基于wlan和mems的室内电子地图构建方法、装置及存储介质

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Algorithm for Dynamic Fingerprinting Radio Map Creation Using IMU Measurements;Brida, Peter et al.;《SENSORS》;第21卷(第7期);1-18 *
机会信号导航综述;黄高明等;《控制与决策》;第34卷(第6期);1121-1131 *
采用EKF与PF的室内融合定位技术;张雨婷;陈璟;;传感技术学报(第02期);89-95 *

Also Published As

Publication number Publication date
CN117308925A (zh) 2023-12-29

Similar Documents

Publication Publication Date Title
US7302345B2 (en) Method and apparatus for generating magnetic field map and method and apparatus for checking pose of mobile body using the magnetic field map
CN107884800B (zh) 观测时滞***的组合导航数据解算方法、装置及导航设备
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN112964291B (zh) 一种传感器标定的方法、装置、计算机存储介质及终端
CN115143954B (zh) 一种基于多源信息融合的无人车导航方法
CN112985391A (zh) 一种基于惯性和双目视觉的多无人机协同导航方法和装置
CN110779514B (zh) 面向仿生偏振导航辅助定姿的分级卡尔曼融合方法及装置
US20220057517A1 (en) Method for constructing point cloud map, computer device, and storage medium
CN114061570A (zh) 车辆定位方法、装置、计算机设备和存储介质
CN114047766B (zh) 面向室内外场景长期应用的移动机器人数据采集***及方法
CN111678513A (zh) 一种超宽带/惯导紧耦合的室内定位装置与***
CN109769206B (zh) 一种室内定位融合方法、装置、存储介质及终端设备
CN113218389B (zh) 一种车辆定位方法、装置、存储介质及计算机程序产品
CN110375740B (zh) 车辆导航方法、装置、设备和存储介质
CN117308925B (zh) 一种频谱地图惯导组合的导航方法、装置、设备和介质
CN114897942B (zh) 点云地图的生成方法、设备及相关存储介质
CN116929343A (zh) 位姿估计方法、相关设备及存储介质
CN114001730B (zh) 融合定位方法、装置、计算机设备和存储介质
US20240134033A1 (en) Method for determining a movement state of a rigid body
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN113701749A (zh) 一种基于立体视觉惯性里程计的车辆定位导航方法及***
Jones et al. Minimizing Relative Position and Attitude Error in UAV Swarms via General Form Consensus
CN113804194B (zh) 一种行驶设备的定位方法、装置、设备及存储介质
Indelman et al. Real-time mosaic-aided aerial navigation: II. Sensor fusion
CN116972834A (zh) 多传感器融合定位方法、装置、***及存储介质

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
GR01 Patent grant
GR01 Patent grant