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

1.本发明属于视觉惯性导航技术领域，具体涉及一种基于深度相机和惯性融合的移动机器人定位方法。

背景技术：

2.近年来，随着计算机视觉的高速发展，基于视觉的同时定位与环境地图构建(简称视觉slam)技术在移动机器人领域已经得到了广泛应用。视觉slam技术无需依赖外界信号，能够通过相机感知环境信息实现地图构建从而估计自身的位姿变化。然而，在机器人纯旋转或急速运动的场景中，由于相机的帧率限制，光照的剧烈变化，常发生特征点跟踪失败导致slam失效的情况，从而无法得到移动机器人的定位信息。此外，由于移动机器人本身主要为平面运动，若采用单目slam方法，系统进行特征点深度计算时会因视差不足而产生较大的深度估计误差，严重影响定位效果。因此，单一的视觉slam算法会因其自身传感器的限制不可避免地有各方面的缺陷，这一现象在应用于复杂场景时尤为明显。
3.综上所述，由于移动机器人上受到机器人运动方式和环境影响，会导致当前视觉slam系统定位失败或获得的定位效果差的问题，因而，采用多传感器融合的方法，充分利用各传感器间测量信息的约束以及机器人的运动学约束来提高系统的位姿估计精度和鲁棒性是当前最佳的应用方案。

技术实现要素：

4.本发明的目的是为解决由于受到移动机器人运动方式和环境的影响，导致当前视觉slam系统容易出现定位失败或获得的定位效果差的问题，而提出的一种基于深度相机和惯性融合的移动机器人定位方法。
5.本发明为解决上述技术问题所采取的技术方案是：
6.一种基于深度相机和惯性融合的移动机器人定位方法，所述方法具体包括以下步骤：
7.步骤1、读取当前来自深度相机的单目rgb图像，深度图像和imu测量序列，再对单目rgb图像，深度图像和imu测量序列进行时间同步；
8.所述步骤1的具体过程为：
9.以单目rgb图像的时间为基准，获得时间戳位于前一帧单目rgb图像到当前帧单目rgb图像时间内的imu测量序列集合，在匹配深度图像时，若深度图像与当前帧单目rgb图像的时间相差在t0以内，则将当前帧单目rgb图像、获得的imu测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目rgb图像的时间相差不小于t0，则去除当前的深度图像，并以单目惯性模式进行处理；
10.步骤2、对前一帧单目rgb图像和当前帧单目rgb图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；
11.步骤3、建立深度相机深度测量误差模型；
12.步骤4、将当前帧单目rgb图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目rgb图像ck具有最大视差的关键帧记为c0，计算关键帧c0与当前帧ck之间的平移和旋转初值其中，为关键帧c0与当前帧之间的平移初值，为关键帧c0与当前帧之间的旋转初值；
13.根据得到的平移和旋转初值得到关键帧c0与当前帧ck间的位姿估计结果
14.步骤5、以关键帧c0作为参考帧，基于关键帧c0与当前帧间的位姿估计结果进行纯视觉位姿估计，得到滑窗内所有的3d路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果
15.步骤6、利用数据集合中的imu测量序列得到滑窗内各关键帧之间的相对位姿变换；
16.步骤7、将步骤5和步骤6的结果对齐，求得参考帧c0到世界坐标系的旋转矩阵以及各帧的速度值
17.再利用旋转矩阵得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值得到各帧相对于世界坐标系的速度；
18.步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；
19.步骤9、根据筛选后的路标点建立点特征投影误差模型；
20.步骤10、建立平面约束误差模型；
21.步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立imu预积分约束模型；
22.步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
23.本发明的有益效果是：
24.本发明利用了深度相机的深度不确定性模型和多视图三角化结合的方法，能够自适应地完成不同距离特征点的深度估计。并且在此基础上，提出了特征点动态分级管理方法，该方法结合跟踪次数和重投影误差动态地进行特征点分类并去除离点。通过构建imu预积分先验误差模型，平面运动约束，特征点深度约束以及3d-2d特征点重投影误差模型，利用多种约束联合实现移动机器人位姿的估计，提高了系统位姿估计的精度和鲁棒性，提高了对移动机器人的定位效果。
附图说明
25.图1是本发明方法的系统框图；
26.图2是测量数据时间同步原理图；
27.图3是移动机器人装置示意图；
28.图4是系统因子图结构；
29.图中，b0…bn
代表imu偏置状态，v0…vn
代表速度状态，t0…
tn代表位姿状态。
具体实施方式
30.具体实施方式一：结合图1说明本实施方式。本实施方式的一种基于深度相机和惯性融合的移动机器人定位方法，该方法具体包括以下步骤：
31.步骤1、读取当前来自深度相机(rgb-d)的单目rgb图像，深度图像和imu测量序列，再对单目rgb图像，深度图像和imu测量序列进行时间同步；
32.如图2所示，所述步骤1的具体过程为：
33.以单目rgb图像的时间为基准，获得时间戳位于前一帧单目rgb图像到当前帧单目rgb图像时间内的imu测量序列集合，在匹配深度图像时，若深度图像与当前帧单目rgb图像的时间相差在t0以内(本发明中取为3ms)，则将当前帧单目rgb图像、获得的imu测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目rgb图像的时间相差不小于t0，则去除当前的深度图像，并以单目惯性模式进行处理；
34.步骤2、采用三层图像金字塔对前一帧单目rgb图像和当前帧单目rgb图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；
35.所述追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置采用的是klt稀疏光流法，若成功追踪的特征点数量小于设定的阈值，则采用shi-tomasi角点提取算法在当前帧下采样后的图像上提取新的特征点，使提取的新特征点与前一帧下采样后图像中未成功追踪的特征点组成特征点对，否则不需要处理。
36.采用shi-tomasi角点提取算法提取新的特征点使得总特征点数量保持在阈值之上。
37.步骤3、建立深度相机深度测量误差模型；
38.步骤4、将当前帧单目rgb图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目rgb图像ck具有最大视差的关键帧记为c0，采用2d-2d对极约束结合ransac(随机采样一致)的方法计算关键帧c0与当前帧ck之间的平移和旋转初值其中，为关键帧c0与当前帧之间的平移初值，为关键帧c0与当前帧之间的旋转初值；
39.根据得到的平移和旋转初值得到关键帧c0与当前帧ck间的位姿估计结果
40.步骤5、以关键帧c0作为参考帧，基于关键帧c0与当前帧间的位姿估计结果进行纯视觉位姿估计，得到滑窗内所有的3d路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果
41.步骤6、利用数据集合中的imu测量序列得到滑窗内各关键帧之间的相对位姿变换；
42.步骤7、利用相邻帧间纯视觉和imu预积分得到的位姿变化一致的特性，将步骤5和步骤6的结果对齐(即，滑窗内帧间纯视觉所得到的位姿和相应的imu预积分的值对齐)，求得参考帧c0到世界坐标系的旋转矩阵以及各帧的速度值
43.再利用旋转矩阵得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值得到各帧相对于世界坐标系的速度；
44.步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；
45.步骤9、根据筛选后的路标点建立点特征投影误差模型；
46.步骤10、考虑到移动机器人平面运动的特性如图3所示，建立平面约束误差模型；
47.步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立imu预积分约束模型；
48.步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行求解得到滑窗内各帧的位姿估计结果。
49.本发明是一种利用rgb-d相机和惯性设备实现移动机器人定位的方法，能够实现gps失效条件下如：地下、室内等复杂环境中高精度定位。本发明方法利用了深度相机深度测量误差模型，提出了自适应深度估计的方法，能够根据特征点深度测量的不同结合融合深度测量值和多视图三角化实现特征点的深度估计，使得该系统突破了rgbd-slam系统在仅在室内应用的限制，在室外也能有较好的效果。此外，针对地面机器人平面运动的特点，提出采用平面运动约束，特征点深度约束，3d-2d重投影约束，imu预积分约束联合实现机器人的六自由度位姿估计，提高了系统位姿估计的精度和鲁棒性，可有效地应用于移动机器人的位姿估计。
50.具体实施方式二、本实施方式与具体实施方式一不同的是：所述深度相机深度测量误差模型为：
51.σz(z)＝a1+a2z+a3z252.其中，z为当前特征点在深度图像上的深度测量值，σz(z)为当前特征点在深度图像上的深度测量值为z时的标准差，a1,a2,a3为待标定的系数；
53.所述系数a1,a2,a3的标定方式为：
54.将深度相机置于滑轨上，当深度相机与白墙的距离为l1时采集多张深度图像，再分别从采集的每张深度图像中截取出感兴趣区域，所述感兴趣区域在每张深度图像中的位置均相同；
55.对于感兴趣区域内某个位置的像素点，计算出该位置的像素点在各个感兴趣区域中的深度测量值的方差，再对感兴趣区域内全部位置上的像素点所对应的方差求平均，将得到的平均值作为标准差σ；再计算出位于感兴趣区域内最中心的像素点在各个感兴趣区域中的深度测量值的均值μ；
56.深度相机在滑轨上滑动，当深度相机与白墙的距离每增加δl时均采集到对应距
离下的多张深度图像，直至深度相机与白墙的距离达到l2时停止，同理，分别计算出每个距离下的标准差和均值；
57.利用获得的各个距离下的标准差和均值对σ＝a1+a2μ+a3μ2进行拟合，得到系数a1,a2,a3的值。
58.本实施方式中，所述的感兴趣区域均为对应的深度图像的中间部分，尺寸大小为450*360。l1的取值为0.3m，l2的取值为2.4m，δl的取值为0.1m，每个距离下采集的深度图像张数为100张。经过拟合得到标定后的参数a1＝0.001087,a2＝0.00058,a3＝0.004034。
59.其它步骤及参数与具体实施方式一相同。
60.具体实施方式三、本实施方式与具体实施方式一或二不同的是：所述根据得到的平移和旋转初值得到关键帧c0与当前帧ck间的位姿估计结果其具体过程为：
61.采用lm算法(levenberg-marquardt算法)对平移和旋转初值进行迭代更新，分别获得各次迭代更新后的平移和旋转值；
62.遍历当前帧单目rgb图像上的特征点，从当前帧单目rgb图像对应的深度图像获得当前帧单目rgb图像上每个特征点的深度测量值；
63.并建立如下的误差方程：
[0064][0065]
其中，代表从各次迭代更新后的平移和旋转值中选择出使误差方程取值最小的平移和旋转值s
3d
表示由深度测量值在3m以内的特征点对组成的集合(每个特征点对均利用当前帧图像上的一个特征点和关键帧图像上的一个特征点形成)，s
2d
表示由全部特征点对组成的集合，r
re
表示重投影误差方程，r
ep
表示对极约束误差方程，表示s
3d
中第l1对特征点在关键帧c0中的深度测量值，表示s
3d
中第l1对特征点在当前帧中的深度测量值，表示s
3d
中第l1对特征点对应的归一化特征点对，表示s
2d
中第l2对特征点对应的归一化特征点对，||
·
||代表范数，ω
re
代表相机测量的重投影误差的协方差矩阵，ω
ep
代表相机测量的对极约束误差的协方差矩阵。
[0066]
其它步骤及参数与具体实施方式一或二相同。
[0067]
具体实施方式四、本实施方式与具体实施方式一至三之一不同的是：所述步骤5中，以关键帧c0作为参考帧，基于c0与当前帧间的位姿估计结果进行纯视觉位姿估计，得到滑窗内所有的3d路标点l1,...lm以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果其具体过程为：
[0068]
步骤51、根据关键帧c0与当前帧间的位姿估计结果以及关键帧c0与当前帧间的特征点对的坐标值，计算出关键帧c0与当前帧间具有共视关系的3d路标点在关键帧c0坐标系下的坐标值；
[0069]
步骤52、根据步骤51中计算出的坐标值以及关键帧c0与当前帧间具有共视关系的3d路标点在滑窗内其它关键帧的相机观测值(包括坐标和深度测量值)，得到滑窗内其它关键帧相对于参考帧的位姿估计初值；
[0070]
步骤53、基于步骤52中的位姿估计初值，利用pnp优化算法得到滑窗内所有3d路标点l1,...lm和其它各关键帧相对于参考帧的视觉位姿估计结果
[0071]
其中，滑窗的长度为10，即滑窗内有包括当前帧在内的10个关键帧，代表滑窗内的各关键帧相对于参考帧的视觉位姿估计结果。
[0072]
其它步骤及参数与具体实施方式一至三之一相同。
[0073]
具体实施方式五、本实施方式与具体实施方式一至四之一不同的是：所述步骤6的具体过程为：
[0074][0075][0076][0077]
其中，和为第i个imu时刻imu测量得到的imu坐标系下加速度和角速度，ba和bg为第i个imu时刻的加速度计和陀螺仪的偏置，bk表示第k帧imu坐标系，和分别为第i个imu时刻相对于bk的位移预积分，速度预积分和旋转预积分，为当前时刻相对于bk的旋转矩阵，代表旋转运算符，δt为当前时刻与上一时刻的时间间隔，是从imu陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值，和分别为第i+1个imu时刻相对于bk的位移预积分，速度预积分和旋转预积分；
[0078]
在理想情况下，视觉运动估计所得的旋转等于imu预积分得到的旋转值，则有目标函数：
[0079][0080]
其中，为由视觉运动估计得到的b
k+1
相对于第c0帧imu坐标系的旋转，为由视觉运动估计得到的bk相对于第c0帧imu坐标系的旋转，b
k+1
表示第k+1帧imu坐标系，为理想情况下imu旋转预积分得到的b
k+1
相对于bk的旋转，b代表滑窗中的所有帧的序号，||
·
||代表范数；
[0081]
由于实际情况下还需要考虑imu中陀螺仪的偏置，因此已知实际的imu旋转预积分的值理想的imu旋转预积分由下式求得：
[0082]
[0083]
其中，为旋转预积分相对于陀螺仪偏置的雅克比矩阵，δbg为将要求得的陀螺仪偏置的变化量；
[0084]
经过化简和cholesky分解得：
[0085][0086]
其中，上角标t代表矩阵的转置，上角标-1代表矩阵的逆；
[0087]
求解出陀螺仪的偏置的变化值δbg后，利用新的偏置bg＝bg+δbg带入到步骤6第一个公式中对imu预积分值进行重新计算。
[0088]
其它步骤及参数与具体实施方式一至四之一相同。
[0089]
具体实施方式六、本实施方式与具体实施方式一至五之一不同的是：所述步骤8的具体过程为：
[0090]
对于滑窗内的任意一个3d路标点，遍历该3d路标点在滑窗中每个关键帧上的测量值，测量值包括该3d路标点在相机归一化平面上的2d坐标和在深度图像上的深度测量值；
[0091]
若该3d路标点在关键帧上存在深度测量值，且深度测量值小于3m，则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差将该3d路标点的2d像素坐标测量噪声方差记为则该3d路标点的三维测量协方差为：
[0092][0093]
其中，i2为2阶单位矩阵，为该3d路标点在相机中2d像素坐标(ui,vi)和深度测量值zi的雅克比；
[0094][0095]
其中，f
x
,fy表示相机x,y方向的焦距，c
x
,cy表示相机x,y方向的光心；
[0096]
以该3d路标点在滑窗中第k帧的观测为例，通过滑窗内帧间的位姿变换将该3d路标点转换到c0帧相机坐标系中，则将协方差转换到c0帧相机坐标系下为：
[0097][0098]
其中，为c0帧相机坐标系下的协方差；
[0099]
利用得到新的深度测量噪声方差将该3d路标点的深度测量值小于3m且重投影后2d观测误差小于设定阈值的n个观测投影到相机坐标系下，每个观测的投影记为di′
，i＝1,2,
·
,n，则该3d路标点在c0帧中的联合深度估计值为
[0100][0101]
那么该3d路标点的x,y方向坐标为该点在c0帧相机坐标系下归一化坐标的前两项，z方向坐标为上面得到的深度估计值
[0102]
若该3d路标点在滑窗内的跟踪次数大于等于4(即联合深度估计的次数大于等于4)，则表明该3d路标点为稳健特征点，将该3d路标点归为a类，a类的3d路标点无需参与下一轮的联合深度估计；
[0103]
若该3d路标点在滑窗内的跟踪次数小于4，则表明该3d路标点仍属新特征点，将该3d路标点归为b类，b类的3d路标点在下一循环仍需进行联合深度估计；
[0104]
对于没有实现联合深度估计的3d路标点，根据共视帧的2d观测值采用多视图三角化的方法实现联合深度估计，对于三角化误差小于等于设定的阈值且跟踪次数大于等于4的3d路标点，将其归为a类，对于三角化误差小于等于设定阈值且跟踪次数小于4的3d路标点，则将其归为b类，对于三角化误差大于设定阈值的3d路标点，则直接将其归为c类，c类特征点为离点不再参与后续优化，在本轮循环结束后被剔除。
[0105]
下一循环是指下一帧到来时，在下一个滑窗内继续进行联合深度估计，将不符合条件的路标点被剔除后，获得筛选出的路标点。
[0106]
其它步骤及参数与具体实施方式一至五之一相同。
[0107]
具体实施方式七、本实施方式与具体实施方式一至六之一不同的是：所述步骤9的具体过程为：
[0108]
对于筛选出的3d路标点lj，遍历在滑窗中所有共视帧中的观测值，若在当前帧ck中路标点lj的观测值包含2d像素坐标(u
k,j
,v
k,j
)和深度测量值d
k,j
，且深度测量值在3m以内，则建立带有深度约束的特征点投影误差模型：
[0109][0110][0111]
其中，r
k,j
为3
×
1的误差向量，[
·
]
1,2
代表误差向量r
k,j
的前两行，[
·
]3代表误差向量的第三行，代表的第三行，π(
·
)表示相机投影方程，是的前两行，为3d路标点lj转换到ck帧相机坐标系下的坐标；
[0112][0113]
由下式求得：
[0114][0115]
其中，和为ck帧到c0帧的旋转和平移，若3d路标点lj在当前帧ck中的深度测量值在3m以外或不含深度测量值，说明当前帧ck中深度测量值不可靠，则采用不含深度约束的特征点投影误差模型：
[0116]
[0117]
其中，r
′
k,j
为2
×
1的误差向量。
[0118]
其它步骤及参数与具体实施方式一至六之一相同。
[0119]
具体实施方式八、本实施方式与具体实施方式一至七之一不同的是：所述步骤10的具体过程为：
[0120]
移动机器人在进行平面运动时将会受到平面约束，即移动机器人的运动变化主要是x,y坐标以及偏航角θ。而移动机器人的z方向坐标，俯仰角和横滚角主要受到移动机器人负载和地形扰动的影响，相比于x,y方向变化较小。
[0121]
将当前帧ck的相机坐标系在世界坐标系下的位姿转换到当前帧ck的imu坐标系(原点为imu的中心，x方向指向机器人右侧，y方向指向机器人下方，z方向指向机器人前进方向)在世界坐标系下的姿态再将转换为平面位姿即将其z方向坐标，俯仰角和横滚角归零，利用表示，则
[0122][0123]
其中，表示移动机器人相机坐标系到imu坐标系的位姿转换，v(
·
)表示对位姿z方向坐标、俯仰角和横滚角进行归零操作；
[0124]
则平面约束误差方程r
plane
为：
[0125][0126]
其中“∨”代表反对称矩阵到向量的变换。
[0127]
考虑到移动机器人实际运动时其z方向，以及自身的俯仰角和横滚角并不始终为零，而是受到自身负载的变化，地面的崎岖不平所带来的扰动。因此，本系统的假设这三个变量分别受到δz,δr,δ
p
为标准差的零均值高斯分布的影响，构建得到相应的协方差矩阵参与运算。
[0128]
其它步骤及参数与具体实施方式一至七之一相同。
[0129]
具体实施方式九、本实施方式与具体实施方式一至八之一不同的是：所述步骤11的具体过程为：
[0130]
利用步骤6中第k帧和第k+1帧之间的imu预积分值，可得第k帧和第k+1帧的位置、速度和旋转(pvq)，用于约束滑窗中帧间的位姿变化。构建imu预积分约束用下式表示：
[0131][0132]
该残差模型由imu预积分建立的数学模型得到，其中，为imu预积分约束模型，为bk到b
k+1
间的预积分值，为bk到b
k+1
间的位移预积分，
为bk到b
k+1
间的速度预积分，为的变化量，为的变化量，δba为ba的变化量，为待优化变量，其中，和分别为bk相对于世界坐标系的平移，速度和旋转值，为b
k+1
相对于世界坐标系的平移值，分别为b
k+1
相对于世界坐标系的速度值，和为bk中加速度计和陀螺仪的偏置，和为b
k+1
中加速度计和陀螺仪的偏置，为对应的四元数形式，为对应的四元数形式，分别为b
k+1
相对于世界坐标系的旋转值，[
·
]
xyz
代表四元数的x,y,z三项的值，δtk为第k帧和第k+1帧间的时间差，gw为世界坐标系下的重力向量。
[0133]
其它步骤及参数与具体实施方式一至八之一相同。
[0134]
具体实施方式十、本实施方式与具体实施方式一至九之一不同的是：所述全局优化目标函数为：
[0135][0136]
其中，第一项为边缘化的先验信息，r
p
为边缘化残差，h
p
为边缘化后的海森矩阵；第二项为imu测量残差项，为imu测量对应的协方差；第三项为带有深度测量约束的视觉重投影残差，为对应的协方差；第四项为不带深度测量的3d-2d视觉重投影残差，为对应的协方差；第五项r
plane
为平面约束残差项，为r
plane
对应的协方差；为滑窗中的3d路标点l在cj帧中的观测值；
[0137]
系统的因子图模型如图4所示。通过对全局优化目标函数进行优化，得到滑窗内各帧的位姿估计结果。
[0138]
其它步骤及参数与具体实施方式一至九之一相同。
[0139]
本发明中的基于深度相机惯性融合的移动机器人定位方法，首先通过建立深度测量误差模型，利用多共视帧量测信息融合的方法提高了路标点深度估计精度。然后在初始化阶段采用优化的方法，利用对极约束和重投影约束利用多模态的特征点联合实现帧间位姿估计。建立了特征点分级管理系统，依据特征点在各帧的重投影误差和追踪次数，将特征点分为a、b和c类，并移除离点。在全局优化模块中采用滑动窗口的方法构建了多模态特征点重投影约束，平面运动学约束，imu预积分约束和边缘化先验约束，联合实现系统的高精度实时位姿估计，提高了系统的鲁棒性。
[0140]
本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。

技术特征：

1.一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述方法具体包括以下步骤：步骤1、读取当前来自深度相机的单目rgb图像，深度图像和imu测量序列，再对单目rgb图像，深度图像和imu测量序列进行时间同步；所述步骤1的具体过程为：以单目rgb图像的时间为基准，获得时间戳位于前一帧单目rgb图像到当前帧单目rgb图像时间内的imu测量序列集合，在匹配深度图像时，若深度图像与当前帧单目rgb图像的时间相差在t0以内，则将当前帧单目rgb图像、获得的imu测量序列集合以及深度图像打包成数据集合，并以深度惯性模式进行处理，即继续执行步骤2；否则，若深度图像与当前帧单目rgb图像的时间相差不小于t0，则去除当前的深度图像，并以单目惯性模式进行处理；步骤2、对前一帧单目rgb图像和当前帧单目rgb图像进行下采样，获得下采样后的图像；再追踪前一帧下采样后图像的特征点在当前帧下采样后图像上的位置，获得各个特征点对；步骤3、建立深度相机深度测量误差模型；步骤4、将当前帧单目rgb图像作为滑动窗口的最后一帧，将滑窗内与当前帧单目rgb图像c
k
具有最大视差的关键帧记为c0，计算关键帧c0与当前帧c
k
之间的平移和旋转初值其中，为关键帧c0与当前帧之间的旋转初值，为关键帧c0与当前帧之间的平移初值；根据得到的平移和旋转初值得到关键帧c0与当前帧c
k
间的位姿估计结果步骤5、以关键帧c0作为参考帧，基于关键帧c0与当前帧间的位姿估计结果进行纯视觉位姿估计，得到滑窗内所有的3d路标点l1,...l
m
以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果步骤6、利用数据集合中的imu测量序列得到滑窗内各关键帧之间的相对位姿变换；步骤7、将步骤5和步骤6的结果对齐，求得参考帧c0到世界坐标系的旋转矩阵以及各帧的速度值再利用旋转矩阵得到滑窗中各帧相对于世界坐标系的平移和旋转，根据各帧的速度值得到各帧相对于世界坐标系的速度；步骤8、基于步骤3中建立的深度相机深度测量误差模型，对滑窗中所有的路标点进行联合深度估计，根据联合深度估计结果对路标点进行分类，根据分类结果对路标点进行筛选；步骤9、根据筛选后的路标点建立点特征投影误差模型；步骤10、建立平面约束误差模型；步骤11、根据滑窗中各帧相对于世界坐标系的平移和旋转以及各帧相对于世界坐标系的速度，建立imu预积分约束模型；步骤12、根据步骤9至步骤11的模型建立全局优化目标函数，对全局优化目标函数进行
求解得到滑窗内各帧的位姿估计结果。2.根据权利要求1所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述深度相机深度测量误差模型为：σ
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的值。3.根据权利要求2所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述根据得到的平移和旋转初值得到关键帧c0与当前帧c
k
间的位姿估计结果其具体过程为：采用lm算法对平移和旋转初值进行迭代更新，分别获得各次迭代更新后的平移和旋转值；遍历当前帧单目rgb图像上的特征点，从当前帧单目rgb图像对应的深度图像获得当前帧单目rgb图像上每个特征点的深度测量值；并建立如下的误差方程：其中，代表从各次迭代更新后的平移和旋转值中选择出使误差方程取值最小的平移和旋转值s
3d
表示由深度测量值在3m以内的特征点对组成的集合，s
2d
表示由全部特征点对组成的集合，r
re
表示重投影误差方程，r
ep
表示对极约束误差方程，表示s
3d
中第l1对特征点在关键帧c0中的深度测量值，表示s
3d
中第l1对特征点在当前帧中的深度测量值，表示s
3d
中第l1对特征点对应的归一化特征点对，表示s
2d
中第l2对特征点对应的归一化特征点对，||
·
||代表范数，ω
re
代表相机测量的重投影误差的协方差矩阵，ω
ep
代表相机测量的对极约束误差的协方差矩阵。4.根据权利要求3所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤5中，以关键帧c0作为参考帧，基于c0与当前帧间的位姿估计结果进行纯视觉位姿估计，得到滑窗内所有的3d路标点l1,...l
m
以及滑窗内其它各个关键帧相对于参考帧的视觉位姿估计结果其具体过程为：步骤51、根据关键帧c0与当前帧间的位姿估计结果以及关键帧c0与当前帧间的特征点对的坐标值，计算出关键帧c0与当前帧间具有共视关系的3d路标点在关键帧c0坐标系下的坐标值；步骤52、根据步骤51中计算出的坐标值以及关键帧c0与当前帧间具有共视关系的3d路标点在滑窗内其它关键帧的相机观测值，得到滑窗内其它关键帧相对于参考帧的位姿估计初值；步骤53、基于步骤52中的位姿估计初值，利用pnp优化算法得到滑窗内所有3d路标点l1,...l
m
和其它各关键帧相对于参考帧的视觉位姿估计结果其中，滑窗的长度为10，即滑窗内有包括当前帧在内的10个关键帧，代表滑窗内的各关键帧相对于参考帧的视觉位姿估计结果。5.根据权利要求4所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤6的具体过程为：征在于，所述步骤6的具体过程为：征在于，所述步骤6的具体过程为：其中，和为第i个imu时刻imu测量得到的imu坐标系下加速度和角速度，b
a
和b
g
为第i个imu时刻的加速度计和陀螺仪的偏置，b
k
表示第k帧imu坐标系，和分别为第i个imu时刻相对于b
k
的位移预积分，速度预积分和旋转预积分，为当前时刻相对于b
k
的旋转矩阵，代表旋转运算符，δt为当前时刻与上一时刻的时间间隔，是从imu陀螺仪得到的当前第i时刻到第i+1时刻的姿态变化估计值，和分别为第i+1个imu时刻相对于b
k
的位移预积分，速度预积分和旋转预积分；在理想情况下，视觉运动估计所得的旋转等于imu预积分得到的旋转值，则有目标函数：其中，为由视觉运动估计得到的b
k+1
相对于第c0帧imu坐标系的旋转，为由视觉运动估计得到的b
k
相对于第c0帧imu坐标系的旋转，b
k+1
表示第k+1帧imu坐标系，为理想情
况下imu旋转预积分得到的b
k+1
相对于b
k
的旋转，b代表滑窗中的所有帧的序号，||
·
||代表范数；已知实际的imu旋转预积分的值理想的imu旋转预积分由下式求得：其中，为旋转预积分相对于陀螺仪偏置的雅克比矩阵，δb
g
为陀螺仪偏置的变化量；经过化简和cholesky分解得：其中，上角标t代表矩阵的转置，上角标-1代表矩阵的逆；求解出陀螺仪的偏置的变化值δb
g
后，利用新的偏置b
g
＝b
g
+δb
g
对imu预积分值进行重新计算。6.根据权利要求5所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤8的具体过程为：对于滑窗内的任意一个3d路标点，遍历该3d路标点在滑窗中每个关键帧上的测量值，测量值包括该3d路标点在相机归一化平面上的2d坐标和在深度图像上的深度测量值；若该3d路标点在关键帧上存在深度测量值，且深度测量值小于3m，则根据步骤3建立的深度相机深度测量误差模型得到深度测量噪声方差将该3d路标点的2d像素坐标测量噪声方差记为则该3d路标点的三维测量协方差为：其中，i2为2阶单位矩阵，为该3d路标点在相机中2d像素坐标(u
i
,v
i
)和深度测量值z
i
的雅克比；其中，f
x
,f
y
表示相机x,y方向的焦距，c
x
,c
y
表示相机x,y方向的光心；通过滑窗内帧间的位姿变换将该3d路标点转换到c0帧相机坐标系中，则将协方差转换到c0帧相机坐标系下为：其中，为c0帧相机坐标系下的协方差；
利用得到新的深度测量噪声方差将该3d路标点的深度测量值小于3m且重投影后2d观测误差小于设定阈值的n个观测投影到相机坐标系下，每个观测的投影记为d
′
i
，i＝1,2,
·
,n，则该3d路标点在c0帧中的联合深度估计值为为若该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类特征点为离点不再参与后续优化，在本轮循环结束后被剔除。7.根据权利要求6所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤9的具体过程为：对于筛选出的3d路标点l
j
，遍历在滑窗中所有共视帧中的观测值，若在当前帧c
k
中路标点l
j
的观测值包含2d像素坐标(u
k,j
,v
k,j
)和深度测量值d
k,j
，且深度测量值在3m以内，则建立带有深度约束的特征点投影误差模型：立带有深度约束的特征点投影误差模型：其中，r
k,j
为3
×
1的误差向量，[
·
]
1,2
代表误差向量r
k,j
的前两行，[
·
]3代表误差向量的第三行，代表的第三行，π(
·
)表示相机投影方程，是的前两行，为3d路标点l
j
转换到c
k
帧相机坐标系下的坐标；帧相机坐标系下的坐标；由下式求得：其中，和为c
k
帧到c0帧的旋转和平移，若3d路标点l
j
在当前帧c
k
中的深度测量值在3m以外或不含深度测量值，说明当前帧c
k
中深度测量值不可靠，则采用不含深度约束的特征点投影误差模型：其中，r
′
k,j
为2
×
1的误差向量。8.根据权利要求7所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤10的具体过程为：
将当前帧c
k
的相机坐标系在世界坐标系下的位姿转换到当前帧c
k
的imu坐标系在世界坐标系下的姿态再将转换为平面位姿则其中，表示移动机器人相机坐标系到imu坐标系的位姿转换，v(
·
)表示对位姿z方向坐标、俯仰角和横滚角进行归零操作；则平面约束误差方程r
plane
为：其中“∨”代表反对称矩阵到向量的变换。9.根据权利要求8所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述步骤11的具体过程为：其中，为imu预积分约束模型，为b
k
到b
k+1
间的预积分值，为b
k
到b
k+1
间的位移预积分，为b
k
到b
k+1
间的速度预积分，为的变化量，为的变化量，δb
a
为b
a
的变化量，为待优化变量，其中，和分别为b
k
相对于世界坐标系的平移，速度和旋转值，为b
k+1
相对于世界坐标系的平移值，分别为b
k+1
相对于世界坐标系的速度值，和为b
k
中加速度计和陀螺仪的偏置，和为b
k+1
中加速度计和陀螺仪的偏置，为对应的四元数形式，为对应的四元数形式，分别为b
k+1
相对于世界坐标系的旋转值，[
·
]
xyz
代表四元数的x,y,z三项的值，δt
k
为第k帧和第k+1帧间的时间差，g
w
为世界坐标系下的重力向量。10.根据权利要求9所述的一种基于深度相机和惯性融合的移动机器人定位方法，其特征在于，所述全局优化目标函数为：其中，第一项为边缘化的先验信息，r
p
为边缘化残差，h
p
为边缘化后的海森矩阵；第二项为imu测量残差项，为imu测量对应的协方差；第三项为带有深度测量约束的视觉重
投影残差，为对应的协方差；第四项为不带深度测量的3d-2d视觉重投影残差，为对应的协方差；第五项r
plane
为平面约束残差项，为r
plane
对应的协方差；为滑窗中的3d路标点l在c
j
帧中的观测值；通过对全局优化目标函数进行优化，得到滑窗内各帧的位姿估计结果。

技术总结

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