CN114915913A - 一种基于滑窗因子图的uwb-imu组合室内定位方法 - Google Patents
一种基于滑窗因子图的uwb-imu组合室内定位方法 Download PDFInfo
- Publication number
- CN114915913A CN114915913A CN202210536034.7A CN202210536034A CN114915913A CN 114915913 A CN114915913 A CN 114915913A CN 202210536034 A CN202210536034 A CN 202210536034A CN 114915913 A CN114915913 A CN 114915913A
- Authority
- CN
- China
- Prior art keywords
- uwb
- imu
- factor
- factor graph
- line
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 238000005457 optimization Methods 0.000 claims abstract description 43
- 230000010354 integration Effects 0.000 claims abstract description 38
- 238000004364 calculation method Methods 0.000 claims abstract description 29
- 238000005259 measurement Methods 0.000 claims description 38
- 230000004927 fusion Effects 0.000 claims description 7
- 230000008030 elimination Effects 0.000 claims description 6
- 238000003379 elimination reaction Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 239000000126 substance Substances 0.000 claims description 4
- 239000013585 weight reducing agent Substances 0.000 claims description 2
- 230000008569 process Effects 0.000 description 15
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 239000002699 waste material Substances 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000000265 homogenisation Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
Images
Classifications
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/021—Services related to particular areas, e.g. point of interest [POI] services, venue services or geofences
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/02—Services making use of location information
- H04W4/025—Services making use of location information using location based information parameters
- H04W4/027—Services making use of location information using location based information parameters using movement velocity, acceleration information
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
- H04W4/30—Services specially adapted for particular environments, situations or purposes
- H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
- H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
-
- 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
- Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
一种基于滑窗因子图的UWB‑IMU组合室内定位方法，具体包含如下步骤：S1.求解系统初始位置，提供系统初值；S2.将IMU的位置和误差状态参数化，作为因子图的优化变量；S3.进行IMU预积分计算；S4.当检测到UWB数据时，进行UWB非视距判别，对UWB视距观测值，添加UWB测距因子，同时在因子图中添加IMU预积分因子；S5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。本发明提供一种基于滑窗因子图的UWB‑IMU组合室内定位方法，将UWB和IMU的组合定位问题转化为图优化问题，将UWB测距值与IMU预积分作为因子图中的因子，考虑UWB的非视距判别和滑窗边缘化，得到具有更高精度的定位结果。
Description
技术领域
本发明属于室内定位技术领域，具体涉及一种基于滑窗因子图的UWB-IMU组合室内定位方法。
背景技术
对于许多室内定位应用而言，可靠且高精度的定位仍然是一个开放的研究问题。目前全球导航卫星系统(global navigation satellite system，GNSS)和惯性导航系统(inertial navigation system，INS)的融合估计已能够满足高精度的要求，但在卫星拒止环境下，GNSS信号难以到达导致其定位功能失效；INS虽具有低成本和不受环境影响等优点，但需对陀螺仪和加速度计输出高频的角速度和加速度信号进行两次积分，容易带来随时间增长的累积误差，无法持续提供可靠的位置信息。现有卫星拒止环境下的定位方法主要有:基于地磁，基于超声，基于视觉和基于激光雷达等。然而地磁信号定位极度依赖精准的环境先验地磁信息，不同场景适应性差；超声定位易受环境影响，且工作距离较短，精度较低；视觉定位运算量大，受光照影响严重，动态易变场景下易失效；激光雷达定位成本昂贵，运算量大，难以推广应用，且受同质化特征影响较为严重。
以蓝牙、超宽带(ultra wide band，UWB)等为代表的无线传感定位方法，成本低，精度较高，通过利用无线信号测量基站和接收站之间的距离，并通过已知基站位置信息估计接收站位置。其中，UWB时间分辨率高、穿透力强、功耗低、抗多径效果好、安全性强，其观测值可以用来抑制并修正INS引起的累积误差。因此，UWB/INS的融合定位成为GNSS拒止环境下一种主流的室内定位解决方案。然而，UWB测量过程中容易出现的非视距(non-line-of-sight，NLOS)现象是影响定位结果的重要因素。非视距指基站与接收站之间的传播路径受到动静态障碍物的遮挡影响，从而导致传播时间变长，测量距离相对实际距离变大。非视距效应会导致UWB观测信号质量变动频繁，影响定位精度。因此，高效的非视距检测判别及自适应融合方法是UWB/INS定位的关键。
现有的数据融合方法多采用卡尔曼滤波(Kalman filter)及其变体。但这类方法依赖于事先建立的固有且准确的数学模型，当观测信号多变时自适应能力较弱。若接收到UWB的非视距信号，固有的滤波模型会带来严重的错误。因此，现有的UWB/INS融合定位方法仍较难解决室内环境下UWB观测信号质量多变带来的定位不准的问题，难以保证室内高精度的定位需求。
发明内容
为解决上述技术问题，本发明提出了一种基于滑窗因子图的UWB-IMU组合室内定位方法，本质上，是利用基于滑窗因子图优化的UWB/IMU组合定位方法对UWB和IMU数据进行融合，同时进行UWB的非视距判别，减小非视距测量值对定位结果的影响。
为实现上述目的，本发明采取的技术方案是：
一种基于滑窗因子图的UWB-IMU组合室内定位方法，其特征在于，具体包含如下步骤：
S1.求解系统初始位置，提供系统初值；
S2.将IMU的位置和误差状态参数化，作为因子图的优化变量；
S3.进行IMU预积分计算；
S4.当检测到UWB数据时，进行UWB非视距判别，对UWB视距观测值，添加UWB测距因子，同时在因子图中添加IMU预积分因子；
S5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。
作为本发明进一步，所述步骤S5中，对UWB和IMU进行数据融合的步骤如下：
滑窗因子图中的IMU状态为X＝[x0，x1，…，xn]，n为滑窗大小，其中xk为IMU状态，
状态xk包括了IMU在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置；
当检测到新的UWB数据时，进行UWB非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的IMU预积分因子和UWB观测值对应的新的UWB观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息；
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
式中的rpre和rUWB分别表示IMU的预积分因子和UWB观测因子，m+1是UWB观测因子的个数，rp-HpX表示边缘化带来的先验信息，利用Ceres框架求解式1中的最小二乘估计，解得系统状态的最优估计。
作为本发明进一步，所述步骤S4中，对UWB测距值进行非视距判别的步骤如下：
通过比较根据因子图求出的标签到基站距离与实际UWB测量值，对非视距信号进行判定，其具体方法如下：在接收到新的UWB数据时，根据前一时刻由IMU因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵得到位置的标准差为
则位置坐标的置信区间分别为
和
因此在k时刻，标签到第i个基站的理论最大距离为：
其中，
为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标，当前时刻标签与第i个基站之间测距值为
标签与该基站之间测距误差为σi，则如果
判定该测距值为非视距测距值，进行降权或者直接剔除。
本发明提供一种基于滑窗因子图的UWB-IMU组合室内定位方法，主要包括:
(1)提出一种基于因子图优化算法的UWB/IMU数据融合算法，因子图算法的灵活性很高，能即插即用的组合使用不同传感器，如果存在传感器失效，能够及时的删除失效传感器对应的因子；
(2)通过利用因子图输出的位置及位置置信度来对非视距信号进行判定，对UWB非视距测量值进行降权或剔除。
附图说明
图1为系统工作流程图；
图2为因子图原理示意图；
图3为系统因子图结构图。
具体实施方式
为了解决这些问题，本方法提出了一种更有效的方法来融合惯性和UWB测量。本质上，是利用基于滑窗因子图优化的UWB/IMU组合定位方法对UWB和IMU数据进行融合，同时进行UWB的非视距判别，减小非视距测量值对定位结果的影响。主要包括:
(1)提出一种基于因子图优化算法的UWB/IMU数据融合算法，因子图算法的灵活性很高，能即插即用的组合使用不同传感器，如果存在传感器失效，能够及时的删除失效传感器对应的因子；
(2)通过利用因子图输出的位置及位置置信度来对非视距信号进行判定，对UWB非视距测量值进行降权或剔除。
因子图概念
通过因式分解使得一个函数转化为多个函数之间的乘积形式，然后将两种表达方式用直线连接起来构成的无向图称为因子图。因子图模型是通过贝叶斯理论推导出的一种概率图模型。
令因子图模型为：H＝(F，X，E)，其中X表示变量节点，即因子图中需要进行优化估计的参数，F是因子图中的因子节点，E是连接因子节点和变量节点的边，是一条无向边，若一个因子节点与一个变量节点通过无向边E相连，则该变量节点就是该因子的变量节点。通过数据集中所有因子的联乘积来表示联合概率分布情况。
设概率分布函数为g(x1，…，xn):
式1中，xi代表变量节点，fj(Xj)表示局部函数。当变量节点和局部函数之间经由无向边相连，且因子图中所有节点全部连接后，即可构成因子图模型，如图2所示。
因子图优化算法流程
如图1所示，是基于因子图的UWB/IMU组合定位算法的算法流程，首先进行IMU的初始预积分，将系统状态转换成具体的位置和误差参数，添加到因子图中作为因子图的变量节点，再进行IMU预积分，当检测到UWB观测数据时，进行UWB的非视距判别和剔除，对视距UWB测量值添加UWB测距因子，同时添加IMU预积分因子，之后判断因子图中同时优化的状态参数个数是否达到设定的窗口大小，如果窗口已满，就进行边缘化操作，边缘化最早的历史状态，最后进行多个状态的联合优化。其中优化过程即图优化的求解过程，对各个因子求各个因子计算的观测值与预测值之差，即残差值，以及求残差对应的雅可比矩阵，利用因子图优化求解方式进行求解。
系统方程
图3为本章中的基于滑窗因子图的UWB/IMU紧组合定位系统因子图结构图，滑窗中的IMU状态为X＝[x0，x1，…，xn]，n为滑窗大小，其中xk为IMU状态，
状态xk包括了IMU在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置。当检测到新的UWB数据时，进行UWB非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的IMU预积分因子和UWB观测值对应的新的UWB观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息，提高数据利用率的同时，也提高定位精度。
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
式中的rpre和rUWB分别表示IMU的预积分因子和UWB观测因子，m+1是UWB观测因子的个数，rp-HpX表示边缘化带来的先验信息，这些内容将在后续小节中介绍。求解式2中的最小二乘估计，解得系统状态的最优估计，求解最小二乘估计这一过程可由Ceres框架进行求解。
IMU预积分因子
在优化过程中，引入IMU数据进行多传感器数据融合可以提高定位的平滑性和准确性。通过对IMU数据进行积分，把上一刻的位置、速度和四元数当作初值，可以得到当前时刻的位置、速度和四元数，更新位置、速度和四元数。但是在因子图优化中，往往是窗口整体优化，在优化过程中，上一刻的初值经过一轮优化后其值会改变，这样对应IMU的积分也要重新计算，这样的话，计算量大，计算过程复杂，浪费计算资源，所以提出预积分方式来提高计算速度，避免浪费算力。预积分的核心思想是把积分中的初值“摘”出去，使得积分结果与初值无关，优化过程中无论初值怎么变化都对IMU数据积分的计算没有影响，提高计算速度。
预积分因子的残差rpre为：
式中
和
分别为预积分的位置、速度和四元数观测值，
和
为位置、速度和四元数的预测值，
和
分别为陀螺仪和加速度计零偏。
非视距信号判别
当UWB测距值受非视距影响，导致测距值大于实际距离时，如果依然简单的将UWB非视距测量值加入因子图中，构造UWB因子，会给定位结果带来很大影响。因此，准确的识别并剔除UWB中的非视距信息对保持定位结果准确可靠十分重要。
本方法中，通过比较根据因子图求出的标签到基站距离与实际UWB测量值，对非视距信号进行判定，其具体方法如下：在接收到新的UWB数据时，根据前一时刻由IMU因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵可以得到位置的标准差为
则位置坐标的置信区间分别为
和
因此在k时刻，标签到第i个基站的理论最大距离为：
其中，
为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标。当前时刻标签与第i个基站之间测距值为
标签与该基站之间测距误差为σi，则如果
可判定该测距值为非视距测距值，可进行降权或者直接剔除。
UWB观测因子
UWB与IMU的主要组合方式有两种，即松组合和紧组合。松组合是由UWB得到定位结果，再将UWB的定位位置作为观测量加入因子图中，这种方式计算简单，容易实现，但UWB与IMU两个系统联系不够紧密，当出现UWB定位异常时，会对组合定位系统造成较大影响。而紧组合这种组合方式使用UWB测距值作为UWB观测值，当发生UWB非视距测量时，可以及时的对UWB非视距测量进行识别、剔除，而并不影响其他正常数据的使用。因此，使用紧组合模式可以得到更高的定位精度，本方法中的UWB/IMU组合方式也是紧组合。在紧组合模式中，可以定义UWB观测因子的残差为UWB测距值与系统推算的标签与基站之间距离之差：
rUWB＝d-||puwb-pbase||
式中d表示UWB的测距值，||puwb-pbase||表示根据系统状态推算的UWB标签到基站的距离。
边缘化
随着系统运行时间的增长，加入窗口中的待优化状态变量个数也会越来越多，如果不对待优化状态的个数做出限制，会导致因子图优化计算复杂度越来越高，运行效率越来越低。因此，为了降低系统计算的复杂度，因子图的优化过程采用了基于滑动窗口思想的方法，对系统的优化状态窗口大小做出限制，设置固定大小，在一定程度上平衡计算量。在窗口滑动的过程中，不断会有新的UWB因子和新的IMU预积分因子被添加到滑窗内，而旧的系统状态、旧的UWB测距因子和旧的IMU预积分因子需要被剔除掉。但是历史时刻的系统状态和观测因子和窗口中的其他状态之间已经存在约束联系，如果直接丢弃窗口之外的数据会导致关联信息的缺失、状态估计不准确，影响系统的精度。所以，在滑窗中删除旧状态的方法并不是直接将旧的状态移除，而是对即将删除的状态，信息进行边缘化，发挥其最后的作用。边缘化的思想就是在移除旧状态的同时，将关联的约束转化为先验信息，再次加入到优化问题中，边缘化的方法是舒尔补。
本发明的总体工作流程如图1所示，一种基于滑窗因子图的UWB-IMU组合室内定位方法，包含如下步骤：
S1.求解系统初始位置，提供系统初值；
S2.将IMU的位置和误差状态参数化，作为因子图的优化变量；
S3.进行IMU预积分计算；
S4.当检测到UWB数据时，进行UWB非视距判别，对UWB视距观测值，添加UWB测距因子，同时在因子图中添加IMU预积分因子；
S5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。
因子图优化算法流程
如图1所示，是基于因子图的UWB/IMU组合定位算法的算法流程，首先进行IMU的初始预积分，将系统状态转换成具体的位置和误差参数，添加到因子图中作为因子图的变量节点，再进行IMU预积分，当检测到UWB观测数据时，进行UWB的非视距判别和剔除，对视距UWB测量值添加UWB测距因子，同时添加IMU预积分因子，之后判断因子图中同时优化的状态参数个数是否达到设定的窗口大小，如果窗口已满，就进行边缘化操作，边缘化最早的历史状态，最后进行多个状态的联合优化。其中优化过程即图优化的求解过程，对各个因子求各个因子计算的观测值与预测值之差，即残差值，以及求残差对应的雅可比矩阵，利用因子图优化求解方式进行求解。
系统方程
图3为本章中的基于滑窗因子图的UWB/IMU紧组合定位系统因子图结构图，滑窗中的IMU状态为X＝[x0，x1，…，xn]，n为滑窗大小，其中xk为IMU状态，
状态xk包括了IMU在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置。当检测到新的UWB数据时，进行UWB非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的IMU预积分因子和UWB观测值对应的新的UWB观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息，提高数据利用率的同时，也提高定位精度。
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
式中的rpre和rUWB分别表示IMU的预积分因子和UWB观测因子，m+1是UWB观测因子的个数，rp-HpX表示边缘化带来的先验信息，这些内容将在后续小节中介绍。求解式6中的最小二乘估计，解得系统状态的最优估计。
IMU预积分因子
在优化过程中，引入IMU数据进行多传感器数据融合可以提高定位的平滑性和准确性。通过对IMU数据进行积分，把上一刻的位置、速度和四元数当作初值，可以得到当前时刻的位置、速度和四元数，更新位置、速度和四元数。但是在因子图优化中，往往是窗口整体优化，在优化过程中，上一刻的初值经过一轮优化后其值会改变，这样对应IMU的积分也要重新计算，这样的话，计算量大，计算过程复杂，浪费计算资源，所以提出预积分方式来提高计算速度，避免浪费算力。预积分的核心思想是把积分中的初值“摘”出去，使得积分结果与初值无关，优化过程中无论初值怎么变化都对IMU数据积分的计算没有影响，提高计算速度。
预积分因子的残差rpre为：
式中
和
分别为预积分的位置、速度和四元数观测值，
和
为位置、速度和四元数的预测值，
和
分别为陀螺仪和加速度计零偏。
非视距信号判别
当UWB测距值受非视距影响，导致测距值大于实际距离时，如果依然简单的将UWB非视距测量值加入因子图中，构造UWB因子，会给定位结果带来很大影响。因此，准确的识别并剔除UWB中的非视距信息对保持定位结果准确可靠十分重要。
本方法中，通过比较根据因子图求出的标签到基站距离与实际UWB测量值，对非视距信号进行判定，其具体方法如下：在接收到新的UWB数据时，根据前一时刻由IMU因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵可以得到位置的标准差为
则位置坐标的置信区间分别为
和
因此在k时刻，标签到第i个基站的理论最大距离为：
其中，
为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标。当前时刻标签与第i个基站之间测距值为
标签与该基站之间测距误差为σi，则如果
可判定该测距值为非视距测距值，可进行降权或者直接剔除。
UWB观测因子
UWB与IMU的主要组合方式有两种，即松组合和紧组合。松组合是由UWB得到定位结果，再将UWB的定位位置作为观测量加入因子图中，这种方式计算简单，容易实现，但UWB与IMU两个系统联系不够紧密，当出现UWB定位异常时，会对组合定位系统造成较大影响。而紧组合这种组合方式使用UWB测距值作为UWB观测值，当发生UWB非视距测量时，可以及时的对UWB非视距测量进行识别、剔除，而并不影响其他正常数据的使用。因此，使用紧组合模式可以得到更高的定位精度，本章的UWB/IMU组合方式也是紧组合。在紧组合模式中，可以定义UWB观测因子的残差为UWB测距值与系统推算的标签与基站之间距离之差：
rUWB＝d-||puwb-pbase||
式中d表示UWB的测距值，||puwb-pbase||表示根据系统状态推算的UWB标签到基站的距离。
边缘化
随着系统运行时间的增长，加入窗口中的待优化状态变量个数也会越来越多，如果不对待优化状态的个数做出限制，会导致因子图优化计算复杂度越来越高，运行效率越来越低。因此，为了降低系统计算的复杂度，因子图的优化过程采用了基于滑动窗口思想的方法，对系统的优化状态窗口大小做出限制，设置固定大小，在一定程度上平衡计算量。在窗口滑动的过程中，不断会有新的UWB因子和新的IMU预积分因子被添加到滑窗内，而旧的系统状态、旧的UWB测距因子和旧的IMU预积分因子需要被剔除掉。但是历史时刻的系统状态和观测因子和窗口中的其他状态之间已经存在约束联系，如果直接丢弃窗口之外的数据会导致关联信息的缺失、状态估计不准确，影响系统的精度。所以，在滑窗中删除旧状态的方法并不是直接将旧的状态移除，而是对即将删除的状态，信息进行边缘化，发挥其最后的作用。边缘化的思想就是在移除旧状态的同时，将关联的约束转化为先验信息，再次加入到优化问题中，边缘化的方法是舒尔补。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。
Claims (3)
1.一种基于滑窗因子图的UWB-IMU组合室内定位方法，其特征在于，具体包含如下步骤：
S1.求解系统初始位置，提供系统初值；
S2.将IMU的位置和误差状态参数化，作为因子图的优化变量；
S3.进行IMU预积分计算；
S4.当检测到UWB数据时，进行UWB非视距判别，对UWB视距观测值，添加UWB测距因子，同时在因子图中添加IMU预积分因子；
S5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。
2.根据权力要求1所述的一种基于滑窗因子图的UWB-IMU组合室内定位方法，其特征在于，所述步骤S5中，对UWB和IMU进行数据融合的步骤如下：
滑窗因子图中的IMU状态为X＝[x0，x1，…，xn]，n为滑窗大小，其中xk为IMU状态，
状态xk包括了IMU在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置；
当检测到新的UWB数据时，进行UWB非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的IMU预积分因子和UWB观测值对应的新的UWB观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息；
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
式中的rpre和rUWB分别表示IMU的预积分因子和UWB观测因子，m+1是UWB观测因子的个数，rp-HpX表示边缘化带来的先验信息，利用Ceres框架求解式1中的最小二乘估计，解得系统状态的最优估计。
3.根据权力要求1所述的一种基于滑窗因子图的UWB-IMU组合室内定位方法，其特征在于，所述步骤S4中，对UWB测距值进行非视距判别的步骤如下：
通过比较根据因子图求出的标签到基站距离与实际UWB测量值，对非视距信号进行判定，其具体方法如下：在接收到新的UWB数据时，根据前一时刻由IMU因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵得到位置的标准差为
则位置坐标的置信区间分别为
和
因此在k时刻，标签到第i个基站的理论最大距离为：
其中，
为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标，当前时刻标签与第i个基站之间测距值为
标签与该基站之间测距误差为σi，则如果
判定该测距值为非视距测距值，进行降权或者直接剔除。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210536034.7A CN114915913A (zh) | 2022-05-17 | 2022-05-17 | 一种基于滑窗因子图的uwb-imu组合室内定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210536034.7A CN114915913A (zh) | 2022-05-17 | 2022-05-17 | 一种基于滑窗因子图的uwb-imu组合室内定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114915913A true CN114915913A (zh) | 2022-08-16 |
Family
ID=82768570
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210536034.7A Pending CN114915913A (zh) | 2022-05-17 | 2022-05-17 | 一种基于滑窗因子图的uwb-imu组合室内定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114915913A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116242372A (zh) * | 2023-01-03 | 2023-06-09 | 东南大学 | 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法 |
| CN116540284A (zh) * | 2023-07-06 | 2023-08-04 | 河北新合芯电子科技有限公司 | 室内导航定位方法、装置、系统及存储介质 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110554396A (zh) * | 2019-10-21 | 2019-12-10 | 深圳市元征科技股份有限公司 | 一种室内场景下激光雷达建图方法、装置、设备及介质 |
| CN111780781A (zh) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
| CN112923919A (zh) * | 2021-01-21 | 2021-06-08 | 湖南格纳微信息科技有限公司 | 基于图优化的行人定位方法及系统 |
| CN113411744A (zh) * | 2021-06-21 | 2021-09-17 | 中国矿业大学 | 一种高精度的室内定位追踪方法 |
| CN113432604A (zh) * | 2021-06-29 | 2021-09-24 | 广东工业大学 | 一种能够灵敏检测故障的imu/gps组合导航方法 |
| US20210373855A1 (en) * | 2020-12-24 | 2021-12-02 | Harbin Engineering University | Rotation Matrix-Based Factor Graph Cooperative Localization Algorithm |
| CN114222366A (zh) * | 2021-08-06 | 2022-03-22 | 深圳技术大学 | 一种基于单基站的室内定位方法及装置 |
-
2022
- 2022-05-17 CN CN202210536034.7A patent/CN114915913A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110554396A (zh) * | 2019-10-21 | 2019-12-10 | 深圳市元征科技股份有限公司 | 一种室内场景下激光雷达建图方法、装置、设备及介质 |
| CN111780781A (zh) * | 2020-06-23 | 2020-10-16 | 南京航空航天大学 | 基于滑动窗口优化的模板匹配视觉和惯性组合里程计 |
| US20210373855A1 (en) * | 2020-12-24 | 2021-12-02 | Harbin Engineering University | Rotation Matrix-Based Factor Graph Cooperative Localization Algorithm |
| CN112923919A (zh) * | 2021-01-21 | 2021-06-08 | 湖南格纳微信息科技有限公司 | 基于图优化的行人定位方法及系统 |
| CN113411744A (zh) * | 2021-06-21 | 2021-09-17 | 中国矿业大学 | 一种高精度的室内定位追踪方法 |
| CN113432604A (zh) * | 2021-06-29 | 2021-09-24 | 广东工业大学 | 一种能够灵敏检测故障的imu/gps组合导航方法 |
| CN114222366A (zh) * | 2021-08-06 | 2022-03-22 | 深圳技术大学 | 一种基于单基站的室内定位方法及装置 |
Non-Patent Citations (1)
| Title |
|---|
| 徐昊玮;廉保旺;刘尚波;: "基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法", 兵工学报, no. 04, 15 April 2019 (2019-04-15) * |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116242372A (zh) * | 2023-01-03 | 2023-06-09 | 东南大学 | 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法 |
| CN116540284A (zh) * | 2023-07-06 | 2023-08-04 | 河北新合芯电子科技有限公司 | 室内导航定位方法、装置、系统及存储介质 |
| CN116540284B (zh) * | 2023-07-06 | 2023-10-20 | 河北新合芯电子科技有限公司 | 室内导航定位方法、装置、系统及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111780755B (zh) | 一种基于因子图和可观测度分析的多源融合导航方法 | |
| CN111272165B (zh) | 一种基于特征点标定的智能车定位方法 | |
| CN110706279B (zh) | 基于全局地图与多传感器信息融合的全程位姿估计方法 | |
| CN108519615B (zh) | 基于组合导航和特征点匹配的移动机器人自主导航方法 | |
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
| CN110411462B (zh) | 一种gnss/惯导/车道线约束/里程计多源融合方法 | |
| CN111721289A (zh) | 车辆定位方法、装置、设备、存储介质及车辆 | |
| CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
| CN114915913A (zh) | 一种基于滑窗因子图的uwb-imu组合室内定位方法 | |
| CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
| CN114323033B (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
| CN112147651B (zh) | 一种异步多车协同目标状态鲁棒估计方法 | |
| CN114264301A (zh) | 车载多传感器融合定位方法、装置、芯片及终端 | |
| CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
| CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN117320148A (zh) | 多源数据融合定位方法、系统、电子设备及存储介质 | |
| CN112229392A (zh) | 一种高冗余的室内煤场导航方法及系统 | |
| CN117268408A (zh) | 一种激光slam定位方法及系统 | |
| CN116482735A (zh) | 一种受限空间内外高精度定位方法 | |
| CN116067369A (zh) | 一种基于距离和角度的行人协同导航方法 | |
| CN113237482B (zh) | 一种基于因子图的城市峡谷环境下车辆鲁棒定位方法 | |
| CN114088104B (zh) | 一种自动驾驶场景下的地图生成方法 | |
| CN117346768B (zh) | 同时适用于室内外的多传感器融合感知定位方法 | |
| CN116380057B (zh) | 一种gnss拒止环境下无人机自主着陆定位方法 | |
| CN114440892B (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 |