CN114440877B - 一种异步多相机视觉惯性里程计定位方法 - Google Patents

一种异步多相机视觉惯性里程计定位方法 Download PDF

Info

Publication number
CN114440877B
CN114440877B CN202210092065.8A CN202210092065A CN114440877B CN 114440877 B CN114440877 B CN 114440877B CN 202210092065 A CN202210092065 A CN 202210092065A CN 114440877 B CN114440877 B CN 114440877B
Authority
CN
China
Prior art keywords
camera
map
time
frames
asynchronous
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
Application number
CN202210092065.8A
Other languages
English (en)
Other versions
CN114440877A (zh
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.)
Beihang University
Original Assignee
Beihang University
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 Beihang University filed Critical Beihang University
Priority to CN202210092065.8A priority Critical patent/CN114440877B/zh
Publication of CN114440877A publication Critical patent/CN114440877A/zh
Application granted granted Critical
Publication of CN114440877B publication Critical patent/CN114440877B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/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
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G01C11/04—Interpretation 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/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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

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)
  • Image Analysis (AREA)
  • Studio Devices (AREA)

Abstract

本发明涉及视觉惯性组合导航技术领域，提供了一种异步多相机视觉惯性里程计定位方法。该方法包括：异步曝光多相机采用LK金字塔将图像帧缩放为多个尺寸不同的图像，获取多相机各自的特征点匹配关系与特征点在图像帧中的移动速度；对IMU测量数据进行预积分处理，获得新的相邻两帧之间的两个时刻的相对位姿约束关系；将异步多相机视觉惯性里程计初始化，获得异步曝光多相机的初始地图以及异步曝光多相机‑IMU系统在初始化后的运动；特征点跟踪出现丢失时，结合特征点匹配关系恢复各相机运动。本发明实现了对异步曝光多相机硬件的支持，恢复了跟踪丢数据的相机相关状态，使得迭代优化结果更客观更准确。

Description

一种异步多相机视觉惯性里程计定位方法
技术领域
本发明涉及视觉惯性组合导航技术领域，尤其涉及一种异步多相机视觉惯性里程计定位方法。
背景技术
近年来，机器人行业发展迅速，越来越多的机器人开始走进人们的日常生活。为完成复杂任务，机器人需要对自身的位置、姿态有准确的估计，这离不开导航定位系统。目前常用的GPS定位方法受环境影响较大，在写字楼等封闭空间内，GPS信号往往不好，不能提供准确的定位信息。而视觉传感器则一定程度上可以解决这一问题，它体积小、质量轻、获取信息丰富，常与惯性测量融合作为视觉惯性里程计，应用于导航定位等领域。
为了提高定位的精确度与鲁棒性，许多视觉惯性里程计采用了多相机系统，增大视觉测量的视野范围。中国专利CN109506642A公开了一种机器人多相机视觉惯性实时定位方法及装置，其使用多个不同视线方向的相机采集图像并提取特征点，获取周围环境中更丰富的数据关联。但是该实时定位系统要求所有相机同步曝光，不支持异步曝光多相机硬件，且该方法没有考虑相机特征点跟踪丢失时的应急处理细节。中国专利CN111791235A公开了一种机器人多相机视觉惯性点线特征定位方法及装置，其使用了多个相机获取环境中的点线特征，在已知地图中匹配特征来估计机器人的位置姿态。但是该方法需要依赖已建立的地图，难以在未知环境中应用。
综上所述，现有多相机视觉惯性定位方案难以应用于相机异步曝光、外界环境未知的情况。
发明内容
有鉴于此，本发明提供了一种异步多相机视觉惯性里程计定位方法，以解决现有技术中不支持异步曝光多相机硬件，且没有考虑相机特征点跟踪丢失时的应急处理细节的问题。
本发明提供了一种异步多相机视觉惯性里程计定位方法，包括：
S1异步曝光多相机采用LK金字塔将图像帧缩放为多个尺寸不同的图像，获取所述多相机各自的特征点匹配关系，并估计所述特征点在图像帧中的移动速度；
S2对IMU测量数据进行预积分处理，将相邻两帧之间的所述IMU测量融合为一次测量，基于新的IMU测量数据以及上一时刻计算的预积分递推，获得新的所述两帧之间的时间段前后两个时刻的相对位姿约束关系；
S3将异步多相机视觉惯性里程计初始化，获得多相机的特征跟踪结果并根据所述特征跟踪结果独立建立各相机的地图，并采用线性插值的方法融合所述各相机的地图，获得所述异步曝光多相机的初始地图；
S4根据所述初始地图、所述特征点匹配关系与所述预积分处理的结果，使用滑动窗口优化的方法估计异步曝光多相机-IMU系统在初始化后的运动，
S41在里程计运行过程中维护一个设定长度的滑动窗口，滑窗内存储了设定数量的所述图像帧及相关的地图点；
S42当新的所述图像帧输入后，根据视差判断次新帧是否可作为关键帧，若是，删除滑窗中的最早帧，否则删除所述次新帧；
S5所述异步曝光多相机对所述特征点跟踪出现丢失时，舍弃跟踪丢失的相机观测数据，直到所述特征点跟踪恢复后，再结合所述特征点匹配关系恢复各所述相机运动，
S51基于特征点跟踪恢复后的跟踪结果，计算相机运动，并基于所述相机运动进行局部初始化，恢复相机相关状态；利用线性插值估算所述相机在原地图中的位置与姿态，将所述局部初始化的结果与原地图对齐；
S52基于所述局部初始化的结果，采用所述运动结构恢复方法计算特征跟踪成功的两帧之间的位姿关系与地图点坐标，利用所述预积分估算陀螺仪零偏与视觉测量尺度；
S53局部初始化结果与原地图对齐时，使用特征跟踪未丢失相机的位姿进行线性插值，估算局部地图坐标系相对于原地图的位姿转换，将新地图中的所述相机运动与特征点坐标转化到所述初始地图。
进一步地，所述S1，包括：
在已有所述特征点周围之外的区域，将所述异步曝光多相机分别检测到的各所述图像帧中的Harris角点作为所述特征点，通过采用LK金字塔方法进行提取；
采用光流法估计所述特征点的移动速度，分别获取所述异步曝光多相机所述各自的特征点匹配关系的跟踪结果。
进一步地，所述光流法估计所述特征点移动速度的表达式如下：
Ixu+Iyv+It＝0
其中，Ix为光强在x方向的梯度变化，u为特征点在x方向的速度，Iy为光强在y方向的梯度变化，v为特征点在y方向的速度，It为光强随时间变化的梯度。
进一步地，所述S2，包括：
将相邻两帧之间的所述IMU测量融合为一次测量，得到所述两帧之间的时间段前后两个时刻的相对位姿约束关系；
基于所述相对位姿关系，当有新的所述IMU测量数据出现时，直接在上一时刻计算的预积分上递推，获得新的所述两帧之间的时间段前后两个时刻的相对位姿约束关系，其中，所述递推的表达式如下：
Figure BDA0003489573220000031
Figure BDA0003489573220000041
/>
Figure BDA0003489573220000042
其中，ΔR(i,j)、Δv(i,j)、Δp(i,j)为预积分计算得到的姿态、速度、位移约束；R(i)为i时刻的绝对姿态，T是矩阵的转置运算，
Figure BDA0003489573220000043
为IMU角速度测量值，bgyr(k)为k时刻IMU的陀螺仪零偏，ηgyr(k)为k时刻IMU的陀螺仪噪声，v(j)为j时刻的绝对速度，v(i)为i时刻的绝对速度，g为重力加速度，Δt(k)为k时刻与k-1时刻的时间间隔，/>
Figure BDA0003489573220000044
为k时刻IMU加速度测量值，bacc(k)为k时刻IMU的加速度计零偏，ηacc(k)为k时刻IMU加速度计噪声，p(i)为i时刻的绝对位置。
进一步地，所述S3，包括：
S31筛选出所述特征点匹配数量较多的一对所述图像帧，利用所述特征点匹配关系计算两帧之间的本质矩阵，根据所述本质矩阵计算所述两帧之间的相对位姿，并基于所述两帧之间的相对位姿，将所述两帧之间共有的特征点进行三角化，根据PnP算法依次计算其它所有所述图像帧的相对位姿，计算出所述特征点的三维坐标；
S32基于预积分方法估算各所述相机在两帧之间的转动，利用所述预积分估算的结果与运动结构恢复方法计算各所述相机在两帧之间转动的误差，利用所述误差与所述预积分估算的结果相对陀螺仪零偏的雅各比矩阵计算陀螺仪零偏，并基于所述陀螺仪零偏，求得陀螺仪零偏校正后的预积分；
S33基于所述陀螺仪零偏校正后的预积分，将所述各时刻的速度、重力加速度、各相机的尺度因数作为初步优化变量，采用所述预积分处理的结果与所述运动结构恢复方法的结果构建残差，通过求解最小二乘问题估计所述初步优化变量；
S34构建与重力加速度向量的方向垂直的一对正交基，把所述重力加速度在所述正交基上的投影作为状态量，重新加入到所述最小二乘问题中进行迭代求解，获得重优化后的所述重力加速度向量的方向、各时刻的速度、各相机的尺度；
S35将异步曝光多相机中任一所述相机作为主相机，通过所述线性插值的方法估算其它相机地图的坐标系相对主相机地图的所述位姿，并利用所述位姿将其它坐标系中的所述相机运动与各相机地图点坐标转移至所述主相机地图中；
S36采用重力加速度对齐方法先计算主相机地图z轴与所述重力加速度之间的夹角，将所述主相机地图中的所有相机位姿与地图点坐标按所述夹角逆向旋转，使得所述主相机地图z轴与重力加速度对齐，获得所述异步曝光多相机的初始地图。
进一步地，S42中所述删除滑窗中的最早帧，包括：
基于上一次优化使用雅各比矩阵计算出的被边缘化变量的舒尔补，舍弃与被边缘化变量相关的残差，删除所述滑窗中的最早帧；
采用所述被边缘化变量的舒尔补与所述残差作为新的优化中考虑的先验估计，在迭代优化中更新对应的新的残差。
进一步地，S42中所述先验估计，是通过边缘化将最早帧相关的数据转化来的，其中，相关的数据，包括重投影误差、预积分误差、先验估计的误差。
本发明与现有技术相比存在的有益效果是：
1.本发明通过多相机异步曝光的方法，实现了对异步曝光多相机硬件的支持；
2.本发明充分考虑了相机特征点跟踪丢失时的应急处理细节，并利用线性插值估算相机在原地图中的位置与姿态，将局部初始化的结果与原地图对齐，恢复了相机相关状态；
3.为了减小异常值的影响，通过在迭代优化中引入鲁棒核函数，使得迭代优化结果更客观更准确。
附图说明
为了更清楚地说明本发明中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明提供的一种异步多相机视觉惯性里程计定位方法的流程示意图；
图2是本发明提供的异步曝光多相机与IMU组合硬件系统的示意图；
图3是本发明提供的滑动窗口示意图。
具体实施方式
以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。
下面将结合附图详细说明根据本发明的一种异步多相机视觉惯性里程计定位方法。
图1是本发明一种异步多相机视觉惯性里程计定位方法的流程图。如图1所示，该方法包括：
S1，异步曝光多相机采用LK金字塔将图像帧缩放为多个尺寸不同的图像，获取多相机各自的特征点匹配关系，并估计特征点在图像帧中的移动速度。
S11，在已有特征点周围之外的区域，将异步曝光多相机分别检测到的各图像帧中的Harris角点作为特征点，通过采用LK金字塔方法进行提取；
S12，采用光流法估计特征点的移动速度，分别获取异步曝光多相机各自的特征点匹配关系的跟踪结果。
其中，异步曝光多相机系统使用多个独立的相机获得比单个相机更大的视场范围；多个相机没有共用的触发信号控制相机同步曝光，每个相机的曝光时刻独立，互不影响；多个相机之间的相对位姿确定且在里程计运行过程中保持不变。
其中，异步曝光的图像帧分别由多相机系统获取；不同相机获取的图像帧曝光时间可以相同也可以不同；不同相机获取的图像帧的帧率接近。
其中，特征点提取与跟踪方法使用LK金字塔将图像帧缩放为多个尺寸不同的图像，分别检测各图像帧中的Harris角点作为特征点，每次提取特征点，都跳过已有特征点周围的小范围区域，这样能够得到分布更加均匀的特征点提取结果。使用光流法估计特征点的移动速度，其表达式如下：
Ixu+Iyv+It＝0
其中，Ix为光强在x方向的梯度变化，u为特征点在x方向的速度，Iy为光强在y方向的梯度变化，v为特征点在y方向的速度，It为光强随时间变化的梯度。
在获取新的图像帧后，使用上一帧估计的特征点移动速度预测其在新图像中的位置，并在预测位置附近搜索上一帧特征点的匹配；在搜索完成后，提取更多特征点并用光流法估计所有特征点的速度。
图2是本发明提供的异步曝光多相机与IMU组合硬件系统的示意图。
S2，对IMU测量数据进行预积分处理，将相邻两帧之间的IMU测量融合为一次测量，基于新的IMU测量数据以及上一时刻计算的预积分递推，获得新的两帧之间的时间段前后两个时刻的相对位姿约束关系。
S21，将相邻两帧之间的IMU测量融合为一次测量，得到两帧之间的时间段前后两个时刻的相对位姿约束关系；
S22，基于相对位姿关系，当有新的IMU测量数据出现时，直接在上一时刻,计算的预积分上递推，获得新的两帧之间的时间段前后两个时刻的相对位姿约束关系。
其中，IMU与多相机的相对位姿确定且在里程计运行过程中保持不变；惯性测量数据的发布频率远高于多相机系统的图像帧率；
其中，经过预积分处理，相邻两帧之间的IMU测量融合为一次测量。
得到时间段前后两个时刻的相对位姿约束关系。每当有新的测量数据到来，均可以直接在上一时刻的计算结果上进行递推，递推的表达式如下：
Figure BDA0003489573220000081
Figure BDA0003489573220000082
Figure BDA0003489573220000083
其中，ΔR(i,j)、Δv(i,j)、Δp(i,j)为预积分计算得到的姿态、速度、位移约束；R(i)为i时刻的绝对姿态，T是矩阵的转置运算，
Figure BDA0003489573220000084
为IMU角速度测量值，bgyr(k)为k时刻IMU的陀螺仪零偏，ηgyr(k)为k时刻IMU的陀螺仪噪声，v(j)为j时刻的绝对速度，v(i)为i时刻的绝对速度，g为重力加速度，Δt(k)为k时刻与k-1时刻的时间间隔，/>
Figure BDA0003489573220000085
为k时刻IMU加速度测量值，bacc(k)为k时刻IMU的加速度计零偏，ηacc(k)为k时刻IMU加速度计噪声，p(i)为i时刻的绝对位置。
传感器噪声对预积分项的影响近似为零均值高斯噪声，其方差也可以在IMU测量数据到来时进行增量式更新。
S3，将异步多相机视觉惯性里程计初始化，获得多相机的特征跟踪结果并根据特征跟踪结果独立建立各相机的地图，并采用线性插值的方法融合各相机的地图，获得异步曝光多相机的初始地图。
S31筛选出特征点匹配数量较多的一对图像帧，利用特征点匹配关系计算两帧之间的本质矩阵，根据本质矩阵计算两帧之间的相对位姿，并基于两帧之间的相对位姿，将两帧之间共有的特征点进行三角化，根据PnP算法依次计算其它所有图像帧的相对位姿，计算出特征点的三维坐标；
S32基于预积分方法估算各相机在两帧之间的转动，利用预积分估算的结果与运动结构恢复方法计算各相机在两帧之间转动的误差，利用误差与预积分估算的结果相对陀螺仪零偏的雅各比矩阵计算陀螺仪零偏，并基于陀螺仪零偏，求得陀螺仪零偏校正后的预积分。
利用预积分进行初步优化，能够独立处理各个相机的地图。
S33，基于陀螺仪零偏校正后的预积分结果，将各时刻的速度、重力加速度、各相机的尺度因数作为初步优化变量，采用预积分处理的结果与运动结构恢复方法的结果构建残差，通过求解最小二乘问题估计初步优化变量；
S34构建与重力加速度向量的方向垂直的一对正交基，把重力加速度在正交基上的投影作为状态量，重新加入到最小二乘问题中进行迭代求解，获得重优化后的重力加速度向量的方向、各时刻的速度、各相机的尺度；
S35将异步曝光多相机中任一相机作为主相机，通过线性插值的方法估算其它相机地图的坐标系相对主相机地图的位姿，并利用位姿将其它坐标系中的相机运动与各相机地图点坐标转移至主相机的地图中；
S36采用重力加速度对齐方法先计算主相机地图z轴与重力加速度之间的夹角，将主相机地图中的所有相机位姿与地图点坐标按夹角逆向旋转，使得主相机地图z轴与重力加速度对齐，获得异步曝光多相机的初始地图。
图3是本发明提供的滑动窗口示意图。
S4，根据初始地图、特征点匹配关系与预积分处理的结果，使用滑动窗口优化的方法估计异步曝光多相机-IMU系统在初始化后的运动。
S41，在里程计运行过程中维护一个长度一定的滑动窗口，滑窗内存储了一定数量的图像帧及相关的地图点；
S42，当新的图像帧输入后，根据视差判断次新帧是否可作为关键帧，若是，删除滑窗中的最早帧，否则删除次新帧。
基于上一次优化使用雅各比矩阵计算出的被边缘化变量的舒尔补，舍弃与被边缘化变量相关的残差，删除滑窗中的最早帧；
采用被边缘化变量的舒尔补与残差作为新的优化中考虑的先验估计，在迭代优化中更新对应的新的残差。
其中，S42中先验估计，是通过边缘化将最早帧相关的数据转化来的，相关的数据，包括重投影误差、预积分误差、先验估计的误差。
其中，滑动窗口如图3所示，它记录了一定数量的相机图像帧中的特征点匹配关系以及图像帧曝光时刻之间的预积分结果；滑动窗口中的状态量包括了：惯性测量元件在图像帧曝光时刻的位姿、速度、传感器零偏、各特征点第一次被观测时的深度。
其中，判断次新帧是否为关键帧时，先要计算次新图像中特征点与上一帧匹配特征点之间的平均像素距离。若平均像素距离大于阈值，则将次新帧作为关键帧，从滑窗中删除最早的图像帧；否则从滑窗中删除次新帧。
其中，将最早帧边缘化，需要先计算上一次优化使用的雅各比矩阵关于被边缘化变量的舒尔补，舍弃与被边缘化变量相关的残差；使用边缘化后的雅各比矩阵与残差作为新的优化中考虑的先验估计，在迭代优化中更新对应的残差项。
其中，优化滑窗状态量时，把滑窗中各图像帧对应的位姿、速度、传感器零偏、所有特征点第一次被观测时的深度作为状态量，同时考虑边缘化产生的先验估计、重投影误差、预积分误差进行优化。
具体如下：
X＝[x0,x1,x2,…,xN-1,λ0,λ1,λ2,…,λm-1,]
xi＝[pi,qi,vi,bacc,i,bgyr,i]
式中，λi表示各地图点第i次被观测时的深度，i表示正整数；
xi表示滑窗中第i帧的状态，它包含了pi,qi,vi,bacc,i,bgyr,i，依次表示滑窗中第i帧的位置、姿态、速度、加速度计零偏、陀螺仪零偏，N表示滑动窗口长度，m表示特征点数量；
同时考虑边缘化产生的先验估计、重投影误差、预积分误差进行优化，优化使用的代价函数如下：
Figure BDA0003489573220000111
式中，rp、Hp为边缘化得到的先验信息，rB为预积分误差，
Figure BDA0003489573220000112
为相邻两帧之间的预积分结果，/>
Figure BDA0003489573220000113
为对应的协方差矩阵，C为图像帧与地图点的组合组成的集合，即C∈{(k,l)|k∈[0,N-1],l∈[0,m-1],k,l∈Z}，rC为重投影误差，/>
Figure BDA0003489573220000114
为地图点在对应图像帧的观测，/>
Figure BDA0003489573220000115
为对应的协方差矩阵，考虑到测量的不确定性，使用了Mahalanobis范数对残差进行处理。
为减小异常值的影响，还可以在优化中引入鲁棒核函数。
其中，考虑先验估计与各个误差进行优化的过程中，需要考虑传感器零偏的变化对预积分测量进行修正，若零偏变化较小，则直接利用Jacobi矩阵修正预积分；若零偏变化较大，则需要重新计算预积分。
S5异步曝光多相机对特征点跟踪出现丢失时，舍弃跟踪丢失的相机观测数据，直到特征点跟踪恢复后，再结合特征点匹配关系恢复各相机运动。
S51基于特征点跟踪恢复后的跟踪结果，计算相机运动，并基于相机运动进行局部初始化，恢复相机相关状态；利用线性插值估算相机在原地图中的位置与姿态，将局部初始化的结果与原地图对齐；
S52基于局部初始化的结果，采用运动结构恢复方法计算特征跟踪成功的两帧之间的位姿关系与地图点坐标，利用预积分估算陀螺仪零偏与视觉测量尺度；
S53局部初始化结果与原地图对齐时，使用特征跟踪未丢失相机的位姿进行线性插值，估算局部地图坐标系相对于原地图的位姿转换，将新地图中的相机运动与特征点坐标转化到初始地图。
利用预积分进行初步优化，能够独立处理各个相机的地图；通过线性插值的方法估算其它相机地图的坐标系相对主相机地图的位姿，再利用相对位姿将其它坐标系中的相机运动与地图点坐标转移到主相机地图中，融合各个相机的地图的方式更简单更便捷；通过在迭代优化中引入鲁棒核函数，使得迭代优化结果更客观更准确。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本发明实施例的实施过程构成任何限定。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (7)

1.一种异步多相机视觉惯性里程计定位方法，其特征在于，包括：
S1异步曝光多相机采用LK金字塔将图像帧缩放为多个尺寸不同的图像，获取所述多相机各自的特征点匹配关系，并估计所述特征点在图像帧中的移动速度；
S2对IMU测量数据进行预积分处理，将相邻两帧之间的所述IMU测量融合为一次测量，基于新的IMU测量数据以及上一时刻计算的预积分递推，获得新的所述两帧之间的时间段前后两个时刻的相对位姿约束关系；
S3将异步多相机视觉惯性里程计初始化，获得多相机的特征跟踪结果并根据所述特征跟踪结果独立建立各相机的地图，并采用线性插值的方法融合所述各相机的地图，获得所述异步曝光多相机的初始地图；
S4根据所述初始地图、所述特征点匹配关系与所述预积分处理的结果，使用滑动窗口优化的方法估计异步曝光多相机-IMU系统在初始化后的运动，
S41在里程计运行过程中维护一个设定长度的滑动窗口，滑窗内存储了设定数量的所述图像帧及相关的地图点；
S42当新的所述图像帧输入后，根据视差判断次新帧是否可作为关键帧，若是，删除滑窗中的最早帧，否则删除所述次新帧；
S5所述异步曝光多相机对所述特征点跟踪出现丢失时，舍弃跟踪丢失的相机观测数据，直到所述特征点跟踪恢复后，再结合所述特征点匹配关系恢复各所述相机运动，
S51基于特征点跟踪恢复后的跟踪结果，计算相机运动，并基于相机运动进行局部初始化，恢复相机相关状态；利用线性插值估算所述相机在原地图中的位置与姿态，将所述局部初始化的结果与原地图对齐；
S52基于所述局部初始化的结果，采用运动结构恢复方法计算特征跟踪成功的两帧之间的位姿关系与地图点坐标，利用所述预积分估算陀螺仪零偏与视觉测量尺度；
S53局部初始化结果与原地图对齐时，使用特征跟踪未丢失相机的位姿进行线性插值，估算局部地图坐标系相对于原地图的位姿转换，将新地图中的所述相机运动与特征点坐标转化到所述初始地图。
2.根据权利要求1所述的方法，其特征在于，所述S1，包括：
在已有所述特征点周围之外的区域，将所述异步曝光多相机分别检测到的各所述图像帧中的Harris角点作为所述特征点，通过采用LK金字塔方法进行提取；
采用光流法估计所述特征点的移动速度，分别获取所述异步曝光多相机所述各自的特征点匹配关系的跟踪结果。
3.根据权利要求2所述的方法，其特征在于，所述光流法估计所述特征点移动速度的表达式如下：
Ixu+Iyv+It＝0
其中，Ix为光强在x方向的梯度变化，u为特征点在x方向的速度，Iy为光强在y方向的梯度变化，v为特征点在y方向的速度，It为光强随时间变化的梯度。
4.根据权利要求1所述的方法，其特征在于，所述S2，包括：
将相邻两帧之间的所述IMU测量融合为一次测量，得到所述两帧之间的时间段前后两个时刻的相对位姿约束关系；基于所述相对位姿关系，当有新的所述IMU测量数据出现时，直接在上一时刻计算的预积分上递推，获得新的所述两帧之间的时间段前后两个时刻的相对位姿约束关系，其中，所述递推的表达式如下：
Figure QLYQS_1
Figure QLYQS_2
Figure QLYQS_3
其中，ΔR(i,j)、Δv(i,j)、Δp(i,j)为预积分计算得到的姿态、速度、位移约束；R(i)为i时刻的绝对姿态，T是矩阵的转置运算，
Figure QLYQS_4
为IMU角速度测量值，bgyr(k)为k时刻IMU的陀螺仪零偏，ηgyr(k)为k时刻IMU的陀螺仪噪声，v(j)为j时刻的绝对速度，v(i)为i时刻的绝对速度，g为重力加速度，Δt(k)为k时刻与k-1时刻的时间间隔，/>
Figure QLYQS_5
为k时刻IMU加速度测量值，bacc(k)为k时刻IMU的加速度计零偏，ηacc(k)为k时刻IMU加速度计噪声，p(i)为i时刻的绝对位置。
5.根据权利要求1所述的方法，其特征在于，所述S3，包括：
S31筛选出所述特征点匹配数量较多的一对所述图像帧，利用所述特征点匹配关系计算两帧之间的本质矩阵，根据所述本质矩阵计算所述两帧之间的相对位姿，并基于所述两帧之间的相对位姿，将所述两帧之间共有的特征点进行三角化，根据PnP算法依次计算其它所有所述图像帧的相对位姿，计算出所述特征点的三维坐标；
S32基于预积分方法估算各所述相机在两帧之间的转动，利用所述预积分估算的结果与运动结构恢复方法计算各所述相机在两帧之间转动的误差，利用所述误差与所述预积分估算的结果相对陀螺仪零偏的雅各比矩阵计算陀螺仪零偏，并基于所述陀螺仪零偏，求得陀螺仪零偏校正后的预积分；
S33基于所述陀螺仪零偏校正后的预积分，将所述各时刻的速度、重力加速度、各相机的尺度因数作为初步优化变量，采用所述预积分处理的结果与所述运动结构恢复方法的结果构建残差，通过求解最小二乘问题估计所述初步优化变量；
S34构建与重力加速度向量的方向垂直的一对正交基，把所述重力加速度在所述正交基上的投影作为状态量，重新加入到所述最小二乘问题中进行迭代求解，获得重优化后的所述重力加速度向量的方向、各时刻的速度、各相机的尺度；
S35将异步曝光多相机中任一所述相机作为主相机，通过所述线性插值的方法估算其它相机地图的坐标系相对主相机地图的所述位姿，并利用所述位姿将其它坐标系中的所述相机运动与各相机地图点坐标转移至所述主相机地图中；
S36采用重力加速度对齐方法先计算主相机地图z轴与所述重力加速度之间的夹角，将所述主相机地图中的所有相机位姿与地图点坐标按所述夹角逆向旋转，使得所述主相机地图z轴与重力加速度对齐，获得所述异步曝光多相机的初始地图。
6.根据权利要求1所述的方法，其特征在于，S42中所述删除滑窗中的最早帧，包括：
基于上一次优化使用雅各比矩阵计算出的被边缘化变量的舒尔补，舍弃与被边缘化变量相关的残差，删除所述滑窗中的最早帧；
采用所述被边缘化变量的舒尔补与所述残差作为新的优化中考虑的先验估计，在迭代优化中更新对应的新的残差。
7.根据权利要求6所述的方法，其特征在于，S42中所述先验估计，是通过边缘化将最早帧相关的数据转化来的，其中，相关的数据，包括重投影误差、预积分误差、先验估计的误差。
CN202210092065.8A 2022-01-26 2022-01-26 一种异步多相机视觉惯性里程计定位方法 Active CN114440877B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210092065.8A CN114440877B (zh) 2022-01-26 2022-01-26 一种异步多相机视觉惯性里程计定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210092065.8A CN114440877B (zh) 2022-01-26 2022-01-26 一种异步多相机视觉惯性里程计定位方法

Publications (2)

Publication Number Publication Date
CN114440877A CN114440877A (zh) 2022-05-06
CN114440877B true CN114440877B (zh) 2023-05-23

Family

ID=81369652

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210092065.8A Active CN114440877B (zh) 2022-01-26 2022-01-26 一种异步多相机视觉惯性里程计定位方法

Country Status (1)

Country Link
CN (1) CN114440877B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117705107B (zh) * 2024-02-06 2024-04-16 电子科技大学 基于两阶段稀疏舒尔补的面向视觉惯性定位方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017131838A2 (en) * 2015-11-13 2017-08-03 Flir Systems, Inc. Sonar sensor fusion and model based virtual and augmented reality systems and methods
CN109506642A (zh) * 2018-10-09 2019-03-22 浙江大学 一种机器人多相机视觉惯性实时定位方法及装置
CN112649016A (zh) * 2020-12-09 2021-04-13 南昌大学 一种基于点线初始化的视觉惯性里程计方法
CN112889091A (zh) * 2018-10-23 2021-06-01 微软技术许可有限责任公司 使用模糊特征的相机位姿估计

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11566896B2 (en) * 2019-08-22 2023-01-31 Leica Geosystems Ag Surveying system with image-based measuring
WO2021077024A1 (en) * 2019-10-18 2021-04-22 Magic Leap, Inc. Gravity estimation and bundle adjustment for visual-inertial odometry

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2017131838A2 (en) * 2015-11-13 2017-08-03 Flir Systems, Inc. Sonar sensor fusion and model based virtual and augmented reality systems and methods
CN109506642A (zh) * 2018-10-09 2019-03-22 浙江大学 一种机器人多相机视觉惯性实时定位方法及装置
CN112889091A (zh) * 2018-10-23 2021-06-01 微软技术许可有限责任公司 使用模糊特征的相机位姿估计
CN112649016A (zh) * 2020-12-09 2021-04-13 南昌大学 一种基于点线初始化的视觉惯性里程计方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于直接法视觉里程计与IMU融合的位姿估计方法;毕浩博;包晗;徐飞;韩天佑;;信息与电脑(理论版)(第07期);全文 *

Also Published As

Publication number Publication date
CN114440877A (zh) 2022-05-06

Similar Documents

Publication Publication Date Title
CN109993113B (zh) 一种基于rgb-d和imu信息融合的位姿估计方法
Wu et al. A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices.
CN112649016B (zh) 一种基于点线初始化的视觉惯性里程计方法
CN111882607B (zh) 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN112240768A (zh) 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN108827341B (zh) 用于确定图像采集装置的惯性测量单元中的偏差的方法
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
US20220051031A1 (en) Moving object tracking method and apparatus
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
CN110660098B (zh) 基于单目视觉的定位方法和装置
CN112749665A (zh) 一种基于图像边缘特征的视觉惯性slam方法
CN111932674A (zh) 一种线激光视觉惯性系统的优化方法
CN111623773B (zh) 一种基于鱼眼视觉和惯性测量的目标定位方法及装置
CN110207693B (zh) 一种鲁棒立体视觉惯性预积分slam方法
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN114136315A (zh) 一种基于单目视觉辅助惯性组合导航方法及系统
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN112731503B (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN111539982B (zh) 一种移动平台中基于非线性优化的视觉惯导初始化方法
CN117073720A (zh) 弱环境与弱动作控制下快速视觉惯性标定与初始化方法及设备
WO2023226156A1 (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