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

一种基于滑窗因子图的UWB

【技术实现步骤摘要】
一种基于滑窗因子图的UWB
‑
IMU组合室内定位方法


[0001]本专利技术属于室内定位
，具体涉及一种基于滑窗因子图的UWB
‑
IMU组合室内定位方法。

技术介绍

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

技术实现思路

[0005]为解决上述技术问题，本专利技术提出了一种基于滑窗因子图的UWB
‑
IMU组合室内定位方法，本质上，是利用基于滑窗因子图优化的UWB/IMU组合定位方法对UWB和IMU数据进行融合，同时进行UWB的非视距判别，减小非视距测量值对定位结果的影响。
[0006]为实现上述目的，本专利技术采取的技术方案是：
[0007]一种基于滑窗因子图的UWB
‑
IMU组合室内定位方法，其特征在于，具体包含如下步
骤：
[0008]S1.求解系统初始位置，提供系统初值；
[0009]S2.将IMU的位置和误差状态参数化，作为因子图的优化变量；
[0010]S3.进行IMU预积分计算；
[0011]S4.当检测到UWB数据时，进行UWB非视距判别，对UWB视距观测值，添加UWB测距因子，同时在因子图中添加IMU预积分因子；
[0012]S5.判断因子图的滑窗是否已满，如果窗口已满，则将窗口中最早的状态进行边缘化删去，减小系统计算量，如果窗口未满，则直接进行联合优化，求解优化结果。
[0013]作为本专利技术进一步，所述步骤S5中，对UWB和IMU进行数据融合的步骤如下：
[0014]滑窗因子图中的IMU状态为X＝[x0，x1，
…
，x
n
]，n为滑窗大小，其中x
k
为IMU状态，状态x
k
包括了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因子得到的位置(x
k
，y
k
，z
k
)，由因子图更新得到的位置协方差矩阵得到位置的标准差为则位置坐标的置信区间分别为和因此在k时刻，标签到第i个基站的理论最大距离为：
[0021][0022]其中，为当前时刻的可能位置坐标，(x
i
，y
i
，z
i
)为第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的非视距判别本文档来自技高网...

【技术保护点】

【技术特征摘要】
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，
…
，x
n
]，n为滑窗大小，其中x
k
为IMU状态，状态x
k
包括了IMU在导航坐标系下的位置，速度，姿态以及陀螺仪和加速度计的漂移和偏置；当检测到新的UWB数据时，进行UWB非视距判别，在剔除非视距测量值之后，就在因子图中添加一个新的IMU预积分因子和UWB观测值对应的新的UWB观测因子，当达到窗口最大值后，需要将最早的状态进行边缘化，控制优化窗口的大小，并将边缘化的状态作为先验信息因子，再加入因子图中，充分利用系统状态中的信息；...

【专利技术属性】
技术研发人员：王庆，李明，杨高朝，
申请(专利权)人：东南大学，
类型：发明
国别省市：