CN112734841B - 一种用轮式里程计-imu和单目相机实现定位的方法 - Google Patents

一种用轮式里程计-imu和单目相机实现定位的方法 Download PDF

Info

Publication number
CN112734841B
CN112734841B CN202011641098.0A CN202011641098A CN112734841B CN 112734841 B CN112734841 B CN 112734841B CN 202011641098 A CN202011641098 A CN 202011641098A CN 112734841 B CN112734841 B CN 112734841B
Authority
CN
China
Prior art keywords
representing
coordinate system
camera
vehicle body
rotation
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
CN202011641098.0A
Other languages
English (en)
Other versions
CN112734841A (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.)
South China University of Technology SCUT
Original Assignee
South China University of Technology SCUT
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 South China University of Technology SCUT filed Critical South China University of Technology SCUT
Priority to CN202011641098.0A priority Critical patent/CN112734841B/zh
Publication of CN112734841A publication Critical patent/CN112734841A/zh
Application granted granted Critical
Publication of CN112734841B publication Critical patent/CN112734841B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/214—Generating training patterns; Bootstrap methods, e.g. bagging or boosting
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/22—Matching criteria, e.g. proximity measures
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G06F18/232—Non-hierarchical techniques
    • G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
    • G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T5/00—Image enhancement or restoration
    • G06T5/80—Geometric correction
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • 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/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30204—Marker
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • 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)
  • Theoretical Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Remote Sensing (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Databases & Information Systems (AREA)
  • Probability & Statistics with Applications (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)
  • Image Processing (AREA)

Abstract

本发明公开了一种用轮式里程计‑IMU和单目相机实现定位的方法，主要包括以下步骤：(1)标定相机内参，相机和轮式里程计外参，IMU和相机的外参；(2)从相机拍摄的图像中选取图像关键帧，同时校正图像的畸变，然后提取特征点，进行系统初始化，最后选择初始化成功时的当前帧位姿作为世界坐标系的位姿；(3)根据车体位姿的预估值，将地图点投影到平面图像上；(4)预测车体位姿；(5)建立局部地图；(6)根据局部地图，建立一个关于车体的所有位姿和地图点的优化问题，然后通过光束平差法得到优化后的位姿和地图点；(7)采用词袋(BoW)检测回环，一旦检测到经过了同一个位置，就进行回环校正。

Description

一种用轮式里程计-IMU和单目相机实现定位的方法
技术领域
本发明属于计算机视觉技术领域，主要是研究的一种用轮式里程计-IMU和单目相机实现定位的方法。
背景技术
随着SLAM技术的发展，地面车辆的定位导航开始由原来在地面铺设一些媒介物(如磁轨、RFID等)实现定位，如[张辰贝西,黄志球,自动导航车(AGV)发展综述[J]信息技术应用.2010.]，现在逐渐地转变为无轨定位的方式。由于相机的成本相对低廉，同时其拍摄的图像能够提供丰富的信息，所以用视觉SLAM(simultaneous localization andmapping)来实现定位导航越来越受到关注，也出现了很多视觉SLAM的方案，如基于RGB_D相机的SLAM方案RGB_D-SLAM[Engelhard N,Endres F,Hess J,et al.Real-time 3D visualSLAM with a hand-held RGB_D camera[C].2011.]；基于特征点法的SLAM方案ORB_SLAM[Murartal R,Tardos J D.ORB-SLAM2:An opensource SLAM system for monocular,stereo and RGB-D cameras[J].2017.]。相关专利有申请号为CN201910919739.5的中国专利《基于视觉SLAM的地图构建的方法、导航系统及装置》，申请号为CN201811318082.9的中国专利《一种基于SLAM的室内定位方法及装置》，但是这些方案都是只使用一个视觉传感器-相机来实现，单目相机会存在尺度不确定的问题，同时在纹理较少的区域，也会出现跟踪丢失的问题。所以后续出现了利用多个传感器融合来解决单一传感器缺点的SLAM方案，如2015年，Michael Bloesch等提出了基于滤波的松耦合方案ROVIO[Bloesch M,Omari S,Hutter M,et al.Robust visual inertial odometry using a direct EKF-basedapproach[C].2015.]。2015年Leutenegger提出了基于图优化的紧耦合方案OKVIS[Leutenegger S,Lynen S,Bosse M,et al.Keyframe-based visual-inertial odometryusing nonlinear optimization[J].2015.]，将IMU的误差以全概率的形式融合到路标的重投影误差里，构成将被优化的联合非线性误差函数，进而最小化从相机帧中观察到的地标的重投影误差。2017年，香港科技大学沈劭劼团队提出了基于图优化的紧耦合方案VINS[Tong Qin,Peiliang Li,and Shaojie Shen.VINS-Mono:A robust and versatilemonocular visual-inertial state estimator[J].2017.]，考虑到优化过程中每次迭代都要计算积分的操作太耗时了，所以VINS引入了预积分的思想，将一个时间段内数百个惯性测量的积分结果作为一个相对运动约束，引入了预积分的IMU模型能完美地融合到视觉－惯性的因子图中。使得VINS成为一套具有自动初始化，在线外参标定，重定位，闭环检测，以及全局位姿图优化功能的完整SLAM系统。
但是，在地面车辆上直接用视觉惯性的方案也存在一些问题，当车辆匀速运动时，惯性测量单元无法检测到加速度，导致此时的尺度不确定性变大，继而导致尺度漂移，定位误差增大。为了解决上面的问题，现提出一种方法，采用了轮式里程计，IMU，单目相机三个传感器，来实现地面车辆的定位。轮式里程计可以预测车体某一时刻的位姿，由于车轮打滑，地面凹凸不平等原理，预测值存在误差，方向角信息由IMU提供，然后根据单目相机的图像数据构建一个重投影误差最小的优化问题，通过优化车体位姿和三维路标点坐标，使得误差最小。
发明内容
本发明提出了一种用轮式里程计-IMU和单目相机实现定位的方法，通过轮式里程计和IMU提供的测量数据预测车体的位姿，然后融合相机的图像数据，用重投影误差来构造一个图优化问题，最后输出优化后的位姿，为了消除累计误差，这里采用了基于词袋的方法来检测回环。
本发明至少通过如下技术方案之一实现
一种用轮式里程计-IMU和单目相机实现定位的方法，包括以下步骤：
(1)标定单目相机的内参矩阵、单目相机和车体之间的外参、单目相机和IMU之间的外参；
(2)对单目相机的图像进行去畸变后，对图像提取ORB特征点，找到对应的最优匹配特征点后，执行单目SLAM初始化操作，选择初始化成功时的当前帧的位姿作为世界坐标系的位姿；
(3)根据轮式里程计和IMU测到的方向角预测车体在某一时刻的位姿；
(4)根据位姿将地图点投影到图像上，进行特征跟踪和匹配；
(5)通过三角化得到地图点的三维坐标，同时插入关键帧，从而维护一个局部地图；
(6)利用相机的图像特征点构建重投影误差最小的优化问题，对局部地图中的地图点和所有位姿进行光束平差法(bundle adjustment，BA)优化，得到优化后的位姿，用优化后的位姿去更新车体位姿；
(7)采用词袋(BoW)的方式检测回环，然后进行回环校正，得到全局一致的车体位姿。
优选的，所述步骤(1)中，车体坐标系(base frame)位于车体的正中心，世界坐标系(world frame)固定在一个位置，相机坐标系(camera frame)位于相机中心，用
Figure BDA0002881168780000031
表示车体坐标系B到世界坐标系W的旋转矩阵，
Figure BDA0002881168780000032
表示车体坐标系B到世界坐标系W的平移向量，则
Figure BDA0002881168780000033
TWB表示从车体坐标系B到世界坐标系W之间的变换矩阵，用r表示车体在地面上的世界坐标(x,y)，Φ表示方向角，则有
Figure BDA0002881168780000034
Figure BDA0002881168780000035
其中，px、py表示取三维向量的x、y部分,
Figure BDA0002881168780000036
表示将矩阵变换为李代数，即旋转向量，
Figure BDA0002881168780000037
表示取旋转向量在z轴部分的分量；
路标在世界坐标系中的坐标l转换为相机坐标系下的坐标lC的公式如下：
Figure BDA0002881168780000038
其中，
Figure BDA0002881168780000039
为世界坐标系W到相机坐标系C的旋转矩阵，
Figure BDA00028811687800000310
为世界坐标系W到相机坐标系C的平移向量，l为路标在世界坐标系W中的坐标，lC为路标在相机坐标系C下的坐标；
通过外参标定，得到相机坐标系C到车体坐标系B之间的旋转矩阵
Figure BDA00028811687800000311
和平移向量
Figure BDA00028811687800000312
用于后续的优化过程。
优选的，在标定单目相机和车体之间的外参之前先将相机坐标系C到车体坐标系B之间的旋转矩阵
Figure BDA00028811687800000313
用对应的旋转四元数
Figure BDA00028811687800000314
描述：
Figure BDA00028811687800000315
其中，α为绕x轴转动的角度，β为绕y轴转动的角度，γ为绕z轴转动的角度，qx(α)表示角度α对应的四元数，qy(β)表示角度β对应的四元数，qz(γ)表示角度γ对应的四元数，
Figure BDA0002881168780000041
表示四元数乘法；标定单目相机和车体之间的外参包括以下3个步骤：
①构建关于旋转的误差项来求解
Figure BDA0002881168780000042
中的分量qyx：
Figure BDA0002881168780000043
Figure BDA0002881168780000044
其中，
Figure BDA0002881168780000045
表示从相机坐标系C到车体坐标系B的旋转四元数，q(.)表示该角度对应的四元数，
Figure BDA0002881168780000046
表示第i时刻到第i+1时刻轮式里程计测到的旋转四元数，
Figure BDA0002881168780000047
表示第i时刻到第i+1时刻相机测到的旋转四元数，
Figure BDA0002881168780000048
表示四元数乘法，ηi表示用轮式里程计测量旋转和用相机测量旋转之间的误差；
②构建误差，获取相机坐标系C到车体坐标系B的三维平移向量在x轴上的分量
Figure BDA0002881168780000049
在y轴上的分量
Figure BDA00028811687800000410
和三维旋转分解到绕z轴的旋转角γ：
Figure BDA00028811687800000411
其中，I3表示三维单位矩阵，
Figure BDA00028811687800000412
表示相机坐标系C到车体坐标系B的三维平移向量，
Figure BDA00028811687800000413
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，
Figure BDA00028811687800000414
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA00028811687800000415
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，∈i表示用轮式里程计测量平移和用相机测量平移之间的误差；
Figure BDA00028811687800000416
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转矩阵增量表示成旋转矩阵的形式，由于
Figure BDA00028811687800000417
Figure BDA00028811687800000418
其中，
Figure BDA0002881168780000051
表示i时刻车体的方向角，
Figure BDA0002881168780000052
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，Rz(γ)表示
Figure BDA0002881168780000053
在z轴上的分量，R(qyx)表示
Figure BDA0002881168780000054
在x轴和y轴上的分量的乘积，将轮式里程计测量的平移和用相机测量的平移之间的误差记为∈i：
Figure BDA0002881168780000055
其中，
Figure BDA0002881168780000056
表示i时刻车体的方向角，
Figure BDA0002881168780000057
表示相机坐标系C到车体坐标系B的三维平移向量，Rz(γ)表示
Figure BDA0002881168780000058
在z轴上的分量，R(qyx)表示
Figure BDA0002881168780000059
在x轴和y轴上的分量的乘积，
Figure BDA00028811687800000510
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA00028811687800000511
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量；
③构造最小二乘问题优化标定的结果：
Figure BDA00028811687800000512
Figure BDA00028811687800000513
Figure BDA00028811687800000514
其中，m代表试验次数，Ei表示平移部分的误差，Fi表示旋转部分的误差，Wi1、Wi2分别是与测量残差相关的协方差矩阵,
Figure BDA00028811687800000515
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转矩阵增量表示成旋转矩阵的形式，I3表示三维单位矩阵，
Figure BDA00028811687800000516
表示相机坐标系C到车体坐标系B的三维平移向量，
Figure BDA00028811687800000517
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，
Figure BDA00028811687800000518
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA00028811687800000519
表示相机在时刻i和时刻i+1之间测量到的旋转四元数增量，
Figure BDA00028811687800000520
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA00028811687800000521
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转四元数增量，
Figure BDA00028811687800000522
表示四元数乘法，
Figure BDA00028811687800000523
表示最终优化后的平移向量，
Figure BDA00028811687800000524
表示最终优化后的旋转四元数。
优选的，所述步骤(2)中，对单目相机的图像进行去畸变后，利用图像特征提取，利用特征点的二进制描述子之间的汉明距离最小，找到对应的最优匹配特征点后，用对极约束的方法实现单目SLAM的初始化操作，选择初始化成功时的当前帧位姿作为世界坐标系的位姿。
优选的，轮式里程计在两个连续图像关键帧k，k+1之间的测量方程如下：
Figure BDA0002881168780000061
其中，
Figure BDA0002881168780000062
表示两个连续图像关键帧k、k+1之间车体姿态的增量测量值，ηvk为噪声，服从正态分布N(0,∑vk)，∑vk为正态分布的方差，因为车体k时刻的姿态量
Figure BDA0002881168780000063
rk位置坐标，
Figure BDA0002881168780000064
为方向角，所以姿态量vk噪声项也可以表示成位置噪声ηrk和方向角噪声ηφk，即
Figure BDA0002881168780000065
而
Figure BDA0002881168780000066
则表示轮式里程计在k时刻实际测量到的车体位置
Figure BDA0002881168780000067
和方向角
Figure BDA0002881168780000068
组成的状态向量；
由轮式里程计k时刻和k+1时刻内的增量测量去推算车体在k+1时刻的位置和方向角的方程如下：
Figure BDA0002881168780000069
其中，rk+1表示k+1时刻车体的世界坐标(x,y)，φk+1表示k+1时刻车体的方向角，rk表示k时刻车体的世界坐标(x,y)，φk表示k时刻车体的方向角，
Figure BDA00028811687800000610
表示k时刻到k+1时刻内轮式里程计测量到的世界坐标增量，
Figure BDA00028811687800000611
表示k时刻到k+1时刻内轮式里程计测量到的方向角增量，ηrk表示k时刻轮式里程计测量的位置噪声，ηφk表示k时刻轮式里程计测量的方向角噪声，同时Φ(φk)表示从k时刻车体的方向角φk得到的旋转矩阵，具体如下所示：
Figure BDA00028811687800000612
由于地面凹凸不平，车体打滑等原因，轮式里程计预测车体下一时刻的位置中的方向角φk是由IMU提供。
优选的，所述局部地图包含图像关键帧的信息和地图点的三维坐标信息。
优选的，所述步骤(6)的优化用马氏距离表示的最小代价函数F(X)：
F(X)＝∑ek(X)T∑kek(X)
X*＝arg min F(X)
其中，用因子图描述优化问题，X表示图的节点，ek(X)表示图中第k条边，信息矩阵∑k作为权重因子,信息矩阵是协方差矩阵的逆矩阵，经过优化后，得到一个优化后的位姿X*，用X*去更新车体位姿。
优选的，单目相机的观测方程如下：
Figure BDA0002881168780000071
其中，u(vi,lω)表示车体在位姿为vi时，相机观测路标lω得到的观测量，lω为路标在世界坐标系下的坐标，车体位姿中的旋转矩阵
Figure BDA0002881168780000072
平移向量
Figure BDA0002881168780000073
Figure BDA0002881168780000074
表示i时刻车体的世界坐标(x,y)，
Figure BDA0002881168780000075
表示i时刻车体的方向角，
Figure BDA0002881168780000076
表示从车体到相机刚体变换的旋转矩阵，ηu为相机的观测噪声，服从正态分布
Figure BDA0002881168780000077
Figure BDA0002881168780000078
表示方差，I2表示二维单位矩阵，简记为
Figure BDA0002881168780000079
π(.)表示相机的投影方程，如下：
Figure BDA00028811687800000710
其中，l表示路标在相机坐标系下的坐标，lz表示路标在相机坐标系下的坐标中z轴上的分量，fx、fy、cx、cy均为相机内部参数。
优选的，在进行回环检测之前先利用DBow开源库离线训练实地拍摄的图片，训练过程是先从这些实地拍摄的图片中提取特征点，然后通过k-mean++聚类方法进行分类，形成一个有k个分支，深度为d的树形结构，并以此作为字典；后续对每一个关键帧都会先提取特征点，然后去这个树形结构的字典中查询得到该关键帧对应的BoW向量，并将BoW向量保存在关键帧数据库中。
优选的，所述回环检测包括以下步骤：
1)当图像检测到的特征点少于一个阈值或超过固定的时间间隔时会生成关键帧，否则不会生成关键帧；
2)通过搜索预先训练得到的字典得到当前关键帧的BoW向量，然后从与当前关键帧有共视关系的所有关键帧中找到与当前关键帧的BoW相似度最低的值，记为minS；
3)从关键帧数据库中除去与当前帧有共视关系的关键帧，最后遍历检索关键帧数据库，在遍历过程中，与当前关键帧BoW相似度小于0.8*minS的关键帧判定为非回环帧，直接跳过，而相似度大于一定阈值的关键帧则标记为回环候选帧，这些标记为回环候选帧中的相似度得分最高的关键帧最后被判定为回环帧，两个BoW向量vA和vB之间的相似度如下：
Figure BDA0002881168780000081
其中,S(vA,vB)表示向量vA和向量vB之间的相似度，向量vA和vB都是N维向量，vAi是向量vA中的第i维的值，vBi是向量vB中的第i维的值，|·|1表示取L1范数。
与现有的技术相比，本发明具有的有益效果如下：
(1)相比在地面铺设标志物来实现地面车辆的定位系统，本发明提供的方法不需要对周围环境做人为改造，成本更低。
(2)利用手眼标定的原理，构造一个最小二乘法的优化问题，从而高效地解决了相机和车体之间的外参的精确标定问题。
(3)采用单目相机，IMU,轮速里程计三者融合的方式来实现车辆的定位和建图，相比单一传感器来说，鲁棒性更强。
(4)相比纯视觉SLAM做的定位系统，本发明是先根据轮式里程计和IMU的数据预测车体在下一时刻的位姿，然后根据视觉重投影误差最小构建一个优化问题，然后利用光束平差法得到优化后的车体位姿和地图点。然后用优化后的值去调整车体的位姿预测值，因此精确性更高，同时解决了单目相机的尺度不确定的问题。
附图说明
图1为本实施的设备配置和坐标系变换关系图；
图2为本实施的相机和车体的外参标定图；
图3为本实施的整体流程框图；
图4为本实施的维护局部地图的流程框图；
图5为本实施的轮式里程计残差和视觉残差构造的因子图；
图6为本实施的回环检测流程图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，故图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。
下面结合实施例和附图对本发明进行详细说明。
一种用轮式里程计-IMU和单目相机实现定位的方法的系统，包括前端VO模块、后端图优化模块、回环检测模块。所述前端VO模块用于预测车体的初步位姿；所述后端图优化模块通过优化车体的初步位姿，使得轮式里程计误差和视觉重投影误差的累加和最小，从而得到一个更准确的车体位姿；所述回环检测模块通过检测车体是否经过了同一个空间位置，可以减小车体位姿估计在长时间内的累计误差，从而得到全局一致的位姿估计。其设备配置如图1所示,车体采用四轮驱动的方式，配备有四个电机和四个编码器，同时在车体中心装有一个IMU用于测量车体的方向角，单目相机以斜向上的方式安装在车体前方，以30Hz的频率采集1280×480分辨率的图像。
所述的一种用轮式里程计-IMU和单目相机实现定位的方法，设备配置和坐标系变换关系图如图1所示，整体流程如图3所示，主要包括以下步骤：
(1)采用张正友的棋盘格标定方法标定单目相机的内参矩阵，然后利用Kalibr工具来标定单目相机和IMU的外参，最后采用手眼标定的方法来标定单目相机和车体之间的外参，如图2，最后得到的外参主要包括旋转矩阵
Figure BDA0002881168780000091
和平移向量
Figure BDA0002881168780000092
标定的前提是车轮的内参，如左轮半径rL，右轮半径rR，轮距b都是已知量。同时相机在不同时刻，都可以通过Apriltag标定板求得带有绝对尺度的位姿，车体在不同时刻的位姿则可以通过轮式里程计求得。
车体坐标系(base frame)位于车体的正中心，世界坐标系(world frame)固定在一个位置，相机坐标系(camera frame)位于相机中心，用
Figure BDA0002881168780000101
表示车体坐标系B到世界坐标系W的旋转矩阵，
Figure BDA0002881168780000102
表示车体坐标系B到世界坐标系W的平移向量，则
Figure BDA0002881168780000103
TWB表示从车体坐标系B到世界坐标系W之间的变换矩阵，用r表示车体在地面上的世界坐标(x,y)，Φ表示方向角，则有
Figure BDA0002881168780000104
Figure BDA0002881168780000105
其中，px、py表示取三维向量的x、y部分,
Figure BDA0002881168780000106
表示将矩阵变换为李代数，即旋转向量，
Figure BDA0002881168780000107
表示取旋转向量在z轴部分的分量；
路标在世界坐标系中的坐标l转换为相机坐标系下的坐标lC的公式如下：
Figure BDA0002881168780000108
其中，
Figure BDA0002881168780000109
为世界坐标系W到相机坐标系C的旋转矩阵，
Figure BDA00028811687800001010
为世界坐标系W到相机坐标系C的平移向量，l为路标在世界坐标系W中的坐标，lC为路标在相机坐标系C下的坐标；
经过外参标定后，可以得到相机坐标系C到车体坐标系B之间的旋转矩阵
Figure BDA00028811687800001011
和平移向量
Figure BDA00028811687800001012
用于后续的优化过程。
在以下的标定步骤中，相机坐标系C到车体坐标系B之间的旋转矩阵
Figure BDA00028811687800001013
用对应的旋转四元数
Figure BDA00028811687800001014
描述，平移向量用
Figure BDA00028811687800001015
描述。标定的目的为求出相机坐标系C到车体坐标系B之间的外参
Figure BDA00028811687800001016
其中，
Figure BDA00028811687800001017
表示从相机坐标系C到车体坐标系B的旋转，用四元数表示，
Figure BDA00028811687800001018
表示从相机坐标系C到车体坐标系B的平移向量。
因为三维空间内的旋转可以分成绕x轴、y轴、z轴分别旋转之后再合成，记绕x轴转动的角度记为α,绕y轴转动的角度记为β，绕z轴转动的角度记为γ，即
Figure BDA00028811687800001019
Figure BDA0002881168780000111
Figure BDA0002881168780000112
表示四元数乘法，q(.)表示该角度对应的四元数，如下(四元数采用虚部在前，实部在后的表示方式)：
Figure BDA0002881168780000113
Figure BDA0002881168780000114
Figure BDA0002881168780000115
关于标定单目相机和车体之间的外参的步骤可以分为如下三步：
①构建关于旋转的误差项来求解
Figure BDA0002881168780000116
中的分量qyx，这里
Figure BDA0002881168780000117
Figure BDA0002881168780000118
其中，
Figure BDA0002881168780000119
表示第i时刻到第i+1时刻轮式里程计测到的旋转四元数，
Figure BDA00028811687800001110
表示第i时刻到第i+1时刻相机测到的旋转四元数，
Figure BDA00028811687800001111
为相机坐标系C到车体坐标系B的旋转四元数，
Figure BDA00028811687800001112
表示四元数乘法，ηi表示用轮式里程计测量旋转和用相机测量旋转之间的误差。
②构建残差,获取相机坐标系C到车体坐标系B的三维平移向量在x轴上的分量
Figure BDA00028811687800001113
在y轴上的分量
Figure BDA00028811687800001114
和三维旋转分解到绕z轴的旋转角γ。
Figure BDA00028811687800001115
其中，
Figure BDA00028811687800001116
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转矩阵增量表示成旋转矩阵的形式，I3表示三维单位矩阵，
Figure BDA00028811687800001117
表示相机坐标系C到车体坐标系B的三维平移向量，
Figure BDA00028811687800001118
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，
Figure BDA00028811687800001119
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA00028811687800001120
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，∈i表示用轮式里程计测量平移和用相机测量平移之间的误差。因为
Figure BDA0002881168780000121
Figure BDA0002881168780000122
其中，
Figure BDA0002881168780000123
表示i时刻车体的方向角，
Figure BDA0002881168780000124
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，Rz(γ)表示
Figure BDA0002881168780000125
在z轴上的分量，R(qyx)表示
Figure BDA0002881168780000126
在x轴和y轴上的分量的乘积，将轮式里程计测量的平移和用相机测量的平移之间的误差记为∈i
Figure BDA0002881168780000127
其中，
Figure BDA0002881168780000128
表示i时刻车体的方向角，
Figure BDA0002881168780000129
表示相机坐标系C到车体坐标系B的三维平移向量，Rz(γ)表示
Figure BDA00028811687800001210
在z轴上的分量，R(qyx)表示
Figure BDA00028811687800001211
在x轴和y轴上的分量的乘积，
Figure BDA00028811687800001212
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA00028811687800001213
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，∈i表示用轮式里程计测量的平移和用相机测量的平移之间的误差。
③最后构造如下的一个最小二乘问题，通过优化多次重复试验的误差之和，达到减小标定误差的目的。
Figure BDA00028811687800001214
Figure BDA00028811687800001215
Figure BDA00028811687800001216
其中，m代表试验次数，Ei表示平移部分的误差，Fi表示旋转部分的误差，Wi1、Wi2分别是与测量残差相关的协方差矩阵,
Figure BDA00028811687800001217
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转矩阵增量表示成旋转矩阵的形式，I3表示三维单位矩阵，
Figure BDA00028811687800001218
表示相机坐标系C到车体坐标系B的三维平移向量，
Figure BDA00028811687800001219
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，
Figure BDA0002881168780000131
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA0002881168780000132
表示相机在时刻i和时刻i+1之间测量到的旋转四元数增量，
Figure BDA0002881168780000133
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，
Figure BDA0002881168780000134
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转四元数增量，
Figure BDA0002881168780000135
表示四元数乘法，
Figure BDA0002881168780000136
表示最终优化后的平移向量，
Figure BDA0002881168780000137
表示最终优化后的旋转四元数。
(2)相机采集到的原始图像，先对其进行去畸变处理，再提取图像的ORB特征，进行特征点匹配，然后利用相邻图像之间的对极约束关系实现单目SLAM的初始化操作，从而将初始化成功时的第一帧作为世界坐标系的位姿。
(3)根据轮式里程计和IMU测到的方向角来预测车体在下一时刻的位姿，这个位姿只是预估值，会含有误差。轮式里程计在两个连续图像关键帧k，k+1之间的测量方程可以写成如下
Figure BDA0002881168780000138
其中，
Figure BDA0002881168780000139
表示两个连续图像关键帧k、k+1之间车体姿态vk的增量测量值，ηvk为噪声，服从正态分布N(0,∑vk)，因车体k时刻的姿态量
Figure BDA00028811687800001310
rk位置坐标，
Figure BDA00028811687800001311
为方向角，所以姿态量vk噪声项也可以表示成位置噪声ηrk和方向角噪声ηφk，即
Figure BDA00028811687800001312
而
Figure BDA00028811687800001313
则表示轮式里程计在k时刻实际测量到的车体位置
Figure BDA00028811687800001314
和方向角
Figure BDA00028811687800001315
组成的状态向量。
由轮式里程计k时刻和k+1时刻内的增量测量去推算车体在k+1时刻的位置和方向角的方程如下：
Figure BDA00028811687800001316
其中，rk+1表示k+1时刻车体的世界坐标(x,y)，φk+1表示k+1时刻车体的方向角，rk表示k时刻车体的世界坐标(x,y)，φk表示k时刻车体的方向角，
Figure BDA00028811687800001317
表示k时刻到k+1时刻内轮式里程计测量到的世界坐标增量，
Figure BDA00028811687800001318
表示k时刻到k+1时刻内轮式里程计测量到的方向角增量，ηrk表示k时刻轮式里程计测量的位置噪声，ηφk表示k时刻轮式里程计测量的方向角噪声。同时Φ(φk)表示从k时刻车体的方向角φk得到的旋转矩阵，当然这是在二维平面上的形式，具体如下所示：
Figure BDA0002881168780000141
由于地面的凹凸不平，轮子侧滑等原因，轮式里程计测到的角度会不太准确，所以角度测量会采用IMU的数据。
(4)根据轮式里程计的测量，得到车体位姿的预估值，利用预估的位姿，将地图点投影到图像上，加快图像特征点的匹配与跟踪过程。这里因为有了一个位姿的预估值，所以在进行特征点匹配时，不需要搜索整个图像关键帧，只需要搜索投影像素点附近区域即可，这样加快了特征匹配的速度，也有助于视觉跟踪。
(5)在完成特征点的正确匹配后，选取图像关键帧，并通过轮式里程计和IMU的数据预测出车体的初步位姿。然后通过三角化的方法得到特征点的三维坐标(即生成新的地图点)，同时维护一个大小有限的局部地图，局部地图保存着图像关键帧的特征点信息和地图点的三维坐标信息。当局部地图中关键帧过多时，会通过边缘化的方式移除多余关键帧，同时将移除的信息作为系统的先验信息，流程图如图4，当图像检测到的特征点少于一个阈值或超过一个固定的时间间隔时会生成关键帧，否则不会生成关键帧；当有新的关键帧加入的时候线程就将自己设置为繁忙状态并立刻处理新的关键帧，当处理完一个关键帧后，就会将自己设置为空闲状态并进入睡眠状态；当某地图点第一次被观测到的关键帧id和当前观测到该地图点的关键帧id相差过大，或者该地图点被观察到的总次数较少，就会剔除该地图点对应的图像特征点。
(6)然后利用相机的图像特征点构建一个重投影误差最小的优化问题，对局部地图中的所有地图点和所有位姿进行光束平差法(bundle adjustment，BA)优化，一个马氏距离表示的最小代价函数F(X)如下
F(X)＝∑ek(X)T∑kek(X)
X*＝arg minF(X)
其中，用因子图描述优化问题，X表示图的节点，ek(X)表示图中第k条边，信息矩阵∑k作为权重因子,信息矩阵是协方差矩阵的逆矩阵，经过优化后，得到一个优化后的位姿X*，用X*去更新车体位姿。
单目相机的观测方程如下：
Figure BDA0002881168780000151
其中，u(vi,lω)表示车体在姿态量vi时，相机观测路标lω得到的观测量，lω为路标在世界坐标系下的坐标，车体位姿变化中的旋转矩阵
Figure BDA0002881168780000152
平移向量
Figure BDA0002881168780000153
Figure BDA0002881168780000154
表示i时刻车体的世界坐标(x,y)，
Figure BDA0002881168780000155
表示i时刻车体的方向角，
Figure BDA0002881168780000156
表示从车体到相机刚体变换的旋转矩阵，
Figure BDA0002881168780000157
为相机的观测噪声，服从正态分布
Figure BDA0002881168780000158
Figure BDA0002881168780000159
表示方差，I2表示二维单位矩阵，π(.)表示相机的投影方程，如下：
Figure BDA00028811687800001510
其中，l表示路标在相机坐标系下的坐标，lz表示路标在相机坐标系下的坐标中z轴上的分量，fx、fy、cx、cy均为相机的内部参数。
由上面的观测方程，可以得到视觉误差定义如下：
误差项(eiω)＝理论值(u(vi,lω)-ηu)-测量值(uiω)
Figure BDA00028811687800001511
其中，eiω为视觉投影的误差项，(u(vi,lω)-ηu)表示车体在姿态量vi时，相机观测路标lω得到的理论值,uiω为车体在姿态量vi时，相机观测路标lω得到的实际测量值，两者的差值就是误差项eiω，同时，lω为路标在世界坐标系下的坐标，车体位姿变化中的旋转矩阵
Figure BDA00028811687800001512
Figure BDA00028811687800001513
平移向量
Figure BDA00028811687800001514
Figure BDA00028811687800001515
表示从车体到相机刚体变换的旋转矩阵，
Figure BDA00028811687800001516
为相机的观测噪声，服从正态分布
Figure BDA00028811687800001517
Figure BDA00028811687800001518
表示方差，I2表示二维单位矩阵，π(.)表示相机的投影方程。
则视觉误差eiω相对于位姿向量vi的雅各比矩阵为：
Figure BDA0002881168780000161
Figure BDA0002881168780000162
Figure BDA0002881168780000163
其中，
Figure BDA0002881168780000164
ei表示单位阵I3的第i列，同时Λ12＝[e1e2]。
eiω相对于lω的雅各比为：
Figure BDA0002881168780000165
轮式里程计在两个连续图像关键帧k，k+1之间的测量方程可以写成如下
Figure BDA0002881168780000166
其中，
Figure BDA0002881168780000167
表示两个连续图像关键帧k、k+1之间车体姿态vk的增量测量值，ηvk为噪声，服从正态分布N(0,∑vk)，因车体k时刻的姿态量
Figure BDA0002881168780000168
rk位置坐标，
Figure BDA0002881168780000169
为方向角，所以姿态量vk噪声项也可以表示成位置噪声ηrk和方向角噪声ηφk，即
Figure BDA00028811687800001610
而
Figure BDA00028811687800001611
则表示轮式里程计在k时刻实际测量到的车体位置
Figure BDA00028811687800001612
和方向角
Figure BDA00028811687800001613
组成的状态向量。
由轮式里程计k时刻和k+1时刻内的增量测量去推算车体在k+1时刻的位置和方向角的方程如下：
Figure BDA00028811687800001614
其中，rk+1表示k+1时刻车体的世界坐标(x,y)，φk+1表示k+1时刻车体的方向角，rk表示k时刻车体的世界坐标(x,y)，φk表示k时刻车体的方向角，
Figure BDA0002881168780000171
表示k时刻到k+1时刻内轮式里程计测量到的世界坐标增量，
Figure BDA0002881168780000172
表示k时刻到k+1时刻内轮式里程计测量到的方向角增量，ηrk表示k时刻轮式里程计测量的位置噪声，ηφk表示k时刻轮式里程计测量的方向角噪声。同时Φ(φk)表示从k时刻车体的方向角φk得到的旋转矩阵，当然这是在二维平面上的形式，具体如下所示：
Figure BDA0002881168780000173
从而得出轮式里程计部分的误差项定义如下：
Figure BDA0002881168780000174
Figure BDA0002881168780000175
其中,eij为轮式里程计在第i时刻和第j时刻的测量误差项，ri为第i时刻的位置坐标，rj为第j时刻的位置坐标，
Figure BDA0002881168780000176
为第i时刻的方向角，
Figure BDA0002881168780000177
为第j时刻的方向角，
Figure BDA0002881168780000178
表示从世界坐标系到第i时刻车体坐标系的变换矩阵，
Figure BDA0002881168780000179
为轮式里程计在第i时刻到第j时刻的时间段内测量到的车体位置坐标增量，
Figure BDA00028811687800001710
为轮式里程计在第i时刻到第j时刻的时间段内测量到的车体方向角增量。
eij相对于vi、vj的雅各比如下：
Figure BDA00028811687800001711
Figure BDA00028811687800001712
其中，
Figure BDA00028811687800001713
由视觉残差约束和轮式里程计残差约束定义的一个因子图如图5所示。
(7)为了减小系统出现的累计误差，采用词袋(BoW)的方式检测回环，在进行回环检测之前先利用DBow开源库离线训练实地拍摄的图片，训练过程是先从这些实地拍摄的图片中提取特征点，然后通过k-mean++聚类方法进行分类，形成一个有k个分支，深度为d的树形结构，并以此作为字典。后续对每一个关键帧都会先提取特征点，然后去这个树形结构的字典中查询得到该关键帧对应的BoW向量，并将其保存在关键帧数据库中。在回环检测的时候，也是先对当前帧图像提取特征点，然后去树形结构的字典中查询得到当前帧对应的BoW向量，最后从关键帧数据库中查找与当前帧的BoW向量最接近且两者相似度不小于一个阈值的关键帧作为回环帧。一旦检测到了回环，说明车体经过同一个位置，然后根据检测到的回环帧去求取当前帧和回环帧之间对应的相似变换矩阵，最后根据这个相似变换矩阵去进行回环校正，减小先前位姿估计中的累计误差，得到全局一致的车体位姿。
回环检测的流程如图6，步骤描述如下：
1)当图像检测到的特征点少于一个阈值或超过固定的时间间隔时会生成关键帧，否则不会生成关键帧；
2)通过搜索预先训练得到的字典得到当前关键帧的BoW向量，然后从与当前关键帧有共视关系的所有关键帧中找到与当前关键帧的BoW相似度最低的值，记为minS；
3)从关键帧数据库中除去与当前帧有共视关系的关键帧，最后遍历检索关键帧数据库，在遍历过程中，与当前关键帧BoW相似度小于0.8*minS的关键帧判定为非回环帧，直接跳过，而相似度大于一定阈值的关键帧则标记为回环候选帧，这些标记为回环候选帧中的相似度得分最高的关键帧最后被判定为回环帧，两个BoW向量vA和vB之间的相似度如下：
Figure BDA0002881168780000181
其中,S(vA,vB)表示向量vA和向量vB之间的相似度，向量vA和vB都是N维向量，vAi是向量vA中的第i维的值，vBi是向量vB中的第i维的值，|·|1表示取L1范数。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，包括以下步骤：
(1)标定单目相机的内参矩阵、单目相机和车体之间的外参、单目相机和IMU之间的外参；
(2)对单目相机的图像进行去畸变后，对图像提取ORB特征点，找到对应的最优匹配特征点后，执行单目SLAM初始化操作，选择初始化成功时的当前帧的位姿作为世界坐标系的位姿；
(3)根据轮式里程计和IMU测到的方向角预测车体在某一时刻的位姿；
(4)根据位姿将地图点投影到图像上，进行特征跟踪和匹配；
(5)通过三角化得到地图点的三维坐标，同时插入关键帧，从而维护一个局部地图；
(6)利用相机的图像特征点构建重投影误差最小的优化问题，对局部地图中的地图点和所有位姿进行光束平差法(bundle adjustment，BA)优化，得到优化后的位姿，用优化后的位姿去更新车体位姿；
(7)采用回环检测和回环校正，得到全局一致的车体位姿。
2.根据权利要求1所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，所述步骤(1)中，车体坐标系(base frame)位于车体的正中心，世界坐标系(worldframe)固定在一个位置，相机坐标系(camera frame)位于相机中心，用
Figure FDA0004060274360000011
表示车体坐标系B到世界坐标系W的旋转矩阵，
Figure FDA0004060274360000012
表示车体坐标系B到世界坐标系W的平移向量，则
Figure FDA0004060274360000013
TWB表示从车体坐标系B到世界坐标系W之间的变换矩阵，用r表示车体在地面上的世界坐标(x,y)，Φ表示方向角，则有
Figure FDA0004060274360000014
Figure FDA0004060274360000015
其中，px、py表示取三维向量的x、y部分,
Figure FDA0004060274360000016
表示将矩阵变换为李代数，即旋转向量，
Figure FDA0004060274360000017
表示取旋转向量在z轴部分的分量；
路标在世界坐标系中的坐标l转换为相机坐标系下的坐标lC的公式如下：
Figure FDA0004060274360000021
其中，
Figure FDA0004060274360000022
为世界坐标系W到相机坐标系C的旋转矩阵，
Figure FDA0004060274360000023
为世界坐标系W到相机坐标系C的平移向量，l为路标在世界坐标系W中的坐标，lC为路标在相机坐标系C下的坐标；
通过外参标定，得到相机坐标系C到车体坐标系B之间的旋转矩阵
Figure FDA0004060274360000024
和平移向量
Figure FDA0004060274360000025
用于后续的优化过程。
3.根据权利要求2所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，在标定单目相机和车体之间的外参之前先将相机坐标系C到车体坐标系B之间的旋转矩阵
Figure FDA0004060274360000026
用对应的旋转四元数
Figure FDA0004060274360000027
描述：
Figure FDA0004060274360000028
其中，α为绕x轴转动的角度，β为绕y轴转动的角度，γ为绕z轴转动的角度，qx(α)表示角度α对应的四元数，qy(β)表示角度β对应的四元数，qz(γ)表示角度γ对应的四元数，
Figure FDA0004060274360000029
表示四元数乘法；标定单目相机和车体之间的外参包括以下3个步骤：
①构建关于旋转的误差项来求解
Figure FDA00040602743600000210
中的分量qyx：
Figure FDA00040602743600000211
Figure FDA00040602743600000212
其中，
Figure FDA00040602743600000213
表示从相机坐标系C到车体坐标系B的旋转四元数，q(.)表示该角度对应的四元数，
Figure FDA00040602743600000214
表示第i时刻到第i+1时刻轮式里程计测到的旋转四元数，
Figure FDA00040602743600000215
表示第i时刻到第i+1时刻相机测到的旋转四元数，
Figure FDA00040602743600000216
表示四元数乘法，ηi表示用轮式里程计测量旋转和用相机测量旋转之间的误差；
②构建误差，获取相机坐标系C到车体坐标系B的三维平移向量在x轴上的分量
Figure FDA00040602743600000217
在y轴上的分量
Figure FDA00040602743600000218
和三维旋转分解到绕z轴的旋转角γ：
Figure FDA0004060274360000031
其中，I3表示三维单位矩阵，
Figure FDA0004060274360000032
表示相机坐标系C到车体坐标系B的三维平移向量，
Figure FDA0004060274360000033
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，
Figure FDA0004060274360000034
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure FDA0004060274360000035
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，∈i表示用轮式里程计测量平移和用相机测量平移之间的误差；
Figure FDA0004060274360000036
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转矩阵增量表示成旋转矩阵的形式，由于
Figure FDA0004060274360000037
Figure FDA0004060274360000038
其中，
Figure FDA0004060274360000039
表示i时刻车体的方向角，
Figure FDA00040602743600000310
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，Rz(γ)表示
Figure FDA00040602743600000311
在z轴上的分量，R(qyx)表示
Figure FDA00040602743600000312
在x轴和y轴上的分量的乘积，将轮式里程计测量的平移和用相机测量的平移之间的误差记为∈i：
Figure FDA00040602743600000313
其中，
Figure FDA00040602743600000314
表示i时刻车体的方向角，
Figure FDA00040602743600000315
表示相机坐标系C到车体坐标系B的三维平移向量，Rz(γ)表示
Figure FDA00040602743600000316
在z轴上的分量，R(qyx)表示
Figure FDA00040602743600000317
在x轴和y轴上的分量的乘积，
Figure FDA00040602743600000318
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure FDA00040602743600000319
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量；
③构造最小二乘问题优化标定的结果：
Figure FDA00040602743600000320
Figure FDA0004060274360000041
Figure FDA0004060274360000042
其中，m代表试验次数，Ei表示平移部分的误差，Fi表示旋转部分的误差，Wi1、Wi2分别是与测量残差相关的协方差矩阵,
Figure FDA0004060274360000043
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转矩阵增量表示成旋转矩阵的形式，I3表示三维单位矩阵，
Figure FDA0004060274360000044
表示相机坐标系C到车体坐标系B的三维平移向量，
Figure FDA0004060274360000045
表示相机坐标系C到车体坐标系B的旋转四元数表示成旋转矩阵的形式，
Figure FDA0004060274360000046
表示相机在时刻i和时刻i+1之间测量到的平移向量增量，
Figure FDA0004060274360000047
表示相机在时刻i和时刻i+1之间测量到的旋转四元数增量，
Figure FDA0004060274360000048
表示轮式里程计在时刻i和时刻i+1之间测量到的平移向量增量，
Figure FDA0004060274360000049
表示轮式里程计在时刻i和时刻i+1之间测量到的旋转四元数增量，
Figure FDA00040602743600000410
表示四元数乘法，
Figure FDA00040602743600000411
表示最终优化后的平移向量，
Figure FDA00040602743600000412
表示最终优化后的旋转四元数。
4.根据权利要求3所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，所述步骤(2)中，对单目相机的图像进行去畸变后，利用图像特征提取，利用特征点的二进制描述子之间的汉明距离最小，找到对应的最优匹配特征点后，用对极约束的方法实现单目SLAM的初始化操作，选择初始化成功时的当前帧位姿作为世界坐标系的位姿。
5.根据权利要求4所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，轮式里程计在两个连续图像关键帧k，k+1之间的测量方程如下：
Figure FDA00040602743600000413
其中，
Figure FDA00040602743600000414
表示两个连续图像关键帧k、k+1之间车体姿态含噪声的增量测量值，
Figure FDA00040602743600000415
表示图像关键帧k、k+1之间车体姿态不含噪声的增量真实值，ηvk为噪声，服从正态分布N(0,∑vk)，∑vk为正态分布的方差，因为车体k时刻的姿态量
Figure FDA00040602743600000416
rk位置坐标，
Figure FDA00040602743600000417
为方向角，所以姿态量vk噪声项也可以表示成位置噪声ηrk和方向角噪声ηφk，即
Figure FDA0004060274360000051
Figure FDA0004060274360000052
由轮式里程计k时刻和k+1时刻内的增量测量去推算车体在k+1时刻的位置和方向角的方程如下：
Figure FDA0004060274360000053
其中，rk+1表示k+1时刻车体的世界坐标(x,y)，φk+1表示k+1时刻车体的方向角，rk表示k时刻车体的世界坐标(x,y)，φk表示k时刻车体的方向角，
Figure FDA0004060274360000054
表示k时刻到k+1时刻内轮式里程计测量到的世界坐标增量，
Figure FDA0004060274360000055
表示k时刻到k+1时刻内轮式里程计测量到的方向角增量，ηrk表示k时刻轮式里程计测量的位置噪声，ηφk表示k时刻轮式里程计测量的方向角噪声，同时Φ(φk)表示从k时刻车体的方向角φk得到的旋转矩阵，具体如下所示：
Figure FDA0004060274360000056
由于地面凹凸不平，车体打滑等原因，轮式里程计预测车体下一时刻的位置中的方向角φk是由IMU提供。
6.根据权利要求5所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，所述局部地图包含图像关键帧的信息和地图点的三维坐标信息。
7.根据权利要求6所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，所述步骤(6)的优化用马氏距离表示的最小代价函数F(X)：
F(X)＝∑ek(X)T∑kek(X)
X*＝argminF(X)
其中，用因子图描述优化问题，X表示图的节点，ek(X)表示图中第k条边，信息矩阵∑k作为权重因子,信息矩阵是协方差矩阵的逆矩阵，经过优化后，得到一个优化后的位姿X*，用X*去更新车体位姿。
8.根据权利要求7所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，单目相机的观测方程如下：
Figure FDA0004060274360000061
其中，u(vi,lω)表示车体在位姿为vi时，相机观测路标lω得到的观测量，lω为路标在世界坐标系下的坐标，车体位姿中的旋转矩阵
Figure FDA0004060274360000062
平移向量
Figure FDA0004060274360000063
Figure FDA0004060274360000064
表示i时刻车体的世界坐标(x,y)，
Figure FDA0004060274360000065
表示i时刻车体的方向角，
Figure FDA0004060274360000066
表示从车体到相机刚体变换的旋转矩阵，ηu为相机的观测噪声，服从正态分布
Figure FDA0004060274360000067
Figure FDA0004060274360000068
表示方差，I2表示二维单位矩阵，简记为
Figure FDA0004060274360000069
π(.)表示相机的投影方程，如下：
Figure FDA00040602743600000610
其中，l表示路标在相机坐标系下的坐标，lz表示路标在相机坐标系下的坐标中z轴上的分量，fx、fy、cx、cy均为相机内部参数。
9.根据权利要求8所述的一种用轮式里程计-IMU和单目相机实现定位的方法，其特征在于，所述回环检测包括以下步骤：
1)当图像检测到的特征点少于一个阈值或超过固定的时间间隔时会生成关键帧，否则不会生成关键帧；
2)通过搜索预先训练得到的字典得到当前关键帧的词袋向量，然后从与当前关键帧有共视关系的所有关键帧中找到与当前关键帧的词袋向量相似度最低的值，记为minS；
3)从关键帧数据库中除去与当前帧有共视关系的关键帧，最后遍历检索关键帧数据库，在遍历过程中，与当前关键帧词袋向量相似度小于0.8*minS的关键帧判定为非回环帧，直接跳过，而相似度大于一定阈值的关键帧则标记为回环候选帧，这些标记为回环候选帧中的相似度得分最高的关键帧最后被判定为回环帧，两个词袋向量vA和vB之间的相似度如下：
Figure FDA0004060274360000071
其中,S(vA,vB)表示向量vA和向量vB之间的相似度，向量vA和vB都是N维向量，vAi是向量vA中的第i维的值，vBi是向量vB中的第i维的值，|·|1表示取L1范数。
CN202011641098.0A 2020-12-31 2020-12-31 一种用轮式里程计-imu和单目相机实现定位的方法 Active CN112734841B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011641098.0A CN112734841B (zh) 2020-12-31 2020-12-31 一种用轮式里程计-imu和单目相机实现定位的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011641098.0A CN112734841B (zh) 2020-12-31 2020-12-31 一种用轮式里程计-imu和单目相机实现定位的方法

Publications (2)

Publication Number Publication Date
CN112734841A CN112734841A (zh) 2021-04-30
CN112734841B true CN112734841B (zh) 2023-04-28

Family

ID=75609062

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011641098.0A Active CN112734841B (zh) 2020-12-31 2020-12-31 一种用轮式里程计-imu和单目相机实现定位的方法

Country Status (1)

Country Link
CN (1) CN112734841B (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113516692A (zh) * 2021-05-18 2021-10-19 上海汽车集团股份有限公司 一种多传感器融合的slam方法和装置
CN114018284B (zh) * 2021-10-13 2024-01-23 上海师范大学 一种基于视觉的轮速里程计校正方法
CN114022561A (zh) * 2021-10-18 2022-02-08 武汉中海庭数据技术有限公司 一种基于gps约束和动态校正的城区单目测图方法和系统
CN114549656A (zh) * 2022-02-14 2022-05-27 希姆通信息技术（上海）有限公司 Ar眼镜相机与imu的标定方法
US20230271555A1 (en) * 2022-02-25 2023-08-31 Gentex Corporation Vehicle rearview display system with orientation sensing
CN115435775A (zh) * 2022-09-23 2022-12-06 福州大学 基于拓展卡尔曼滤波的多传感器融合slam方法
CN116592897B (zh) * 2023-07-17 2023-09-22 河海大学 基于位姿不确定性的改进orb-slam2定位方法
CN117288115A (zh) * 2023-11-23 2023-12-26 中信重工开诚智能装备有限公司 一种基于激光点云的巡检机器人巷道形变检测方法

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112150547A (zh) * 2019-06-28 2020-12-29 北京初速度科技有限公司 一种确定车体位姿的方法、装置及环视视觉里程计系统

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109671119A (zh) * 2018-11-07 2019-04-23 中国科学院光电研究院 一种基于slam的室内定位方法及装置
CN109887087B (zh) * 2019-02-22 2021-02-19 广州小鹏汽车科技有限公司 一种车辆的slam建图方法及系统
CN110631586A (zh) * 2019-09-26 2019-12-31 珠海市一微半导体有限公司 基于视觉slam的地图构建的方法、导航系统及装置
CN111415375B (zh) * 2020-02-29 2023-03-21 华南理工大学 一种基于多鱼眼摄像机和双针孔投影模型的slam方法
CN111750864B (zh) * 2020-06-30 2022-05-13 杭州海康机器人技术有限公司 一种基于视觉地图的重定位方法和装置
CN111928861B (zh) * 2020-08-07 2022-08-09 杭州海康威视数字技术股份有限公司 地图构建方法及装置

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112150547A (zh) * 2019-06-28 2020-12-29 北京初速度科技有限公司 一种确定车体位姿的方法、装置及环视视觉里程计系统

Also Published As

Publication number Publication date
CN112734841A (zh) 2021-04-30

Similar Documents

Publication Publication Date Title
CN112734841B (zh) 一种用轮式里程计-imu和单目相机实现定位的方法
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
CN111739063B (zh) 一种基于多传感器融合的电力巡检机器人定位方法
CN111156984B (zh) 一种面向动态场景的单目视觉惯性slam方法
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN109631855B (zh) 基于orb-slam的高精度车辆定位方法
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
CN111595333A (zh) 视觉惯性激光数据融合的模块化无人车定位方法及系统
CN111462135A (zh) 基于视觉slam与二维语义分割的语义建图方法
CN107193279A (zh) 基于单目视觉和imu信息的机器人定位与地图构建系统
CN108051002A (zh) 基于惯性测量辅助视觉的运输车空间定位方法及系统
CN111862673B (zh) 基于顶视图的停车场车辆自定位及地图构建方法
Yin et al. Dynam-SLAM: An accurate, robust stereo visual-inertial SLAM method in dynamic environments
CN112649016A (zh) 一种基于点线初始化的视觉惯性里程计方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN112767546B (zh) 移动机器人基于双目图像的视觉地图生成方法
CN112101160B (zh) 一种面向自动驾驶场景的双目语义slam方法
CN114234967B (zh) 一种基于多传感器融合的六足机器人定位方法
CN106574836A (zh) 用于在定位平面中定位机器人的方法
CN111998862A (zh) 一种基于bnn的稠密双目slam方法
CN111307146A (zh) 一种基于双目相机和imu的虚拟现实头戴显示设备定位系统
Zhang LILO: A novel LiDAR–IMU SLAM system with loop optimization

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