一种聚焦距离的相机-IMU-UWB融合精准定位方法技术

本发明专利技术是一种聚焦距离的相机

【技术实现步骤摘要】
一种聚焦距离的相机
‑
IMU
‑
UWB融合精准定位方法


[0001]本专利技术涉及室内定位
，具体为一种聚焦距离的相机
‑
IMU
‑
UWB融合精准定位方法。

技术介绍

[0002]对于许多室内定位应用而言，可靠且全球一致的定位仍然是一个开放的研究问题。近年来，由于相机和惯性测量元件(IMU)传感器的互补性，视觉惯性里程计(VIO)或同时定位和建图(VI
‑
SLAM)是用于此目的的流行方法。尽管最先进的方法可以实现非常准确和高速率的姿态和速度估计，但传感器噪声和计算错误使系统易于随时间累积漂移。这个问题的流行解决方案是包括一个额外的全球传感器，如GPS。对于GPS不可用的情况(室内、隧道、走廊等)，超宽带(UWB)是适合小规模操作的替代选项。
[0003]一些研究已经为各种应用和场景提出了融合UWB数据和VIO的方法。这些方法以松耦合的方式工作，这意味着UWB范围和相机
‑
IMU数据首先在单独的定位系统中计算，然后由UWB和相机
‑
IMU子系统获得的位置估计被对齐并融合在一起。虽然这些方法可以直接构建，但如果一次性融合所有传感器数据以利用可用信息之间的相关性，结果可以得到改善。此外，它们需要设置多个已知的UWB基站以进行基于距离的定位，这可能成本高昂，并且会限制在许多空间受限的场景(例如室内、隧道、走廊等)中的适用性。
[0004]一些研究已经引入了仅使用位置未知的单个UWB基站的方法。由于不需要设置时间来校准基站位置，这样的系统将享有用于精确定位的无漂移距离测量以及在实际应用中易于使用的优点。结果表明，通过在联合优化问题中紧耦合UWB、相机和激光雷达或IMU测量，可以实现更准确和稳健的定位。然而，这些方法以类似模拟的方式处理UWB数据：每个相机位置与一个距离测量配对，并且不考虑两个连续相机帧之间的任何其他范围。由于多种原因，这种观点并不能反映现实生活中的传感器系统：真正的UWB传感器独立于相机/IMU传感器，因此距离/图像消息之间总是存在时间偏移；遵循VINS
‑
Mono的边缘化策略，当该关键帧被边缘化时，连接到第一个关键帧的UWB因素与视觉和惯性因素一起转化为线性先验。UWB测距速率不符合标准相机或IMU速率，UWB数据速率往往比相机高几倍；由于非视距，UWB数据速率在实际操作期间可能会有所不同，这意味着可用的UWB数据量也会有所不同。
[0005]在现有的定位系统中引入超宽带的方法有很多。UWB测距可独立定位，并与单目相机、IMU、RGB
‑
D相机、IMU和RGB
‑
D、LiDAR等相结合，提高了SLAM系统的精度和鲁棒性。要获得唯一的3D距离定位方法，需要以下两种方法:
[0006]1)至少有四个已知位置的UWB基站；
[0007]2)三个已知基站位置和载体高度数据。
[0008]这种假设限制了系统的适用场景，因为操作区域需要安装UWB基站，而且每个新环境都需要额外的时间和精力来校准基站的位置。为了满足这一要求，一些方法尝试在操作过程中估计基站地图，因为载体可以使用附加基站间范围的公制尺度里程计，或者仅使用公制尺度里程计，甚至只有标准尺度里程计。然而，这些解决方案仍然以次最佳的方式处理
UWB数据。

技术实现思路

[0009]为解决上述技术问题，本专利技术提出了一种聚焦距离的相机
‑
IMU
‑
UWB融合精准定位方法，本方法提出了一种更有效的方法来融合视觉、惯性和超宽带测量。本质上，是利用VIO方法中的现有状态传播过程有效地为每个测距数据制定UWB误差。
[0010]一种聚焦距离的相机
‑
IMU
‑
UWB融合精准定位方法，包括：
[0011]S1.初始化视觉惯性里程计VIO，等待VIO状态稳定；
[0012]S2.使用VIO初期提供的准确的里程测量，进行解算，获取载体在VIO的坐标系中的位置信息；
[0013]S3.利用载体在多个位置上得到的载体到UWB基站距离信息，估计UWB基站位置。当UWB基站位置不确定性低于某一阈值时，基站的位置被认为是固定的；
[0014]S4.在步骤S3获得了固定的基站位置后，随后可将UWB测量数据与视觉和惯性数据紧密融合在一起，通过基于联合关键帧的优化，获得准确和减少漂移的长期里程测量结果。
[0015]作为本专利技术进一步改进，所述步骤S3中，获取UWB基站位置的步骤如下：
[0016]为了估算UWB基站在世界框架中的位置ap，使用短期准确的VIO数据包括位姿和速度，本方法在数据窗口上制定了一个优化问题:
[0017][0018]由VIO方法的距离数据和相应的IMU积分输出位置组成，需要最小化的代价函数为
[0019][0020]其中ρ为Huber损失。UWB残差用IMU输出位置计算：
[0021][0022]对于一个低成本系统有噪声的IMU数据，IMU积分的状态传播可能是不可靠的，因此，用来代替进行更稳定的相机状态估计；
[0023]估计结果将取决于覆盖所有三维轴的轨迹以及基站相对于运动半径的距离，将x轴上位置的样本方差表示为y轴和z轴类似，当载体移动时，每个新位置数据都会递归更新，为了保证优化的性能，在启动或跳过优化过程时，需要检查以下条件：
[0024]检查载体是否在移动，如果载体是静态的，新的数据会是一样的，优化结果不会提高；
[0025]检查各轴位置的样本方差是否足够大，即载体的移动是否覆盖了所有方向；v
min
和是用户定义的参数。对每个新VIO数据检查第一个条件，直到满足终止条件，对第二个条件进行检查，直到第一次满足为止，通过加强这些条件，估计获得令人满
意的结果，无需费力的初始猜测基站的位置尽管如此，良好的初始猜测提高收敛时间，并可在实践中考虑提高性能；
[0026]一旦启动，使用标准Levenberg
‑
Marquardt算法和Ceres求解器优化代价函数，由于系统是在线运行的，因此引入了终止判据来确定解的不确定度：
[0027]σ
max
＜σ
p
#(4)
[0028]σ
max
是协方差矩阵的最大奇异值，σ
p
是一个给定的阈值，一旦满足标准，UWB基站定位操作完成，固定，由于σ
max
的计算不是必需的，但由于雅可比矩阵的秩不足检查和反演，可能会很耗时，因此这个终止检查在一个单独的线程中以较慢的速度运行，因此，优化可能会运行几次额外的时间，但处理时间不会受到影响。
[0029]作为本专利技术进一步改进，所述步骤S4中，UWB测量数据与视觉和惯性数据融合方法如下：
[0030]一旦找到UWB基站位置，相机
‑...

【技术保护点】

【技术特征摘要】
1.一种聚焦距离的相机
‑
IMU
‑
UWB融合精准定位方法，包括，其特征在于：S1.初始化视觉惯性里程计VIO，等待VIO状态稳定；S2.使用VIO初期提供的准确的里程测量，进行解算，获取载体在VIO的坐标系中的位置信息；S3.利用载体在多个位置上得到的载体到UWB基站距离信息，估计UWB基站位置，当UWB基站位置不确定性低于某一阈值时，基站的位置被认为是固定的；S4.在步骤S3获得了固定的基站位置后，随后可将UWB测量数据与视觉和惯性数据紧密融合在一起，通过基于联合关键帧的优化，获得准确和减少漂移的长期里程测量结果。2.根据权利要求1所述的一种聚焦距离的相机
‑
IMU
‑
UWB融合精准定位方法，其特征在于：所述步骤S3中，获取UWB基站位置的步骤如下：为了估算UWB基站在世界框架中的位置ap，使用短期准确的VIO数据包括位姿和速度，本方法在数据窗口上制定了一个优化问题:由VIO方法的距离数据和相应的IMU积分输出位置组成，需要最小化的代价函数为其中ρ为Huber损失，UWB残差用IMU输出位置计算：对于一个低成本系统有噪声的IMU数据，IMU积分的状态传播可能是不可靠的，因此，用来代替进行更稳定的相机状态估计；估计结果将取决于覆盖所有三维轴的轨迹以及基站相对于运动半径的距离，将x轴上位置的样本方差表示为y轴和z轴类似，当载体移动时，每个新位置数据都会递归更新，为了保证优化的性能，在启动或跳过优化过程时，需要检查以下条件：检查载体是否在移动，如果载体是静态的，新的数据会是一样的，优化结果不会提高；检查各轴位置的样本方差是否足够大，即载体的移动是否覆盖了所有方向；v
min
和是用户定义的参数，对每个新VIO数据检查第一个条件，直到满足终止条件，对第二个条件进行检查，直到第一次满足为止，通过加强这些条件，估计获得令人满意的结果，无需费力的初始猜测基站的位置尽管如此，良好的初始猜测提高收敛时间，并可在实践中考虑提高性能；一旦启动，使用标准Levenberg
‑
Marquardt算法和Ceres求解器优化代价函数，由于系统是在线运行的，因此引入了终止判据来确定解的不确定度：σ
max
＜σ
p
#(4)σ
max<...

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