VSLAM(VINS)+EKF+MPC=停车场无限循环

1 背景

很奇妙，从一个群里看到一个人的朋友圈，搞定了【VSLAM(VINS)+EKF+MPC=停车场无限循环】，挺感兴趣的，在这里简单分析一下。

视频截图：

2 分析

2.1 VSLAM

根据经验，电脑屏幕上显示的是rviz，VSLAM具体是VINS，但不确定是VINS-mono，还是VINS-fusion，也不确定是单目+IMU，还是双目+IMU。

问题：

• VINS是用的在线的定位模式还是加载地图的重定位模式？

猜测：

• 根据视频展示，车辆在地下停车场自动驾驶，其实是循迹，也就是先建图，并记录轨迹，之后加载地图进行重定位，再使用EKF融合其他传感器，得到最终的pose，最后再使用MPC循迹
• 当然也不能排除就是在线的定位模式的可能性

2.2 EKF

EKF是扩展卡尔曼滤波，SLAM可分为基于滤波的方法和基于优化的方法，但是已经有了基于优化方法的VINS，那么这里的EKF很可能用来做多传感器融合，比如robot_pose_ekf和robot_localization都是用的EKF做多传感器融合，可以融合Odometry，IMU，VO和GPS等进行定位。

2.3 MPC

MPC(Model Predict Control)是一种反馈控制（feedback control）算法，使用模型来预测过程的未来输出。

MPC的特点：

• 支持MIMO系统，便于构建输入输出间的相互影响参数
• 支持方便添加约束条件
• 具有预测能力
• 需要较好的处理器和较大的内存，因为需要大量的在线优化，存储大量的变量

相关参考资料：

• https://blog.csdn.net/zhkmxx930xperia/article/details/87742479

2.4 VSLAM(VINS)+EKF+MPC

从视频效果来看，能够实现在停车场环境的无限循环自动驾驶。

从轨迹来看，也是有误差的，每次的行驶轨迹有差异，尤其是右上角部分，轨迹明显不重合。（轨迹不重合也可能是为了避障）

从这个角度看，VINS又可能是使用的加载地图的重定位模式，不然怎么消除的累计误差？

3 扩展

Autoware的waypoint_follower中使用的轨迹跟踪算法为Pure Pursuit。

参考资料：

• 使用pure pursuit实现无人车轨迹追踪
• Autoware学习笔记waypoint_follower之pure_pursuit