CN114018284B - 一种基于视觉的轮速里程计校正方法 - Google Patents
一种基于视觉的轮速里程计校正方法 Download PDFInfo
- Publication number
- CN114018284B CN114018284B CN202111191236.4A CN202111191236A CN114018284B CN 114018284 B CN114018284 B CN 114018284B CN 202111191236 A CN202111191236 A CN 202111191236A CN 114018284 B CN114018284 B CN 114018284B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- wheel speed
- camera
- pose
- matrix
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000012937 correction Methods 0.000 title claims abstract description 8
- 230000000007 visual effect Effects 0.000 claims abstract description 16
- 238000004364 calculation method Methods 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 111
- 238000013519 translation Methods 0.000 claims description 28
- 238000000354 decomposition reaction Methods 0.000 claims description 20
- 238000012545 processing Methods 0.000 claims description 15
- 230000003416 augmentation Effects 0.000 claims description 7
- 230000014509 gene expression Effects 0.000 claims description 4
- 230000000903 blocking effect Effects 0.000 claims description 3
- 230000006835 compression Effects 0.000 claims description 3
- 238000007906 compression Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 3
- 238000003672 processing method Methods 0.000 claims description 3
- 230000003190 augmentative effect Effects 0.000 claims description 2
- 230000004069 differentiation Effects 0.000 claims description 2
- 238000009825 accumulation Methods 0.000 abstract 1
- 238000005516 engineering process Methods 0.000 description 6
- 230000004927 fusion Effects 0.000 description 6
- 238000001914 filtration Methods 0.000 description 2
- 238000006467 substitution reaction Methods 0.000 description 2
- 101100248200 Arabidopsis thaliana RGGB gene Proteins 0.000 description 1
- 241000270666 Testudines Species 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000007499 fusion processing Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- 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/20—Instruments for performing navigational calculations
-
- 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)
- Manufacturing & Machinery (AREA)
- Image Processing (AREA)
Abstract
本发明涉及一种基于视觉的轮速里程计校正方法，该方法包括以下步骤：步骤1：获取系统的内参数和外参数；步骤2：获取并同步视觉信息和轮速里程计信息；步骤3：基于MSCKF算法对视觉信息和轮速里程计信息进行融合，即根据轮速里程计信息进行状态的预测并根据视觉信息进行状态的更新，以校正轮速里程计；步骤4：判断是否执行完所有的特征点更新；步骤5：若是，则得到当前局部地图位姿，进而得到系统全局定位，若否，则返回步骤2，与现有技术相比，本发明具有校正轮速里程计的时间累积误差、提高定位精度和计算效率高等优点。
Description
技术领域
本发明涉及多传感器融合技术，尤其是涉及一种基于视觉的轮速里程计校正方法。
背景技术
随着扫地机器人、自动餐车等移动机器人的兴起，人们对移动机器人的运用越来越广。而对移动机器人运动过程中的定位和导航是其完成各种任务的基础。移动机器人的定位主要依赖于各种传感器感知周围环境信息，从而分析获得其与周围环境中各物体的位置关系，进而实现实时的自我定位。但单一传感器物理条件上存在局限性，在实际应用中倘若某一传感器发生错误或误差，SLAM系统则可能出现定位偏差等现象，多传感器信息融合技术可以有效地解决以上问题。
多传感器融合技术可以将多个传感器的定位信息进行有效的数据融合，综合优化，处理得到所需的估计与决策的数据，以增强数据的可靠性和利用性。随着近年来无人驾驶，无人机和移动机器人领域的不断发展，多传感器融合技术也得到迅速的发展。目前得到广泛应用的多传感器信息融合算法有，卡尔曼滤波算法，扩展卡尔曼滤波算法，经典推理法，神经网络算法等。
基于单一传感器如轮速里程计的SLAM技术已经能够有效地提取周围环境信息，完成机器人的定位需求。因此该技术在Turtle Bot等机器人中取得了令人满意的实验结果。尽管如此，该技术目前还存在以下缺陷：
第一，由于差速轮速里程计与真实位姿存在差距，定位误差随时间累积，在大场景长时间的环境中机器人不能获得可靠定位，需要进行人工校正。
第二，基于图像的视觉里程计同样能满足简单的定位需求，但相较于轮速里程计实时性差、高动态环境下同样易出现误差。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种基于视觉的轮速里程计校正方法。
本发明的目的可以通过以下技术方案来实现：
一种基于视觉的轮速里程计校正方法，该方法包括以下步骤：
步骤1：获取系统的内参数和外参数；
步骤2：获取并同步视觉信息和轮速里程计信息；
步骤3：基于MSCKF算法对视觉信息和轮速里程计信息进行融合，即根据轮速里程计信息进行状态的预测并根据视觉信息进行状态的更新，以校正轮速里程计；
步骤4：判断是否执行完所有的特征点更新；
步骤5：若是，则得到当前局部地图位姿，进而得到系统全局定位，若否，则返回步骤2。
所述的步骤1中，系统的内参数包括相机内参、左右轮轴距b、左轮速系数kl和右轮速系数kr，系统的外参数包括轮速坐标系到相机坐标系的旋转矩阵和轮速坐标系到相机坐标系的平移矩阵CpO。
所述的步骤3中，具体包括以下步骤：
步骤31：轮速里程计预测；
步骤32：状态增广；
步骤33：轮速里程计更新。
所述的步骤31中，轮速里程计预测的过程具体包括以下步骤：
步骤311：初始化状态向量和协方差，将滑动窗口的状态分为里程计位姿和相机位姿，总状态为当前里程计位姿增广上N帧的相机位姿，里程计位姿和相机位姿的表达式分别为：
其中，R为旋转矩阵，G为全局坐标系，O为轮速坐标系，C为相机坐标系，为轮速坐标系到全局坐标系的里程计位姿，/>为相机坐标系到全局坐标系的相机位姿，/>为轮速坐标系到地面坐标系的旋转矩阵，GpO为轮速坐标系到全局坐标系的平移矩阵，GpC为相机坐标系到地面坐标系的平移矩阵，SE3为欧式群；
步骤312：根据轮速里程计信息进行状态的预测，基于IMU处理方法对轮速里程计信息进行处理并建立常微分方程：
其中，OωO为相对轮速坐标系的瞬时角速度，OvO为相对轮速坐标系的瞬时速度，[OωO]×为瞬时角速度的反对称矩阵，为轮速坐标系到地面坐标系的旋转矩阵，GvO为轮速坐标系到全局坐标系下的瞬时速度，/>为旋转矩阵的微分，/>为平移矩阵的微分，kl和kr分别为左轮速系数和右轮速系数，b为左右轮轴距，vl和vr分别为左轮的速度和右轮的速度；
步骤313：采用欧拉积分对步骤312得到的常微分方程进行积分，以进行均值的预测：
其中，为k+1时刻轮速坐标系到地面坐标系的旋转矩阵，GPOk+1为k+1时刻轮速坐标系到全局坐标系的平移矩阵，/>为k时刻轮速坐标系到地面坐标系的旋转矩阵，GPOk为k时刻轮速坐标系到全局坐标系的平移矩阵，Δt为k时刻与k+1时刻之间的时间差，{ΔR，Δp}为位姿增量；
步骤314：对于协方差进行预测，定义旋转的误差为：
其中，δθ为旋转误差量，为轮速坐标系到地面坐标系的旋转矩阵的估计值；
步骤315：得到相对于里程计位姿的雅克比和相对于位姿增量{ΔR，Δp}的雅克比分别为：
其中，Φ为相对于里程计位姿的雅克比，F为相对于位姿增量的雅克比；
步骤316：根据雅克比对协方差进行预测，协方差矩阵的预测公式为：
其中，Q为位姿增量{ΔR，Δp}的噪声，根据位姿增量的大小进行设置，为里程计位姿/>对应的协方差，维度为6×6，/>为N帧相机位姿对应的协方差矩阵，维度为6N×6N，/>为里程计位姿与N帧相机位姿之间的关联协方差。
所述的步骤311中，总状态的状态向量为：
其中，χ为状态向量，N为相机的帧数，为相机坐标系到全局坐标系的第N帧相机位姿；
协方差的分块矩阵为：
其中，为里程计位姿/>对应的协方差，维度为6×6，/>为N帧相机位姿对应的协方差，维度为6N×6N，/>为里程计位姿与N帧相机位姿之间的关联协方差。
所述的步骤32中，状态增广的过程具体包括以下步骤：
步骤321：当新产生一帧图像时，通过里程计位姿计算得到相机位姿：
其中，为相机坐标系到全局坐标系的旋转矩阵，/>为轮速坐标系到全局坐标系的旋转矩阵，/>为相机坐标系到轮速坐标系的旋转矩阵，GpC为相机坐标系到全局坐标系的平移矩阵，GpO为轮速坐标系到全局坐标系的平移矩阵，OpC为相机坐标系到轮速坐标系的平移矩阵；
步骤322：将相机位姿放到原状态向量χ里得到增广状态向量，并对协方差矩阵进行扩展：
其中，J为协方差矩阵相对于原状态向量χ的雅克比，J的第一列和第二列为相对于里程计位姿的雅克比，/>为轮速坐标系到相机坐标系的旋转矩阵，[OpC]×为轮速坐标系到相机坐标系的平移矩阵的反对称矩阵。
所述的步骤33中，轮速里程计更新的过程包括对一个特征点的处理、对多个特征点的处理以及边缘化处理。
所述的对一个特征点的处理过程具体包括以下步骤：
步骤331：根据特征点对多个历史相机状态的约束更新状态向量，每个用以进行更新的特征点Gpf均被滑窗内的M帧相机观测到，特征点Gpf在其中一帧图像上的投影方程为：
其中，π为相机的投影函数，为相机坐标系到全局坐标系的旋转矩阵的转置，Zi为特征点Gpf在相机第i帧图像上的投影，/>为第i帧图像的特征点，i为相机图像的帧数，/>为通过里程计位姿计算得到相机位姿；
步骤332：将特征点在其中一帧图像上的投影方程线性化：
其中，ri为残差，表示特征点在相机第i帧图像上的投影误差，为残差ri对于第i帧状态向量χi的雅克比，/>为残差ri对第i帧图像的特征点/>的雅克比，δχi为第i帧状态向量χi的差值，/>为第i帧图像的特征点/>的差值；
步骤333：将特征点的3D位置恢复，一个特征点具有M帧相机的观测，将M帧相机的观测堆叠在一起得到一个线性化方程：
r＝Hχδχ+HfδGpf
其中，Hχ为M帧相机的观测堆叠后的残差r对整个状态向量χ的雅克比，Hf为M帧相机的观测堆叠后的残差r对特征点Gpf的雅克比；
步骤334：在线性化方程左右两边同时乘以一个满足条件ATHf＝0的矩阵AT，以消除线性化方程中关于特征点的部分：
ATr＝ATHχδχ+ATHfδGpf
ATHf＝0
ATr＝ATHχδχ
其中，AT为Hf的左零空间，δχ为状态向量χ的差值，用以消除线性化方程中关于特征点的部分；
步骤335：求解矩阵A，即对Hf进行QR分解，再对Hf左乘得到一个不含特征点的线性方程/>
A＝Q2
其中，Q2与Q1为对Hf进行QR分解后的标准正交矩阵，Q2与Q1正交，R1为对Hf进行QR分解后的上三角矩阵，为Hf左乘AT后的矩阵，/>为不含特征点的线性方程，/>和/>分别表示残差r和雅克比Hf在左零空间上的投影。
所述的对多个特征点的处理过程具体包括以下步骤：
步骤336：一个特征点对应一个线性方程，将所有特征点对应的线性方程堆叠起来得到一个总线性方程：
r*＝H*δχ
其中，r*为所有特征点堆叠后的残差，H*为残差r*对所有特征点的雅克比；
步骤337：对总线性方程进行QR分解，进行一次压缩，具体是对H*进行QR分解，并将分解后的H*代入到总线性方程中：
R2＝TH
其中，Q3与Q4为对H*进行QR分解后的标准正交矩阵，Q3与Q4正交，R1为对H*进行QR分解后的上三角矩阵，TH表示行数为H的上三角矩阵；
步骤339：总线性方程的左右两边同时乘以[Q3 Q4]T，最后得到一个压缩后的总线性方程，压缩后的总线性方程的行数最大值与状态的维度相同，用以进行EKF更新：
其中，rn为压缩后的总线性方程。
所述的边缘化处理具体为：
将滑动窗口的状态中最旧的一帧状态及其对应的协方差去掉，去掉的这帧里的所有特征点均用以进行更新。
与现有技术相比，本发明具有以下优点：
第一，本方法使用MSCKF(多状态约束下的卡尔曼滤波器)算法，在特征层将视觉信息与轮速信息进行融合，能够有效校正轮速里程计的时间累积误差，最终达到提高定位精度的效果，并且具有鲁棒性好的特点。
第二，本方法的定位准确率优于在KAIST URBAN DATA SET数据集上的主流算法，并且使用Givens Rotation(吉文斯旋转)方法求解，具有较高的计算效率，以达到较高的实时性。
附图说明
图1为本发明的流程示意图。
图2为MSCKF信息融合过程的示意图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。
实施例
如图2所示，本发明提供了一种基于视觉的轮速里程计校正方法，该方法包括以下步骤：
步骤1：获取系统的内参数和外参数；
步骤2：获取并同步视觉信息和轮速里程计信息；
步骤3：基于MSCKF(多状态约束下的卡尔曼滤波器)算法对视觉信息和轮速里程计信息进行融合，即根据轮速里程计信息进行状态的预测并根据视觉信息进行状态的更新，以校正轮速里程计；
步骤4：判断是否执行完所有的特征点更新；
步骤5：若是，则得到当前局部地图位姿，进而得到系统全局定位，若否，则返回步骤2。
在步骤3中，对视觉信息和轮速里程计信息进行融合的过程具体包括以下步骤：
步骤301：轮速里程计预测：将滑动窗口(Sliding Windows Filter，SWF)的状态分为里程计位姿和相机位姿，里程计位姿的表达式为：
其中，R为旋转矩阵，G为全局坐标系，O为轮速坐标系，C为相机坐标系，为轮速坐标系到全局坐标系的里程计位姿，/>为轮速坐标系到地面坐标系的旋转矩阵，GpO为轮速坐标系到全局坐标系的平移矩阵，SE3为特殊欧式群(Special Euclidean Group)。
相机位姿的表达式为：
其中，为相机坐标系到全局坐标系的相机位姿，GpC为相机坐标系到地面坐标系的平移矩阵；
总状态为当前里程计位姿增广上N帧的相机位姿，状态向量为：
其中，χ为状态向量，N为状态向量里增广相机位姿的帧数，为相机坐标系到全局坐标系的第N帧相机位姿；
协方差的分块矩阵为：
其中，为里程计位姿/>的协方差，维度为6×6，/>为对应N帧相机位姿的协方差矩阵，维度为6N×6N，/>为里程计位姿与N帧相机位姿之间的关联协方差；
根据轮速里程计信息进行状态的预测并根据视觉信息进行状态的更新，首先根据轮速里程计信息进行状态的预测，基于IMU处理方法对轮速里程计信息进行处理并建立常微分方程：
其中，OωO为相对轮速坐标系的顺时角速度，类似于IMU gyrosope的测量，是个3D量，但实际上轮速计只能测到2D的旋转，只有能测到绕z轴的角速度，OvO为瞬时速度，同样是个3D量，但轮速也只能测到沿x轴方向的速度，kl和kr分别为左轮的速度和右轮的速度，[OωO]×为瞬时角速度的反对称矩阵，为轮速坐标系到全局坐标系下的瞬时速度，vl和vr分别为左轮的速度和右轮的速度；
采用欧拉积分对常微分方程进行积分，以进行均值的预测：
其中，为k+1时刻轮速坐标系到地面坐标系的旋转矩阵，GPOk+1为k+1时刻轮速坐标系到全局坐标系的平移矩阵，/>为k时刻轮速坐标系到地面坐标系的旋转矩阵，GPOk为k时刻轮速坐标系到全局坐标系的平移矩阵，Δt为k时刻与k+1时刻之间的时间差，{ΔR，Δp}为位姿增量；
对于协方差的预测，先求雅克比，首先定义旋转矩阵的误差为：
其中，δθ为旋转误差量，为轮速坐标系到地面坐标系的旋转矩阵的估计值；那么可以求得，相对于里程计位姿/>的雅克比为：
其中，Φ为相对于里程计位姿的雅克比，OωO为相对轮速坐标系的顺时角速度，OvO为瞬时速度；
相对于位姿增量{ΔR，Δp}的雅克比为：
其中，F为相对于位姿增量的雅克比；
得到协方差矩阵的预测公式：
其中，Q为位姿增量{ΔR，Δp}的噪声，根据位姿增量的大小进行设置，为里程计位姿/>的协方差，维度为6×6，/>为对应N帧相机位姿的协方差矩阵，维度为6N×6N，/>为里程计位姿与N帧相机位姿之间的关联协方差。
步骤302：进行状态增广：当新来一帧图像时，通过里程计位姿计算得到相机位姿：
其中，为相机坐标系到全局坐标系的旋转矩阵，/>为轮速坐标系到全局坐标系的旋转矩阵，/>为相机坐标系到轮速坐标系的旋转矩阵，GpC为相机坐标系到全局坐标系的平移矩阵，GpO为轮速坐标系到全局坐标系的平移矩阵，OpC为相机坐标系到轮速坐标系的平移矩阵；
将相机位姿放到原状态向量里得到增广之后的状态向量，并对协方差矩阵进行扩展：
其中，J为协方差矩阵相对于原状态向量χ(增广之前的状态向量)的雅克比，χ为原状态向量，J的第一列和第二列为相对于里程计位姿的雅克比，/>为轮速坐标系到相机坐标系的旋转矩阵，[OpC]×为轮速坐标系到相机坐标系的平移矩阵的反对称矩阵。
步骤303：对轮速里程计进行更新：
步骤A：一个特征点的处理：
每个用以做更新的特征点Gpf会被滑窗内的M帧相机观测到，特征点Gpf在其中一帧图像上的投影方程为：
其中，π为相机的投影函数，为相机坐标系到全局坐标系的旋转矩阵的转置，Zi为特征点/>在相机第i帧图像上的投影，/>为第i帧图像的特征点，i为相机图像的帧数，/>为通过里程计位姿计算得到相机位姿；
将投影方程线性化：
其中，ri为残差，表示特征点在相机第i帧图像上的投影误差，为残差ri对于第i帧状态向量χi的雅克比，/>为残差ri对第i帧图像的特征点/>的雅克比，δχi为第i帧状态向量χi的差值，/>为第i帧图像的特征点/>的差值；
计算对整个状态向量χ的雅克比需要获得特征点/>所以需要将特征点的3D位置恢复，因为一个特征点具有M帧相机的观测，所以将M帧相机的观测堆叠在一起得到一个线性化方程：
r＝Hχδχ+HfδGpf
但是线性化方程里有特征点Gpf，而状态里面没有特征点Gpf，不能直接用以进行EKF更新，通过在线性化方程左右两边同时乘以一个满足ATHf＝0的矩阵AT将HfδGpf消掉：
ATr＝ATHχδχ+ATHfδGpf
ATHf＝0
ATr＝ATHχδχ
其中，AT为Hf的左零空间，用以消除线性化方程中关于特征点的部分。
进而求解矩阵A，即对Hf进行QR分解，对Hf左乘Q2与Q1正交，得到一个不含特征点的线性方程/>
A＝Q2
其中，Q2与Q1为对Hf进行QR分解后的标准正交矩阵，Q2与Q1正交，R1为对Hf进行QR分解后的上三角矩阵，为Hf左乘AT后的矩阵，/>为不含特征点的线性方程，/>和/>分别表示残差r和雅克比Hf在左零空间上的投影。
总线性方程的行数很大，直接用来做EKF更新效率很低，因此对总线性方程进行QR分解，进行一次压缩，具体是对H*做QR分解：
带入到总线性方程中得到：
R2＝TH
总线性方程的左右两边同时乘以[Q1 Q2]T得到：
最后得到一个压缩后的总线性方程：
其中，Q3与Q4为对H*进行QR分解后的标准正交矩阵，Q3与Q4正交，R1为对H*进行QR分解后的上三角矩阵，TH表示行数为H的上三角矩阵。
压缩后的总线性方程的行数最大值能够与状态向量的维度相同，最终压缩后的总线性方程用以进行EKF更新。
步骤C：边缘化处理
进行边缘化操作，即如何删除滑窗里的状态，将最旧的一帧状态及其对应的协方差去掉，去掉的这一帧里的所有特征点都被用以进行更新操作。
为了验证本发明的性能，本实施例使用FLIR公司的FL3-U3-20E4C-C款全局快门彩色双目相机，相机频率为10Hz，相机分辨率为1289x 560，双目相机图像以10Hz的频率采集，并以无损PNG格式存储在未校正的8位拜耳阵列图像中，图像的拜耳阵列为RGGB，轮速编码器为RLS公司的LM13款磁性旋转编码器，编码器频率为100Hz，编码器精度为4096PPR，基于上述传感器数据，本发明比纯轮速里程计精度更好。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的工作人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
Claims (2)
1.一种基于视觉的轮速里程计校正方法，其特征在于，该方法包括以下步骤：
步骤1：获取系统的内参数和外参数；
步骤2：获取并同步视觉信息和轮速里程计信息；
步骤3：基于MSCKF算法对视觉信息和轮速里程计信息进行融合，即根据轮速里程计信息进行状态的预测并根据视觉信息进行状态的更新，以校正轮速里程计；
步骤4：判断是否执行完所有的特征点更新；
步骤5：若是，则得到当前局部地图位姿，进而得到系统全局定位，若否，则返回步骤2；
所述的步骤3中，具体包括以下步骤：
步骤31：轮速里程计预测；
步骤32：状态增广；
步骤33：轮速里程计更新；
所述的步骤31中，轮速里程计预测的过程具体包括以下步骤：
步骤311：初始化状态向量和协方差，将滑动窗口的状态分为里程计位姿和相机位姿，总状态为当前里程计位姿增广上N帧的相机位姿，里程计位姿和相机位姿的表达式分别为：
其中，R为旋转矩阵，G为全局坐标系，O为轮速坐标系，C为相机坐标系，为轮速坐标系到全局坐标系的里程计位姿，/>为相机坐标系到全局坐标系的相机位姿，/>为轮速坐标系到地面坐标系的旋转矩阵，GpO为轮速坐标系到全局坐标系的平移矩阵，GpC为相机坐标系到地面坐标系的平移矩阵，SE3为欧式群；
步骤312：根据轮速里程计信息进行状态的预测，基于IMU处理方法对轮速里程计信息进行处理并建立常微分方程：
其中，OωO为相对轮速坐标系的瞬时角速度，OvO为相对轮速坐标系的瞬时速度，[OωO]×为瞬时角速度的反对称矩阵，为轮速坐标系到地面坐标系的旋转矩阵，GvO为轮速坐标系到全局坐标系下的瞬时速度，/>为旋转矩阵的微分，/>为平移矩阵的微分，kl和kr分别为左轮速系数和右轮速系数，b为左右轮轴距，vl和vr分别为左轮的速度和右轮的速度；
步骤313：采用欧拉积分对步骤312得到的常微分方程进行积分，以进行均值的预测：
其中，为k+1时刻轮速坐标系到地面坐标系的旋转矩阵，GPOk+1为k+1时刻轮速坐标系到全局坐标系的平移矩阵，/>为k时刻轮速坐标系到地面坐标系的旋转矩阵，GPOk为k时刻轮速坐标系到全局坐标系的平移矩阵，Δt为k时刻与k+1时刻之间的时间差，{ΔR，Δp}为位姿增量；
步骤314：对于协方差进行预测，定义旋转的误差为：
其中，δθ为旋转误差量，为轮速坐标系到地面坐标系的旋转矩阵的估计值；
步骤315：得到相对于里程计位姿的雅克比和相对于位姿增量{ΔR，Δp}的雅克比分别为：
其中，Φ为相对于里程计位姿的雅克比，F为相对于位姿增量的雅克比；
步骤316：根据雅克比对协方差进行预测，协方差矩阵的预测公式为：
其中，Q为位姿增量{ΔR，Δp}的噪声，根据位姿增量的大小进行设置，为里程计位姿/>对应的协方差，维度为6×6，/>为N帧相机位姿对应的协方差矩阵，维度为6N×6N，/>为里程计位姿与N帧相机位姿之间的关联协方差；
所述的步骤311中，总状态的状态向量为：
其中，χ为状态向量，N为相机的帧数，为相机坐标系到全局坐标系的第N帧相机位姿；
协方差的分块矩阵为：
其中，为里程计位姿/>对应的协方差，维度为6×6，/>为N帧相机位姿/>对应的协方差，维度为6N×6N，/>为里程计位姿与N帧相机位姿之间的关联协方差；
所述的步骤32中，状态增广的过程具体包括以下步骤：
步骤321：当新产生一帧图像时，通过里程计位姿计算得到相机位姿：
其中，为相机坐标系到全局坐标系的旋转矩阵，/>为轮速坐标系到全局坐标系的旋转矩阵，/>为相机坐标系到轮速坐标系的旋转矩阵，GpC为相机坐标系到全局坐标系的平移矩阵，GpO为轮速坐标系到全局坐标系的平移矩阵，OpC为相机坐标系到轮速坐标系的平移矩阵；
步骤322：将相机位姿放到原状态向量χ里得到增广状态向量，并对协方差矩阵进行扩展：
其中，J为协方差矩阵相对于原状态向量χ的雅克比，J的第一列和第二列为相对于里程计位姿的雅克比，/>为轮速坐标系到相机坐标系的旋转矩阵，[OpC]×为轮速坐标系到相机坐标系的平移矩阵的反对称矩阵；
所述的步骤33中，轮速里程计更新的过程包括对一个特征点的处理、对多个特征点的处理以及边缘化处理；
所述的对一个特征点的处理过程具体包括以下步骤：
步骤331：根据特征点对多个历史相机状态的约束更新状态向量，每个用以进行更新的特征点Gpf均被滑窗内的M帧相机观测到，特征点Gpf在其中一帧图像上的投影方程为：
其中，π为相机的投影函数，为相机坐标系到全局坐标系的旋转矩阵的转置，Zi为特征点Gpf在相机第i帧图像上的投影，/>为第i帧图像的特征点，i为相机图像的帧数，为通过里程计位姿计算得到相机位姿；
步骤332：将特征点在其中一帧图像上的投影方程线性化：
其中，ri为残差，表示特征点在相机第i帧图像上的投影误差，为残差ri对于第i帧状态向量χi的雅克比，/>为残差ri对第i帧图像的特征点/>的雅克比，δχi为第i帧状态向量χi的差值，/>为第i帧图像的特征点/>的差值；
步骤333：将特征点的3D位置恢复，一个特征点具有M帧相机的观测，将M帧相机的观测堆叠在一起得到一个线性化方程：
r＝Hχδχ+HfδGpf
其中，Hχ为M帧相机的观测堆叠后的残差r对整个状态向量χ的雅克比，Hf为M帧相机的观测堆叠后的残差r对特征点Gpf的雅克比；
步骤334：在线性化方程左右两边同时乘以一个满足条件ATHf＝0的矩阵AT，以消除线性化方程中关于特征点的部分：
ATr＝ATHχδχ+ATHfδGpf
ATHf＝0
ATr＝ATHχδχ
其中，AT为Hf的左零空间，δχ为状态向量χ的差值，用以消除线性化方程中关于特征点的部分；
步骤335：求解矩阵A，即对Hf进行QR分解，再对Hf左乘得到一个不含特征点的线性方程/>
A＝Q2
其中，Q2与Q1为对Hf进行QR分解后的标准正交矩阵，Q2与Q1正交，R1为对Hf进行QR分解后的上三角矩阵，为Hf左乘AT后的矩阵，/>为不含特征点的线性方程，/>和/>分别表示残差r和雅克比Hf在左零空间上的投影；
所述的对多个特征点的处理过程具体包括以下步骤：
步骤336：一个特征点对应一个线性方程，将所有特征点对应的线性方程堆叠起来得到一个总线性方程：
r*＝H*δχ
其中，r*为所有特征点堆叠后的残差，H*为残差r*对所有特征点的雅克比；
步骤337：对总线性方程进行QR分解，进行一次压缩，具体是对H*进行QR分解，并将分解后的H*代入到总线性方程中：
R2＝TH
其中，Q3与Q4为对H*进行QR分解后的标准正交矩阵，Q3与Q4正交，R1为对H*进行QR分解后的上三角矩阵，TH表示行数为H的上三角矩阵；
步骤339：总线性方程的左右两边同时乘以[Q3 Q4]T，最后得到一个压缩后的总线性方程，压缩后的总线性方程的行数最大值与状态的维度相同，用以进行EKF更新：
其中，rn为压缩后的总线性方程；
所述的边缘化处理具体为：
将滑动窗口的状态中最旧的一帧状态及其对应的协方差去掉，去掉的这帧里的所有特征点均用以进行更新。
2.根据权利要求1所述的一种基于视觉的轮速里程计校正方法，其特征在于，所述的步骤1中，系统的内参数包括相机内参、左右轮轴距b、左轮速系数kl和右轮速系数kr，系统的外参数包括轮速坐标系到相机坐标系的旋转矩阵和轮速坐标系到相机坐标系的平移矩阵CpO。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111191236.4A CN114018284B (zh) | 2021-10-13 | 2021-10-13 | 一种基于视觉的轮速里程计校正方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111191236.4A CN114018284B (zh) | 2021-10-13 | 2021-10-13 | 一种基于视觉的轮速里程计校正方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114018284A CN114018284A (zh) | 2022-02-08 |
| CN114018284B true CN114018284B (zh) | 2024-01-23 |
Family
ID=80055862
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111191236.4A Active CN114018284B (zh) | 2021-10-13 | 2021-10-13 | 一种基于视觉的轮速里程计校正方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114018284B (zh) |
Citations (28)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104864889A (zh) * | 2015-05-29 | 2015-08-26 | 山东鲁能智能技术有限公司 | 一种基于视觉的机器人里程计校正系统及方法 |
| JP2017122960A (ja) * | 2016-01-05 | 2017-07-13 | マツダ株式会社 | 車両位置推定装置 |
| CN107942748A (zh) * | 2017-11-28 | 2018-04-20 | 上海师范大学 | 一种机械臂空间动态避障感应手环及控制系统 |
| KR20190040818A (ko) * | 2017-10-11 | 2019-04-19 | 재단법인대구경북과학기술원 | 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템 |
| CN109945858A (zh) * | 2019-03-20 | 2019-06-28 | 浙江零跑科技有限公司 | 用于低速泊车驾驶场景的多传感融合定位方法 |
| CN110070712A (zh) * | 2019-04-12 | 2019-07-30 | 同济大学 | 一种低速清扫车全局定位系统及方法 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| CN110411457A (zh) * | 2019-08-27 | 2019-11-05 | 纵目科技（上海）股份有限公司 | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 |
| CN110458885A (zh) * | 2019-08-27 | 2019-11-15 | 纵目科技（上海）股份有限公司 | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 |
| CN110986939A (zh) * | 2020-01-02 | 2020-04-10 | 东南大学 | 一种基于imu预积分的视觉惯性里程计方法 |
| KR20200107382A (ko) * | 2019-03-07 | 2020-09-16 | 에스케이텔레콤 주식회사 | 차량용 센서의 보정 정보 획득 장치 및 방법 |
| CN111707272A (zh) * | 2020-06-28 | 2020-09-25 | 湖南大学 | 一种地下车库自动驾驶激光定位系统 |
| CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN111811506A (zh) * | 2020-09-15 | 2020-10-23 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
| CN111986506A (zh) * | 2020-07-20 | 2020-11-24 | 苏州易航远智智能科技有限公司 | 基于多视觉系统的机械式停车位泊车方法 |
| CN112014113A (zh) * | 2019-05-29 | 2020-12-01 | 上海淞泓智能汽车科技有限公司 | 一种室内整车硬件在环试验装置 |
| CN112050809A (zh) * | 2020-10-08 | 2020-12-08 | 吉林大学 | 轮式里程计与陀螺仪信息融合的无人车定向定位方法 |
| CN112105961A (zh) * | 2019-07-26 | 2020-12-18 | 深圳市大疆创新科技有限公司 | 基于多数据融合的定位方法、可移动平台及存储介质 |
| CN112102406A (zh) * | 2020-09-09 | 2020-12-18 | 东软睿驰汽车技术（沈阳）有限公司 | 单目视觉的尺度修正方法、装置及运载工具 |
| CN112129297A (zh) * | 2020-09-25 | 2020-12-25 | 重庆大学 | 一种多传感器信息融合的自适应校正室内定位方法 |
| CN112506195A (zh) * | 2020-12-02 | 2021-03-16 | 吉林大学 | 基于视觉和底盘信息的车辆自主定位系统及定位方法 |
| CN112734841A (zh) * | 2020-12-31 | 2021-04-30 | 华南理工大学 | 一种用轮式里程计-imu和单目相机实现定位的方法 |
| CN112734852A (zh) * | 2021-03-31 | 2021-04-30 | 浙江欣奕华智能科技有限公司 | 一种机器人建图方法、装置及计算设备 |
| CN112985450A (zh) * | 2021-02-09 | 2021-06-18 | 东南大学 | 一种具有同步时间误差估计的双目视觉惯性里程计方法 |
| CN113223161A (zh) * | 2021-04-07 | 2021-08-06 | 武汉大学 | 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法 |
| CN113358112A (zh) * | 2021-06-03 | 2021-09-07 | 北京超星未来科技有限公司 | 一种地图构建方法及一种激光惯性里程计 |
| CN113483755A (zh) * | 2021-07-09 | 2021-10-08 | 北京易航远智科技有限公司 | 一种基于非全局一致地图的多传感器组合定位方法及系统 |
| CN115790617A (zh) * | 2022-10-29 | 2023-03-14 | 武汉中海庭数据技术有限公司 | 基于相机感知的车道边线修正方法及系统 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| AU2018326507A1 (en) * | 2017-08-29 | 2020-03-19 | Comet Electronics, LLC | High speed thermal imaging system and method |
-
2021
- 2021-10-13 CN CN202111191236.4A patent/CN114018284B/zh active Active
Patent Citations (28)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104864889A (zh) * | 2015-05-29 | 2015-08-26 | 山东鲁能智能技术有限公司 | 一种基于视觉的机器人里程计校正系统及方法 |
| JP2017122960A (ja) * | 2016-01-05 | 2017-07-13 | マツダ株式会社 | 車両位置推定装置 |
| KR20190040818A (ko) * | 2017-10-11 | 2019-04-19 | 재단법인대구경북과학기술원 | 차량 내부 센서, 카메라, 및 gnss 단말기를 이용한 3차원 차량 항법 시스템 |
| CN107942748A (zh) * | 2017-11-28 | 2018-04-20 | 上海师范大学 | 一种机械臂空间动态避障感应手环及控制系统 |
| KR20200107382A (ko) * | 2019-03-07 | 2020-09-16 | 에스케이텔레콤 주식회사 | 차량용 센서의 보정 정보 획득 장치 및 방법 |
| CN109945858A (zh) * | 2019-03-20 | 2019-06-28 | 浙江零跑科技有限公司 | 用于低速泊车驾驶场景的多传感融合定位方法 |
| CN110070712A (zh) * | 2019-04-12 | 2019-07-30 | 同济大学 | 一种低速清扫车全局定位系统及方法 |
| CN112014113A (zh) * | 2019-05-29 | 2020-12-01 | 上海淞泓智能汽车科技有限公司 | 一种室内整车硬件在环试验装置 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| CN112105961A (zh) * | 2019-07-26 | 2020-12-18 | 深圳市大疆创新科技有限公司 | 基于多数据融合的定位方法、可移动平台及存储介质 |
| CN110458885A (zh) * | 2019-08-27 | 2019-11-15 | 纵目科技（上海）股份有限公司 | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 |
| CN110411457A (zh) * | 2019-08-27 | 2019-11-05 | 纵目科技（上海）股份有限公司 | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 |
| CN110986939A (zh) * | 2020-01-02 | 2020-04-10 | 东南大学 | 一种基于imu预积分的视觉惯性里程计方法 |
| CN111795686A (zh) * | 2020-06-08 | 2020-10-20 | 南京大学 | 一种移动机器人定位与建图的方法 |
| CN111707272A (zh) * | 2020-06-28 | 2020-09-25 | 湖南大学 | 一种地下车库自动驾驶激光定位系统 |
| CN111986506A (zh) * | 2020-07-20 | 2020-11-24 | 苏州易航远智智能科技有限公司 | 基于多视觉系统的机械式停车位泊车方法 |
| CN112102406A (zh) * | 2020-09-09 | 2020-12-18 | 东软睿驰汽车技术（沈阳）有限公司 | 单目视觉的尺度修正方法、装置及运载工具 |
| CN111811506A (zh) * | 2020-09-15 | 2020-10-23 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
| CN112129297A (zh) * | 2020-09-25 | 2020-12-25 | 重庆大学 | 一种多传感器信息融合的自适应校正室内定位方法 |
| CN112050809A (zh) * | 2020-10-08 | 2020-12-08 | 吉林大学 | 轮式里程计与陀螺仪信息融合的无人车定向定位方法 |
| CN112506195A (zh) * | 2020-12-02 | 2021-03-16 | 吉林大学 | 基于视觉和底盘信息的车辆自主定位系统及定位方法 |
| CN112734841A (zh) * | 2020-12-31 | 2021-04-30 | 华南理工大学 | 一种用轮式里程计-imu和单目相机实现定位的方法 |
| CN112985450A (zh) * | 2021-02-09 | 2021-06-18 | 东南大学 | 一种具有同步时间误差估计的双目视觉惯性里程计方法 |
| CN112734852A (zh) * | 2021-03-31 | 2021-04-30 | 浙江欣奕华智能科技有限公司 | 一种机器人建图方法、装置及计算设备 |
| CN113223161A (zh) * | 2021-04-07 | 2021-08-06 | 武汉大学 | 一种基于imu和轮速计紧耦合的鲁棒全景slam系统和方法 |
| CN113358112A (zh) * | 2021-06-03 | 2021-09-07 | 北京超星未来科技有限公司 | 一种地图构建方法及一种激光惯性里程计 |
| CN113483755A (zh) * | 2021-07-09 | 2021-10-08 | 北京易航远智科技有限公司 | 一种基于非全局一致地图的多传感器组合定位方法及系统 |
| CN115790617A (zh) * | 2022-10-29 | 2023-03-14 | 武汉中海庭数据技术有限公司 | 基于相机感知的车道边线修正方法及系统 |
Non-Patent Citations (7)
| Title |
|---|
| Internal features in basin of attraction fof the simplest walking model;An , K 等;《JOURNAL OF MACHNICAL SCIENCE AND TECHNOLOGY》;第29卷(第11期);4913-4921 * |
| 单目视觉人工路标辅助INS的组合导航定位方法;李传立;尚俊娜;李芳;;传感技术学报(01);72-77 * |
| 基于SLAM的移动机器人自主返航控制系统设计;高波;施家栋;王建中;闫国栋;;机器人技术与应用(05);23-29 * |
| 基于多传感器融合的移动机器人SLAM算法研究;周阳;《中国国优秀硕士学位论文全文数据库信息科技辑》(第9期);1140-27 * |
| 基于多传感器融合视觉SLAM的人员跟随问题的研究;危义坤;《中国优秀硕士学位论文全文数据库信息科技辑》(第10期);1138-252 * |
| 多传感器信息融合的自动驾驶车辆定位与速度估计;彭文正;敖银辉;黄晓涛;王鹏飞;;传感技术学报(08);66-74 * |
| 无人驾驶车转向系统自抗扰控制的研究;李一染 等;《上海师范大学学报》;第39卷(第2期);156-160 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114018284A (zh) | 2022-02-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110009681B (zh) | 一种基于imu辅助的单目视觉里程计位姿处理方法 | |
| CN111561923B (zh) | 基于多传感器融合的slam制图方法、系统 | |
| CN111795686B (zh) | 一种移动机器人定位与建图的方法 | |
| CN106679648B (zh) | 一种基于遗传算法的视觉惯性组合的slam方法 | |
| CN112197770B (zh) | 一种机器人的定位方法及其定位装置 | |
| CN107516326B (zh) | 融合单目视觉和编码器信息的机器人定位方法和系统 | |
| CN110517324B (zh) | 基于变分贝叶斯自适应算法的双目vio实现方法 | |
| JP6534664B2 (ja) | カメラ動き予測及び修正のための方法 | |
| CN110595466B (zh) | 轻量级的基于深度学习的惯性辅助视觉里程计实现方法 | |
| CN112183171A (zh) | 一种基于视觉信标建立信标地图方法、装置 | |
| CN112837352B (zh) | 基于图像的数据处理方法、装置及设备、汽车、存储介质 | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN109631894A (zh) | 一种基于滑动窗口的单目视觉惯性紧耦合方法 | |
| CN114964276B (zh) | 一种融合惯导的动态视觉slam方法 | |
| JP2005182834A (ja) | 移動体の回転移動量利用方法とその装置、及びコンピュータプログラムを保存するコンピュータ読取り可能な記録媒体 | |
| CN114111818A (zh) | 一种通用视觉slam方法 | |
| CN110793526A (zh) | 基于可穿戴单目视觉和惯性传感器融合的行人导航方法及系统 | |
| CN115371665A (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
| CN108827287B (zh) | 一种复杂环境下的鲁棒视觉slam系统 | |
| CN114764830A (zh) | 一种基于四元数ekf和未标定手眼系统的物体位姿估算方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN114018284B (zh) | 一种基于视觉的轮速里程计校正方法 | |
| CN116958198A (zh) | 基于视觉惯性里程计的运动轨迹漂移检测方法及装置 | |
| CN116429098A (zh) | 低速无人飞行器视觉导航定位方法及系统 | |
| CN112712107B (zh) | 一种基于优化的视觉和激光slam融合定位方法 |
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 |