一种基于滑窗因子图的UWB-IMU组合室内定位方法

一种基于滑窗因子图的uwb-imu组合室内定位方法
技术领域
1.本发明属于室内定位技术领域，具体涉及一种基于滑窗因子图的uwb-imu组合室内定位方法。


背景技术：

2.对于许多室内定位应用而言，可靠且高精度的定位仍然是一个开放的研究问题。目前全球导航卫星系统(global navigation satellite system，gnss)和惯性导航系统(inertial navigation system，ins)的融合估计已能够满足高精度的要求，但在卫星拒止环境下，gnss信号难以到达导致其定位功能失效；ins虽具有低成本和不受环境影响等优点，但需对陀螺仪和加速度计输出高频的角速度和加速度信号进行两次积分，容易带来随时间增长的累积误差，无法持续提供可靠的位置信息。现有卫星拒止环境下的定位方法主要有:基于地磁，基于超声，基于视觉和基于激光雷达等。然而地磁信号定位极度依赖精准的环境先验地磁信息，不同场景适应性差；超声定位易受环境影响，且工作距离较短，精度较低；视觉定位运算量大，受光照影响严重，动态易变场景下易失效；激光雷达定位成本昂贵，运算量大，难以推广应用，且受同质化特征影响较为严重。
3.以蓝牙、超宽带(ultra wide band，uwb)等为代表的无线传感定位方法，成本低，精度较高，通过利用无线信号测量基站和接收站之间的距离，并通过已知基站位置信息估计接收站位置。其中，uwb时间分辨率高、穿透力强、功耗低、抗多径效果好、安全性强，其观测值可以用来抑制并修正ins引起的累积误差。因此，uwb/ins的融合定位成为gnss拒止环境下一种主流的室内定位解决方案。然而，uwb测量过程中容易出现的非视距(non-line-of-sight，nlos)现象是影响定位结果的重要因素。非视距指基站与接收站之间的传播路径受到动静态障碍物的遮挡影响，从而导致传播时间变长，测量距离相对实际距离变大。非视距效应会导致uwb观测信号质量变动频繁，影响定位精度。因此，高效的非视距检测判别及自适应融合方法是uwb/ins定位的关键。
4.现有的数据融合方法多采用卡尔曼滤波(kalman filter)及其变体。但这类方法依赖于事先建立的固有且准确的数学模型，当观测信号多变时自适应能力较弱。若接收到uwb的非视距信号，固有的滤波模型会带来严重的错误。因此，现有的uwb/ins融合定位方法仍较难解决室内环境下uwb观测信号质量多变带来的定位不准的问题，难以保证室内高精度的定位需求。


技术实现要素：

5.为解决上述技术问题，本发明提出了一种基于滑窗因子图的uwb-imu组合室内定位方法，本质上，是利用基于滑窗因子图优化的uwb/imu组合定位方法对uwb和imu数据进行融合，同时进行uwb的非视距判别，减小非视距测量值对定位结果的影响。
6.为实现上述目的，本发明采取的技术方案是：
7.一种基于滑窗因子图的uwb-imu组合室内定位方法，其特征在于，具体包含如下步
骤：
8.s1.求解系统初始位置，提供系统初值；
9.s2.将imu的位置和误差状态参数化，作为因子图的优化变量；
10.s3.进行imu预积分计算；
11.s4.当检测到uwb数据时，进行uwb非视距判别，对uwb视距观测值，添加uwb测距因子，同时在因子图中添加imu预积分因子；
12.s5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。
13.作为本发明进一步，所述步骤s5中，对uwb和imu进行数据融合的步骤如下：
14.滑窗因子图中的imu状态为x＝[x0，x1，
…
，xn]，n为滑窗大小，其中xk为imu状态，状态xk包括了imu在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置；
[0015]
当检测到新的uwb数据时，进行uwb非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的imu预积分因子和uwb观测值对应的新的uwb观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息；
[0016]
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
[0017][0018]
式中的r
pre
和r
uwb
分别表示imu的预积分因子和uwb观测因子，m+1是uwb观测因子的个数，r
p-h
p
x表示边缘化带来的先验信息，利用ceres框架求解式1中的最小二乘估计，解得系统状态的最优估计。
[0019]
作为本发明进一步，所述步骤s4中，对uwb测距值进行非视距判别的步骤如下：
[0020]
通过比较根据因子图求出的标签到基站距离与实际uwb测量值，对非视距信号进行判定，其具体方法如下：在接收到新的uwb数据时，根据前一时刻由imu因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵得到位置的标准差为则位置坐标的置信区间分别为和因此在k时刻，标签到第i个基站的理论最大距离为：
[0021][0022]
其中，为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标，当前时刻标签与第i个基站之间测距值为标签与该基站之间测距误差为σi，则如果判定该测距值为非视距测距值，进行降权或者直接剔除。
[0023]
本发明提供一种基于滑窗因子图的uwb-imu组合室内定位方法，主要包括:
[0024]
(1)提出一种基于因子图优化算法的uwb/imu数据融合算法，因子图算法的灵活性很高，能即插即用的组合使用不同传感器，如果存在传感器失效，能够及时的删除失效传感
器对应的因子；
[0025]
(2)通过利用因子图输出的位置及位置置信度来对非视距信号进行判定，对uwb非视距测量值进行降权或剔除。
附图说明
[0026]
图1为系统工作流程图；
[0027]
图2为因子图原理示意图；
[0028]
图3为系统因子图结构图。
具体实施方式
[0029]
为了解决这些问题，本方法提出了一种更有效的方法来融合惯性和uwb测量。本质上，是利用基于滑窗因子图优化的uwb/imu组合定位方法对uwb和imu数据进行融合，同时进行uwb的非视距判别，减小非视距测量值对定位结果的影响。主要包括:
[0030]
(1)提出一种基于因子图优化算法的uwb/imu数据融合算法，因子图算法的灵活性很高，能即插即用的组合使用不同传感器，如果存在传感器失效，能够及时的删除失效传感器对应的因子；
[0031]
(2)通过利用因子图输出的位置及位置置信度来对非视距信号进行判定，对uwb非视距测量值进行降权或剔除。
[0032]
因子图概念
[0033]
通过因式分解使得一个函数转化为多个函数之间的乘积形式，然后将两种表达方式用直线连接起来构成的无向图称为因子图。因子图模型是通过贝叶斯理论推导出的一种概率图模型。
[0034]
令因子图模型为：h＝(f，x，e)，其中x表示变量节点，即因子图中需要进行优化估计的参数，f是因子图中的因子节点，e是连接因子节点和变量节点的边，是一条无向边，若一个因子节点与一个变量节点通过无向边e相连，则该变量节点就是该因子的变量节点。通过数据集中所有因子的联乘积来表示联合概率分布情况。
[0035]
设概率分布函数为g(x1，
…
，xn):
[0036][0037]
式1中，xi代表变量节点，fj(xj)表示局部函数。当变量节点和局部函数之间经由无向边相连，且因子图中所有节点全部连接后，即可构成因子图模型，如图2所示。
[0038]
因子图优化算法流程
[0039]
如图1所示，是基于因子图的uwb/imu组合定位算法的算法流程，首先进行imu的初始预积分，将系统状态转换成具体的位置和误差参数，添加到因子图中作为因子图的变量节点，再进行imu预积分，当检测到uwb观测数据时，进行uwb的非视距判别和剔除，对视距uwb测量值添加uwb测距因子，同时添加imu预积分因子，之后判断因子图中同时优化的状态参数个数是否达到设定的窗口大小，如果窗口已满，就进行边缘化操作，边缘化最早的历史状态，最后进行多个状态的联合优化。其中优化过程即图优化的求解过程，对各个因子求各个因子计算的观测值与预测值之差，即残差值，以及求残差对应的雅可比矩阵，利用因子图
优化求解方式进行求解。
[0040]
系统方程
[0041]
图3为本章中的基于滑窗因子图的uwb/imu紧组合定位系统因子图结构图，滑窗中的imu状态为x＝[x0，x1，
…
，xn]，n为滑窗大小，其中xk为imu状态，状态xk包括了imu在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置。当检测到新的uwb数据时，进行uwb非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的imu预积分因子和uwb观测值对应的新的uwb观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息，提高数据利用率的同时，也提高定位精度。
[0042]
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
[0043][0044]
式中的r
pre
和r
uwb
分别表示imu的预积分因子和uwb观测因子，m+1是uwb观测因子的个数，r
p-h
p
x表示边缘化带来的先验信息，这些内容将在后续小节中介绍。求解式2中的最小二乘估计，解得系统状态的最优估计，求解最小二乘估计这一过程可由ceres框架进行求解。
[0045]
imu预积分因子
[0046]
在优化过程中，引入imu数据进行多传感器数据融合可以提高定位的平滑性和准确性。通过对imu数据进行积分，把上一刻的位置、速度和四元数当作初值，可以得到当前时刻的位置、速度和四元数，更新位置、速度和四元数。但是在因子图优化中，往往是窗口整体优化，在优化过程中，上一刻的初值经过一轮优化后其值会改变，这样对应imu的积分也要重新计算，这样的话，计算量大，计算过程复杂，浪费计算资源，所以提出预积分方式来提高计算速度，避免浪费算力。预积分的核心思想是把积分中的初值“摘”出去，使得积分结果与初值无关，优化过程中无论初值怎么变化都对imu数据积分的计算没有影响，提高计算速度。
[0047]
预积分因子的残差r
pre
为：
[0048][0049]
式中和分别为预积分的位置、速度和四元数观测值，分别为预积分的位置、速度和四元数观测值，和为位置、速度和四元数的预测值，和分别为陀螺仪和加速度计零偏。
[0050]
非视距信号判别
[0051]
当uwb测距值受非视距影响，导致测距值大于实际距离时，如果依然简单的将uwb非视距测量值加入因子图中，构造uwb因子，会给定位结果带来很大影响。因此，准确的识别并剔除uwb中的非视距信息对保持定位结果准确可靠十分重要。
[0052]
本方法中，通过比较根据因子图求出的标签到基站距离与实际uwb测量值，对非视距信号进行判定，其具体方法如下：在接收到新的uwb数据时，根据前一时刻由imu因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵可以得到位置的标准差为则位置坐标的置信区间分别为和因此在k时刻，标签到第i个基站的理论最大距离为：
[0053][0054]
其中，为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标。当前时刻标签与第i个基站之间测距值为标签与该基站之间测距误差为σi，则如果可判定该测距值为非视距测距值，可进行降权或者直接剔除。
[0055]
uwb观测因子
[0056]
uwb与imu的主要组合方式有两种，即松组合和紧组合。松组合是由uwb得到定位结果，再将uwb的定位位置作为观测量加入因子图中，这种方式计算简单，容易实现，但uwb与imu两个系统联系不够紧密，当出现uwb定位异常时，会对组合定位系统造成较大影响。而紧组合这种组合方式使用uwb测距值作为uwb观测值，当发生uwb非视距测量时，可以及时的对uwb非视距测量进行识别、剔除，而并不影响其他正常数据的使用。因此，使用紧组合模式可以得到更高的定位精度，本方法中的uwb/imu组合方式也是紧组合。在紧组合模式中，可以定义uwb观测因子的残差为uwb测距值与系统推算的标签与基站之间距离之差：
[0057]ruwb
＝d-||p
uwb-p
base
||
[0058]
式中d表示uwb的测距值，||p
uwb-p
base
||表示根据系统状态推算的uwb标签到基站的距离。
[0059]
边缘化
[0060]
随着系统运行时间的增长，加入窗口中的待优化状态变量个数也会越来越多，如果不对待优化状态的个数做出限制，会导致因子图优化计算复杂度越来越高，运行效率越来越低。因此，为了降低系统计算的复杂度，因子图的优化过程采用了基于滑动窗口思想的方法，对系统的优化状态窗口大小做出限制，设置固定大小，在一定程度上平衡计算量。在窗口滑动的过程中，不断会有新的uwb因子和新的imu预积分因子被添加到滑窗内，而旧的系统状态、旧的uwb测距因子和旧的imu预积分因子需要被剔除掉。但是历史时刻的系统状态和观测因子和窗口中的其他状态之间已经存在约束联系，如果直接丢弃窗口之外的数据会导致关联信息的缺失、状态估计不准确，影响系统的精度。所以，在滑窗中删除旧状态的方法并不是直接将旧的状态移除，而是对即将删除的状态，信息进行边缘化，发挥其最后的作用。边缘化的思想就是在移除旧状态的同时，将关联的约束转化为先验信息，再次加入到优化问题中，边缘化的方法是舒尔补。
[0061]
本发明的总体工作流程如图1所示，一种基于滑窗因子图的uwb-imu组合室内定位方法，包含如下步骤：
[0062]
s1.求解系统初始位置，提供系统初值；
[0063]
s2.将imu的位置和误差状态参数化，作为因子图的优化变量；
[0064]
s3.进行imu预积分计算；
[0065]
s4.当检测到uwb数据时，进行uwb非视距判别，对uwb视距观测值，添加uwb测距因子，同时在因子图中添加imu预积分因子；
[0066]
s5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。
[0067]
因子图优化算法流程
[0068]
如图1所示，是基于因子图的uwb/imu组合定位算法的算法流程，首先进行imu的初始预积分，将系统状态转换成具体的位置和误差参数，添加到因子图中作为因子图的变量节点，再进行imu预积分，当检测到uwb观测数据时，进行uwb的非视距判别和剔除，对视距uwb测量值添加uwb测距因子，同时添加imu预积分因子，之后判断因子图中同时优化的状态参数个数是否达到设定的窗口大小，如果窗口已满，就进行边缘化操作，边缘化最早的历史状态，最后进行多个状态的联合优化。其中优化过程即图优化的求解过程，对各个因子求各个因子计算的观测值与预测值之差，即残差值，以及求残差对应的雅可比矩阵，利用因子图优化求解方式进行求解。
[0069]
系统方程
[0070]
图3为本章中的基于滑窗因子图的uwb/imu紧组合定位系统因子图结构图，滑窗中的imu状态为x＝[x0，x1，
…
，xn]，n为滑窗大小，其中xk为imu状态，状态xk包括了imu在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置。当检测到新的uwb数据时，进行uwb非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的imu预积分因子和uwb观测值对应的新的uwb观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息，提高数据利用率的同时，也提高定位精度。
[0071]
根据因子图理论，将系统整体的残差最小化，得到最小二乘估计如下：
[0072][0073]
式中的r
pre
和r
uwb
分别表示imu的预积分因子和uwb观测因子，m+1是uwb观测因子的个数，r
p-h
p
x表示边缘化带来的先验信息，这些内容将在后续小节中介绍。求解式6中的最小二乘估计，解得系统状态的最优估计。
[0074]
imu预积分因子
[0075]
在优化过程中，引入imu数据进行多传感器数据融合可以提高定位的平滑性和准确性。通过对imu数据进行积分，把上一刻的位置、速度和四元数当作初值，可以得到当前时刻的位置、速度和四元数，更新位置、速度和四元数。但是在因子图优化中，往往是窗口整体优化，在优化过程中，上一刻的初值经过一轮优化后其值会改变，这样对应imu的积分也要重新计算，这样的话，计算量大，计算过程复杂，浪费计算资源，所以提出预积分方式来提高计算速度，避免浪费算力。预积分的核心思想是把积分中的初值“摘”出去，使得积分结果与初值无关，优化过程中无论初值怎么变化都对imu数据积分的计算没有影响，提高计算速度。
[0076]
预积分因子的残差r
pre
为：
[0077][0078]
式中和分别为预积分的位置、速度和四元数观测值，分别为预积分的位置、速度和四元数观测值，和为位置、速度和四元数的预测值，和分别为陀螺仪和加速度计零偏。
[0079]
非视距信号判别
[0080]
当uwb测距值受非视距影响，导致测距值大于实际距离时，如果依然简单的将uwb非视距测量值加入因子图中，构造uwb因子，会给定位结果带来很大影响。因此，准确的识别并剔除uwb中的非视距信息对保持定位结果准确可靠十分重要。
[0081]
本方法中，通过比较根据因子图求出的标签到基站距离与实际uwb测量值，对非视距信号进行判定，其具体方法如下：在接收到新的uwb数据时，根据前一时刻由imu因子得到的位置(xk，yk，zk)，由因子图更新得到的位置协方差矩阵可以得到位置的标准差为则位置坐标的置信区间分别为和因此在k时刻，标签到第i个基站的理论最大距离为：
[0082][0083]
其中，为当前时刻的可能位置坐标，(xi，yi，zi)为第i个基站坐标。当前时刻标签与第i个基站之间测距值为标签与该基站之间测距误差为σi，则如果可判定该测距值为非视距测距值，可进行降权或者直接剔除。
[0084]
uwb观测因子
[0085]
uwb与imu的主要组合方式有两种，即松组合和紧组合。松组合是由uwb得到定位结果，再将uwb的定位位置作为观测量加入因子图中，这种方式计算简单，容易实现，但uwb与imu两个系统联系不够紧密，当出现uwb定位异常时，会对组合定位系统造成较大影响。而紧组合这种组合方式使用uwb测距值作为uwb观测值，当发生uwb非视距测量时，可以及时的对uwb非视距测量进行识别、剔除，而并不影响其他正常数据的使用。因此，使用紧组合模式可以得到更高的定位精度，本章的uwb/imu组合方式也是紧组合。在紧组合模式中，可以定义uwb观测因子的残差为uwb测距值与系统推算的标签与基站之间距离之差：
[0086]ruwb
＝d-||p
uwb-p
base
||
[0087]
式中d表示uwb的测距值，||p
uwb-p
base
||表示根据系统状态推算的uwb标签到基站的距离。
[0088]
边缘化
[0089]
随着系统运行时间的增长，加入窗口中的待优化状态变量个数也会越来越多，如果不对待优化状态的个数做出限制，会导致因子图优化计算复杂度越来越高，运行效率越来越低。因此，为了降低系统计算的复杂度，因子图的优化过程采用了基于滑动窗口思想的方法，对系统的优化状态窗口大小做出限制，设置固定大小，在一定程度上平衡计算量。在
窗口滑动的过程中，不断会有新的uwb因子和新的imu预积分因子被添加到滑窗内，而旧的系统状态、旧的uwb测距因子和旧的imu预积分因子需要被剔除掉。但是历史时刻的系统状态和观测因子和窗口中的其他状态之间已经存在约束联系，如果直接丢弃窗口之外的数据会导致关联信息的缺失、状态估计不准确，影响系统的精度。所以，在滑窗中删除旧状态的方法并不是直接将旧的状态移除，而是对即将删除的状态，信息进行边缘化，发挥其最后的作用。边缘化的思想就是在移除旧状态的同时，将关联的约束转化为先验信息，再次加入到优化问题中，边缘化的方法是舒尔补。
[0090]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。