基于改进VINS-Mono算法的状态估计方法

doi:10.12068/j.issn.1005-3026.2024.09.009

摘要/Abstract

摘要：

针对传统的即时定位并建图（simultaneous localization and mapping, SLAM）算法在位姿识别过程中对精度的要求，在VINS-Mono（visual inertial system-Mono）的前端部分，增加了逆向光流法；针对SLAM算法对实时性的要求，在VINS-Mono的后端滑窗优化方法中，融合了一种边缘化优化算法，再对除相机位姿的部分进行边缘化，然后边缘化相机的位姿部分，从而加速边缘化的过程.再使用EuRoc（European robotics challenge）数据集进行实验，结果发现针对前端的改进策略，精度提升不明显，并分析了原因；针对后端的改进策略，改进算法的边缘化时间平均减少了25.9%，又对比了改进算法与源码的轨迹精度，发现误差可控.最后验证了对VINS-Mono后端的改进策略在实时性上具有优越性.

关键词: 状态估计方法, 滑窗, 边缘化, 实时性, ROS（robot operating system）机器人仿真平台

Abstract:

Aiming at the requirement of precision in pose recognition process of traditional SLAM（simultaneous localization and mapping）， the reverse optical flow algorithm is added to the front?end part of VINS-Mono. To meet the real?time requirement of SLAM， a marginal optimization algorithm is integrated into the back?end sliding window optimization method of VINS-Mono， the parts excluding camera pose are marginalized firstly， and then pose parts of cameras are marginalized， to accelerate marginalization process. And experiments are carried out using EuRoc（European robotics challenge）datasets. The results show that accuracy is improved slightly by improvement strategy of the front end， and the possible reasons are analyzed. For the improvement strategy of the back end， it is found that compared with the source code， the time used for the marginalization of the improved algorithm is reduced by 25.9% on average， and then compare their trajectory accuracy. It is found that the error is controllable. Finally， it is verified that the improved strategy for the back?end of VINS-Mono is superior in real?time performance.

Key words: state estimation method, sliding window, marginalization, real?time performance, ROS robot simulation platform

中图分类号:

TP 301

王海芳, 乔鼎杰, 吴天浩, 黄鹏. 基于改进VINS-Mono算法的状态估计方法[J]. 东北大学学报（自然科学版）, 2024, 45(9): 1287-1293.

Hai-fang WANG, Ding-jie QIAO, Tian-hao WU, Peng HUANG. State Estimation Method Based on Improved VINS-Mono Algorithm[J]. Journal of Northeastern University(Natural Science), 2024, 45(9): 1287-1293.

图/表 10

参考文献 16

1	Davison A J， Reid I D， Molton N D，et al.MonoSLAM：real‑time single camera SLAM［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2007，29（6）：1052-1067.
2	Klein G， Murray D.Parallel tracking and mapping for small AR workspaces［C］//IEEE and ACM International Symposium on Mixed and Augmented Reality.Nara，2007：225-234.
3	Engel J， Schöps T， Cremers D.LSD-SLAM：large‑scale direct monocular SLAM［C］//European Conference on Computer Vision.Cham：Springer，2014：834-849.
4	Engel J， Sturm J， Cremers D.Semi‑dense visual odometry for a monocular camera［C］//IEEE International Conference on Computer Vision.Sydney，2013：1449-1456.
5	Forster C， Pizzoli M， Scaramuzza D.SVO：fast semi‑direct monocular visual odometry［C］//IEEE International Conference on Robotics and Automation.Hong Kong，2014：15-22.
6	Newcombe R A， Lovegrove S J， Davison A J.DTAM：dense tracking and mapping in real‑time［C］//2011 International Conference on Computer Vision.Barcelona，2011：2320-2327.
7	Engel J， Koltun V， Cremers D.Direct sparse odometry［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2018，40（3）：611-625.
8	Kerl C， Sturm J， Cremers D.Dense visual SLAM for RGB-D cameras［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Tokyo，2013：2100-2106.
9	Mur‑Artal R， Montiel J M， Tardós J D.ORB-SLAM：a versatile and accurate monocular SLAM system［J］.IEEE Transactions on Robotics，2015，31（5）：1147-1163.
10	Mourikis A I， Roumeliotis S I.A multi‑state constraint Kalman filter for vision‑aided inertial navigation［C］//IEEE International Conference on Robotics and Automation.Rome，2007：3565-3572.
11	Bloesch M， Omari S， Hutter M，et al.Robust visual inertial odometry using a direct EKF‑based approach［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS）.Hamburg，2015：298-304.
12	Lynen S， Achtelik M W， Weiss S，et al.A robust and modular multi‑sensor fusion approach applied to MAV navigation［C］//IEEE/RSJ International Conference on Intelligent Robots and Systems.Tokyo，2013：3923-3929.
13	Leutenegger S， Lynen S， Bosse M，et al.Keyframe‑based visual‑inertial odometry using nonlinear optimization［J］.The International Journal of Robotics Research，2015，34（3）：314-334.
14	Qin T， Li P L， Shen S J.VINS‑Mono：a robust and versatile monocular visual‑inertial state estimator［J］.IEEE Transactions on Robotics，2018，34（4）：1004-1020.
15	Martinelli A.Closed‑form solution of visual‑inertial structure from motion［J］.International Journal of Computer Vision，2014，106（2）：138-152.
16	Lucas B D， Kanade T.An iterative image registration technique with an application to stereo vision［C］//International Joint Conference on Artificial Intelligence.Vancouver，1981：674-679.

数据集	RMSE		对比结果/%
数据集	源码	改进算法	对比结果/%
MH_01	0.181 302	0.179 712	0.88
MH_02	0.182 778	0.180 029	1.50
MH_03	0.405 599	0.404 163	0.35
MH_04	0.393 492	0.402 433	-2.27

数据集	RMSE		对比结果/%
数据集	源码	改进算法	对比结果/%
MH_01	0.181 302	0.179 712	0.88
MH_02	0.182 778	0.180 029	1.50
MH_03	0.405 599	0.404 163	0.35
MH_04	0.393 492	0.402 433	-2.27

步骤	边缘化优化
1	localSize（parameter_block_size[it.first]）>4
2	marg_pose_index_size<-pos,localSize （parameter_block_size[it.first]） marg_pose_size<-marg_pose_size+localSize （parameter_block_size[it.first]） //对要边缘化的变量参数根据维度赋予id
3	m₁<-m-marg_pose_size; n₁<-n+marg_pose_size;
4	for （auto iter: marg_pose_index_size） //遍历marg_pose_index_size的所有元素
5	swap（A_reorder.block（idx,0,size,pos）, A_reorder.block（idx+size,0,m-idx-size,pos）） //将row i移到矩阵的最下面 swap（A_reorder.block（0,idx,pos,size）,A_reorder.block（0,idx+size,pos,m-idx-size）） //将col i移到矩阵的最右面 swap（b_reorder.segment（idx,size）,b_reorder. segment（idx+size,m?idx?size）） //将b i移到向量的最下面
6	tempA<-Arm1 * Amm_inv1; A<-Arr1-tempA * Amr1; b<-brr1-tempA * bmm1; //用Schur消元进行边缘化
7	m₂<-m-m₁;n₂<-n;
8	MatrixXd tempB=Arm * Amm_inv; A=Arr-tempB * Amr; b=brr-tempB * bmm;
	//对剩余的部分进行边缘化

步骤	边缘化优化
1	localSize（parameter_block_size[it.first]）>4
2	marg_pose_index_size<-pos,localSize （parameter_block_size[it.first]） marg_pose_size<-marg_pose_size+localSize （parameter_block_size[it.first]） //对要边缘化的变量参数根据维度赋予id
3	m₁<-m-marg_pose_size; n₁<-n+marg_pose_size;
4	for （auto iter: marg_pose_index_size） //遍历marg_pose_index_size的所有元素
5	swap（A_reorder.block（idx,0,size,pos）, A_reorder.block（idx+size,0,m-idx-size,pos）） //将row i移到矩阵的最下面 swap（A_reorder.block（0,idx,pos,size）,A_reorder.block（0,idx+size,pos,m-idx-size）） //将col i移到矩阵的最右面 swap（b_reorder.segment（idx,size）,b_reorder. segment（idx+size,m?idx?size）） //将b i移到向量的最下面
6	tempA<-Arm1 * Amm_inv1; A<-Arr1-tempA * Amr1; b<-brr1-tempA * bmm1; //用Schur消元进行边缘化
7	m₂<-m-m₁;n₂<-n;
8	MatrixXd tempB=Arm * Amm_inv; A=Arr-tempB * Amr; b=brr-tempB * bmm;
	//对剩余的部分进行边缘化

数据集	源码用时				改进算法用时
数据集	中位数	平均数	最小值	最大值	中位数	平均数	最小值	最大值
MH_01	11 918.92	11 877.99	11 706.68	11 977.77	8 550.97	8 578.00	8 481.91	8 677.89
MH_02	9 715.99	9 738.60	9 567.80	9 931.28	7 105.28	7 059.44	6 643.53	7 436.98
MH_03	9 405.12	9 427.55	9 289.16	9 649.91	7 257.35	7 129.18	6 726.81	7 448.28
MH_04	6 812.63	6 904.28	6 658.04	7 311.03	5 227.38	5 251.68	5 204.86	5 318.54