CN115540854A - 一种基于uwb辅助的主动定位方法、设备和介质 - Google Patents

一种基于uwb辅助的主动定位方法、设备和介质 Download PDF

Info

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
Application number
CN202211526026.0A
Other languages
English (en)
Inventor
谢晓娜
徐智勇
常政威
陈明举
邓元实
熊兴中
谢正军
蒲维
吴杰
丁宣文
张葛祥
张江林
刘甲甲
王振玉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chengdu University of Information Technology
Electric Power Research Institute of State Grid Sichuan Electric Power Co Ltd
Original Assignee
Chengdu University of Information Technology
Electric Power Research Institute of State Grid Sichuan Electric Power Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Chengdu University of Information Technology, Electric Power Research Institute of State Grid Sichuan Electric Power Co Ltd filed Critical Chengdu University of Information Technology
Priority to CN202211526026.0A priority Critical patent/CN115540854A/zh
Publication of CN115540854A publication Critical patent/CN115540854A/zh
Pending legal-status Critical Current

Links

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辅助的主动定位方法、设备和介质
技术领域
本发明涉及定位技术领域，具体涉及一种基于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数据，同时，在优化的时候，固定回环帧已经获取的位置和姿态。因而，此时建立的优化方程如下：
Figure 77786DEST_PATH_IMAGE002
其中：
Figure 198188DEST_PATH_IMAGE003
是关于第i个回环帧建立的特征的重投影误差，
Figure 446767DEST_PATH_IMAGE004
是第i个回环帧的重投影误差的协方差矩阵。
Figure 310818DEST_PATH_IMAGE005
与
Figure 92567DEST_PATH_IMAGE006
是回环帧之前求解得到的姿态和位置，在此次优化中它们固定不变。采用Levenberg Marquardt方法对以上优化方程进行优化求解，获得载体的位姿信息。完成优化求解后，将该关键帧的特征三角化后加入点云中进行点云构建，并与其回环的关键帧之间建立起特征及位姿的关系。
而为了更加有效的将回环检测的误差消除，当一个关键帧被边缘化出滑动窗口后，会将其加入位姿图中。在位姿图中，关键帧作为图的节点，而图的边则是两个关键帧之间的相对位姿关系。第i帧和第j帧之间的相对位姿关系具体如下：
Figure 383871DEST_PATH_IMAGE007
Figure 854167DEST_PATH_IMAGE008
其中，
Figure 256329DEST_PATH_IMAGE009
为第i帧和第j帧之间的相对位移，而
Figure 394049DEST_PATH_IMAGE010
是第i帧和第j帧之间的相对航向角。由于惯性/视觉***两个水平姿态角可观，因而在位姿图中仅需考虑4个自由度。此外，对于一个关键帧，它只与其连续的前后两个关键帧及其对应的回环关键帧建立边的关系。位姿图中被加入新的三个关键帧后，对所有关键帧的位姿做一个优化。优化方程如下：
Figure 856255DEST_PATH_IMAGE011
Figure 345005DEST_PATH_IMAGE012
其中，
Figure 789674DEST_PATH_IMAGE013
是构建的相对位姿残差，
Figure 781900DEST_PATH_IMAGE014
是需要优化的量，F𝐺𝑟𝑎𝑝h是构建的优化方程，
Figure 946165DEST_PATH_IMAGE015
是协方差矩阵，根据经验值设置。该优化，能够将回环检测出的累积误差的影响均匀的分摊到所有关键帧上，从而尽可能的减小其影响。
当视觉传感器失效时，回环检测算法无法进行，匹配上的点线特征也急剧下降，优化方程中，IMU测量的残差成为主要部分。此时，根据视觉/惯性/超宽带组合模型依然能够依靠IMU预积分进行位姿的估计，但是，估计精度将会大大降低。而当视觉传感器恢复时，如果***路过曾经经过的地方，利用重定位算法能够立刻计算出较为精确的载***姿。但是，若***一直不经过曾经经过的地方，该段时间的位姿误差将一直存在。
在三维空间中，至少需要布置3个基站，这里使用4个基站能够取得较好的定位效果。根据基站与标签之间距离信息进行定位的方式其基本原理都是基于最小二乘法，构建距离差进行优化方程的建立。具体如下：针对上述超宽带基站位置、测距信息及待求标签位置，可以构建如下残差：
Figure 656632DEST_PATH_IMAGE016
其中，i对应的是第i个基站。据此，能够以标签位置
Figure 400598DEST_PATH_IMAGE017
为优化量，构建如下优化方程：
Figure 512910DEST_PATH_IMAGE019
其中，
Figure 316918DEST_PATH_IMAGE021
是第i个基站与标签之间的测量协方差，可根据超宽带传感器参数设置。在的***中，将其均设为0.001。同时，利用Huber核函数增加优化的鲁棒性。
不论是最小二乘法还是上述优化方程，对于任意一时刻标签的位置都只有基站与标签间测量距离的约束，如果测量值不准，很容易造成定位精度的下降。因此，考虑利用标签移动速度作为一个弱约束，增加两个连续时刻间标签位置的约束。具体优化方程如下：
Figure 514681DEST_PATH_IMAGE023
其中，
Figure 560872DEST_PATH_IMAGE024
新增的两个连续时刻间标签位置的平滑约束。通过其关于速度的协方差矩阵
Figure 527691DEST_PATH_IMAGE025
来对其进行加权：
Figure 768180DEST_PATH_IMAGE026
其中，
Figure 187660DEST_PATH_IMAGE027
是设置的最大载体最大速度，∆𝑡是k和k+1时刻间的时间间隔，由于该约束是一个弱约束，因此，可以通过调节因子s调节其大小，减小其权重。
对于以上优化方程，同样采用基于滑动窗口的Levenberg Marquardt方法对其进行优化，滑动窗口大小设为20，而超宽带标签的初始位置，利用最小二乘法计算得到。
对于超宽带定位***，利用载体速度对连续两个时刻间的载***姿进行了弱约束，但是该约束并不完全准确。视觉/惯性/超宽带组合模型的本质就是估计两个时刻间载体的相对位姿，它缺少的是载***姿的全局约束。而超宽带定位***则可以提供全局的位姿约束。因此，将这两种***结合起来，设计视觉/惯性/超宽带的组合定位***。
如图4所示，圆圈代表载体的位姿信息，矩形代表超宽带传感器的基站，他们能够与载体上搭载的传感器标签进行通信，计算出基站与标签之间的距离，六边形则代表了两个时刻之间，由视觉/惯性/超宽带组合模型提供的位姿约束。可以看到，该组合***利用W系下固定位置的超宽带基站对载体进行全局位置的约束，而通过视觉/惯性/超宽带组合模型提供连续时刻载体间相对位姿的约束。整个组合***的定位都以世界坐标系W为参考坐标系。而这里的每个时刻对应的是超宽带传感器的测量时刻。
视觉/惯性/超宽带组合模型同样利用图优化的思想进行设计。整个位姿图里包含两种端点，一是固定的超宽带基站的位置，二是载体的位姿。此外，位姿图里包含两种边，一是超宽带基站与载体之间的边。另一种是两个连续时刻载体之间的边，它们通过相对位姿变化进行描述为：
Figure 539007DEST_PATH_IMAGE029
其中，
Figure 94753DEST_PATH_IMAGE030
为需要估计的世界坐标系下的以超宽带标签为基准的载体的位姿。
Figure 974984DEST_PATH_IMAGE031
则为视觉/惯性/超宽带组合模型测量得到的导航坐标系下的载***姿。
Figure 114716DEST_PATH_IMAGE032
则为起始时刻导航坐标系与超宽带标签坐标系之间的杆臂误差，它同样可以通过优化得到。
Figure 676279DEST_PATH_IMAGE033
描述了视觉/惯性/超宽带组合模型测量得到的连续两个时刻间的位值与姿态相对边缘与需要估计的世界坐标系下位姿变换的残差。我们的目的是估计出合适的世界坐标系下的载***姿，能够使得该残差最接近零。
根据以上分析，视觉/惯性/超宽带组合模型的优化状态量为：
Figure 352111DEST_PATH_IMAGE035
优化方程可以写成如下形式：
Figure 668823DEST_PATH_IMAGE036
其中，
Figure 30272DEST_PATH_IMAGE037
为视觉/惯性/超宽带组合模型测量的协方差矩阵，根据其定位精度与经验值综合设定。在本项目中，将位置的x轴和y轴方向对应的协方差值设为0.0001，将位置的z轴方向对应的协方差矩阵设为0.00001，将姿态三个方向对应的协方差值设为0.000001。而一旦相机失效，将位置三个方向对应的协方差矩阵值设为0.0025。
一般情况下，在组合***中，使用的超宽带传感器的数据频率小于或等于视觉/惯性/超宽带组合模型。并且，它们很难做到时间上的绝对对齐。因此，以超宽带传感器的输出时刻为基准，对于某一基准时刻的视觉/惯性/超宽带组合模型测量值，利用该时刻前后两帧的视觉/惯性组合的输出做线性插值获取。
每次进行优化时，滑窗内的
Figure 723421DEST_PATH_IMAGE038
、
Figure 988180DEST_PATH_IMAGE039
等视觉/惯性/超宽带组合模型的测量值均使用最新的估计值。每次优化结束后，根据估计出的
Figure 741373DEST_PATH_IMAGE040
和杆臂误差
Figure 622741DEST_PATH_IMAGE042
以及视觉/惯性/超宽带组合模型输出的
Figure 119581DEST_PATH_IMAGE038
、
Figure 35585DEST_PATH_IMAGE039
能够计算出世界坐标系W与导航坐标系N之间的转换矩阵：
Figure 944633DEST_PATH_IMAGE043
对于超宽带传感器输出频率低于视觉/惯性/超宽带组合模型的情况，能够将两个超宽带传感器测量时刻之间的视觉/惯性/超宽带组合模型估计出的载***姿、特征通过
Figure 47719DEST_PATH_IMAGE044
转换到世界坐标系下，从而获得更高的世界坐标系下的输出频率。在***运行结束时，对所有的载体状态进行一个全局的优化，从而估计出最为精确的
Figure 82671DEST_PATH_IMAGE045
，然后将建立的点云通过
Figure 587601DEST_PATH_IMAGE046
转换至世界坐标系下。
此外，当***开始运行时，首先进行的是视觉/惯性/超宽带组合模型的初始化，初始化完成后，视觉/惯性/超宽带组合模型开始进行载体（以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辅助的主动定位方法。
CN202211526026.0A 2022-12-01 2022-12-01 一种基于uwb辅助的主动定位方法、设备和介质 Pending CN115540854A (zh)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116930864A (zh) * 2023-06-27 2023-10-24 中铁第四勘察设计院集团有限公司 一种室内外无缝统一基准构建方法及装置

Citations (8)

* Cited by examiner, † Cited by third party
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和视觉信息融合的智能泊车定位***及方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
杨博: "视觉/惯性/超宽带组合定位***关键技术研究", 《中国博士学位论文全文数据库信息科技辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
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
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109991636A (zh) 基于gps、imu以及双目视觉的地图构建方法及***
KR102226846B1 (ko) Ｉｍｕ 센서와 카메라를 이용한 하이브리드 실내 측위 시스템
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN105931275A (zh) 基于移动端单目和imu融合的稳定运动跟踪方法和装置
CN108844533A (zh) 一种基于多传感器融合和姿态解算的自由姿态pdr定位方法
CN103776446A (zh) 一种基于双mems-imu的行人自主导航解算算法
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
CN112525197B (zh) 基于图优化算法超宽带惯性导航融合位姿估计方法
CN107941212B (zh) 一种视觉与惯性联合定位方法
CN114693754B (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与***
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN115183762A (zh) 一种机场仓库内外建图方法、***、电子设备及介质
CN103076023A (zh) 一种步长计算方法和装置
Adam et al. Fusion of fixation and odometry for vehicle navigation
CN115540854A (zh) 一种基于uwb辅助的主动定位方法、设备和介质
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN109387198A (zh) 一种基于序贯检测的惯性/视觉里程计组合导航方法
CN110672103A (zh) 一种多传感器目标跟踪滤波方法及***
CN113865584A (zh) 一种基于视觉惯性里程计的uwb三维寻物方法和装置
CN112731503B (zh) 一种基于前端紧耦合的位姿估计方法及***
CN106767770A (zh) 一种基于便携智能设备的用户行走方向检测与追踪方法
CN117075158A (zh) 基于激光雷达的无人变形运动平台的位姿估计方法及***
Forno et al. Techniques for improving localization applications running on low-cost IoT devices

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