CN113865584B - 一种基于视觉惯性里程计的uwb三维寻物方法和装置 - Google Patents
一种基于视觉惯性里程计的uwb三维寻物方法和装置 Download PDFInfo
- Publication number
- CN113865584B CN113865584B CN202110972559.0A CN202110972559A CN113865584B CN 113865584 B CN113865584 B CN 113865584B CN 202110972559 A CN202110972559 A CN 202110972559A CN 113865584 B CN113865584 B CN 113865584B
- Authority
- CN
- China
- Prior art keywords
- pose data
- intelligent terminal
- uwb
- base station
- virtual base
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 43
- 230000000007 visual effect Effects 0.000 title claims abstract description 37
- 238000001514 detection method Methods 0.000 claims abstract description 20
- 238000005457 optimization Methods 0.000 claims description 22
- 230000001133 acceleration Effects 0.000 claims description 5
- 230000002146 bilateral effect Effects 0.000 claims description 5
- 238000004891 communication Methods 0.000 claims description 4
- 238000013528 artificial neural network Methods 0.000 claims description 3
- 230000005540 biological transmission Effects 0.000 claims 1
- 238000011895 specific detection Methods 0.000 abstract description 2
- 238000005259 measurement Methods 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 4
- 238000009825 accumulation Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000002457 bidirectional effect Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 230000009977 dual effect Effects 0.000 description 1
- 230000001575 pathological effect Effects 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
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/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
- G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
- G01C11/02—Picture taking arrangements specially adapted for photogrammetry or photographic surveying, e.g. controlling overlapping of pictures
-
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Navigation (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种基于视觉惯性里程计的UWB三维寻物方法和装置。其中，方法包括：基于惯性导航系统和视觉里程计来确定智能终端在不同检测位置上的位姿数据，并将特定检测位置作为虚拟基站的建立位置；根据UWB定位单元收发到的UWB标签的脉冲信号，确定各虚拟基站与待搜索物品之间的距离；根据各虚拟基站的坐标信息和各虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息。通过利用智能终端集成的惯性传感器和单目相机来实时的解算终端的移动轨迹，能够极大程度保证终端轨迹跟踪的精度；通过建立虚拟基站的方式组建局部小型定位网络来求解待搜寻物品相对于移动终端的位置，能够帮助用户进行精准的位置判断。
Description
技术领域
本发明实施例涉及物品定位技术领域，尤其涉及一种基于视觉惯性里程计的UWB的三维寻物方法和装置。
背景技术
当前智能终端(主要是智能手机)寻物方案典型有两种，一种是利用低功耗蓝牙，基于是否接收信号或者接收的信号的强度来进行寻物操作，另一种是采用超宽带(Ultra-Wide Bandwidth，UWB)双天线方案，基于到达相位差(Phase Difference of Arrival，PDOA)来确定待搜寻物品的位置信息。
然而，低功耗蓝牙只能做存在性检测，不能测距(RSS测距不准，测距精度依赖模型参数)、测角；UWB双天线只能测一个小范围的角度，一旦超过这一范围，角度测量精度急剧下降，无法标明待搜索物品的具体方向，包括水平面和纵截面两个方向的角度，且无法判别物品的前后位置和上下位置。
发明内容
为了解决现有技术中的缺陷，本发明提供一种基于视觉惯性里程计的UWB的三维寻物方法和装置，以确定待搜寻物品精确的三维位置信息。
第一方面，本发明实施例提供了一种基于视觉惯性里程计的UWB的三维寻物方法，由三维寻物系统的控制器执行，所述三维寻物系统包括设置在智能终端内的惯性传感器、UWB 定位单元、单目相机以及设置在待搜索物品上的UWB标签，所述惯性传感器用于测量所述智能终端移动过程中的加速度和角速度信息，所述UWB定位单元与所述UWB标签通信连接，用于发射UWB脉冲信号和接收所述UWB标签发送的脉冲信号，所述方法包括：
基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，并将相邻间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置；
根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各虚拟基站与待搜索物品之间的距离；
根据各虚拟基站的坐标信息和各虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息。
第二方面，一种基于视觉惯性里程计的UWB的三维寻物装置，配置于三维寻物系统的控制器中，所述三维寻物系统包括设置在智能终端内的惯性传感器、UWB定位单元、单目相机以及设置在待搜索物品上的UWB标签，所述惯性传感器用于测量所述智能终端移动过程中的速度信息，所述UWB定位单元与所述UWB标签通信连接，用于发射UWB脉冲信号和接收所述UWB标签发送的脉冲信号，所述装置包括：
智能终端位姿确定模块，用于基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，并将相邻间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置；
距离确定模块，用于根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各虚拟基站与待搜索物品之间的距离；
待搜索物品位置信息确定模块，用于根据各虚拟基站的坐标信息和各虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息。
本发明的有益效果：通过建立虚拟基站的方式组建局部小型定位网络来求解待搜寻物品相对于移动终端的位置，利用智能终端集成的惯性传感器和单目相机来实时的解算终端的移动轨迹，单目相机和惯性组合既能弥补单目相机的尺度问题和图像模糊导致轨迹跟踪不连续等问题，又能克服惯性的误差累积问题，该组合方案位姿能够极大程度保证终端轨迹跟踪的精度；此外，针对网形较差的小型定位网络定位解算问题，通过残差校正的方式来进行精确求解，以确定待搜寻物品的精确三维位置信息。
附图说明
图1为本发明实施例提供的一种基于视觉惯性里程计的UWB的三维寻物方法的流程图；
图2为为本发明实施例提供的智能终端轨迹推估的流程图；
图3为本发明实施例提供的基于视觉惯性里程计的UWB的三维寻物方法的基础原理图；
图4为本发明实施例提供的基于双边双向测量模式的测量原理图；
图5为本发明实施例提供的一种基于视觉惯性里程计的UWB三维寻物装置的结构示意图；
图6为本发明实施例提供的一种关键帧图像特征点稀疏时深度信息计算的流程图。
具体实施方式
下面结合附图和实施例对本发明作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部结构。
实施例
图1为本发明实施例提供的基于UWB的三维寻物方法的流程图，本实施例可适用于通过智能终端进行物品定位的情况，该方法可以由三维寻物系统的控制器执行，所述三维寻物系统包括设置在智能终端内的惯性传感器、UWB定位单元、单目相机以及设置在待搜索物品上的UWB标签，所述惯性传感器用于测量所述智能终端移动过程中的速度信息，所述UWB定位单元与所述UWB标签通信连接，用于发射UWB脉冲信号和接收所述UWB标签发送的脉冲信号。
其中，所述惯性传感器包括加速度计和陀螺仪，加速度计用来测量智能终端移动过程中的加速度，陀螺仪用来测量智能终端移动过程中的角速度。本实施例中的UWB定位单元为UWB单天线结构，也可以替换为其它具有定位功能的设备，如蓝牙、可见光等等。
所述方法具体包括如下步骤：
S110、基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，并将相邻间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置。
其中，所述智能终端包括但不限于智能手机、移动电脑、平板电脑等智能设备。
具体的，本实施例中的加速度计和陀螺仪内置在智能终端的微电子机械系统(Micro-Electro-Mechanical System，MEMS)中，该MEMS系统中还可以设置磁力计，用来测量智能设备与东南西北四个方向上的夹角。
待搜索物品上对应的设置有UWB标签，用于向设置在智能终端内部的UWB定位单元发送脉冲信号。
本实施例中，智能终端在寻物的过程中尽量保持移动，并且移动轨迹最好是一个完整的闭环。进一步参见图2，基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，包括：
从所述单目相机采集的图像流中确定关键帧图像；
对所述关键帧图像之间的惯性传感器数据进行预积分，确定所述智能终端的第一位姿数据；其中，上述惯性传感器数据包括加速度计获取的加速度，以及陀螺仪获取的角速度，通过对获取到的加速度和角度度进行预积分，可以得到智能终端当前时刻的位姿、速度以及旋转角，将上述求解得到的位姿作为智能终端的第一位姿数据。并同时计算在后续对位姿数据进行优化过程中所要用到的相邻帧间的预积分增量、预积分的协方差矩阵和雅可比矩阵。
接着，基于视觉里程计，根据所述关键帧图像确定所述智能终端的第二位姿数据；具体的，提取所述关键帧图像中的特征点；将所述关键帧图像对应的特征点进行特征匹配以解算出所述单目相机的位姿数据，并将所述单目相机的位姿数据作为所述智能终端的第二位姿数据。进一步参见图6，在利用视觉里程计计算相机位姿时，可能面临特征点稀疏、图像模糊等问题从而无法有效估计相机移动过程中的位姿，本实施例中使用深度估计神经网络估计深度信息和不确定度，来辅助滑窗滤波器进行位姿估计。
在求得第一位姿数据和第二位姿数据之后，对所述第一位姿数据和所述第二位姿数据进行初始化，以得到所述智能终端的位姿数据；具体的，先对所述第二位姿数据进行初始化，即先进行视觉的初始化，解算出所述单目相机的相对位姿数据，由于使用单目视觉无尺度信息；需要再与IMU预积分进行对齐，来给单目相机的相对位姿数据加上尺度信息，从而得到智能终端初的位姿数据。
进一步的，在初始化之后对所述智能终端的位姿数据进行优化，以得到所述智能终端的最终位姿数据。具体的，上述优化步骤包括：基于滑动窗口的视觉惯导对所述智能终端的位姿数据进行局部优化，以得到局部优化后的位姿数据。本实施例中在进行局部优化时，将视觉约束、IMU约束放在一个大目标函数中进行优化，并且本实施例中的局部优化也只优化当前帧及之前的n帧的窗口中的变量，局部非线性优化输出较为精确的位姿。
接着，对智能终端的运动轨迹进行回环检测，即检测所述智能终端的移动轨迹是否为闭合轨迹。具体的检测步为：将前面检测的关键帧图像保存起来，当再回到原来经过的同一个地方，通过特征点的匹配关系，判断是否已经来过这里。若存在回环，则对所述局部优化后的位姿数据进行全局优化，将全局优化得到的位姿数据作为所述移动终端的最终位姿数据；其中，全局优化利用相机约束和IMU约束，再加上回环检测的约束，进行非线性优化。全局优化在局部优化的基础上进行，能够输出更为精确的位姿。
若不存在回环，则将所述局部优化后的位姿数据作为所述移动终端的最终位姿数据。
本实施例通过利用智能终端中集成的惯性传感器和单目相机来实时的解算终端的移动轨迹，单目相机和惯性组合既能弥补单目相机的尺度问题和图像模糊导致轨迹跟踪不连续等问题，又能克服惯性的误差累积问题，该组合方案位姿能够极大程度保证终端轨迹跟踪的精度。
继续参见图3，在确定出上述检测位置的坐标之后，抽取轨迹中两两检测位置的间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置。图3中的A、B、C、D表示智能终端移动过程中停留的检测位置。
S120、根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各虚拟基站与待搜索物品之间的距离。
本实施例中，UWB定位单元根据接收到的UWB标签发出的脉冲信号可以直接得到各虚拟基站与待搜索物品之间的距离。为了提高距离测量的精度，本实施例采用双边双向测距模式，来确定各虚拟基站与待搜索物品之间的距离，从而消除时间不同步带来的误差。
基于双边双向测量模式的测量原理图详见图4，图4中的虚线表示信号传输走向，每个检测位置与待搜寻物品之间距离的计算公式如下：
式中c表示光速。
S130、根据各虚拟基站的坐标信息和各虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息。
本实施例中，基于信号到达时间差的定位算法，根据各所述虚拟基站的坐标信息和各所述虚拟基站与所述待搜索物品之间的距离，采用残差校正法计算所述待搜索物品的三维位置信息。
所述信号到达时间差定位算法的计算公式为：
其中，(x,y,z),(xi,yi,zi)分别为所述待搜索物品坐标和各所述虚拟基站坐标，di表示第 i个虚拟基站与所述待搜索物品之间的距离。
进一步的，由于建立起的虚拟基站间的间距较小，基站同标签组成的网形结构较差，即上面求解方程组是一个较为典型的病态方程组，相较于常规求解方法，可以进行一些迭代操作以保证求解精度。示例性的，本发明实施例采用残差校正法进行计算：
上式可以进一步化简至：
进一步简化：
Y＝F·X
步骤1：直接求Y＝F·X，得到第一步的一个近似解X1。
步骤2:得到残差Δ1＝Y-F·X1，求解F·ΔX1＝Δ1，修正X2＝X1+ΔX1。
步骤3：得到残差Δ2＝Y-F·X2，求解F·ΔX2＝Δ2，修正X3＝X2+ΔX2。
步骤4:迭代。
步骤5：直到Δn小于某一阈值，最终输出结果X＝Xn+ΔXn。
这其中可能出现的空间奇异的问题，可以通过要求用户终端移动过程中使各个停留位置最终不在一个平面来避免。通过上述计算过程，最终求解的向量结果为待搜寻物品的三维空间坐标，通过得到的三维坐标不仅可以判断距离，同时可以判断物品相较于终端是处于前还是后、处于上还是下，并且针对上下、前后都有精确的角度输出以帮助用户进行精准的位置判断。
参见图5，本发明实施例还提供了一种基于视觉惯性里程计的UWB三维寻物装置，该装置配置于三维寻物系统的控制器中，所述三维寻物系统包括设置在智能终端内的惯性传感器、UWB定位单元、单目相机以及设置在待搜索物品上的UWB标签，所述惯性传感器用于测量所述智能终端移动过程中的速度信息，所述UWB定位单元与所述UWB标签通信连接，用于发射UWB脉冲信号和接收所述UWB标签发送的脉冲信号，所述装置包括：
智能终端位姿确定模块210，用于基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，并将相邻间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置；
距离确定模块220，用于根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各所述虚拟基站与所述待搜索物品之间的距离；
待搜索物品位置信息确定模块230，用于根据各所述虚拟基站的坐标信息和各所述虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息。
其中，所述智能终端位姿确定模块210具体用于：
从所述单目相机采集的图像流中确定关键帧图像；
对所述关键帧图像之间的惯性传感器数据进行预积分，确定所述智能终端的第一位姿数据；
基于视觉里程计，根据所述关键帧图像确定所述智能终端的第二位姿数据；
对所述第一位姿数据和所述第二位姿数据进行初始化，以得到所述智能终端的位姿数据；
对所述智能终端的位姿数据进行优化，以得到所述智能终端的最终位姿数据。
所述智能终端位姿确定模块210还具体用于：
基于视觉里程计，根据所述关键帧图像确定所述智能终端的第二位姿数据，包括：
提取所述关键帧图像中的特征点；
将所述关键帧图像对应的特征点进行特征匹配以解算出所述单目相机的位姿数据，并将所述单目相机的位姿数据作为所述智能终端的第二位姿数据。
所述智能终端位姿确定模块210还具体用于：
若所述关键帧图像中的特征点稀疏或者关键帧图像模糊时，则采用深度估计神经网络来确定所述单目相机的位姿数据。
所述智能终端位姿确定模块210还具体用于：对所述第一位姿数据和所述第二位姿数据进行初始化，以得到所述智能终端的位姿数据，包括：
对所述第二位姿数据进行初始化，解算出所述单目相机的相对位姿数据；
将所述单目相机的相对位姿数据与所述第一位姿数据进行对齐，以得到所述智能终端的位姿数据。
所述智能终端位姿确定模块210还具体用于：对所述智能终端的位姿数据进行优化，以得到所述智能终端的最终位姿数据，包括：
基于滑动窗口的视觉惯导对所述智能终端的位姿数据进行局部优化，以得到局部优化后的位姿数据；
检测所述智能终端的移动轨迹是否为闭合轨迹；
若是，则对所述局部优化后的位姿数据进行全局优化，将全局优化得到的位姿数据作为所述移动终端的最终位姿数据；
若否，则将所述局部优化后的位姿数据作为所述移动终端的最终位姿数据。
所述距离确定模块220用于基于双边双向测距模式，根据所述UWB定位单元收发到的 UWB标签的脉冲信号，确定各所述虚拟基站与所述待搜索物品之间的距离。
所述待搜索物品位置信息确定模块230具体用于：基于信号到达时间差的定位算法，根据各所述虚拟基站的坐标信息和各所述虚拟基站与所述待搜索物品之间的距离，采用残差校正法计算所述待搜索物品的三维位置信息。
其中，所述信号到达时间差定位算法的计算公式为：
其中，(x,y,z),(xi,yi,zi)分别为所述待搜索物品坐标和各所述虚拟基站坐标，di表示第 i个虚拟基站与所述待搜索物品之间的距离。
本发明实施例所提供的一种基于视觉惯性里程计的UWB的三维寻物装置可执行本发明任意实施例所提供的一种基于视觉惯性里程计的UWB的三维寻物方法，具备执行方法相应的功能模块和有益效果。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。
Claims (9)
1.一种基于视觉惯性里程计的UWB三维寻物方法，其特征在于，由三维寻物系统的控制器执行，所述三维寻物系统包括设置在智能终端内的惯性传感器、UWB定位单元、单目相机以及设置在待搜索物品上的UWB标签，所述惯性传感器用于测量所述智能终端移动过程中的加速度、角速度信息，所述UWB定位单元与所述UWB标签通信连接，用于发射UWB脉冲信号和接收所述UWB标签发送的脉冲信号，所述方法包括：
基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，并将相邻间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置；
根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各所述虚拟基站与所述待搜索物品之间的距离；
根据各所述虚拟基站的坐标信息和各所述虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息；
基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，包括：
从所述单目相机采集的图像流中确定关键帧图像；
对所述关键帧图像之间的惯性传感器数据进行预积分，确定所述智能终端的第一位姿数据；
基于视觉里程计，根据所述关键帧图像确定所述智能终端的第二位姿数据；
对所述第一位姿数据和所述第二位姿数据进行初始化，以得到所述智能终端的位姿数据；
对所述智能终端的位姿数据进行优化，以得到所述智能终端的最终位姿数据。
2.根据权利要求1所述的方法，其特征在，基于视觉里程计，根据所述关键帧图像确定所述智能终端的第二位姿数据，包括：
提取所述关键帧图像中的特征点；
将所述关键帧图像对应的特征点进行特征匹配以解算出所述单目相机的位姿数据，并将所述单目相机的位姿数据作为所述智能终端的第二位姿数据。
3.根据权利要求2所述的方法，其特征在于，若所述关键帧图像中的特征点稀疏或者关键帧图像模糊时，则采用深度估计神经网络来确定所述单目相机的位姿数据。
4.根据权利要求2所述的方法，其特征在于，对所述第一位姿数据和所述第二位姿数据进行初始化，以得到所述智能终端的位姿数据，包括：
对所述第二位姿数据进行初始化，解算出所述单目相机的相对位姿数据；
将所述单目相机的相对位姿数据与所述第一位姿数据进行对齐，以得到所述智能终端的位姿数据。
5.根据权利要求3所述的方法，其特征在于，对所述智能终端的位姿数据进行优化，以得到所述智能终端的最终位姿数据，包括：
基于滑动窗口的视觉惯导对所述智能终端的位姿数据进行局部优化，以得到局部优化后的位姿数据；
检测所述智能终端的移动轨迹是否为闭合轨迹；
若是，则对所述局部优化后的位姿数据进行全局优化，将全局优化得到的位姿数据作为所述智能终端的最终位姿数据；
若否，则将所述局部优化后的位姿数据作为所述智能终端的最终位姿数据。
6.根据权利要求1所述的方法，其特征在于，根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各所述虚拟基站与所述待搜索物品之间的距离，包括：
基于双边双向测距模式，根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各所述虚拟基站与所述待搜索物品之间的距离。
7.根据权利要求1所述的方法，其特征在于，根据各所述虚拟基站的坐标信息和各所述虚拟基站与所述待搜索物品之间的距离，确定所述待搜索物品的三维位置信息，包括：
基于信号到达时间差的定位算法，根据各所述虚拟基站的坐标信息和各所述虚拟基站与所述待搜索物品之间的距离，采用残差校正法计算所述待搜索物品的三维位置信息。
8.根据权利要求7所述的方法，其特征在于，所述信号到达时间差定位算法的计算公式为：
其中，(x,y,z),(xi,yi,zi)分别为所述待搜索物品坐标和各所述虚拟基站坐标，di表示第i个虚拟基站与所述待搜索物品之间的距离。
9.一种基于视觉惯性里程计的UWB的三维寻物装置，其特征在于，配置于三维寻物系统的控制器中，所述三维寻物系统包括设置在智能终端内的惯性传感器、UWB定位单元、单目相机以及设置在待搜索物品上的UWB标签，所述惯性传感器用于测量所述智能终端移动过程中的速度信息，所述UWB定位单元与所述UWB标签通信连接，用于发射UWB脉冲信号和接收所述UWB标签发送的脉冲信号，所述装置包括：
智能终端位姿确定模块，用于基于惯性导航系统和视觉里程计来确定所述智能终端在不同检测位置上的位姿数据，并将相邻间距大于距离阈值的至少四个检测位置作为虚拟基站的建立位置；
距离确定模块，用于根据所述UWB定位单元收发到的UWB标签的脉冲信号，确定各所述虚拟基站与所述待搜索物品之间的距离；
待搜索物品位置信息确定模块，用于根据各所述虚拟基站的坐标信息和各所述虚拟基站与待搜索物品之间的距离，确定待搜索物品的三维位置信息；
所述智能终端位姿确定模块具体用于：
从所述单目相机采集的图像流中确定关键帧图像；
对所述关键帧图像之间的惯性传感器数据进行预积分，确定所述智能终端的第一位姿数据；
基于视觉里程计，根据所述关键帧图像确定所述智能终端的第二位姿数据；
对所述第一位姿数据和所述第二位姿数据进行初始化，以得到所述智能终端的位姿数据；
对所述智能终端的位姿数据进行优化，以得到所述智能终端的最终位姿数据。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110972559.0A CN113865584B (zh) | 2021-08-24 | 2021-08-24 | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110972559.0A CN113865584B (zh) | 2021-08-24 | 2021-08-24 | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113865584A CN113865584A (zh) | 2021-12-31 |
| CN113865584B true CN113865584B (zh) | 2024-05-03 |
Family
ID=78988161
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110972559.0A Active CN113865584B (zh) | 2021-08-24 | 2021-08-24 | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113865584B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114485623B (zh) * | 2022-02-16 | 2024-02-23 | 东南大学 | 一种聚焦距离的相机-imu-uwb融合精准定位方法 |
| CN114623817B (zh) * | 2022-02-21 | 2024-04-26 | 武汉大学 | 基于关键帧滑窗滤波的含自标定的视觉惯性里程计方法 |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104697517A (zh) * | 2015-03-26 | 2015-06-10 | 江南大学 | 一种室内停车场多目标跟踪定位系统 |
| CN106912003A (zh) * | 2017-02-22 | 2017-06-30 | 上海斐讯数据通信技术有限公司 | 一种短距离搜寻系统和物件搜寻方法 |
| CN107609005A (zh) * | 2017-07-24 | 2018-01-19 | 中国科学院光电研究院 | 一种基于机器视觉的物品定位方法及定位装置 |
| CN108303673A (zh) * | 2018-02-01 | 2018-07-20 | 杭州球帆科技有限公司 | 一种基于视频辅助定位的uwb三维定位系统 |
| CN108827315A (zh) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
| CN109389343A (zh) * | 2018-09-14 | 2019-02-26 | 上海物联网有限公司 | 基于uwb技术的智能制造货物定位跟踪及追溯管理系统 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN111880543A (zh) * | 2020-08-05 | 2020-11-03 | 蒙泽新 | 一种基于uwb的室内机器人定位控制系统 |
| CN112051544A (zh) * | 2020-08-03 | 2020-12-08 | 云南电网有限责任公司楚雄供电局 | 一种基于uwb的电力仓库物资三维定位方法及装置 |
| KR20210026795A (ko) * | 2019-09-02 | 2021-03-10 | 경북대학교 산학협력단 | Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템 |
| CN112484725A (zh) * | 2020-11-23 | 2021-03-12 | 吉林大学 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
| CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
-
2021
- 2021-08-24 CN CN202110972559.0A patent/CN113865584B/zh active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104697517A (zh) * | 2015-03-26 | 2015-06-10 | 江南大学 | 一种室内停车场多目标跟踪定位系统 |
| CN106912003A (zh) * | 2017-02-22 | 2017-06-30 | 上海斐讯数据通信技术有限公司 | 一种短距离搜寻系统和物件搜寻方法 |
| CN107609005A (zh) * | 2017-07-24 | 2018-01-19 | 中国科学院光电研究院 | 一种基于机器视觉的物品定位方法及定位装置 |
| CN108303673A (zh) * | 2018-02-01 | 2018-07-20 | 杭州球帆科技有限公司 | 一种基于视频辅助定位的uwb三维定位系统 |
| CN108827315A (zh) * | 2018-08-17 | 2018-11-16 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
| CN109389343A (zh) * | 2018-09-14 | 2019-02-26 | 上海物联网有限公司 | 基于uwb技术的智能制造货物定位跟踪及追溯管理系统 |
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| KR20210026795A (ko) * | 2019-09-02 | 2021-03-10 | 경북대학교 산학협력단 | Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템 |
| CN112051544A (zh) * | 2020-08-03 | 2020-12-08 | 云南电网有限责任公司楚雄供电局 | 一种基于uwb的电力仓库物资三维定位方法及装置 |
| CN111880543A (zh) * | 2020-08-05 | 2020-11-03 | 蒙泽新 | 一种基于uwb的室内机器人定位控制系统 |
| CN112484725A (zh) * | 2020-11-23 | 2021-03-12 | 吉林大学 | 一种基于多传感器融合的智能汽车高精度定位与时空态势安全方法 |
| CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113865584A (zh) | 2021-12-31 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111024066B (zh) | 一种无人机视觉-惯性融合室内定位方法 | |
| EP3090407B1 (en) | Methods and systems for determining estimation of motion of a device | |
| CN113865584B (zh) | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 | |
| CN111462231B (zh) | 一种基于rgbd传感器和imu传感器的定位方法 | |
| EP2946167B1 (en) | Method and apparatus for determination of misalignment between device and pedestrian | |
| WO2017215024A1 (zh) | 一种基于新型多传感器融合技术的行人导航装置和方法 | |
| US10072956B2 (en) | Systems and methods for detecting and handling a magnetic anomaly | |
| CN113108791B (zh) | 一种导航定位方法及导航定位设备 | |
| CN109471096B (zh) | 多传感器目标匹配方法、装置及汽车 | |
| US10228252B2 (en) | Method and apparatus for using multiple filters for enhanced portable navigation | |
| CN113551670A (zh) | 一种基于uwb的三维寻物方法和装置 | |
| KR102226846B1 (ko) | Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템 | |
| CN110553648A (zh) | 一种用于室内导航的方法和系统 | |
| CN112525197B (zh) | 基于图优化算法超宽带惯性导航融合位姿估计方法 | |
| WO2016016731A2 (en) | Method and apparatus for categorizing device use case | |
| CN108106613B (zh) | 基于视觉辅助的定位方法与系统 | |
| CN112967392A (zh) | 一种基于多传感器触合的大规模园区建图定位方法 | |
| CN112529962A (zh) | 一种基于视觉算法的室内空间关键定位技术方法 | |
| CN117268370A (zh) | 确定移动方向与传感器行进方向间的失准角进行航位推算 | |
| KR20230008000A (ko) | 차선 및 특징 포인트에 기초한 위치확정방법 및 장치, 전자장비, 저장매체, 컴퓨터 프로그램 및 자율주행차량 | |
| CN114217268A (zh) | 一种基于机器学习的复杂环境下无线定位方法 | |
| EP3227634B1 (en) | Method and system for estimating relative angle between headings | |
| US10197402B2 (en) | Travel direction information output apparatus, map matching apparatus, travel direction information output method, and computer readable medium | |
| CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
| CN113483752A (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 |