一种基于非线性优化的视觉里程计方法

1.本发明属于图像处理与自动化控制领域，特别是一种基于非线性优化的视觉里程计方法。


背景技术：

2.随着计算机视觉、人工智能等领域的高速发展，移动机器人定位技术的研究引起了广泛的关注。视觉里程计是判断机器人在环境中所在位置的一个十分重要方法之一，视觉里程计融合imu和轮式编码器能够更好的优化相机运动轨迹，同时，视觉里程计能够适应非结构化场景和相似场景。因此，研究视觉里程计在移动机器人中十分重要，如：人机交互、虚拟现实和自动驾驶等。
3.广义的里程计泛指对于机器人定位的研究。一般来说，里程计方法可以分为基于激光雷达的方法和基于视觉的方法两大类。基于激光雷达的方法的基本思想是激光雷达探测障碍物距离然后反射等特征，结合机器人的先验信息，估计机器人的位置。而基于视觉的方法则是直接提取图像特征，匹配图像然后根据icp算法估计机器人运动，从而进行定位。通过对比分析，基于激光雷达的方法获得的精度较高，但对环境要求也较高，例如在相似环境中基于激光雷达里程计的机器人定位失败的概率相较于基于视觉里程计的高；而基于视觉的方法对大场景、相似环境和非结构化场景更好，但模型的定位精度低于激光雷达里程计。随着多传感器数据融合技术的不断发展，基于非线性优化的视觉里程计受到了广泛的关注。
4.目前，虽然对机器人定位技术的研究取得了很大的进步，但由于环境的复杂性和非结构化，通用模型获得的精度有限，同时，设备的机械形变会对实验结果产生较大影响，会降低定位精度。目前主流的视觉里程计是通过将imu紧耦合到视觉框架中，其中具有代表性的视觉惯性里程计系统有orb-slam3算法和vins-mono算法。但在平整的地面上采取如匀速运动时，相机运动绝对尺度估计误差较大。


技术实现要素：

5.本发明旨在解决以上现有技术的问题。提出了一种基于非线性优化的视觉里程计方法。本发明的技术方案如下：
6.一种基于非线性优化的视觉里程计方法，其包括以下步骤：
7.s1，对相机图像流进行灰度图转换、去畸变预处理，利用orb(面向fast角点和旋转二进制描述子)特征检测进行图像特征点提取以及利用opencv(开源计算机视觉库)进行图像特征点匹配；
8.s2，利用icp(迭代最近点)算法进行初步相机运动估计；
9.s3，利用imu测量移动机器人的加速度和角速度；
10.s4，利用轮式编码测量移动机器人的左右轮线速度；
11.s5，利用非线性优化将获得的imu数据和轮式编码器数据约束相机轨迹，输出最终
相机估计结果。
12.进一步的，所述步骤s1的图像特征点提取具体包括：
13.对于图像提取分为两步，关键点和描述子，使用改进的fast角点算法提取关键点，用于检测局部像素灰度变化明显的地方，改进的fast角点算法流程为：
14.a)在图像中选取像素p，假设它的亮度为i
p
；
15.b)设置一个亮度阈值t；
16.c)以像素p为中心，选取半径为3的圆上的16个像素点；
17.d)假设选取的圆上有连续n个点的亮度大于t+i
p
或小于t-i
p
那么像素p就认为是一个fast角点；
18.e)对图像中的所有像素点执行步骤d)。
19.进一步的，所述步骤s1的图像特征点提取具体包括：
20.对于图像提取分为两步，关键点和描述子，使用brief(二进制鲁棒独立初级特征)描述所有点，brief算法流程为：
21.a)对图像进行高斯滤波(方差为2，高斯窗口为9x9)；
22.b)以特征点为中心，取sxs(一般s＝31)的领域大窗口。在大窗口中随机选取两个5
×
5的子窗口，比较子窗口内的像素和，进行二进制赋值：
[0023][0024]
其中，p(x)，p(y)分别为随机点x＝(u1,v1)和y＝(u2,v2)所在5
×
5窗口的像素和；
[0025]
c)在大窗口中随机选取n(一般取n＝256)对子窗口，重复步骤b的二进制赋值，形成一个二进制编码，这个编码就是对该点的描述，即描述子；
[0026]
进一步的，所述步骤s2中利用icp(迭代最近点)算法进行初步相机运动估计，具体为：
[0027]
采用icp方法，将365
×
480的图像流ik,k＝1,2,3,
…
,n作为输入，其中365
×
480表示每帧图像的大小，对图像进行特征点提取，利用快速近似最邻近算法进行特征点匹配后，计算得到初步相机运动轨迹为：
[0028]
t
0:t
∈{r3×
30:t
,p3×1}
ꢀꢀꢀ
(11)
[0029]
其中，为0到t时刻三维旋转矩阵，p3×1为平移向量；
[0030]
在k时刻，从图像帧ik中提取一组特征点；重投影函数π(
·
)将当前相机帧中表示的第l个地图点投影到图像坐标系
[0031][0032]
其中，表示特征点的测量坐标，η
kl
表示测量噪声，它服从均值为零，协方差为的正态分布(c
kl
表示在第k帧中特征点l)，重投影函数π(
·
)的参数由相机的内参决定，这些内参可以通过相机标定得到，相机的重投影误差表示为：
[0033][0034]
其中，表示相机测量得到的坐标。
[0035]
进一步的，所述步骤s3，利用imu测量移动机器人的加速度和角速度，具体包括：
[0036]
非线性优化中包含imu数据和左右轮编码器数据误差项，共同约束相机运动轨迹估计，利用预积分模型将误差项分离得到imu测量数据误差项为：
[0037][0038]
其中表示在第i到第j关键帧的imu测量误差，它由旋转、速度和平移三部分误差组成，分别表示旋转矩阵误差、速度误差以及平移误差，log(
·
)为指数映射，将旋转矩阵映射为一个向量、rj、分别表示第j关键帧的旋转矩阵和它的逆，vj为第j关键帧速度，vi为第i关键帧速度、g为加速度、δt
ij
为第j关键帧和第i关键帧之间的时间差值，为第j关键帧和第i关键帧之间的速度差值，为第i关键帧角速度偏移，为第i关键帧的加速度偏移，pj、pi、分别为第j关键帧和第i关键帧的平移以及第i关键帧和第j关键帧之间的平移量，和表示预积分测量值(预积分模型是指，测量值发生变化时不需要进行重新积分，而是通过修正进行调整预积分量)。
[0039]
进一步的，所述s4，利用轮式编码测量移动机器人的左右轮线速度，具体包括：非线性优化中包含左右轮编码器数据误差项，共同约束相机运动轨迹估计，利用预积分模型将误差项分离得到编码器测量数据误差项为：
[0040][0041]
其中，r
(
·
)
表示(
·
)的误差项，ei或e
iej
表示i在编码器系下的i帧或i到j帧，p
(
·
)
表示(
·
)的平移，δ
(
·
)
表示(
·
)的增量，r
(
·
)
表示(
·
)的旋转矩阵，表示(
·
)的测量值。
[0042]
进一步的，所述步骤s5利用非线性优化将获得的imu数据和轮式编码器数据约束相机轨迹，输出最终相机估计结果，具体包括：
[0043]
非线性优化采用三线程并行的结构，而并行结构的每个分支都实时采集传感器数据，每个线程将计算得到的传感器误差项加入到非线性优化系统中，约束初步估计的相机轨迹，将优化后的轨迹作为新的相机轨迹进行机器人定位。
[0044]
进一步的，所述步骤s5非线性系统将相机轨迹误差项、imu测量误差项和轮式编码器车辆误差项作为输入进行优化，其公式表示为：
[0045][0046]
其中，是输入，是经过非线性优化之后的输出，r
p
、和分别为相机投影误差、imu测量误差和轮式编码器测量误差，σ
p
、σi和σe分别为相机误差协方差、imu误差
项协方差和编码器误差项协方差。
[0047]
本发明的优点及有益效果如下：
[0048]
本发明针对于不同环境存在差异性，通用模型获得精度有限，且在单一传感器获取数据少的状态下，移动机器人定位精度欠佳的问题，提出了一种非线性优化的视觉里程计方法，对初步估计的相机轨迹进行优化。并在模型中融合imu和轮式编码器的误差项，增强视觉里程计的鲁棒性。
[0049]
通过在公开数据集tum上测试，实验表明，当融imu和轮式编码器的信息，利用非线性优化进行数据融合，移动机器人相机轨迹的绝对轨迹误差最小，可见非线性优化的引入可以有效地校准定位估计结果，降低运动估计的绝对轨迹误差，且融合imu和轮式编码器信息可以使系统对移动机器人定位具有较好的鲁棒性。利用三线程技术在同一时刻对三个传感器的数据进行同时处理，利用式(14)和式(15)可以快速得到imu和轮式编码器的误差项，利用式(16)进行迭代优化，最终得到包含imu和轮式编码器误差项限制的相机运动轨迹。为了更明确地进行不同模型对移动机器人的定位估计效果的对比，将本文提出的算法模型与其他基于非线性优化的移动机器人定位估计方法进行对比，本文提出的模型对运动估计的绝对轨迹误差更小，取得了优越的性能。
附图说明
[0050]
图1是本发明提供优选实施例基于非线性优化的视觉里程计框架图；
[0051]
图2为非线性优化的因子图。
具体实施方式
[0052]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、详细地描述。所描述的实施例仅仅是本发明的一部分实施例。
[0053]
本发明解决上述技术问题的技术方案是：
[0054]
s1，对相机图像流进行预处理，利用orb算法进行图像特征点提取以及图像特征点匹配。具体为，对于图像提取分为两步，关键点和描述子。我们使用改进的fast算法提取关键点，主要检测局部像素灰度变化明显的地方，检测过程为：
[0055]
a)在图像中选取像素点p，假设它的亮度为i
p
；
[0056]
b)设置一个亮度阈值t；
[0057]
c)以像素p为中心，选取半径为3的圆上的16个像素点；
[0058]
d)假设选取的圆上有连续n个点的亮度大于t+i
p
或小于t-i
p
那么像素p就认为是一个fast角点；
[0059]
e)对图像中的所有像素点执行步骤d)。
[0060]
对于图像提取分为两步，关键点和描述子，使用brief(二进制鲁棒独立初级特征)描述所有点，brief算法流程为：
[0061]
a)对图像进行高斯滤波(方差为2，高斯窗口为9x9)；
[0062]
b)以特征点为中心，取sxs(一般s＝31)的领域大窗口。在大窗口中随机选取两个5
×
5的子窗口，比较子窗口内的像素和，进行二进制赋值：
[0063][0064]
其中，p(x)，p(y)分别为随机点x＝(u1,v1)和y＝(u2,v2)所在5
×
5窗口的像素和；在大窗口中随机选取n(一般取n＝256)对子窗口，重复步骤b的二进制赋值，形成一个二进制编码，这个编码就是对该点的描述，即描述子；
[0065]
s2，利用icp算法进行初步相机运动估计，具体为，将365
×
480的图像流ik,k＝1,2,3,
…
,n作为输入，其中365
×
480表示每帧图像的大小，对图像进行特征点提取预，利用快速近似最邻近算法进行特征点匹配后，计算得到初步相机运动轨迹为：
[0066]
t
0:t
∈{r3×
30:t
,p3×1}
ꢀꢀꢀ
(18)
[0067]
其中，为0到t时刻三维旋转矩阵，p3×1为平移向量。
[0068]
然后使用针孔相机模型，在k时刻，从图像帧ik中提取一组特征点。重投影函数π(
·
)将当前相机帧中表示的第l个地图点投影到图像坐标系
[0069][0070]
其中，表示特征点的测量坐标，η
kl
表示测量噪声，它服从均值为零，协方差为的正态分布。重投影函数π(
·
)的参数由相机的内参决定，这些内参可以通过相机标定得到，相机的重投影误差表示为：
[0071][0072]
s3，包含imu数据和左右轮编码器数据误差项，共同约束相机运动轨迹估计，利用预积分模型得到imu测量数据误差项和左右编码器测量数据分别为：
[0073][0074]
其中表示在第i到第j关键帧的imu测量误差，它由旋转、速度和平移三部分误差组成，分别表示旋转矩阵误差、速度误差以及平移误差，log(
·
)为指数映射，将旋转矩阵映射为一个向量、rj、分别表示第j关键帧的旋转矩阵和它的逆，vj为第j关键帧速度，vi为第i关键帧速度、g为加速度、δt
ij
为第j关键帧和第i关键帧之间的时间差值，为第j关键帧和第i关键帧之间的速度差值，为第i关键帧角速度偏移，为第i关键帧的加速度偏移，pj、pi、分别为第j关键帧和第i关键帧的平移以及第i关键帧和第j关键帧之间的平移量，和表示预积分测量值(预积分模型是
指，测量值发生变化时不需要进行重新积分，而是通过修正进行调整预积分量)。
[0075][0076]
其中，r
(
·
)
表示(
·
)的误差项，ei或e
iej
表示i在编码器系下的i帧或i到j帧，p
(
·
)
表示(
·
)的平移，δ
(
·
)
表示(
·
)的增量，r
(
·
)
表示(
·
)的旋转矩阵，表示(
·
)的测量值。
[0077]
s4，所述非线性优化采用三线程并行的结构，而并行结构的每个分支都实时采集传感器数据，每个线程将计算得到的传感器误差项加入到非线性优化系统中，约束初步估计的相机轨迹。将优化后的轨迹作为新的相机轨迹进行机器人定位。
[0078]
s5，所述非线性系统将相机轨迹误差项、imu测量误差项和轮式编码器车辆误差项作为输入进行优化，其公式表示为：
[0079][0080]
其中，是输入，是经过非线性优化之后的输出，r
p
、和分别为相机投影误差、imu测量误差和轮式编码器测量误差，σ
p
、σi和σe分别为相机误差协方差、imu误差项协方差和编码器误差项协方差。
[0081]
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
[0082]
以上这些实施例应理解为仅用于说明本发明而不用于限制本发明的保护范围。在阅读了本发明的记载的内容之后，技术人员可以对本发明作各种改动或修改，这些等效变化和修饰同样落入本发明权利要求所限定的范围。