精华内容
下载资源
问答
  • 22.IMU里程计融合

    万次阅读 多人点赞 2018-06-22 08:47:18
    实际使用中会出现轮子打滑和累计误差的情况,这里单单使用编码器得到里程计会出现一定的偏差,虽然激光雷达会纠正,但一个准确的里程对这个系统还是较为重要 2.IMU数据获取 IMU即为 惯性测量单元,一般包含了三...

    1. 概述

    实际使用中会出现轮子打滑和累计误差的情况,这里单单使用编码器得到里程计会出现一定的偏差,虽然激光雷达会纠正,但一个准确的里程对这个系统还是较为重要

    2. IMU数据获取

    IMU即为 惯性测量单元,一般包含了三个单轴的加速度计和三个单轴的陀螺仪,简单理解通过加速度二次积分就可以得到位移信息、通过角速度积分就可以得到三个角度,实时要比这个复杂许多

    2.1 PIBOT IMU

    PIBOT在嵌入式程序提供出原始的数据接口,通过配置可以输出原始raw_imu topic
    raw_imu
    topic类型为自定义具体如下,即为三轴加速度三轴陀螺仪和三轴磁力计的原始数据
    raw_imu
    raw_imu

    通过对原始数据处理得到一个/imu/data_raw数据类型为sensor_msgs/Imu

    通过ROS提供的相关包imu_tools进行滤波
    可以看到complementary_filter_gain_node会订阅该topic,即该topic作为输入滤波得到最终数据(发布/imu/data topic 类型同样为sensor_msgs/Imu)

    输出该topic可以看到得到的值波动已经较小了,且静止的时候接近于0
    /imu/data

    3. 两种融合的方法

    3.1 一种简单的方法

    从imu得到的数据为一个相对角度(主要使用yawrollpitch 后面不会使用到),使用该角度来替代由编码器计算得到的角度。
    这个方法较为简单,出现打滑时候因yaw不会受到影响,即使你抬起机器人转动一定的角度,得到的里程也能正确反映出来

    3.2 扩展的卡尔曼滤波

    官方提供了个扩展的卡尔曼滤波的包robot_pose_ekf,robot_pose_ekf开启扩展卡尔曼滤波器生成机器人姿态,支持

    • odom(编码器)
    • imu_data(IMU)
    • vo(视觉里程计)
      还可以支持GPS
      引用官方图片

      PR2从实际初始点(绿色)溜达一圈回到初始点(绿色),编码器的里程(蓝色)发生了漂移,而使用robot_pose_ekf融合出来的里程(红色)则跟实际位置基本重合(后面我们会针对这个测试下效果)

    中间的圆是小圆放大的展示效果

    再回去看下该包的输出

    • 发布一个topic, 类型需要注意下是PoseWithCovarianceStamped并非Odometry
      后面会用到这个作为显示,所以还需要一个转换

      查看该topic信息可以看到odom_ekf订阅了该topic
      再次查看该节点信息可以看到
      ,他会发出一个Odometrytopic

    • 发出一个tf

    robot_pose_ekf配置时,做了些映射处理,这样可以保证导航层在使用和不用imu的时候无需修改就可以工作

    bringup.lauch或者bringup_with_imu.launch 输出的tf都为odom → base_footprint ;发出的里程也都是odom
    bringup_with_imu.launch轮子的里程topic 映射为wheel_odom

    这里很重要,后面的对该包的验证会使用到

    下2张图展示了未使用IMU和使用IMU时候的tf tree情况, 可以看到用了一致的frame
    no imu
    use imu

    展开全文
  • 单独使用轮子编码器得到的里程计与融合IMU数据的里程计最终效果如何,我们这里做个测试来对比下。 有2种方式测试: - 手动控制机器人走一圈然后回到之前的原点,通过观察模拟器(RViz)中里程与初始点的偏差 - ...

    里程计直接会作为建图或者导航的时候的输入,所以起着至关重要的做,准确性直接影响建图和导航的效果。单独使用轮子编码器得到的里程计与融合了IMU数据的里程计最终效果如何,我们这里做个测试来对比下。
    有2种方式测试:

    • 手动控制机器人走一圈然后回到之前的原点,通过观察模拟器(RViz)中里程与初始点的偏差
    • 程序控制机器人行走(例如走一个方形),通过观察最终机器人时间与最初原点的偏差

    下面我们使用的都是第一种及标记个原点,控制机器人随机行走一段时间后再控制其回到标记的原点上,打开RViz观察程序计算的机器人位置姿态是否回到原点

    编码器里程计测试

    手动控制

    手动控制apollo"溜达"了两圈回到原点,观察tf坐标系可以看到

    程序显示base_link没有回到原点有一定的误差(位置和姿态都有误差)

    融合IMU的里程测试

    手动控制

    未做对比我们在使用融合IMU的时候,把使用编码器计算出来的里程计显示出来作对比(大的红色箭头就是编码器里程计),同上面我们控制apollo溜达一圈回到原点

    • 初始的时候三个是重合的

    • 行走了一段时间

    可以发现没走多远融合出来的里程(黄色框)和编码器里程(红色框)就有了较大的差距

    -回到原点
    我们继续控制使得apollo回到原点

    这张图可以看出跟官方提供的图较为类似了

    展开全文
  • 主要学习的是港科大今年开源的LINS算法. 该算法是直接在lego上面改的,主要针对的还是16线激光. 算法主要的核心是在激光里程计节点, 只需要重点学习这部分就可以了.

    主要学习的是港科大今年开源的LINS算法.
    该算法是直接在lego上面改的,主要针对的还是16线激光.
    主要模块是:

    1. Feature Extraction Module: 点云分割以及特征提取, 和legoloam的操作一样
    2. LIO module: 融合IMU与激光的里程计.
    3. Mapping Module: 局部地图优化以及回环检测与后端优化, 和legoloam一样
      该算法的核心还是IMU与激光融合的LIO模块, 重点学习这部分就可以了.
      在这里插入图片描述

    1. 开始

    首先理清LIO module相关的文件:
    src/lins_fusion_node.cpp: LIO 节点, 构造了 fusion::LinsFusion 对象.
    src/lib/Estimator.cpp: fusion::LinsFusion 类的实现, 该类相当于ROS接口, 将传感器数据通过ROS传入到算法内部中.
    include/Estimator.h: fusion::LinsFusion 类的声明.
    include/integrationBase.h: IMU 运动学模型相关.
    include/KalmanFilter.hpp: 这里好像只是实现了使用IMU进行EKF状态预测的部分.
    include/StateEstimator.hpp: LIO算法底层库的实现, 包含特征提取等.

    整个结构关系可以简单的表示为:
    在这里插入图片描述

    首先,
    进入lins_fusion_node.cpp, main函数主要的处理是:

    parameter::readParameters(pnh);
    fusion::LinsFusion lins(nh, pnh);
    lins.run();
    
    1. 读取参数 parameter::readParameters(pnh);
    2. 实例化lins对象 fusion::LinsFusion lins(nh, pnh);
    3. 执行run().
      其中 ,run()函数为
    void LinsFusion::run() { initialization(); }
    

    initialization()中主要的操作是:

    1. 构建紧耦合估计器 estimator = new StateEstimator();
    2. 订阅话题
    // Subscribe to IMU, segmented point clouds, and map-refined odometry feedback
      subMapOdom_ = pnh_.subscribe<nav_msgs::Odometry>(
          LIDAR_MAPPING_TOPIC, 5, &LinsFusion::mapOdometryCallback, this);     
      subImu = pnh_.subscribe<sensor_msgs::Imu>(IMU_TOPIC, 100,
                                                &LinsFusion::imuCallback, this);
      subLaserCloud = pnh_.subscribe<sensor_msgs::PointCloud2>(
          "/segmented_cloud", 2, &LinsFusion::laserCloudCallback, this);
      subLaserCloudInfo = pnh_.subscribe<cloud_msgs::cloud_info>(
          "/segmented_cloud_info", 2, &LinsFusion::laserCloudInfoCallback, this);
      subOutlierCloud = pnh_.subscribe<sensor_msgs::PointCloud2>(
          "/outlier_cloud", 2, &LinsFusion::outlierCloudCallback, this);
    

    其中, LIDAR_MAPPING_TOPIC, IMU_TOPIC在 void readParameters(ros::NodeHandle& n) 函数中设置, 进入参数文件可以看到
    在这里插入图片描述

    “/segmented_cloud”, “/segmented_cloud_info”, "/outlier_cloud"话题在 image_projection_node 中发布, 发布函数如下:

    void publishCloud() {
    
        segMsg.header = cloudHeader;
        pubSegmentedCloudInfo.publish(segMsg);       // /segmented_cloud_info
    
        sensor_msgs::PointCloud2 laserCloudTemp;
    
        pcl::toROSMsg(*outlierCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubOutlierCloud.publish(laserCloudTemp);     // 话题 /outlier_cloud  
    
        pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
        laserCloudTemp.header.stamp = cloudHeader.stamp;
        laserCloudTemp.header.frame_id = "base_link";
        pubSegmentedCloud.publish(laserCloudTemp);       // 话题 /segmented_cloud 
        ......
        ......
    

    /segmented_cloud_info话题上发布的是 segMsg,
    /outlier_cloud上发布的是 outlierCloud,
    /segmented_cloud 上发布的是 segmentedCloud
    它们的形成要追溯到 image_projection_node 中的 cloudSegmentation()函数

    void cloudSegmentation() {
        // 每个像素进行BFS聚类   
        for (size_t i = 0; i < LINE_NUM; ++i)
          for (size_t j = 0; j < SCAN_NUM; ++j)
            if (labelMat.at<int>(i, j) == 0) labelComponents(i, j);
    
        int sizeOfSegCloud = 0;                   // 分割后所有分割点的计数  
        for (size_t i = 0; i < LINE_NUM; ++i) {
          // 每一环 (ring) 的有效起始点序号   即提取特征点的起始位置  
          segMsg.startRingIndex[i] = sizeOfSegCloud - 1 + 5;
    
          for (size_t j = 0; j < SCAN_NUM; ++j) {
            // 地面点或分割点  
            if (labelMat.at<int>(i, j) > 0 || groundMat.at<int8_t>(i, j) == 1) {
              // 设置  outlierCloud   聚类失败的点中非地面点且进行5倍降采样   
              if (labelMat.at<int>(i, j) == 999999) {
                if (i > groundScanInd && j % 5 == 0) {   
                  outlierCloud->push_back(fullCloud->points[j + i * SCAN_NUM]);
                  continue;
                } else {
                  continue;
                }
              }
              // 对于地面点也进行降采样  
              if (groundMat.at<int8_t>(i, j) == 1) {
                if (j % 5 != 0 && j > 5 && j < SCAN_NUM - 5) continue;
              }
              // 是否为地面点 
              segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i, j) == 1);
              // 指示当前点的在距离图像上的列数   
              segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
              segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i, j);
              // 分割点    包含 所有有效的聚类     
              segmentedCloud->push_back(fullCloud->points[j + i * SCAN_NUM]);
              ++sizeOfSegCloud;              // 记录分割点的总数  
            }
          }
          // 每一环 (ring) 的有效结束点序号   即提取特征点的终止位置  
          segMsg.endRingIndex[i] = sizeOfSegCloud - 1 - 5;
        }
    
        // 将所有非地面点且为有效聚类的点 组成  segmentedCloudPure  
        if (pubSegmentedCloudPure.getNumSubscribers() != 0) {
          for (size_t i = 0; i < LINE_NUM; ++i) {
            for (size_t j = 0; j < SCAN_NUM; ++j) {
              if (labelMat.at<int>(i, j) > 0 && labelMat.at<int>(i, j) != 999999) {
                segmentedCloudPure->push_back(fullCloud->points[j + i * SCAN_NUM]);
                segmentedCloudPure->points.back().intensity =
                    labelMat.at<int>(i, j);
              }
            }
          }
        }
      }
    

    可以看到,
    outlierCloud 是无效聚类点中 挑选行数大于groundScanInd 并且 经过5倍降采样 所形成的的点云.
    segmentedCloud 是所有有效聚类点+地面点进行5倍降采样所形成的点云.
    segMsg 是自定义消息, 定义如下:

    Header header 
    
    # 每一个ring 特征提取的起始和终止位置的序号 
    int32[] startRingIndex    
    int32[] endRingIndex
    # 当前scan 起始点的角度与终止点的角度   以及总的角度差  
    float32 startOrientation
    float32 endOrientation
    float32 orientationDiff
    # 指示是否为地面点
    bool[]    segmentedCloudGroundFlag
    # 当前点处于距离图像的列数  
    uint32[]  segmentedCloudColInd 
    # 当前点的range 
    float32[] segmentedCloudRange
    

    了解了这些话题的数据之后,下面看看它们的回调函数分别是怎么处理的.

    2. 回调函数的处理

    紧耦合所有的回调函数实现在 Estimator.cpp的LinsFusion类中, 这个类相当于一个ros壳.

    一帧点云的回调 laserCloudCallback();
    每一帧发过来的点云都添加到了 pclBuf_ ,
    这个点云是什么 ?
    话题是: /segmented_cloud 发布: segmentedCloud
    segmentedCloud 是所有有效聚类点+地面点进行5倍降采样所形成的点云.

    void LinsFusion::laserCloudCallback(
        const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) {
      // Add a new segmented point cloud
      pclBuf_.addMeas(laserCloudMsg, laserCloudMsg->header.stamp.toSec());
    }
    

    点云信息的回调 laserCloudInfoCallback();
    话题是/segmented_cloud_info
    发布的是 segMsg

    void LinsFusion::laserCloudInfoCallback(
        const cloud_msgs::cloud_infoConstPtr& cloudInfoMsg) {
      // Add segmentation information of the point cloud
      cloudInfoBuf_.addMeas(*cloudInfoMsg, cloudInfoMsg->header.stamp.toSec());
    }
    

    外点
    话题是/outlier_cloud上
    发布 outlierCloud
    outlierCloud 是无效聚类点中 挑选行数大于groundScanInd 并且 经过5倍降采样 所形成的的点云.

    // 外点点云
    void LinsFusion::outlierCloudCallback(
        const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg) {
      outlierBuf_.addMeas(laserCloudMsg, laserCloudMsg->header.stamp.toSec());
    }
    

    Map优化后里程计

    // 地图优化的结果   
    void LinsFusion::mapOdometryCallback(
        const nav_msgs::Odometry::ConstPtr& odometryMsg) {
      // Map优化后的旋转 
      geometry_msgs::Quaternion geoQuat = odometryMsg->pose.pose.orientation;
      Q4D q_yzx(geoQuat.w, geoQuat.x, geoQuat.y, geoQuat.z);
      // Map优化后的xyz    
      V3D t_yzx(odometryMsg->pose.pose.position.x,
                odometryMsg->pose.pose.position.y,
                odometryMsg->pose.pose.position.z);
      // 改变轴的顺序   
      V3D t_xyz = estimator->Q_yzx_to_xyz * t_yzx;
      // ?????????????????????????
      Q4D q_xyz =
          estimator->Q_yzx_to_xyz * q_yzx * estimator->Q_yzx_to_xyz.inverse();
    
      V3D rpy = math_utils::Q2rpy(q_xyz);
    }
    

    2.1 IMU的回调处理

    可以看到上面点云的回调函数中都没进行什么实质的处理, 因此可能IMU的回调部分就比较关键了.
    流程:

    1. 将IMU数据与车体系对齐.
    // 将IMU与车体坐标对齐  
    alignIMUtoVehicle(misalign_euler_angles_, acc_raw_, gyr_raw_, acc_aligned_,
                      gyr_aligned_);
    
    1. 将IMU数据添加到 imuBuf_ 中.
    // Add a new IMU measurement
    Imu imu(imuMsg->header.stamp.toSec(), acc_aligned_, gyr_aligned_);
    imuBuf_.addMeas(imu, imuMsg->header.stamp.toSec());                 // MapRingBuffer<Imu>
    
    1. 执行状态估计 performStateEstimation();
      果然,执行了一个看起来有点东西的函数, 应该里面大有文章, 下面进行分析.

    3. performStateEstimation()

    流程:

    1. 检查数据是否充分
    if (imuBuf_.empty() || pclBuf_.empty() || cloudInfoBuf_.empty() ||
       outlierBuf_.empty())
     return;
    
    1. 检查是否进行初始化 ,没执行初始化 则执行 processFirstPointCloud();
    2. while()循环 , 没搞懂!!! 反复执行 processPointClouds();

    3.1 processFirstPointCloud()

    如果初始化未完成,则进入这个函数进行处理.
    流程:

    1. 获取最新的分割后的点云为 distortedPointCloud, 以及外点点云 outlierMsg, 当前点云信息 cloudInfoMsg , 以及最新的imu数据.

    2. 调用 下面函数完成处理

       estimator->processPCL(scan_time_, imu, distortedPointCloud, cloudInfoMsg,
       outlierPointCloud);
      

    processPCL() 主要完成分割后点云的处理,包括点云分线束模型化, 特征提取等等,详细分析见第4节。

    3.2 processPointClouds()

    流程:
    1、获取上一次估计后第一个点云数据

    // Obtain the next PCL  获取上一次估计后第一个点云数据
      pclBuf_.itMeas_ = pclBuf_.measMap_.upper_bound(estimator->getTime());    // 二分查找第一个大于或等于num的数字 
      sensor_msgs::PointCloud2::ConstPtr pclMsg = pclBuf_.itMeas_->second;
      scan_time_ = pclBuf_.itMeas_->first;
      distortedPointCloud->clear();
      pcl::fromROSMsg(*pclMsg, *distortedPointCloud);
    
      outlierBuf_.itMeas_ = outlierBuf_.measMap_.upper_bound(estimator->getTime());
      sensor_msgs::PointCloud2::ConstPtr outlierMsg = outlierBuf_.itMeas_->second;
      outlierPointCloud->clear();
      pcl::fromROSMsg(*outlierMsg, *outlierPointCloud);
    
      cloudInfoBuf_.itMeas_ =
          cloudInfoBuf_.measMap_.upper_bound(estimator->getTime());
      cloud_msgs::cloud_info cloudInfoMsg = cloudInfoBuf_.itMeas_->second;
    

    2、检查最后一个IMU的时间戳是否晚于该点云时间,如果比点云的时间早,认为IMU没有覆盖两帧点云直接退出。
    3、对于当前帧时间戳之前的IMU数据,循环执行IMU预测过程,
    estimator->processImu(dt, imu.acc, imu.gyr);
    4、执行IESKF的观测更新:

    // Update the iterative-ESKF using a new PCL
      estimator->processPCL(scan_time_, imu, distortedPointCloud, cloudInfoMsg,
                            outlierPointCloud)
    

    `

    大概的流程就是这样了,可以看到最后数据都流向了StateEstimator类的对象estimator中,下面学习这个类。

    4. StateEstimator类

    首先看初始化:
    构造函数中,最需要注意的是 filter_ = new StatePredictor(); filter_是 StatePredictor类的对象,是卡尔曼滤波的实现类。

    4.1 状态预测 processImu()

    switch (status_) {
          case STATUS_INIT:
            break;
          case STATUS_FIRST_SCAN:   // 第一帧-第二帧之间
            preintegration_->push_back(dt, acc, gyr);    // 进行预积分  
            filter_->time_ += dt;
            acc_0_ = acc;
            gyr_0_ = gyr;
            break;
          case STATUS_RUNNING:     // 第二帧后 
            filter_->predict(dt, acc, gyr, true);
            break;
          default:
            break;
        }
    

    这里都是常规操作。。

    4.2 观测校正 processPCL()

    在这里插入图片描述

    4.2.1 processSecondScan()

    在这里插入图片描述

    下面这一段用预积分求解帧间预测 感觉好像有些问题 ????? 重力应该要转换到上一帧坐标系中呀??? 这里使用的重力 linState_.gn_,貌似在 processFirstScan中:

    // Set the relative transform to identity
    linState_.setIdentity();
    
    // Calculate relative transform, linState_, using ICP method
        V3D pl;
        Q4D ql;
        V3D v0, v1, ba0, bw0;
        ba0.setZero();
        ql = preintegration_->delta_q;
        // gn_ ??????????????
        pl = preintegration_->delta_p +
             0.5 * linState_.gn_ * preintegration_->sum_dt *
                 preintegration_->sum_dt -
             0.5 * ba0 * preintegration_->sum_dt * preintegration_->sum_dt;
    

    核心自然就是 void estimateTransform(ScanPtr lastScan, ScanPtr newScan, V3D& t, Q4D& q) 点云匹配, 后续再统一分析…

    4.2.2 processScan()

    在这里插入图片描述重点是:performIESKF()
    疑问 :

    1. filter_->reset(1)中 初始化协方差矩阵:
    covariance_.setZero();
     
    covariance_.block<3, 3>(GlobalState::pos_, GlobalState::pos_) =
        covPos.asDiagonal();  // pos
    // ??????????????????????????
    covariance_.block<3, 3>(GlobalState::vel_, GlobalState::vel_) =
        state_.qbn_.inverse() * vel_cov * state_.qbn_;  // vel
    
    covariance_.block<3, 3>(GlobalState::att_, GlobalState::att_) =
        V3D(covRoll, covPitch, covYaw).asDiagonal();  // att
    
    covariance_.block<3, 3>(GlobalState::acc_, GlobalState::acc_) = acc_cov;
    covariance_.block<3, 3>(GlobalState::gyr_, GlobalState::gyr_) = gyr_cov;
    // ?????????????????????????
    covariance_.block<3, 3>(GlobalState::gra_, GlobalState::gra_) =
        state_.qbn_.inverse() * gra_cov * state_.qbn_;
    

    4. StatePredictor类

    实现卡尔曼滤波的类,类内部有一个重要的成员变量:GlobalState state_;
    GlobalState 状态的定义为:

     V3D rn_;   // position in n-frame    相对于上一时刻IMU坐标系的平移
     V3D vn_;   // velocity in n-frame    相对于上一时刻IMU坐标系的速度
     Q4D qbn_;  // rotation from b-frame to n-frame    相对于上一时刻IMU坐标系的旋转 
     V3D ba_;   // acceleartion bias                   加速度偏置 
     V3D bw_;   // gyroscope bias                      角速度偏置
     V3D gn_;   // gravity                             重力 
    

    初始化:

     StatePredictor() { reset(); }    // 构造函数
    

    reset()中:

    void reset(int type = 0) {
        if (type == 0) {
          state_.rn_.setZero();
          state_.vn_ = state_.qbn_.inverse() * state_.vn_;
          state_.qbn_.setIdentity();
          initializeCovariance();
    

    协方差矩阵的初始化:

    展开全文
  • 视觉惯性里程计SVO融合IMU

    千次阅读 2019-12-03 16:48:31
    7月初的时候尝试把IMU作为视觉导航的辅助项,加入到SVO的框架中去,目前已完成基于半直接单目视觉里程计SVO 的 视觉惯性融合 的程序框架搭建。以下围绕 ‘IMU数据以什么样的方式参与视觉系统导航?’、‘IMU可作用于...

    基于视觉惯性融合的SVO
    7月初的时候尝试把IMU作为视觉导航的辅助项,加入到SVO的框架中去,目前已完成基于半直接单目视觉里程计SVO 的 视觉惯性融合 的程序框架搭建。以下围绕 ‘IMU数据以什么样的方式参与视觉系统导航?’、‘IMU可作用于视觉导航的哪些部分?’、‘融合IMU数据可以得到怎样的结果?’几个方面进行讲述。
    本文仅代表个人观点,并作为工作学习记录,若有错误或者不妥之处欢迎指出!!!

    在HeYijia对于SVO 的改进版本上https://github.com/HeYijia/svo_edgelet进行修改,并且参考VI-ORB代码https://github.com/jingpang/LearnVIORB以及VINS-Mono https://github.com/HKUST-Aerial-Robotics/VINS-Mono中的IMU使用。

    #程序流程图

    原来SVO框架
    在这里插入图片描述在这里插入图片描述视觉惯性融合的半直接视觉里程计程序流程图大致如上,在原有的SVO框架上添加了视觉惯性联合初始化模块和后端优化模块。
    #1IMU数据以什么样的方式参与视觉系统导航?
    ##IMU预积分
    作用: IMU预积分算法可将积分增量与世界系下的惯性状态解耦,避免在线性化点更新时对测量值的重复积分计算,减少计算复杂度并获取连续时间内的相对运动增量
    IMU测量频率在200Hz,视觉图像频率约20Hz,在两帧图像之间,对IMU采用预积分的方式进行运动增量计算。IMU就以这种预积分的方式参与视觉系统各个环节的计算。
    在纯视觉系统中,我们只能得到视觉的状态估计量,无法构建运动模型,因此通过视觉测量方程构建重投影误差进行运动优化估计。在加上IMU测量数据之后,相当于引入了运动测量。在VIO中,就可以通过IMU的运动测量与视觉测量进行联合估计,有效提高系统鲁棒性。

    #2 IMU可作用于视觉导航的哪些部分?
    ##2.1视觉惯性联合初始化
    视觉惯性联合初始化是VIO必不可少的一部分,通过将视觉数据与IMU数据对齐,获取导航必须的初始参数估计。
    最最重要的一点就是:单目视觉的尺度恢复!!
    其实在SVO上添加IMU 的初衷也是为了恢复单目尺度。在原SVO中,作者将首帧固定,作为第一个关键帧,并在后续普通帧上进行光流跟踪,计算匹配特征点的个数以及平均视差,当平移量足够大 平均视差>50个像素时,才认为找到第二个关键帧,随后再通过视差的方差是否超过阈值来判断计算E矩阵还是H矩阵。通过前两个关键帧完成视觉初始化之后,得到两关键帧之间的平移量与旋转量t,R ,也得到了由两个关键帧中多有特帧点构建的初始地图。
    在这一环节中,环境的尺度(包括特征点的坐标以及相机位姿)均以两关键帧之间的平移量t作为单位1。
    而IMU得到的测量数据(加速度与角速度)均为世界系下的真实尺度的运动测量,通过IMU 预积分获取一段时间内的运动增量,并将其与视觉位姿估计的运动增量对比,可以恢复出视觉的真实尺度。这是单目尺度恢复的基本思想。

    联合初始化的理论部分参考论文:Visual-Inertial Monocular SLAM with Map Reuse 通过 4 个步骤将视觉与惯性信息对齐,解决了视觉惯性系统的初始化问题。具体公式就不进行展开了,找论文都能看到。这篇论文是ORB-SLAM的作者写的,相应程序中也是使用同样的初始化方法。
    我在程序中同样使用这种联合初始化方法,对关键帧处的陀螺仪偏差、单目尺度、重力矢量、加速度偏差以及速度进行估计,但让我很迷的一点就是“初始化的终止条件”??Visual-Inertial Monocular SLAM with Map Reuse一文中作者把初始化的时间设为一个定值,貌似是做过实验:15s的初始化时间可以使得所有待估计参数可观。 但是在仿真过程中发现,15s并不是一个十分靠谱的数。之前Closed-Form Solution of Visual-Inertial Structure from Motion文章中提到过,当机体处于静止或匀速运动状态下时,VI-SfM 问题将存在无穷解,此时是无法进行视觉惯性联合初始化的。这是否表明,若是采用给定15s的初始化时间,那此区间内不能存在静止或者匀速运动???? 我尝试过采用不同的起始帧进行图像跟踪,初始化估计出来的尺度时好时坏,并且与起始帧有着巨大的关联。使用某些起始帧,基本上在6、7s很快就能达到初始化参数收敛,用不到15s。而使用相差不远的帧作为起始帧,却估计出误差巨大的尺度。
    关于这一点,目前还无法解决,希望有哪位研究过这类问题的同学可以解答一下!!

    ##2.2运动先验
    视觉静止的先验模型:SVO 、LSD-SLAM
    匀速运动的先验模型:ORB-SLAM、PTAM、DSO
    在视觉惯性联合初始化之后,IMU就可以光明正大地参与运动跟踪了。
    在VIO中,通常都会拿IMU作为运动先验,因为在联合初始化完成之后,陀螺仪和加速度计的偏置都可以被估计出来啦,而且重力矢量也可观测。此时的IMU除了一些小的测量误差以及器件的随机游走之外,基本上是没什么其他误差的。由于两帧图像之间的时间很短,其间的传感器偏差可视为定值,通过IMU测量数据预积分,可以在新一帧图像到来之前对运动进行先验估计。
    SVO原程序中,采用的是位姿变化为0的先验模型,也就是静止模型:将上一帧位姿作为当前帧的位姿先验,进行图像匹配。这种方法虽然在能动性上差了一点但是也是有好处的,比如在运动缓慢的情况下、或者静止的情况下,视觉静止模型就表现得非常稳定。
    而反观IMU作为先验,虽然在机体快速运动过程中IMU的测量精度很高,但是也要考虑缓慢运动状态下的IMU漂移可能引入累积误差的情况。
    出于这一点考虑,我认为将IMU先验模型与其他静态更稳定的模型进行组合,再作为运动先验是一个比较合适的想法。

    此处说明一下先验模型的误差计算方法:
    在这里插入图片描述
    数据集中一般都提供了groundtruth,使用其中的姿态四元数以及位移矢量作为真值。但是要验证运动先验额准确性的话需要基于上一帧的位姿,为避免位姿估计误差的影响,可以采用计算单帧运动增量的方法验证先验模型。
    通过先验模型估计运动增量,再与真实增量求绝对误差,通过整条运动轨迹求单帧先验误差可以得到关于先验模型误差的对比曲线。

    ##2.3后端非线性优化
    到上述为止都是关于前端的内容,视觉也好,IMU测量也好,主要目的都是为了估计当前帧的位姿。
    在处理完最新一帧图像之后,就要进入一个局部的优化环节。
    SVO原程序中在bundle_adjustment.cpp中是有写优化的内容的,但是没有使用,他自带的local_ba也是采用一种比较简单的局部优化方法。
    当看到VI-ORB中的Optimizer.cc中的各种图优化函数时,我凌乱了···加了IMU测量的图优化模型开始有了千奇百怪的顶点和边,赶紧补了一波g2o的知识,在其他CSDN上可以找到一些特定类型的函数模板,但是对应到自身程序上时还是需要进一步的加工和考量。
    定义顶点和边的时候比较容易,确定好参与优化的变量以及顶点类型基本上就没什么问题;边的话要确定好每个边关联的顶点以及误差项的计算方法,接下去就直接调用g2o库就可以了,但其中的部分参数还是有点伤脑筋。例如设定误差边时的信息矩阵该如何选取?信息矩阵代表的是这个测量值的可信程度,信息矩阵的大小影响着对应边的误差在整个目标函数中的占比。参考版本函数大多都设为weight*InvCov(权重乘协方差的逆)这样的形式,但是当一个图中含有多种类型的边时,不同边的权重需要如何权衡,如何在多类型边的图中选择合适的权重可能要经过较长时间的测试进行调整。

    在视觉后端优化中,为了提高计算效率,通常采用的是滑动窗口优化的方法。SVO 仅在关键帧上提取特征点,因此考虑使用基于关键帧的滑动窗口进行优化。
    所以我在localBA的基础上又添加了一个windowBA的函数,在windowBA中结合了IMU数据进行联合优化,具体步骤类似VI-ORB中的Optimizer::LocalBundleAdjustmentNavState,用到了IMU的PVR以及bias。

    就运行结果来说,没有想象中那么好。
    每确定一个关键帧后都要进行一次滑窗优化显然有些浪费资源,并且图优中包含各种误差边的计算、变量优化反复迭代,消耗了较长的时间。
    从效果层面,滑窗确实可以提高局部运动的鲁棒性,但是无法避免误差累积。 显然依靠简单的滑窗是无法达到高精度的目标的,尤其是在运动剧烈、旋转较大的数据集上,优化的方法经常出现无法运行的情况···

    #总结
    目前SVO与IMU融合的算法虽然在部分数据集中可以跑出比较好的效果,但还是存在很多漏洞和不当的地方。只能说搭建好了框架,可以使用,但无法普遍适用,后续将持续进行改进···

    展开全文
  • 里程计imu数据融合

    千次阅读 2019-03-05 21:31:13
    https://blog.csdn.net/baimei4833953/article/details/82423407 https://blog.csdn.net/xiekaikaibing/article/details/80402113 https://blog.csdn.net/baimei4833953/article/details/80768762
  • 现代电子 IMU 已高度集成化,可包含多种类型的传感器——加速、陀螺仪和磁力仪;其基于微机电系统 (MEMS) 技术,因此体积小、重量轻且相对坚固。这些新一代 IMU 可作为板安装元件提供,经证明非常适合嵌入式应用。...
  • 文章目录基于Bundle Adjustment的VIO融合视觉SLAM中的Bundle Adjustment问题最小二乘问题的...状态初始值,根据里程计大致位置,可以计算出机器人的位置特征的位置。如: q = [0.96, 0, 0, 0.25], p = [1, 2, 0],...
  • 使用 robot_pose_ekf 对imu和odom进行融合

    千次阅读 多人点赞 2019-05-10 15:00:40
      robot_pose_ekf 是 ROS Navigation stack 中的一个包,通过扩展卡尔曼滤波器对 imu里程计 odom、视觉里程计 vo 的数据进行融合,来估计平面移动机器人的真实位置姿态,输出 odom_combined 消息。robot_pose_ekf ...
  • cartographer 处理IMU(激光,里程计等)流程

    千次阅读 热门讨论 2018-08-03 16:19:27
    std::unique_ptr是标准库的智能指针,auto_ptr真的非常类似,auto_ptr可以说可以随便赋值,但赋值完后原来的对象就不知不觉报废,而unique_ptr不让你随便赋值,只能move内存转移。 11、注意到在collator.h中一...
  • 目录里程计(Odometry)视觉里程计(Visual Odometry)单目视觉里程计(monocular Visual Odometry)双目视觉里程计(stereo Visual Odometry)视觉惯性里程计(visual-inertial Odometry)轮式里程计odometry 编码器 编码...
  • IMU的输出频率很高(100-1KHz),所以优化变量会快速增长,使得实时优化变得infeasible。Christian Forster提出用将两帧图像间的IMU采样数据用预积分的方法变为一个constraint,从而降低优化变量。对于两帧图像间的...
  • =====================================... Odometry, IMU and Robot Localization Packages <0>.关于 <robot> 包 0.1.该包基本上是为 bot 制作的,因为它使用 3 个传感器值 0.1.0.Odometry_data 0.1.1.Imu_data

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,408
精华内容 563
关键字:

imu与里程计融合