CN115540854A - 一种基于uwb辅助的主动定位方法、设备和介质 - Google Patents
一种基于uwb辅助的主动定位方法、设备和介质 Download PDFInfo
- Publication number
- CN115540854A CN115540854A CN202211526026.0A CN202211526026A CN115540854A CN 115540854 A CN115540854 A CN 115540854A CN 202211526026 A CN202211526026 A CN 202211526026A CN 115540854 A CN115540854 A CN 115540854A
- Authority
- CN
- China
- Prior art keywords
- key frame
- ultra
- pose
- wideband
- loop
- 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 35
- 238000005457 optimization Methods 0.000 claims description 34
- 238000004422 calculation algorithm Methods 0.000 claims description 25
- 238000001514 detection method Methods 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 13
- 230000009466 transformation Effects 0.000 claims description 8
- 230000000007 visual effect Effects 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 6
- 239000000969 carrier Substances 0.000 claims description 4
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 238000005259 measurement Methods 0.000 description 7
- 230000007613 environmental effect Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 230000008030 elimination Effects 0.000 description 2
- 238000003379 elimination reaction Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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
- G01C21/1656—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 with passive imaging devices, e.g. cameras
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
- G01S5/0257—Hybrid positioning
- G01S5/0268—Hybrid positioning by deriving positions from different combinations of signals or of estimated positions in a single positioning system
- G01S5/02685—Hybrid positioning by deriving positions from different combinations of signals or of estimated positions in a single positioning system involving dead reckoning based on radio wave measurements
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10004—Still image; Photographic image
- G06T2207/10012—Stereo images
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于UWB辅助的主动定位方法、设备和介质,涉及定位技术领域,通过环境图像数据的点特征和线特征、拍摄设备的角速度和加速度,构建双目视觉和惯性里程计模型进行初次定位,引入回环检测与重定位算法,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位,结合对目标环境图像进行回环修正与重定位数据和优化定位数据,基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型,通过视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位,减小位置误差,提高估计精度,获得更为精确的载体位姿。
Description
技术领域
本发明涉及定位技术领域,具体涉及一种基于UWB辅助的主动定位方法、设备和介质。
背景技术
考虑到电力作业人员在复杂、电磁干扰严重、多遮挡的环境中工作,当视觉传感器失效时,回环检测算法无法进行,匹配上的点线特征也急剧下降,优化方程中,IMU测量的残差成为主要部分。此时,根据视觉/惯性/超宽带组合模型依然能够依靠IMU预积分进行位姿的估计,但是,估计精度将会大大降低。而当视觉传感器恢复时,如果系统路过曾经经过的地方,利用重定位算法能够立刻计算出较为精确的载体位姿。但是,若系统一直不经过曾经经过的地方,该段时间的位姿误差将一直存在。
发明内容
本发明所要解决的技术问题是通过IMU预积分进行位姿的估计无法对已经获取的数据进行修正,位姿误差大,目的在于提供一种基于UWB辅助的主动定位方法、设备和介质,通过构建双目视觉和惯性里程计模型,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位,建立视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位,减小位姿误差,提高估计精度,获得更为精确的载体位姿。
本发明通过下述技术方案实现:
本发明第一方面提供一种基于UWB辅助的主动定位方法,包括以下具体步骤:
S1、获取目标环境图像,获得环境图像数据的点特征和线特征,获取拍摄设备的角速度和加速度;
S2、构建双目视觉和惯性里程计模型,结合环境图像数据的点特征和线特征、拍摄设备的角速度和加速度,进行初次定位;
S3、引入回环检测与重定位算法,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位;
S4、结合对目标环境图像进行回环修正与重定位数据和优化定位数据,基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型,通过视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位。
本发明通过环境图像数据的点特征和线特征、拍摄设备的角速度和加速度,构建双目视觉和惯性里程计模型进行初次定位,引入回环检测与重定位算法,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位。结合目标环境图像进行回环修正与重定位数据和优化定位,基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型,通过视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位,提高估计精度,获得更为精确的载体位姿。
进一步的,所述回环检测与重定位算法具体包括:
获取当前输入的环境图像数据,若当前输入的环境图像数据被判断为当前关键帧:
获取当前关键帧的特征,将当前关键帧对应的特征与滑动窗口中的特征进行匹配,建立重投影误差;
对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系。
进一步的,所述当前输入的环境图像数据被判断为当前关键帧包括:
对当前窗口中关键帧以外的所有关键帧进行检测,检测是否存在回环:
若不存在回环,则按照视觉/惯性里程计算法进行基于滑动窗口的位姿估计;
若存在回环,将回环关键帧确定为当前关键帧,将当前关键帧加入当前的滑动窗口之中。
进一步的,还包括对所述重投影误差进行误差消除:
获取当前回环帧中的图像数据,确定当前回环帧的重投影误差的协方差矩阵,确定当前回环帧建立的特征的重投影误差,采用最小二乘法对获得载体的位姿信息进行优化。
进一步的,所述对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系包括:
将一个关键帧与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系,
将建立关系的三个关键帧加入位姿图中,对所有关键帧的位姿进行优化,获得优化后的当前关键帧与回环的关键帧之间构建的相对位姿残差。
进一步的,所述优化的超宽带定位算法具体包括:
获取超宽带基站位置、测距信息和待求标签位置,进行残差模型的构建;
以标签位置为优化量,输入到构建的残差模型中构建优化方程;
获取标签移动速度作为弱约束,确定两个连续时刻间标签位置的平滑约束,确定标签移动速度的协方差矩阵,对优化方程进行求解。
进一步的,所述通过视觉/惯性/超宽带组合模型对初次定位数据进行修正具体包括:
获取位姿图中固定的超宽带基站的位置和载体的位姿,获取超宽带基站与载体之间的边和两个连续时刻载体之间的边,构建相对位姿变化方程;
获取连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差,根据连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差确定载体在世界坐标系下的实时位姿。
进一步的,所述确定载体在世界坐标系下的实时位姿还包括:
将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过关键帧转换到世界坐标系下,从而获得世界坐标系下的输出频率;
根据当前关键帧的特征三角化后建立的点云转换至世界坐标系下。
本发明第二方面提供一种电子设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时用于实现一种基于UWB辅助的主动定位方法。
本发明第三方面提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时用于实现一种基于UWB辅助的主动定位方法。
本发明与现有技术相比,具有如下的优点和有益效果:
通过构建双目视觉和惯性里程计模型,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位,建立视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位,实现估计精度的提高,获得更为精确的载体位姿。
附图说明
为了更清楚地说明本发明示例性实施方式的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本发明的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他相关的附图。在附图中:
图1为本发明实施例中的UWB辅助的主动定位流程;
图2为本发明实施例中的UWB辅助的主动视频定位方法;
图3为本发明实施例中的回环检测与重定位算法流程图;
图4为本发明实施例中的视觉/惯性/超宽带组合模型示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白,下面结合实施例和附图,对本发明作进一步的详细说明,本发明的示意性实施方式及其说明仅用于解释本发明,并不作为对本发明的限定。
实施例1
如图1和图2所示,本实施例第一方面提供一种基于UWB辅助的主动定位方法,包括以下具体步骤:
S1、获取目标环境图像,获得环境图像数据的点特征和线特征,获取拍摄设备的角速度和加速度;
S2、构建双目视觉和惯性里程计模型,结合环境图像数据的点特征和线特征、拍摄设备的角速度和加速度,进行初次定位;
S3、引入回环检测与重定位算法,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位;
S4、结合对目标环境图像进行回环修正与重定位数据和优化定位数据,基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型,通过视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位。
在一些可能的实施例中,回环检测与重定位算法具体包括:
获取当前输入的环境图像数据,若当前输入的环境图像数据被判断为当前关键帧:
当前输入的环境图像数据被判断为关键帧包括:
对当前窗口中关键帧以外的所有关键帧进行检测,检测是否存在回环:
若不存在回环,则按照视觉/惯性里程计算法进行基于滑动窗口的位姿估计;
若存在回环,将回环关键帧确定为当前关键帧,将当前关键帧加入当前的滑动窗口之中。
获取当前关键帧的特征,将当前关键帧对应的特征与滑动窗口中的特征进行匹配,建立重投影误差;
对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系:
对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系包括:
将一个关键帧与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系,
将建立关系的三个关键帧加入位姿图中,对所有关键帧的位姿进行优化,获得优化后的当前关键帧与回环的关键帧之间构建的相对位姿残差。
在一些可能的实施例中,还包括对重投影误差进行误差消除:
获取当前回环帧中的图像数据,确定当前回环帧的重投影误差的协方差矩阵,确定当前回环帧建立的特征的重投影误差,采用最小二乘法对获得载体的位姿信息进行优化。
在一些可能的实施例中,优化的超宽带定位算法具体包括:
获取超宽带基站位置、测距信息和待求标签位置,进行残差模型的构建;
以标签位置为优化量,输入到构建的残差模型中构建优化方程;
获取标签移动速度作为弱约束,确定两个连续时刻间标签位置的平滑约束,确定标签移动速度的协方差矩阵,对优化方程进行求解;
待求标签位置为即将要进行位姿测量的图像数据。
在一些可能的实施例中,通过视觉/惯性/超宽带组合模型对初次定位数据进行修正具体包括:
获取位姿图中固定的超宽带基站的位置和载体的位姿,获取超宽带基站与载体之间的边和两个连续时刻载体之间的边,构建相对位姿变化方程;
获取连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差,根据连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差确定载体在世界坐标系下的实时位姿。
确定载体在世界坐标系下的实时位姿还包括:
将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过关键帧转换到世界坐标系下,从而获得世界坐标系下的输出频率;
根据当前关键帧的特征三角化后建立的点云转换至世界坐标系下。
本实施例第二方面提供一种电子设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,处理器执行程序时用于实现一种基于UWB辅助的主动定位方法。
本实施例第三方面提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时用于实现一种基于UWB辅助的主动定位方法。
实施例2
如图3所示,考虑到算法实时性、效率与准确性之间的平衡,以及电力场景光照、季节、天气等带来的环境变化和视角变化都具有很好的鲁棒性,回环检测与重定位算法具体包括:
当新来的一帧被判断为关键帧后,就会先进入回环检测线程中,利用基于图像块均匀采样的回环检测算法,在除现有窗口中关键帧以外的所有关键帧中对其进行检测。如果没有发现回环,则按照视觉/惯性里程计的算法进行基于滑动窗口的位姿估计。如果检测出回环并返回了回环对应的关键帧,则将回环关键帧加入当前的滑动窗口之中,并将回环关键帧对应的特征与滑动窗口中的特征进行匹配,建立重投影误差。但是,并不使用该回环关键帧附近的IMU数据,同时,在优化的时候,固定回环帧已经获取的位置和姿态。因而,此时建立的优化方程如下:
其中:是关于第i个回环帧建立的特征的重投影误差,是第i个回环帧的重投影误差的协方差矩阵。与是回环帧之前求解得到的姿态和位置,在此次优化中它们固定不变。采用Levenberg Marquardt方法对以上优化方程进行优化求解,获得载体的位姿信息。完成优化求解后,将该关键帧的特征三角化后加入点云中进行点云构建,并与其回环的关键帧之间建立起特征及位姿的关系。
而为了更加有效的将回环检测的误差消除,当一个关键帧被边缘化出滑动窗口后,会将其加入位姿图中。在位姿图中,关键帧作为图的节点,而图的边则是两个关键帧之间的相对位姿关系。第i帧和第j帧之间的相对位姿关系具体如下:
其中,为第i帧和第j帧之间的相对位移,而是第i帧和第j帧之间的相对航向角。由于惯性/视觉系统两个水平姿态角可观,因而在位姿图中仅需考虑4个自由度。此外,对于一个关键帧,它只与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系。位姿图中被加入新的三个关键帧后,对所有关键帧的位姿做一个优化。优化方程如下:
当视觉传感器失效时,回环检测算法无法进行,匹配上的点线特征也急剧下降,优化方程中,IMU测量的残差成为主要部分。此时,根据视觉/惯性/超宽带组合模型依然能够依靠IMU预积分进行位姿的估计,但是,估计精度将会大大降低。而当视觉传感器恢复时,如果系统路过曾经经过的地方,利用重定位算法能够立刻计算出较为精确的载体位姿。但是,若系统一直不经过曾经经过的地方,该段时间的位姿误差将一直存在。
在三维空间中,至少需要布置3个基站,这里使用4个基站能够取得较好的定位效果。根据基站与标签之间距离信息进行定位的方式其基本原理都是基于最小二乘法,构建距离差进行优化方程的建立。具体如下:针对上述超宽带基站位置、测距信息及待求标签位置,可以构建如下残差:
不论是最小二乘法还是上述优化方程,对于任意一时刻标签的位置都只有基站与标签间测量距离的约束,如果测量值不准,很容易造成定位精度的下降。因此,考虑利用标签移动速度作为一个弱约束,增加两个连续时刻间标签位置的约束。具体优化方程如下:
对于以上优化方程,同样采用基于滑动窗口的Levenberg Marquardt方法对其进行优化,滑动窗口大小设为20,而超宽带标签的初始位置,利用最小二乘法计算得到。
对于超宽带定位系统,利用载体速度对连续两个时刻间的载体位姿进行了弱约束,但是该约束并不完全准确。视觉/惯性/超宽带组合模型的本质就是估计两个时刻间载体的相对位姿,它缺少的是载体位姿的全局约束。而超宽带定位系统则可以提供全局的位姿约束。因此,将这两种系统结合起来,设计视觉/惯性/超宽带的组合定位系统。
如图4所示,圆圈代表载体的位姿信息,矩形代表超宽带传感器的基站,他们能够与载体上搭载的传感器标签进行通信,计算出基站与标签之间的距离,六边形则代表了两个时刻之间,由视觉/惯性/超宽带组合模型提供的位姿约束。可以看到,该组合系统利用W系下固定位置的超宽带基站对载体进行全局位置的约束,而通过视觉/惯性/超宽带组合模型提供连续时刻载体间相对位姿的约束。整个组合系统的定位都以世界坐标系W为参考坐标系。而这里的每个时刻对应的是超宽带传感器的测量时刻。
视觉/惯性/超宽带组合模型同样利用图优化的思想进行设计。整个位姿图里包含两种端点,一是固定的超宽带基站的位置,二是载体的位姿。此外,位姿图里包含两种边,一是超宽带基站与载体之间的边。另一种是两个连续时刻载体之间的边,它们通过相对位姿变化进行描述为:
其中,为需要估计的世界坐标系下的以超宽带标签为基准的载体的位姿。则为视觉/惯性/超宽带组合模型测量得到的导航坐标系下的载体位姿。则为起始时刻导航坐标系与超宽带标签坐标系之间的杆臂误差,它同样可以通过优化得到。描述了视觉/惯性/超宽带组合模型测量得到的连续两个时刻间的位值与姿态相对边缘与需要估计的世界坐标系下位姿变换的残差。我们的目的是估计出合适的世界坐标系下的载体位姿,能够使得该残差最接近零。
根据以上分析,视觉/惯性/超宽带组合模型的优化状态量为:
优化方程可以写成如下形式:
其中,为视觉/惯性/超宽带组合模型测量的协方差矩阵,根据其定位精度与经验值综合设定。在本项目中,将位置的x轴和y轴方向对应的协方差值设为0.0001,将位置的z轴方向对应的协方差矩阵设为0.00001,将姿态三个方向对应的协方差值设为0.000001。而一旦相机失效,将位置三个方向对应的协方差矩阵值设为0.0025。
一般情况下,在组合系统中,使用的超宽带传感器的数据频率小于或等于视觉/惯性/超宽带组合模型。并且,它们很难做到时间上的绝对对齐。因此,以超宽带传感器的输出时刻为基准,对于某一基准时刻的视觉/惯性/超宽带组合模型测量值,利用该时刻前后两帧的视觉/惯性组合的输出做线性插值获取。
每次进行优化时,滑窗内的、等视觉/惯性/超宽带组合模型的测量值均使用最新的估计值。每次优化结束后,根据估计出的和杆臂误差以及视觉/惯性/超宽带组合模型输出的、能够计算出世界坐标系W与导航坐标系N之间的转换矩阵:
对于超宽带传感器输出频率低于视觉/惯性/超宽带组合模型的情况,能够将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过转换到世界坐标系下,从而获得更高的世界坐标系下的输出频率。在系统运行结束时,对所有的载体状态进行一个全局的优化,从而估计出最为精确的,然后将建立的点云通过转换至世界坐标系下。
此外,当系统开始运行时,首先进行的是视觉/惯性/超宽带组合模型的初始化,初始化完成后,视觉/惯性/超宽带组合模型开始进行载体(以IMU坐标系为基准)在导航坐标系下的估计。与此同时,超宽带传感器也在不断的进行基站与标签之间的测量。当视觉/惯性/超宽带组合模型设置的滑动窗口具有20帧时,视觉/惯性/超宽带组合模型开始进行优化,并实时估计出载体(以超宽带标签坐标系为基准)在世界坐标系下的位姿。
以上所述的具体实施方式,对本发明的目的、技术方案和有益效果进行了进一步详细说明,所应理解的是,以上所述仅为本发明的具体实施方式而已,并不用于限定本发明的保护范围,凡在本发明的精神和原则之内,所做的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (10)
1.一种基于UWB辅助的主动定位方法,其特征在于,包括以下具体步骤:
S1、获取目标环境图像,获得环境图像数据的点特征和线特征,获取拍摄设备的角速度和加速度;
S2、构建双目视觉和惯性里程计模型,结合环境图像数据的点特征和线特征、拍摄设备的角速度和加速度,进行初次定位;
S3、引入回环检测与重定位算法,对目标环境图像进行回环修正与重定位,引入优化的超宽带定位算法,对目标环境图像中的待测标签与基站间的距离进行优化定位;
S4、结合对目标环境图像进行回环修正与重定位数据和优化定位数据,基于双目视觉和惯性里程计模型构建视觉/惯性/超宽带组合模型,通过视觉/惯性/超宽带组合模型对初次定位数据进行修正,获得精确定位。
2.根据权利要求1所述的基于UWB辅助的主动定位方法,其特征在于,所述回环检测与重定位算法具体包括:
获取当前输入的环境图像数据,若当前输入的环境图像数据被判断为当前关键帧:
获取当前关键帧的特征,将当前关键帧对应的特征与滑动窗口中的特征进行匹配,建立重投影误差;
对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系。
3.根据权利要求2所述的基于UWB辅助的主动定位方法,其特征在于,所述当前输入的环境图像数据被判断为当前关键帧包括:
对当前窗口中关键帧以外的所有关键帧进行检测,检测是否存在回环:
若不存在回环,则按照视觉/惯性里程计算法进行基于滑动窗口的位姿估计;
若存在回环,将回环关键帧确定为当前关键帧,将当前关键帧加入当前的滑动窗口之中。
4.根据权利要求2所述的基于UWB辅助的主动定位方法,其特征在于,还包括对所述重投影误差进行误差消除:
获取当前回环帧中的图像数据,确定当前回环帧的重投影误差的协方差矩阵,确定当前回环帧建立的特征的重投影误差,采用最小二乘法对获得载体的位姿信息进行优化。
5.根据权利要求2所述的基于UWB辅助的主动定位方法,其特征在于,所述对当前关键帧的特征三角化后与回环的关键帧之间建立特征及位姿的关系包括:
将一个关键帧与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系;
将建立关系的三个关键帧加入位姿图中,对所有关键帧的位姿进行优化,获得优化后的当前关键帧与回环的关键帧之间构建的相对位姿残差。
6.根据权利要求1所述的基于UWB辅助的主动定位方法,其特征在于,所述优化的超宽带定位算法具体包括:
获取超宽带基站位置、测距信息和待求标签位置,进行残差模型的构建;
以标签位置为优化量,输入到构建的残差模型中构建优化方程;
获取标签移动速度作为弱约束,确定两个连续时刻间标签位置的平滑约束,确定标签移动速度的协方差矩阵,对优化方程进行求解。
7.根据权利要求1所述的基于UWB辅助的主动定位方法,其特征在于,所述通过视觉/惯性/超宽带组合模型对初次定位数据进行修正具体包括:
获取位姿图中固定的超宽带基站的位置和载体的位姿,获取超宽带基站与载体之间的边和两个连续时刻载体之间的边,构建相对位姿变化方程;
获取连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差,根据连续两个时刻间的位值与姿态相对边缘和需要估计的世界坐标系下位姿变换的残差确定载体在世界坐标系下的实时位姿。
8.根据权利要求7所述的基于UWB辅助的主动定位方法,其特征在于,所述确定载体在世界坐标系下的实时位姿还包括:
将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载体位姿、特征通过关键帧转换到世界坐标系下,从而获得世界坐标系下的输出频率;
根据当前关键帧的特征三角化后建立的点云转换至世界坐标系下。
9.一种电子设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,其特征在于,所述处理器执行所述程序时实现如权利要求1至8任一项所述的一种基于UWB辅助的主动定位方法。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现如权利要求1至8任一项所述的一种基于UWB辅助的主动定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211526026.0A CN115540854A (zh) | 2022-12-01 | 2022-12-01 | 一种基于uwb辅助的主动定位方法、设备和介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211526026.0A CN115540854A (zh) | 2022-12-01 | 2022-12-01 | 一种基于uwb辅助的主动定位方法、设备和介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115540854A true CN115540854A (zh) | 2022-12-30 |
Family
ID=84722261
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211526026.0A Pending CN115540854A (zh) | 2022-12-01 | 2022-12-01 | 一种基于uwb辅助的主动定位方法、设备和介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115540854A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116930864A (zh) * | 2023-06-27 | 2023-10-24 | 中铁第四勘察设计院集团有限公司 | 一种室内外无缝统一基准构建方法及装置 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110672093A (zh) * | 2019-08-23 | 2020-01-10 | 华清科盛(北京)信息技术有限公司 | 一种基于uwb与惯导融合的车载导航定位方法 |
CN110849367A (zh) * | 2019-10-08 | 2020-02-28 | 杭州电子科技大学 | 基于融合uwb的视觉slam的室内定位与导航的方法 |
CN113109772A (zh) * | 2021-04-07 | 2021-07-13 | 成都信息工程大学 | 一种超宽带高动态目标信号模拟方法 |
CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
CN113503873A (zh) * | 2021-07-14 | 2021-10-15 | 北京理工大学 | 一种多传感器融合的视觉定位方法 |
CN114485623A (zh) * | 2022-02-16 | 2022-05-13 | 东南大学 | 一种聚焦距离的相机-imu-uwb融合精准定位方法 |
WO2022183665A1 (zh) * | 2021-03-02 | 2022-09-09 | 浙江商汤科技开发有限公司 | 初始化方法及装置、电子设备、存储介质和程序产品 |
CN115235452A (zh) * | 2022-07-22 | 2022-10-25 | 上海师范大学 | 基于uwb/imu和视觉信息融合的智能泊车定位系统及方法 |
-
2022
- 2022-12-01 CN CN202211526026.0A patent/CN115540854A/zh active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110672093A (zh) * | 2019-08-23 | 2020-01-10 | 华清科盛(北京)信息技术有限公司 | 一种基于uwb与惯导融合的车载导航定位方法 |
CN110849367A (zh) * | 2019-10-08 | 2020-02-28 | 杭州电子科技大学 | 基于融合uwb的视觉slam的室内定位与导航的方法 |
WO2022183665A1 (zh) * | 2021-03-02 | 2022-09-09 | 浙江商汤科技开发有限公司 | 初始化方法及装置、电子设备、存储介质和程序产品 |
CN113109772A (zh) * | 2021-04-07 | 2021-07-13 | 成都信息工程大学 | 一种超宽带高动态目标信号模拟方法 |
CN113124856A (zh) * | 2021-05-21 | 2021-07-16 | 天津大学 | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 |
CN113503873A (zh) * | 2021-07-14 | 2021-10-15 | 北京理工大学 | 一种多传感器融合的视觉定位方法 |
CN114485623A (zh) * | 2022-02-16 | 2022-05-13 | 东南大学 | 一种聚焦距离的相机-imu-uwb融合精准定位方法 |
CN115235452A (zh) * | 2022-07-22 | 2022-10-25 | 上海师范大学 | 基于uwb/imu和视觉信息融合的智能泊车定位系统及方法 |
Non-Patent Citations (1)
Title |
---|
杨博: "视觉/惯性/超宽带组合定位系统关键技术研究", 《中国博士学位论文全文数据库信息科技辑》 * |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116930864A (zh) * | 2023-06-27 | 2023-10-24 | 中铁第四勘察设计院集团有限公司 | 一种室内外无缝统一基准构建方法及装置 |
CN116930864B (zh) * | 2023-06-27 | 2024-02-23 | 中铁第四勘察设计院集团有限公司 | 一种室内外无缝统一基准构建方法及装置 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
CN109991636B (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
KR102226846B1 (ko) | Imu 센서와 카메라를 이용한 하이브리드 실내 측위 시스템 | |
US8320616B2 (en) | Image-based system and methods for vehicle guidance and navigation | |
CN109579840A (zh) | 一种点线特征融合的紧耦合双目视觉惯性slam方法 | |
CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
US20210183100A1 (en) | Data processing method and apparatus | |
CN108844533A (zh) | 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法 | |
CN113763548B (zh) | 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及系统 | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN112525197B (zh) | 基于图优化算法超宽带惯性导航融合位姿估计方法 | |
CN111932674A (zh) | 一种线激光视觉惯性系统的优化方法 | |
CN114693754B (zh) | 一种基于单目视觉惯导融合的无人机自主定位方法与系统 | |
CN109059907A (zh) | 轨迹数据处理方法、装置、计算机设备和存储介质 | |
CN114323033A (zh) | 基于车道线和特征点的定位方法、设备及自动驾驶车辆 | |
CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
Adam et al. | Fusion of fixation and odometry for vehicle navigation | |
CN103076023A (zh) | 一种步长计算方法和装置 | |
CN117739972B (zh) | 一种无全球卫星定位系统的无人机进近阶段定位方法 | |
Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
CN110672103A (zh) | 一种多传感器目标跟踪滤波方法及系统 | |
CN113865584A (zh) | 一种基于视觉惯性里程计的uwb三维寻物方法和装置 | |
CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
CN117075158A (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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20221230 |
|
RJ01 | Rejection of invention patent application after publication |