CN115371665B - 一种基于深度相机和惯性融合的移动机器人定位方法 - Google Patents

一种基于深度相机和惯性融合的移动机器人定位方法 Download PDF

Info

Publication number
CN115371665B
CN115371665B CN202211119147.3A CN202211119147A CN115371665B CN 115371665 B CN115371665 B CN 115371665B CN 202211119147 A CN202211119147 A CN 202211119147A CN 115371665 B CN115371665 B CN 115371665B
Authority
CN
China
Prior art keywords
depth
frame
imu
coordinate system
sliding window
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
CN202211119147.3A
Other languages
English (en)
Other versions
CN115371665A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202211119147.3A priority Critical patent/CN115371665B/zh
Publication of CN115371665A publication Critical patent/CN115371665A/zh
Application granted granted Critical
Publication of CN115371665B publication Critical patent/CN115371665B/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/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/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
    • 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

Abstract

一种基于深度相机和惯性融合的移动机器人定位方法，它属于视觉惯性导航技术领域。本发明解决了由于受到移动机器人运动方式和环境的影响，导致当前视觉SLAM系统容易出现定位失败或获得的定位效果差的问题。本发明利用了深度相机的深度不确定性模型和多视图三角化结合的方法，能够自适应地完成不同距离特征点的深度估计。并且在此基础上，提出了特征点动态分级管理方法，该方法结合跟踪次数和重投影误差动态地进行特征点分类并去除离群点。通过构建IMU预积分先验误差模型，平面运动约束，特征点深度约束以及3D‑2D特征点重投影误差模型，利用多种约束联合实现移动机器人位姿的估计。本发明方法可以应用于视觉惯性导航技术领域。

Description

一种基于深度相机和惯性融合的移动机器人定位方法
技术领域
本发明属于视觉惯性导航技术领域，具体涉及一种基于深度相机和惯性融合的移动机器人定位方法。
背景技术
近年来，随着计算机视觉的高速发展，基于视觉的同时定位与环境地图构建(简称视觉SLAM)技术在移动机器人领域已经得到了广泛应用。视觉SLAM技术无需依赖外界信号，能够通过相机感知环境信息实现地图构建从而估计自身的位姿变化。然而，在机器人纯旋转或急速运动的场景中，由于相机的帧率限制，光照的剧烈变化，常发生特征点跟踪失败导致SLAM失效的情况，从而无法得到移动机器人的定位信息。此外，由于移动机器人本身主要为平面运动，若采用单目SLAM方法，系统进行特征点深度计算时会因视差不足而产生较大的深度估计误差，严重影响定位效果。因此，单一的视觉SLAM算法会因其自身传感器的限制不可避免地有各方面的缺陷，这一现象在应用于复杂场景时尤为明显。
综上所述，由于移动机器人上受到机器人运动方式和环境影响，会导致当前视觉SLAM系统定位失败或获得的定位效果差的问题，因而，采用多传感器融合的方法，充分利用各传感器间测量信息的约束以及机器人的运动学约束来提高系统的位姿估计精度和鲁棒性是当前最佳的应用方案。
发明内容
本发明的目的是为解决由于受到移动机器人运动方式和环境的影响，导致当前视觉SLAM系统容易出现定位失败或获得的定位效果差的问题，而提出的一种基于深度相机和惯性融合的移动机器人定位方法。
本发明为解决上述技术问题所采取的技术方案是：
一种基于深度相机和惯性融合的移动机器人定位方法，所述方法具体包括以下步骤：
步骤1、读取当前来自深度相机的单目RGB图像，深度图像和IMU测量序列，再对单目RGB图像，深度图像和IMU测量序列进行时间同步；
所述步骤1的具体过程为：
以单目RGB图像的时间为基准，获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合，在匹配深度图像时，若深度图像与当前帧单目RGB图像的时间相差在t0以内，则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目RGB图像的时间相差不小于t0，则去除当前的深度图像，并以单目惯性模式进行处理；
步骤2、对前一帧单目RGB图像和当前帧单目RGB图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；
步骤3、建立深度相机深度测量误差模型；
步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目RGB图像ck具有最大视差的关键帧记为c0，计算关键帧c0与当前帧ck之间的平移和旋转初值
Figure BDA0003843234250000021
其中，/>
Figure BDA0003843234250000022
为关键帧c0与当前帧之间的平移初值，/>
Figure BDA0003843234250000023
为关键帧c0与当前帧之间的旋转初值；
根据得到的平移和旋转初值
Figure BDA0003843234250000024
得到关键帧c0与当前帧ck间的位姿估计结果/>
Figure BDA0003843234250000025
步骤5、以关键帧c0作为参考帧，基于关键帧c0与当前帧间的位姿估计结果
Figure BDA0003843234250000026
进行纯视觉位姿估计，得到滑窗内所有的3D路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果/>
Figure BDA0003843234250000027
步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换；
步骤7、将步骤5和步骤6的结果对齐，求得参考帧c0到世界坐标系的旋转矩阵
Figure BDA0003843234250000028
以及各帧的速度值/>
Figure BDA0003843234250000029
再利用旋转矩阵
Figure BDA00038432342500000210
得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值/>
Figure BDA00038432342500000211
得到各帧相对于世界坐标系的速度；
步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；
步骤9、根据筛选后的路标点建立点特征投影误差模型；
步骤10、建立平面约束误差模型；
步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立IMU预积分约束模型；
步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
本发明的有益效果是：
本发明利用了深度相机的深度不确定性模型和多视图三角化结合的方法，能够自适应地完成不同距离特征点的深度估计。并且在此基础上，提出了特征点动态分级管理方法，该方法结合跟踪次数和重投影误差动态地进行特征点分类并去除离群点。通过构建IMU预积分先验误差模型，平面运动约束，特征点深度约束以及3D-2D特征点重投影误差模型，利用多种约束联合实现移动机器人位姿的估计，提高了系统位姿估计的精度和鲁棒性，提高了对移动机器人的定位效果。
附图说明
图1是本发明方法的系统框图；
图2是测量数据时间同步原理图；
图3是移动机器人装置示意图；
图4是系统因子图结构；
图中，b0…bn代表IMU偏置状态，V0…Vn代表速度状态，T0…Tn代表位姿状态。
具体实施方式
具体实施方式一：结合图1说明本实施方式。本实施方式的一种基于深度相机和惯性融合的移动机器人定位方法，该方法具体包括以下步骤：
步骤1、读取当前来自深度相机(RGB-D)的单目RGB图像，深度图像和IMU测量序列，再对单目RGB图像，深度图像和IMU测量序列进行时间同步；
如图2所示，所述步骤1的具体过程为：
以单目RGB图像的时间为基准，获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合，在匹配深度图像时，若深度图像与当前帧单目RGB图像的时间相差在t0以内(本发明中取为3ms)，则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目RGB图像的时间相差不小于t0，则去除当前的深度图像，并以单目惯性模式进行处理；
步骤2、采用三层图像金字塔对前一帧单目RGB图像和当前帧单目RGB图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；
所述追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置采用的是KLT稀疏光流法，若成功追踪的特征点数量小于设定的阈值，则采用Shi-Tomasi角点提取算法在当前帧下采样后的图像上提取新的特征点，使提取的新特征点与前一帧下采样后图像中未成功追踪的特征点组成特征点对，否则不需要处理。
采用Shi-Tomasi角点提取算法提取新的特征点使得总特征点数量保持在阈值之上。
步骤3、建立深度相机深度测量误差模型；
步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目RGB图像ck具有最大视差的关键帧记为c0，采用2D-2D对极约束结合RANSAC(随机采样一致)的方法计算关键帧c0与当前帧ck之间的平移和旋转初值
Figure BDA0003843234250000041
其中，/>
Figure BDA0003843234250000042
为关键帧c0与当前帧之间的平移初值，/>
Figure BDA0003843234250000043
为关键帧c0与当前帧之间的旋转初值；
根据得到的平移和旋转初值
Figure BDA0003843234250000044
得到关键帧c0与当前帧ck间的位姿估计结果/>
Figure BDA0003843234250000045
步骤5、以关键帧c0作为参考帧，基于关键帧c0与当前帧间的位姿估计结果
Figure BDA0003843234250000046
进行纯视觉位姿估计，得到滑窗内所有的3D路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果/>
Figure BDA0003843234250000047
步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换；
步骤7、利用相邻帧间纯视觉和IMU预积分得到的位姿变化一致的特性，将步骤5和步骤6的结果对齐(即，滑窗内帧间纯视觉所得到的位姿和相应的IMU预积分的值对齐)，求得参考帧c0到世界坐标系的旋转矩阵
Figure BDA0003843234250000048
以及各帧的速度值/>
Figure BDA0003843234250000049
再利用旋转矩阵
Figure BDA00038432342500000410
得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值/>
Figure BDA00038432342500000411
得到各帧相对于世界坐标系的速度；
步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；
步骤9、根据筛选后的路标点建立点特征投影误差模型；
步骤10、考虑到移动机器人平面运动的特性如图3所示，建立平面约束误差模型；
步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立IMU预积分约束模型；
步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
本发明是一种利用RGB-D相机和惯性设备实现移动机器人定位的方法，能够实现GPS失效条件下如：地下、室内等复杂环境中高精度定位。本发明方法利用了深度相机深度测量误差模型，提出了自适应深度估计的方法，能够根据特征点深度测量的不同结合融合深度测量值和多视图三角化实现特征点的深度估计，使得该系统突破了RGBD-SLAM系统在仅在室内应用的限制，在室外也能有较好的效果。此外，针对地面机器人平面运动的特点，提出采用平面运动约束，特征点深度约束，3D-2D重投影约束，IMU预积分约束联合实现机器人的六自由度位姿估计，提高了系统位姿估计的精度和鲁棒性，可有效地应用于移动机器人的位姿估计。
具体实施方式二、本实施方式与具体实施方式一不同的是：所述深度相机深度测量误差模型为：
σz(z)＝a1+a2z+a3z2
其中，z为当前特征点在深度图像上的深度测量值，σz(z)为当前特征点在深度图像上的深度测量值为z时的标准差，a1,a2,a3为待标定的系数；
所述系数a1,a2,a3的标定方式为：
将深度相机置于滑轨上，当深度相机与白墙的距离为L1时采集多张深度图像，再分别从采集的每张深度图像中截取出感兴趣区域，所述感兴趣区域在每张深度图像中的位置均相同；
对于感兴趣区域内某个位置的像素点，计算出该位置的像素点在各个感兴趣区域中的深度测量值的方差，再对感兴趣区域内全部位置上的像素点所对应的方差求平均，将得到的平均值作为标准差σ；再计算出位于感兴趣区域内最中心的像素点在各个感兴趣区域中的深度测量值的均值μ；
深度相机在滑轨上滑动，当深度相机与白墙的距离每增加ΔL时均采集到对应距离下的多张深度图像，直至深度相机与白墙的距离达到L2时停止，同理，分别计算出每个距离下的标准差和均值；
利用获得的各个距离下的标准差和均值对σ＝a1+a2μ+a3μ2进行拟合，得到系数a1,a2,a3的值。
本实施方式中，所述的感兴趣区域均为对应的深度图像的中间部分，尺寸大小为450*360。L1的取值为0.3m，L2的取值为2.4m，ΔL的取值为0.1m，每个距离下采集的深度图像张数为100张。经过拟合得到标定后的参数a1＝0.001087,a2＝0.00058,a3＝0.004034。
其它步骤及参数与具体实施方式一相同。
具体实施方式三、本实施方式与具体实施方式一或二不同的是：所述根据得到的平移和旋转初值
Figure BDA0003843234250000061
得到关键帧c0与当前帧ck间的位姿估计结果/>
Figure BDA0003843234250000062
其具体过程为：
采用LM算法(Levenberg-Marquardt算法)对平移和旋转初值
Figure BDA0003843234250000063
进行迭代更新，分别获得各次迭代更新后的平移和旋转值；
遍历当前帧单目RGB图像上的特征点，从当前帧单目RGB图像对应的深度图像获得当前帧单目RGB图像上每个特征点的深度测量值；
并建立如下的误差方程：
Figure BDA0003843234250000064
其中，
Figure BDA0003843234250000065
代表从各次迭代更新后的平移和旋转值中选择出使误差方程取值最小的平移和旋转值/>
Figure BDA0003843234250000066
S3D表示由深度测量值在3m以内的特征点对组成的集合(每个特征点对均利用当前帧图像上的一个特征点和关键帧图像上的一个特征点形成)，S2D表示由全部特征点对组成的集合，rre表示重投影误差方程，rep表示对极约束误差方程，/>
Figure BDA0003843234250000067
表示S3D中第l1对特征点在关键帧c0中的深度测量值，/>
Figure BDA0003843234250000068
表示S3D中第l1对特征点在当前帧中的深度测量值，/>
Figure BDA0003843234250000069
表示S3D中第l1对特征点对应的归一化特征点对，/>
Figure BDA00038432342500000610
表示S2D中第l2对特征点对应的归一化特征点对，||·||代表范数，Ωre代表相机测量的重投影误差的协方差矩阵，Ωep代表相机测量的对极约束误差的协方差矩阵。
其它步骤及参数与具体实施方式一或二相同。
具体实施方式四、本实施方式与具体实施方式一至三之一不同的是：所述步骤5中，以关键帧c0作为参考帧，基于c0与当前帧间的位姿估计结果
Figure BDA0003843234250000071
进行纯视觉位姿估计，得到滑窗内所有的3D路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果/>
Figure BDA0003843234250000072
其具体过程为：
步骤51、根据关键帧c0与当前帧间的位姿估计结果
Figure BDA0003843234250000073
以及关键帧c0与当前帧间的特征点对的坐标值，计算出关键帧c0与当前帧间具有共视关系的3D路标点在关键帧c0坐标系下的坐标值；
步骤52、根据步骤51中计算出的坐标值以及关键帧c0与当前帧间具有共视关系的3D路标点在滑窗内其它关键帧的相机观测值(包括坐标和深度测量值)，得到滑窗内其它关键帧相对于参考帧的位姿估计初值；
步骤53、基于步骤52中的位姿估计初值，利用PnP优化算法得到滑窗内所有3D路标点l1,...lm和其它各关键帧相对于参考帧的视觉位姿估计结果
Figure BDA0003843234250000074
其中，滑窗的长度为10，即滑窗内有包括当前帧在内的10个关键帧，
Figure BDA0003843234250000075
代表滑窗内的各关键帧相对于参考帧的视觉位姿估计结果。
其它步骤及参数与具体实施方式一至三之一相同。
具体实施方式五、本实施方式与具体实施方式一至四之一不同的是：所述步骤6的具体过程为：
Figure BDA0003843234250000076
Figure BDA0003843234250000077
Figure BDA0003843234250000078
其中，
Figure BDA0003843234250000079
和/>
Figure BDA00038432342500000710
为第i个IMU时刻IMU测量得到的IMU坐标系下加速度和角速度，ba和bg为第i个IMU时刻的加速度计和陀螺仪的偏置，bk表示第k帧IMU坐标系，/>
Figure BDA00038432342500000711
和/>
Figure BDA00038432342500000712
分别为第i个IMU时刻相对于bk的位移预积分，速度预积分和旋转预积分，/>
Figure BDA00038432342500000713
为当前时刻相对于bk的旋转矩阵，/>
Figure BDA0003843234250000081
代表旋转运算符，δt为当前时刻与上一时刻的时间间隔，/>
Figure BDA0003843234250000082
是从IMU陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值，/>
Figure BDA0003843234250000083
和/>
Figure BDA0003843234250000084
分别为第i+1个IMU时刻相对于bk的位移预积分，速度预积分和旋转预积分；
在理想情况下，视觉运动估计所得的旋转等于IMU预积分得到的旋转值，则有目标函数：
Figure BDA0003843234250000085
其中，
Figure BDA0003843234250000086
为由视觉运动估计得到的bk+1相对于第c0帧IMU坐标系的旋转，/>
Figure BDA0003843234250000087
为由视觉运动估计得到的bk相对于第c0帧IMU坐标系的旋转，bk+1表示第k+1帧IMU坐标系，/>
Figure BDA0003843234250000088
为理想情况下IMU旋转预积分得到的bk+1相对于bk的旋转，B代表滑窗中的所有帧的序号，||·||代表范数；
由于实际情况下还需要考虑IMU中陀螺仪的偏置，因此已知实际的IMU旋转预积分的值
Figure BDA0003843234250000089
理想的IMU旋转预积分由下式求得：
Figure BDA00038432342500000810
其中，
Figure BDA00038432342500000811
为旋转预积分相对于陀螺仪偏置的雅克比矩阵，δbg为将要求得的陀螺仪偏置的变化量；
经过化简和Cholesky分解得：
Figure BDA00038432342500000812
其中，上角标T代表矩阵的转置，上角标-1代表矩阵的逆；
求解出陀螺仪的偏置的变化值δbg后，利用新的偏置bg＝bg+δbg带入到步骤6第一个公式中对IMU预积分值进行重新计算。
其它步骤及参数与具体实施方式一至四之一相同。
具体实施方式六、本实施方式与具体实施方式一至五之一不同的是：所述步骤8的具体过程为：
对于滑窗内的任意一个3D路标点，遍历该3D路标点在滑窗中每个关键帧上的测量值，测量值包括该3D路标点在相机归一化平面上的2D坐标和在深度图像上的深度测量值；
若该3D路标点在关键帧上存在深度测量值，且深度测量值小于3m，则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差
Figure BDA0003843234250000091
将该3D路标点的2D像素坐标测量噪声方差记为/>
Figure BDA0003843234250000092
则该3D路标点的三维测量协方差/>
Figure BDA0003843234250000093
为：
Figure BDA0003843234250000094
其中，I2为2阶单位矩阵，
Figure BDA0003843234250000095
为该3D路标点在相机中2D像素坐标(ui,vi)和深度测量值zi的雅克比；
Figure BDA0003843234250000096
其中，fx,fy表示相机x,y方向的焦距，cx,cy表示相机x,y方向的光心；
以该3D路标点在滑窗中第k帧的观测为例，通过滑窗内帧间的位姿变换
Figure BDA0003843234250000097
将该3D路标点转换到c0帧相机坐标系中，则将协方差/>
Figure BDA0003843234250000098
转换到c0帧相机坐标系下为：
Figure BDA0003843234250000099
其中，
Figure BDA00038432342500000910
为c0帧相机坐标系下的协方差；
利用
Figure BDA00038432342500000911
得到新的深度测量噪声方差/>
Figure BDA00038432342500000915
将该3D路标点的深度测量值小于3m且重投影后2D观测误差小于设定阈值的N个观测投影到相机坐标系下，每个观测的投影记为di′，i＝1,2,·,N，则该3D路标点在c0帧中的联合深度估计值为/>
Figure BDA00038432342500000912
Figure BDA00038432342500000913
那么该3D路标点的x,y方向坐标为该点在c0帧相机坐标系下归一化坐标的前两项，z方向坐标为上面得到的深度估计值
Figure BDA00038432342500000914
若该3D路标点在滑窗内的跟踪次数大于等于4(即联合深度估计的次数大于等于4)，则表明该3D路标点为稳健特征点，将该3D路标点归为A类，A类的3D路标点无需参与下一轮的联合深度估计；
若该3D路标点在滑窗内的跟踪次数小于4，则表明该3D路标点仍属新特征点，将该3D路标点归为B类，B类的3D路标点在下一循环仍需进行联合深度估计；
对于没有实现联合深度估计的3D路标点，根据共视帧的2D观测值采用多视图三角化的方法实现联合深度估计，对于三角化误差小于等于设定的阈值且跟踪次数大于等于4的3D路标点，将其归为A类，对于三角化误差小于等于设定阈值且跟踪次数小于4的3D路标点，则将其归为B类，对于三角化误差大于设定阈值的3D路标点，则直接将其归为C类，C类特征点为离群点不再参与后续优化，在本轮循环结束后被剔除。
下一循环是指下一帧到来时，在下一个滑窗内继续进行联合深度估计，将不符合条件的路标点被剔除后，获得筛选出的路标点。
其它步骤及参数与具体实施方式一至五之一相同。
具体实施方式七、本实施方式与具体实施方式一至六之一不同的是：所述步骤9的具体过程为：
对于筛选出的3D路标点lj，遍历在滑窗中所有共视帧中的观测值，若在当前帧ck中路标点lj的观测值包含2D像素坐标(uk,j,vk,j)和深度测量值dk,j，且深度测量值在3m以内，则建立带有深度约束的特征点投影误差模型：
Figure BDA0003843234250000101
Figure BDA0003843234250000102
其中，rk,j为3×1的误差向量，[·]1,2代表误差向量rk,j的前两行，[·]3代表误差向量的第三行，
Figure BDA0003843234250000103
代表/>
Figure BDA0003843234250000104
的第三行，π(·)表示相机投影方程，/>
Figure BDA0003843234250000105
是/>
Figure BDA0003843234250000106
的前两行，/>
Figure BDA0003843234250000107
为3D路标点lj转换到ck帧相机坐标系下的坐标；
Figure BDA0003843234250000108
Figure BDA0003843234250000109
由下式求得：
Figure BDA00038432342500001010
其中，
Figure BDA00038432342500001011
和/>
Figure BDA00038432342500001012
为ck帧到c0帧的旋转和平移，若3D路标点lj在当前帧ck中的深度测量值在3m以外或不含深度测量值，说明当前帧ck中深度测量值不可靠，则采用不含深度约束的特征点投影误差模型：
Figure BDA0003843234250000111
其中，r′k,j为2×1的误差向量。
其它步骤及参数与具体实施方式一至六之一相同。
具体实施方式八、本实施方式与具体实施方式一至七之一不同的是：所述步骤10的具体过程为：
移动机器人在进行平面运动时将会受到平面约束，即移动机器人的运动变化主要是x,y坐标以及偏航角θ。而移动机器人的z方向坐标，俯仰角和横滚角主要受到移动机器人负载和地形扰动的影响，相比于x,y方向变化较小。
将当前帧ck的相机坐标系在世界坐标系下的位姿
Figure BDA0003843234250000112
转换到当前帧ck的IMU坐标系(原点为IMU的中心，x方向指向机器人右侧，y方向指向机器人下方，z方向指向机器人前进方向)在世界坐标系下的姿态/>
Figure BDA0003843234250000113
再将/>
Figure BDA0003843234250000114
转换为平面位姿/>
Figure BDA0003843234250000115
即将其z方向坐标，俯仰角和横滚角归零，利用/>
Figure BDA0003843234250000116
表示，则
Figure BDA0003843234250000117
其中，
Figure BDA0003843234250000118
表示移动机器人相机坐标系到IMU坐标系的位姿转换，v(·)表示对位姿z方向坐标、俯仰角和横滚角进行归零操作；
则平面约束误差方程rplane为：
Figure BDA0003843234250000119
其中“∨”代表反对称矩阵到向量的变换。
考虑到移动机器人实际运动时其z方向，以及自身的俯仰角和横滚角并不始终为零，而是受到自身负载的变化，地面的崎岖不平所带来的扰动。因此，本系统的假设这三个变量分别受到δz,δr,δp为标准差的零均值高斯分布的影响，构建得到相应的协方差矩阵参与运算。
其它步骤及参数与具体实施方式一至七之一相同。
具体实施方式九、本实施方式与具体实施方式一至八之一不同的是：所述步骤11的具体过程为：
利用步骤6中第k帧和第k+1帧之间的IMU预积分值，可得第k帧和第k+1帧的位置、速度和旋转(PVQ)，用于约束滑窗中帧间的位姿变化。构建IMU预积分约束用下式表示：
Figure BDA0003843234250000121
该残差模型由IMU预积分建立的数学模型得到，其中，
Figure BDA0003843234250000122
为IMU预积分约束模型，/>
Figure BDA0003843234250000123
为bk到bk+1间的预积分值，/>
Figure BDA0003843234250000124
为bk到bk+1间的位移预积分，/>
Figure BDA0003843234250000125
为bk到bk+1间的速度预积分，/>
Figure BDA0003843234250000126
为/>
Figure BDA0003843234250000127
的变化量，/>
Figure BDA0003843234250000128
为/>
Figure BDA0003843234250000129
的变化量，δba为ba的变化量，
Figure BDA00038432342500001210
为待优化变量，其中，/>
Figure BDA00038432342500001211
和/>
Figure BDA00038432342500001212
分别为bk相对于世界坐标系的平移，速度和旋转值，/>
Figure BDA00038432342500001213
为bk+1相对于世界坐标系的平移值，/>
Figure BDA00038432342500001214
分别为bk+1相对于世界坐标系的速度值，/>
Figure BDA00038432342500001215
和/>
Figure BDA00038432342500001216
为bk中加速度计和陀螺仪的偏置，/>
Figure BDA00038432342500001217
和/>
Figure BDA00038432342500001218
为bk+1中加速度计和陀螺仪的偏置，/>
Figure BDA00038432342500001219
为/>
Figure BDA00038432342500001220
对应的四元数形式，/>
Figure BDA00038432342500001221
为/>
Figure BDA00038432342500001222
对应的四元数形式，/>
Figure BDA00038432342500001223
分别为bk+1相对于世界坐标系的旋转值，[·]xyz代表四元数的x,y,z三项的值，Δtk为第k帧和第k+1帧间的时间差，gw为世界坐标系下的重力向量。
其它步骤及参数与具体实施方式一至八之一相同。
具体实施方式十、本实施方式与具体实施方式一至九之一不同的是：所述全局优化目标函数为：
Figure BDA00038432342500001224
其中，第一项为边缘化的先验信息，rp为边缘化残差，Hp为边缘化后的海森矩阵；第二项为IMU测量残差项，
Figure BDA00038432342500001225
为IMU测量对应的协方差；第三项/>
Figure BDA00038432342500001226
为带有深度测量约束的视觉重投影残差，/>
Figure BDA0003843234250000131
为/>
Figure BDA0003843234250000132
对应的协方差；第四项/>
Figure BDA0003843234250000133
为不带深度测量的3D-2D视觉重投影残差，/>
Figure BDA0003843234250000134
为/>
Figure BDA0003843234250000135
对应的协方差；第五项rplane为平面约束残差项，/>
Figure BDA0003843234250000136
为rplane对应的协方差；/>
Figure BDA0003843234250000137
为滑窗中的3D路标点l在cj帧中的观测值；
系统的因子图模型如图4所示。通过对全局优化目标函数进行优化，得到滑窗内各帧的位姿估计结果。
其它步骤及参数与具体实施方式一至九之一相同。
本发明中的基于深度相机惯性融合的移动机器人定位方法，首先通过建立深度测量误差模型，利用多共视帧量测信息融合的方法提高了路标点深度估计精度。然后在初始化阶段采用优化的方法，利用对极约束和重投影约束利用多模态的特征点联合实现帧间位姿估计。建立了特征点分级管理系统，依据特征点在各帧的重投影误差和追踪次数，将特征点分为A、B和C类，并移除离群点。在全局优化模块中采用滑动窗口的方法构建了多模态特征点重投影约束，平面运动学约束，IMU预积分约束和边缘化先验约束，联合实现系统的高精度实时位姿估计，提高了系统的鲁棒性。
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

Claims (5)

1.一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述方法具体包括以下步骤：
步骤1、读取当前来自深度相机的单目RGB图像，深度图像和IMU测量序列，再对单目RGB图像，深度图像和IMU测量序列进行时间同步；
所述步骤1的具体过程为：
以单目RGB图像的时间为基准，获得时间戳位于前一帧单目RGB图像到当前帧单目RGB图像时间内的IMU测量序列集合，在匹配深度图像时，若深度图像与当前帧单目RGB图像的时间相差在t0以内，则将当前帧单目RGB图像、获得的IMU测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目RGB图像的时间相差不小于t0，则去除当前的深度图像，并以单目惯性模式进行处理；
步骤2、对前一帧单目RGB图像和当前帧单目RGB图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；
步骤3、建立深度相机深度测量误差模型；
所述深度相机深度测量误差模型为：
σz(z)＝a1+a2z+a3z2
其中，z为当前特征点在深度图像上的深度测量值，σz(z)为当前特征点在深度图像上的深度测量值为z时的标准差，a1,a2,a3为待标定的系数；
所述系数a1,a2,a3的标定方式为：
将深度相机置于滑轨上，当深度相机与白墙的距离为L1时采集多张深度图像，再分别从采集的每张深度图像中截取出感兴趣区域，所述感兴趣区域在每张深度图像中的位置均相同；
对于感兴趣区域内某个位置的像素点，计算出该位置的像素点在各个感兴趣区域中的深度测量值的方差，再对感兴趣区域内全部位置上的像素点所对应的方差求平均，将得到的平均值作为标准差σ；再计算出位于感兴趣区域内最中心的像素点在各个感兴趣区域中的深度测量值的均值μ；
深度相机在滑轨上滑动，当深度相机与白墙的距离每增加ΔL时均采集到对应距离下的多张深度图像，直至深度相机与白墙的距离达到L2时停止，同理，分别计算出每个距离下的标准差和均值；
利用获得的各个距离下的标准差和均值对σ＝a1+a2μ+a3μ2进行拟合，得到系数a1,a2,a3的值；
步骤4、将当前帧单目RGB图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目RGB图像ck具有最大视差的关键帧记为c0，计算关键帧c0与当前帧ck之间的平移和旋转初值
Figure FDA0004169522360000021
其中，/>
Figure FDA0004169522360000022
为关键帧c0与当前帧之间的旋转初值，/>
Figure FDA0004169522360000023
为关键帧c0与当前帧之间的平移初值；
根据得到的平移和旋转初值
Figure FDA0004169522360000024
得到关键帧c0与当前帧ck间的位姿估计结果
Figure FDA0004169522360000025
所述根据得到的平移和旋转初值
Figure FDA0004169522360000026
得到关键帧c0与当前帧ck间的位姿估计结果/>
Figure FDA0004169522360000027
其具体过程为：
采用LM算法对平移和旋转初值
Figure FDA0004169522360000028
进行迭代更新，分别获得各次迭代更新后的平移和旋转值；
遍历当前帧单目RGB图像上的特征点，从当前帧单目RGB图像对应的深度图像获得当前帧单目RGB图像上每个特征点的深度测量值；
并建立如下的误差方程：
Figure FDA0004169522360000029
其中，
Figure FDA00041695223600000210
代表从各次迭代更新后的平移和旋转值中选择出使误差方程取值最小的平移和旋转值/>
Figure FDA00041695223600000211
S3D表示由深度测量值在3m以内的特征点对组成的集合，S2D表示由全部特征点对组成的集合，rre表示重投影误差方程，rep表示对极约束误差方程，/>
Figure FDA00041695223600000212
表示S3D中第l1对特征点在关键帧c0中的深度测量值，/>
Figure FDA00041695223600000213
表示S3D中第l1对特征点在当前帧中的深度测量值，/>
Figure FDA00041695223600000214
表示S3D中第l1对特征点对应的归一化特征点对，/>
Figure FDA00041695223600000215
表示S2D中第l2对特征点对应的归一化特征点对，||·||代表范数，Ωre代表相机测量的重投影误差的协方差矩阵，Ωep代表相机测量的对极约束误差的协方差矩阵；
步骤5、以关键帧c0作为参考帧，基于关键帧c0与当前帧间的位姿估计结果
Figure FDA0004169522360000031
进行纯视觉位姿估计，得到滑窗内所有的3D路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果/>
Figure FDA0004169522360000032
所述步骤5的具体过程为：
步骤51、根据关键帧c0与当前帧间的位姿估计结果
Figure FDA0004169522360000033
以及关键帧c0与当前帧间的特征点对的坐标值，计算出关键帧c0与当前帧间具有共视关系的3D路标点在关键帧c0坐标系下的坐标值；
步骤52、根据步骤51中计算出的坐标值以及关键帧c0与当前帧间具有共视关系的3D路标点在滑窗内其它关键帧的相机观测值，得到滑窗内其它关键帧相对于参考帧的位姿估计初值；
步骤53、基于步骤52中的位姿估计初值，利用PnP优化算法得到滑窗内所有3D路标点l1,...lm和其它各关键帧相对于参考帧的视觉位姿估计结果
Figure FDA0004169522360000034
其中，滑窗的长度为10，即滑窗内有包括当前帧在内的10个关键帧，
Figure FDA0004169522360000035
代表滑窗内的各关键帧相对于参考帧的视觉位姿估计结果；
步骤6、利用数据集合中的IMU测量序列得到滑窗内各关键帧之间的相对位姿变换；
所述步骤6的具体过程为：
Figure FDA0004169522360000036
Figure FDA0004169522360000037
Figure FDA0004169522360000038
其中，
Figure FDA0004169522360000039
和/>
Figure FDA00041695223600000310
为第i个IMU时刻IMU测量得到的IMU坐标系下加速度和角速度，ba和bg为第i个IMU时刻的加速度计和陀螺仪的偏置，bk表示第k帧IMU坐标系，/>
Figure FDA00041695223600000311
和/>
Figure FDA00041695223600000312
分别为第i个IMU时刻相对于bk的位移预积分，速度预积分和旋转预积分，/>
Figure FDA00041695223600000313
为当前时刻相对于bk的旋转矩阵，/>
Figure FDA00041695223600000314
代表旋转运算符，δt为当前时刻与上一时刻的时间间隔，/>
Figure FDA0004169522360000041
是从IMU陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值，/>
Figure FDA0004169522360000042
和/>
Figure FDA0004169522360000043
分别为第i+1个IMU时刻相对于bk的位移预积分，速度预积分和旋转预积分；
在理想情况下，视觉运动估计所得的旋转等于IMU预积分得到的旋转值，则有目标函数：
Figure FDA0004169522360000044
其中，
Figure FDA0004169522360000045
为由视觉运动估计得到的bk+1相对于第c0帧IMU坐标系的旋转，/>
Figure FDA0004169522360000046
为由视觉运动估计得到的bk相对于第c0帧IMU坐标系的旋转，bk+1表示第k+1帧IMU坐标系，/>
Figure FDA0004169522360000047
为理想情况下IMU旋转预积分得到的bk+1相对于bk的旋转，B代表滑窗中的所有帧的序号，||·||代表范数；
已知实际的IMU旋转预积分的值
Figure FDA0004169522360000048
理想的IMU旋转预积分由下式求得：
Figure FDA0004169522360000049
其中，
Figure FDA00041695223600000410
为旋转预积分相对于陀螺仪偏置的雅克比矩阵，δbg为陀螺仪偏置的变化量；
经过化简和Cholesky分解得：
Figure FDA00041695223600000411
其中，上角标T代表矩阵的转置，上角标-1代表矩阵的逆；
求解出陀螺仪的偏置的变化值δbg后，利用新的偏置bg＝bg+δbg对IMU预积分值进行重新计算；
步骤7、将步骤5和步骤6的结果对齐，求得参考帧c0到世界坐标系的旋转矩阵
Figure FDA00041695223600000412
以及各帧的速度值/>
Figure FDA00041695223600000413
再利用旋转矩阵
Figure FDA00041695223600000414
得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值/>
Figure FDA00041695223600000415
得到各帧相对于世界坐标系的速度；
步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；
所述步骤8的具体过程为：
对于滑窗内的任意一个3D路标点，遍历该3D路标点在滑窗中每个关键帧上的测量值，测量值包括该3D路标点在相机归一化平面上的2D坐标和在深度图像上的深度测量值；
若该3D路标点在关键帧上存在深度测量值，且深度测量值小于3m，则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差
Figure FDA0004169522360000051
将该3D路标点的2D像素坐标测量噪声方差记为/>
Figure FDA0004169522360000052
则该3D路标点的三维测量协方差/>
Figure FDA0004169522360000053
为：
Figure FDA0004169522360000054
其中，I2为2阶单位矩阵，
Figure FDA0004169522360000055
为该3D路标点在相机中2D像素坐标(ui,vi)和深度测量值zi的雅克比；
Figure FDA0004169522360000056
其中，fx,fy表示相机x,y方向的焦距，cx,cy表示相机x,y方向的光心；
通过滑窗内帧间的位姿变换
Figure FDA0004169522360000057
将该3D路标点转换到c0帧相机坐标系中，则将协方差/>
Figure FDA0004169522360000058
转换到c0帧相机坐标系下为：
Figure FDA0004169522360000059
其中，
Figure FDA00041695223600000510
为c0帧相机坐标系下的协方差；
利用
Figure FDA00041695223600000511
得到新的深度测量噪声方差/>
Figure FDA00041695223600000512
将该3D路标点的深度测量值小于3m且重投影后2D观测误差小于设定阈值的N个观测投影到相机坐标系下，每个观测的投影记为di′，i＝1,2,…,N，则该3D路标点在c0帧中的联合深度估计值为/>
Figure FDA00041695223600000513
Figure FDA00041695223600000514
若该3D路标点在滑窗内的跟踪次数大于等于4，则表明该3D路标点为稳健特征点，将该3D路标点归为A类，A类的3D路标点无需参与下一轮的联合深度估计；
若该3D路标点在滑窗内的跟踪次数小于4，则表明该3D路标点仍属新特征点，将该3D路标点归为B类，B类的3D路标点在下一循环仍需进行联合深度估计；
对于没有实现联合深度估计的3D路标点，根据共视帧的2D观测值采用多视图三角化的方法实现联合深度估计，对于三角化误差小于等于设定的阈值且跟踪次数大于等于4的3D路标点，将其归为A类，对于三角化误差小于等于设定阈值且跟踪次数小于4的3D路标点，则将其归为B类，对于三角化误差大于设定阈值的3D路标点，则直接将其归为C类，C类特征点为离群点不再参与后续优化，在本轮循环结束后被剔除；
步骤9、根据筛选后的路标点建立点特征投影误差模型；
步骤10、建立平面约束误差模型；
步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立IMU预积分约束模型；
步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
2.根据权利要求1所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤9的具体过程为：
对于筛选出的3D路标点lj，遍历在滑窗中所有共视帧中的观测值，若在当前帧ck中路标点lj的观测值包含2D像素坐标(uk,j,vk,j)和深度测量值dk,j，且深度测量值在3m以内，则建立带有深度约束的特征点投影误差模型：
Figure FDA0004169522360000061
Figure FDA0004169522360000062
其中，rk,j为3×1的误差向量，[·]1,2代表误差向量rk,j的前两行，[·]3代表误差向量的第三行，
Figure FDA0004169522360000063
代表/>
Figure FDA0004169522360000064
的第三行，π(·)表示相机投影方程，/>
Figure FDA0004169522360000065
是/>
Figure FDA0004169522360000066
的前两行，
Figure FDA0004169522360000067
为3D路标点lj转换到ck帧相机坐标系下的坐标；
Figure FDA0004169522360000068
Figure FDA0004169522360000069
由下式求得：
Figure FDA00041695223600000610
其中，
Figure FDA0004169522360000071
和/>
Figure FDA0004169522360000072
为ck帧到c0帧的旋转和平移，若3D路标点lj在当前帧ck中的深度测量值在3m以外或不含深度测量值，说明当前帧ck中深度测量值不可靠，则采用不含深度约束的特征点投影误差模型：
Figure FDA0004169522360000073
其中，rk′,j为2×1的误差向量。
3.根据权利要求2所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤10的具体过程为：
将当前帧ck的相机坐标系在世界坐标系下的位姿
Figure FDA0004169522360000074
转换到当前帧ck的IMU坐标系在世界坐标系下的姿态/>
Figure FDA0004169522360000075
再将/>
Figure FDA0004169522360000076
转换为平面位姿/>
Figure FDA0004169522360000077
则
Figure FDA0004169522360000078
其中，
Figure FDA0004169522360000079
表示移动机器人相机坐标系到IMU坐标系的位姿转换，v(·)表示对位姿z方向坐标、俯仰角和横滚角进行归零操作；
则平面约束误差方程rplane为：
Figure FDA00041695223600000710
其中“∨”代表反对称矩阵到向量的变换。
4.根据权利要求3所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤11的具体过程为：
Figure FDA00041695223600000711
其中，
Figure FDA00041695223600000712
为IMU预积分约束模型，/>
Figure FDA00041695223600000713
为bk到bk+1间的预积分值，/>
Figure FDA00041695223600000714
为bk到bk+1间的位移预积分，/>
Figure FDA00041695223600000715
为bk到bk+1间的速度预积分，/>
Figure FDA00041695223600000716
为/>
Figure FDA00041695223600000717
的变化量，
Figure FDA00041695223600000718
为/>
Figure FDA00041695223600000719
的变化量，δba为ba的变化量，/>
Figure FDA00041695223600000720
为待优化变量，其中，
Figure FDA0004169522360000081
和/>
Figure FDA0004169522360000082
分别为bk相对于世界坐标系的平移，速度和旋转值，/>
Figure FDA0004169522360000083
为bk+1相对于世界坐标系的平移值，/>
Figure FDA0004169522360000084
分别为bk+1相对于世界坐标系的速度值，/>
Figure FDA0004169522360000085
和/>
Figure FDA0004169522360000086
为bk中加速度计和陀螺仪的偏置，/>
Figure FDA0004169522360000087
和/>
Figure FDA0004169522360000088
为bk+1中加速度计和陀螺仪的偏置，/>
Figure FDA0004169522360000089
为/>
Figure FDA00041695223600000810
对应的四元数形式，
Figure FDA00041695223600000811
为/>
Figure FDA00041695223600000812
对应的四元数形式，/>
Figure FDA00041695223600000813
分别为bk+1相对于世界坐标系的旋转值，[·]xyz代表四元数的x,y,z三项的值，Δtk为第k帧和第k+1帧间的时间差，gw为世界坐标系下的重力向量。
5.根据权利要求4所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述全局优化目标函数为：
Figure FDA00041695223600000814
其中，第一项为边缘化的先验信息，rp为边缘化残差，Hp为边缘化后的海森矩阵；第二项为IMU测量残差项，
Figure FDA00041695223600000822
为IMU测量对应的协方差；第三项/>
Figure FDA00041695223600000815
为带有深度测量约束的视觉重投影残差，/>
Figure FDA00041695223600000816
为/>
Figure FDA00041695223600000823
对应的协方差；第四项/>
Figure FDA00041695223600000817
为不带深度测量的3D-2D视觉重投影残差，/>
Figure FDA00041695223600000818
为/>
Figure FDA00041695223600000819
对应的协方差；第五项rplane为平面约束残差项，/>
Figure FDA00041695223600000820
为rplane对应的协方差；/>
Figure FDA00041695223600000821
为滑窗中的3D路标点l在cj帧中的观测值；
通过对全局优化目标函数进行优化，得到滑窗内各帧的位姿估计结果。
CN202211119147.3A 2022-09-13 2022-09-13 一种基于深度相机和惯性融合的移动机器人定位方法 Active CN115371665B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211119147.3A CN115371665B (zh) 2022-09-13 2022-09-13 一种基于深度相机和惯性融合的移动机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211119147.3A CN115371665B (zh) 2022-09-13 2022-09-13 一种基于深度相机和惯性融合的移动机器人定位方法

Publications (2)

Publication Number Publication Date
CN115371665A CN115371665A (zh) 2022-11-22
CN115371665B true CN115371665B (zh) 2023-06-23

Family

ID=84072528

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211119147.3A Active CN115371665B (zh) 2022-09-13 2022-09-13 一种基于深度相机和惯性融合的移动机器人定位方法

Country Status (1)

Country Link
CN (1) CN115371665B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115657062B (zh) * 2022-12-27 2023-03-17 理工雷科智途(北京)科技有限公司 一种设备快速重定位方法、装置及存储介质
CN117421384A (zh) * 2023-10-24 2024-01-19 哈尔滨理工大学 基于共视投影匹配的视觉惯性slam系统滑窗优化方法
CN117705107B (zh) * 2024-02-06 2024-04-16 电子科技大学 基于两阶段稀疏舒尔补的面向视觉惯性定位方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN111288989B (zh) * 2020-02-25 2021-11-05 浙江大学 一种小型无人机视觉定位方法
CN112240768A (zh) * 2020-09-10 2021-01-19 西安电子科技大学 基于Runge-Kutta4改进预积分的视觉惯导融合SLAM方法
CN114529576A (zh) * 2022-01-04 2022-05-24 重庆邮电大学 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法

Also Published As

Publication number Publication date
CN115371665A (zh) 2022-11-22

Similar Documents

Publication Publication Date Title
CN111024066B (zh) 一种无人机视觉-惯性融合室内定位方法
CN108827315B (zh) 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN115371665B (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN108717712B (zh) 一种基于地平面假设的视觉惯导slam方法
CN111156998B (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
US10762643B2 (en) Method for evaluating image data of a vehicle camera
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN113052908B (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN111795686A (zh) 一种移动机器人定位与建图的方法
US20180075614A1 (en) Method of Depth Estimation Using a Camera and Inertial Sensor
CN112254729A (zh) 一种基于多传感器融合的移动机器人定位方法
CN111161337A (zh) 一种动态环境下的陪护机器人同步定位与构图方法
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
He et al. Relative motion estimation using visual–inertial optical flow
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
CN114608554A (zh) 一种手持slam设备以及机器人即时定位与建图方法
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN112945233B (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN109785428A (zh) 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN113701750A (zh) 一种井下多传感器的融合定位系统
CN112907633A (zh) 动态特征点识别方法及其应用
Panahandeh et al. Vision-aided inertial navigation using planar terrain features

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