文摘

提出了一个算法基于多约束卡尔曼滤波器(MSCKF)算法来构造机器人特殊的地图穷人GPS信号环境。我们可以计算出机器人的位置与收集到的数据和惯性测量单元的特征提取的相机MSCKF算法紧一些。本文侧重于优化位置的方式,因为我们采用卡尔曼增益计算更新机器人的状态。为了减少处理时间,我们设计一种新颖的快速高斯牛顿MSCKF完成非线性优化算法。与传统MSCKF算法的性能相比,小说fast-location算法可以减少kitti数据集的处理时间。

1。介绍

机器人的里程表可以分为两个部分:运动传播和状态更新结束1,2]。运动传播计算运动矩阵数据的传感器。然后,通过更新结果更新结束。许多机器人采用构造视觉里程计的相机,因为相机的安装更方便。(3]视觉里程计从视频帧中提取的匹配特性传播运动矩阵。然而,有许多缺点在视觉里程计等长的处理时间和由于失配误差特性。为了减少处理时间,一些研究者应用两个线程来计算运动方程和更新结果,分别是(4,5]。然而,它将提高定位系统的硬件成本。为了提高处理时间没有额外的硬件,Chansoo角落特性用来代替普通的特性(6]。然而,信息的功能有时会失去很难计算机器人的位置。此外,很难提取角特性的结构缺陷的环境。通过这种方式,系统的误差变大。为了减少处理时间,提高视觉里程计的准确性的同时,越来越多的研究人员开始采用不同的传感器,以弥补缺点的视觉里程计(7- - - - - -12]。GPS可以得到机器人的精确定位的户外环境。惯性测量单元(IMU)可以弥补这个错误在可怜的GPS信号环境中(13,14]。对机器人的应用程序在穷人GPS信号环境下,许多研究人员采用IMU构建视觉惯性里程表(VIO) [15- - - - - -19]。为了计算运动矩阵和构建VIO机器人的地图,我们采用MSCKF算法。MSCKF算法使用数据的运动传播IMU传播运动矩阵。的update-end MSCKF算法下的观测约束功能更新运动矩阵扩展卡尔曼滤波器(EKF)框架(16,20.,21]。然而,传统的性能MSCKF处理时间和准确性是不好的。他等人扩大了范围的观察到的特性来改善MSCKF算法的准确性(22]。李等人构建的模型误差对重力加速器和IMU的偏移误差改善MSCKF算法的准确性(23,24]。一些研究人员加入闭环检测更新机器人的初始点(24,25]。这些方法提高了系统的准确性,但增加处理时间(25]。为了提高处理时间,Eckenhoff等人提出了在线验证减少更新时间和改善的准确性MSCKF算法虽然会增加硬件成本(26]。世宗Heo和其他应用的滑动窗口来代替普通的方法提取的特征MSCKF算法减少计算量,提高了处理时间(17,27,28]。然而,处理时间没有显著减少。一些研究人员专注于改善MSCKF算法的后端改善处理时间因为MSCKF结束更新算法基于EKF滤波将花费太多的时间过程。一些研究者应用容积卡尔曼滤波器(位置)和最大似然估计(标定)方法来取代EKF虽然处理时间的性能仍不理想(29日,30.]。Alibay等人应用阶段随机抽样共识(RANSAC)方法传播运动矩阵满足实时的要求(31日]。尼克里奇应用现场可编程门阵列(FPGA)完成数据的预处理IMU减少处理时间(32]。然而,这些方法会增加硬件成本。

为了减少处理时间,我们设计新颖快速高斯牛顿MSCKF算法无需额外的硬件成本。我们工作的重点是计算位置和姿态新颖的非线性优化的方法。然后,我们用结果来计算卡尔曼增益卡尔曼滤波器框架。

MSCKF算法是基于卡尔曼滤波器结构。该算法由三个部分组成:传播,增加和更新16]。我们使用的IMU传播状态的机器人状态传播。传播方程如下(16]: 在哪里F意味着IMU状态矩阵的系数矩阵计算的加速器IMU和陀螺仪数据矩阵和矩阵G意味着噪声矩阵的系数矩阵。然后,我们计算雅克比矩阵完成增加。我们定义的矩阵的状态。j帧的象征 。协方差矩阵的矩阵构造乌兹别克斯坦伊斯兰运动的状态 ,IMU和相机之间的协方差矩阵 ,和协方差矩阵的摄像机 :

考虑之间的关联特性,MSCKF算法更新传播通过观察到的特性的集合。同时,MSCKF算法消除了功能未被注意的,一文不值。然后,我们可以计算出reprojection错误的观察特性与非线性优化更新状态矩阵。现在,观测方程表示如下: 在哪里r意味着残留误差的特性,H表示雅各宾派的矩阵, 通过上面的运动方程表示国家传播。现在,我们需要计算矩阵H卡尔曼增益。的残留误差可以计算的实际位置之间的区别j功能和预期价值的位置。这个过程表示如下:

通过这种方式, 表示的估计位置。j没有观察到的特性。我相机。 就意味着没有的位置。j特性。为了减少计算量,QR分解 加速方程的解决方案。然后,我们得到上三角矩阵 这是满秩矩阵:

现在,我们可以使用 卡尔曼增益: 在哪里P相机的状态矩阵和手段 意味着噪声矩阵。协方差的状态。j+ 1帧可以更新如下:

3所示。我们的方法

的价值 决定了卡尔曼增益的结果 在方程6。计算 成本最处理时间,传统方法采用高斯牛顿优化方法计算的价值 ,用来计算 。为了减少处理时间,我们采用快速高斯牛顿法进行非线性优化。优化的整个过程分为两个阶段。

在第一阶段,区别太大了,我们使用梯度下降法来优化。我们做的泰勒展开方程如下:

我们可以得到的梯度下降法 是 。然而,梯度下降方法可能会导致不同的结果,因为迭代步长太大了。

在第二阶段,我们使用高斯牛顿法 当优化值非常接近前值保持算法的收敛性。高斯牛顿算法最小化的异议 。因此,我们的解决方案推导。我们得到了高斯牛顿方程:

然后,我们可以计算的价值 如下:

为了计算运动的真正价值矩阵,我们把之前的价值坐标到上面的方程。如果我们描述的prior-estimated价值的位置。j框架 如下: 在哪里 意味着prior-estimated值的位置jth框架和 意味着旋转四元数的矩阵,我们可以改变上述方程通过归一化如下:

为了更清晰的表达,我们描述如下:

现在,我们可以更新 如下:

现在,我们更新与前值之间的误差估计价值如下:

之前的值 是归一化值。我们可以使用 为的影响迭代站。我们描述 如下:

在上面的方程中,R表示噪音的重量包括陀螺仪的偏差和偏差的加速器速度。为了反映推导的效果,我们描述的区别之前价值和优化的价值 :

在第一阶段,当 大于阈值 ,我们选择梯度下降法的价值 。顺便说一下,的价值 经验值为0.5。现在,我们描述了一步的衍生物 如下: 在哪里 意味着错误的雅可比矩阵 。在第二阶段,当优化值接近真实的那些手段的价值 小于 。为了保证收敛的矩阵,我们选择高斯牛顿完成非线性优化算法。的步骤 表示如下:

如果 小于0.01,该算法是收敛的,我们可以停止迭代。

4所示。实验

本文基于kitti数据集的实验。数据的预处理是提取特征和检查摄像机的外部参数(33]。处理的计算,我们选择的价值偏见的陀螺仪和加速度 。我们也选择相同的值作为加速器的噪声和陀螺仪。结果如表所示1。

表1
小说MSCKF算法的性能与第一数据集。

有98帧1号表的数据集1。比较传统的MSCKF算法的处理时间,我们发现处理时间减少了20.7%,当我们采用小说快速高斯牛顿MSCKF算法。然后,我们计算出最终的错误由不同的算法计算结果之间的差异和实际价值。比较的最终错误MSCKF算法仅从IMU数据,最后传统MSCKF误差降低了56.5%。小说的最后一个错误快速高斯牛顿MSCKF减少了55.1%。最后的误差计算的小说MSCKF算法增加3.0%比传统MSCKF计算的算法。

有239帧在表2号数据集2。比较传统MSCKF算法的处理时间,小说的处理时间快速高斯牛顿MSCKF下降了6.0%。比较传统的最终误差MSCKF仅与IMU的数据,传统的最终误差MSCKF增加了55.0%。小说的最后一个错误高斯牛顿MSCKF算法相比,增加了6.0%的最终误差传统MSCKF算法。

表2
改进MSCKF算法的性能与2号数据集。

然后,我们可以比较传统MSCKF轨迹计算的算法和计算的小说MSCKF算法。为了明确结果,我们也比较传统的轨迹计算MSCKF使用数据仅IMU与全球真实轨迹。

有计算出的轨迹不同的算法。蓝色的轨迹计算的传统MSCKF算法如图1。蓝色的人物之一2意味着小说高斯牛顿MSCKF轨迹计算的算法。有红色的轨迹计算数据的常规MSCKF IMU独自住在两个数字。有全球真实轨迹在绿色两个数字。我们可以得到的结论是,蓝色的轨迹接近红色的轨迹比绿色的轨迹图1。这意味着传统MSCKF算法可以从IMU数据融合和立体相机来减少错误。蓝色的轨迹也接近绿色的轨迹比红色的轨迹图2。这意味着小说MSCKF算法也能够融合的数据IMU和相机的特性来减少计算的误差MSCKF乌兹别克斯坦伊斯兰运动的数据。当我们蓝色的轨迹在数据相比,蓝色的轨迹图2更接近图的红色比蓝色的轨迹1在开始。然而,小说的误差算法变得更大,距离变得不再图2。我们也注意到,有一个突然的变化的轨迹图2因为有两种优化方法之间的转换。


图1
传统的轨迹MSCKF算法与数据集。

图2
小说MSCKF算法的轨迹与第一数据集。

我们也计算轨迹通过上述算法2号数据集测试小说MSCKF算法的性能在VIO运行时更长。红色意味着传统MSCKF算法计算的轨迹与IMU数据仅在2号数据集在图3和4。绿色的轨迹意味着地面真理轨迹数据3和4。蓝色的轨迹图3意味着传统MSCKF轨迹计算的算法和2号数据集。蓝色的人物之一4意味着小说的轨迹计算高斯牛顿MSCKF算法与2号数据集。我们注意到蓝色的轨迹图3更接近绿色的。然而,蓝色轨迹就偏离了红色轨迹图的距离就变长时3。这意味着小说MSCKF算法的误差就大签证官运行时更长。蓝色的轨迹变得接近红色的轨迹图4在第一位。然而,当距离x在50米和100米之间,蓝色的轨迹接近绿色的。这意味着小说MSCKF减少计算误差的计算时仍在继续。当的距离x大于100蓝色轨迹的错误变得更大。然而,有更明显的臀部变化比蓝色的轨迹图2。


图3
MSCKF的轨迹与2号数据集。

图4
改善MSCKF的轨迹与2号数据集。

5。结论

小说的表现比较MSCKF算法与传统MSCKF算法,这部小说MSCKF算法的处理时间减少20.7%∼6.0%。最后的错误计算了小说MSCKF算法增加了2.0%∼6.0%比传统MSCKF计算的算法。我们可以得到的结论是,这部小说MSCKF可以减少处理时间。然而,最终误差计算的快速高斯牛顿MSCKF变得更大,当运行时间更长。这意味着小说MSCKF将增加最后的错误。我们可以使用它在低精度和速度快的场合。然后,有明显的时尚变化轨迹计算的小说MSCKF算法因为交换的两种优化方法。我们还发现计算的误差小说MSCKF变得更大的VIO运行时更长。为了减少积累误差,我们可以添加闭环检测。

数据可用性

手稿是kitti数据集的数据可以通过MATLAB。可以获得的数据https://figshare.com/s/395bc65e815a4e4b3f2f。

的利益冲突

作者宣称没有利益冲突。

确认

这项工作是福建科技支持的部门(批准2018 h0003),福建科技大学(格兰特GY-Z17011),泉州科技局(批准2017 g012),教育部福建省(格兰特JT180339)和福建大学技术(JGBK201911)。