Vins-mono概述

本文详细解析了VINS-Mono视觉惯性导航系统的关键模块,包括数据预处理(光流追踪和预积分)、初始化、滑窗非线性优化、闭环检测、以及图优化过程。通过实例展示了如何使用ROS和相关函数实现这些功能。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

        上图为vins-mono算法框图,来自vins相关论文。

         VINS的功能模块可包括五个部分:从上到下从左到右分别为,数据预处理、初始化、滑窗非线性优化BA、闭环检测及全局图优化。

        代码中主要开启了四个线程,分别是:前端图像跟踪、滑窗非线性优化BA(其中初始化和 IMU 预积分在这个线程中)、闭环检测、全局图优化。

  1. 数据预处理
    1. 1光流追踪

        Harris角点加KLT金字塔光流跟踪,输出跟踪上的特征点信息给滑窗。

        Harris角点提取,采用opencv的goodFeatureToTrack()函数。

        KLT金字塔光流跟踪,采用opencv的CalcOpticalFlowPyrLK()函数。

        输出特征点信息为如下图。

             1.2预积分

        预积分只关心两帧之间的变化量,与上一状态无关。

        位移P,姿态Q,速度V采用预积分公式计算。姿态Q采用四元素表示。预积分公式,参考vins论文,用的知识,分别为:a.加速度计、陀螺仪模型;b. 加速度、速度、位移积分公式属于初中数学知识;c. 姿态Q的积分,用了四元素对时间求导以及四元素乘法的相关知识。

        协方差传递,采用误差卡尔曼滤波的predict的协方差传递公式。

     2. 初始化

        初始化如下五个参数:gyro零偏、外参(旋转量)、重力加速度初始方向、尺度和速度。

       初始化过程:视觉和IMU积分的松耦合,各自计算轨迹,凑成一对。用视觉初始化惯导相关参数。先初始化外参(旋转量)、gyro零偏、重力加速度初始方向,因为此三个参数与尺度无关。最后初始化尺度和速度。

      3. 滑窗

        滑窗非线性优化BA将视觉约束、IMU约束和闭环约束放在一个大的目标函数中进行非线性优化,求解滑窗 内所有帧最优位姿。

滑窗优化示意图

        上图来自vins相关论文。

        x0到x4,代表机器的位姿,是被优化量。机器分别在这几个位姿拍摄了图片,观察到了一些路标f0到f3 。黄色的线条代表惯导测到的位姿之间的约束关系,黑色的线条代表视觉测到的位姿和路标之间的约束关系。xv0到xv4为闭环检测从全局图中找到回环帧。

        非线性优化BA的目的,就是通过滑窗内多副图片的视觉、惯导约束关系来求解位姿x0到x4最优解,以保证机器位姿的准确性。

        滑窗输出关键帧给后端回环。输出的关键帧,包括关键帧的姿态信息,地图点信息。

     4. 回环检测

        滑窗输出的的关键帧进行如下处理。

        用fast额外提取了些特征点。在原图上计算BRIEF描述符。

        DBoW查找4个得分最高的备选帧,从大到小排序。得分筛查。同时还要看id,id要尽量小。这里只选一帧,最近50帧不进行回环检测。

        接下来备选帧与当前关键帧进行匹配。先是描述符匹配,大于25个匹配对时继续,PNP及随机一致性(opencv的函数)去除误匹配同时求出回环帧在当前vio下的位姿,如果匹配数大于25个就算成功,同时更新两帧之间的转换关系存在loop_info中并发布回环帧到滑窗。两帧不在同一个sequence中时,要进行sequence合并。

    5. 图优化

 

       上图来自vins相关论文。

        每个关键帧为图中的一个节点。

        顺序边,每个关键帧往前找5帧,建立顺序边。顺序边的建立方法为两个关键帧的位姿差,原点为当前关键帧。

        残差构建,就是优化后的位姿差与建立的顺序边之间的差值。

        采用ceres库,构建顶点和边,以及残差。然后求最优解。

​​​​​​​    6. 数据集运行

        启动文件launch,启动各个节点。

        配置文件yaml,配置所有参数。

        打开三个终端,分别启动vins_estimator,rviz和播放bag文件。以MH_01为例:

       roslaunch vins_estimator euroc.launch

       roslaunch vins_estimator vins_rviz.launch

       rosbag play YOUR_PATH_TO_DATASET/MH_01_easy.bag

Ros 的rviz的数据流图。5个path应该是对应5个sequence地图。

### VINS-Mono 中时间戳同步错误及其解决方案 #### 时间戳同步的重要性 在VINS-Mono中,传感器之间的时间同步至关重要。由于该系统依赖于来自不同源的数据融合——即相机图像和IMU测量值之间的紧密协作,任何一方的时间偏差都会影响到整体性能以及最终的状态估计精度[^1]。 #### 常见的时间戳问题描述 当处理ROS消息时,如果发现时间戳异常,则会触发系统的自我保护机制:清空当前状态并向外界广播重启信号。这种设计是为了防止因时间错乱而导致更严重的后果,比如位置估算失真或完全失败[^3]。 #### 解决方案概述 针对上述提到的时间戳不一致情况,建议采取如下措施: - **验证硬件同步** 确认所使用的摄像头与IMU设备本身具备良好的硬件级同步能力。对于某些特定型号的组合,可能需要查阅制造商文档来了解其内置的时间对齐特性。 - **软件层面调整** 如果确认不是由硬件引起的问题,在软件方面可以通过以下方式尝试修正: - 调整`/config/vins.yaml`文件中的参数设置,特别是那些涉及时间窗口大小(`time_window_size`) 和最大允许延迟 (`max_delay`) 的选项; - 对接收到的消息实施额外的时间戳校验逻辑,确保每条记录都处于合理范围内;一旦超出预设阈值立即丢弃并发出警告日志; - **中间件优化** 使用如Chrony这样的网络时间协议(NTP)服务保持各个节点间钟表的一致性,减少因为计算平台内部差异带来的误差累积效应。 ```cpp // 示例代码片段展示如何检查并过滤掉具有不合理时间戳的消息 void processImuData(const sensor_msgs::ImuConstPtr& imu_msg){ double current_time = ros::Time::now().toSec(); if (abs(imu_msg->header.stamp.toSec() - current_time) > max_allowed_delay_){ ROS_WARN("Detected IMU message with invalid timestamp, skipping..."); return; } // 继续后续处理... } ```
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值