自己造轮子——基于图优化的轮速+IMU里程计

于 2024-05-14 20:36:16 发布
阅读量989 收藏 11
点赞数 22
分类专栏： SLAM工具 文章标签： c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_45432823/article/details/138629944
版权

一个简单的IWO

背景

之前一直在做传统的单目VIO，自己也在VINS框架的基础上自己加进去了不少东西，但在挑战环境下(光线较暗，纹理不丰富)整个轨迹的漂移还是很明显的。。而且感觉存在很明显的尺度偏差。我用的移动底盘autolabor M2自己是有里程计数据的，不过和自己买轮速计搭起来的环境不同，底盘自己的轮速计输出最高就到25Hz，鉴于自己没有很高的硬件水平去拔轮速的原始数据，联系那边的技术人员也不支持高频输出的驱动，所以只得就凑合使用这个低频的轮速数据了。网上的资料主要还是基于滤波框架对IMU+轮速的处理，不过之前学高博的课的时候，有注意到高博的组合导航程序里是有加轮速的，而且加的也很简单，所以这里用自己比较熟悉的Ceres把IMU+轮速的东西又单独写了一遍，整体效果看起来还可以

IMU预积分处理

预积分这里没什么好说的，我一直用的港科大VINS的那套预积分，不同框架可能会有些许不同，不过本质上都是处理了两个时刻间的IMU数据，这里就不再单独写了。

void IWO::AddIMU(const IMU &imu)
{
    if (!first_imu_received_) {
        first_imu_time_ = imu.getTime();
        first_imu_received_ = true;
        return;
    }

    if (!is_imu_inited_) {
        if (imu.getTime() - first_imu_time_ < 1) {
            imu_vec_.push_back(imu);
            return;
        }
        else {
            is_imu_inited_ = imu_init_->initialize(imu_vec_, Ri0w_);
            if (is_imu_inited_) {
                init_ba_ = imu_init_->init_ba();
                ROS_DEBUG("init_ba: %f, %f, %f", init_ba_(0), init_ba_(1), init_ba_(2));
                init_bg_ = imu_init_->init_bg();
                pre_integ_ = std::make_shared<ImuIntegration>(imu, std::make_pair(init_ba_, init_bg_));

                imu_state_ = last_state_ = NavState {
                    .t_ = imu.getTime(),
                    .p_ = Vector3d::Zero(),
                    .q_ = Quaterniond(Ri0w_.transpose()).normalized(),
                    .v_ = Vector3d::Zero(),
                    .ba_ = init_ba_,
                    .bg_ = init_bg_
                };

                cur_time_ = imu.getTime();
                last_imu_ = imu;
            }
            else {
                first_imu_time_ = imu.getTime();
            }
            return;
        }
    }

    if (is_imu_inited_) {
        pre_integ_->push_back(imu.getTime() - cur_time_, imu);
        imu_state_ = fastPredictIMU(imu);
    }

    last_imu_ = imu;
    cur_time_ = imu.getTime();
}

NavState IWO::fastPredictIMU(const IMU &imu) {
    double dt = imu.getTime() - imu_state_.t_;
    Vector3d un_acc_0 = imu_state_.q_ * (last_imu_.getRawData().first - imu_state_.ba_) - Gv_;
    Vector3d un_gyr = 0.5 * (last_imu_.getRawData().second + imu.getRawData().second) - imu_state_.bg_;
    auto latest_q_ = imu_state_.q_ * deltaQ(un_gyr * dt);
    Vector3d un_acc_1 = latest_q_ * (imu.getRawData().first - imu_state_.ba_) - Gv_;
    auto un_acc = 0.5 * (un_acc_0 + un_acc_1);
    auto latest_p_ = imu_state_.p_ +  imu_state_.v_ * dt + 0.5 * un_acc * dt * dt;
    auto latest_v_ = imu_state_.v_ + un_acc * dt;

    return NavState {
        .t_ = imu.getTime(),
        .p_ = latest_p_,
        .q_ = latest_q_,
        .v_ = latest_v_,
        .ba_ = imu_state_.ba_,
        .bg_ = imu_state_.bg_
    };
}

轮速处理

比起VINS-Wheel等文献，这里借鉴了高博的思路，利用轮速的速度对速度进行约束，其实最开始还考虑了把轮速用来做个yaw的约束，不过可能是因为我这个IMU还不错，或者水平方向给的激励还可以，感觉IMU自己积出来的yaw角感觉挺准了(蓝线是IWO结果，绿线是里程计的yaw角结果，甚至感觉IWO的准一些)，所以就没再用轮速加姿态的约束，只是简单的约束了下速度，这里都是线性的，用Ceres实现非常简单
在这里插入图片描述

bool WheelFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    Vector3d cur_v(parameters[0][0], parameters[0][1], parameters[0][2]);
    Vector3d cur_ba(parameters[0][3], parameters[0][4], parameters[0][5]);
    Vector3d cur_bg(parameters[0][6], parameters[0][7], parameters[0][8]);

    Vector3d Vj = cur_state_.q_ * (cur_odom_.v);
    if (plane_) Vj(2) = 0;

    Eigen::Map<Eigen::Matrix<double, 9, 1>> residuals_map(residuals);
    residuals_map.block<3, 1>(0, 0) = cur_v - Vj;
    residuals_map.block<3, 1>(3, 0) = cur_ba - cur_state_.ba_;
    residuals_map.block<3, 1>(6, 0) = cur_bg - cur_state_.bg_;
    Eigen::Matrix<double, 9, 9> sqrt_info = 1e3 * Eigen::Matrix<double, 9, 9>::Identity();
    residuals_map = sqrt_info * residuals_map;

    if (jacobians)
    {
        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 9, 9, Eigen::RowMajor>> jacobian_speed_bias(jacobians[0]);
            jacobian_speed_bias.setZero();
            jacobian_speed_bias.block<3, 3>(0, 0) = Matrix3d::Identity();
            jacobian_speed_bias.block<3, 3>(3, 3) = Matrix3d::Identity();
            jacobian_speed_bias.block<3, 3>(6, 6) = Matrix3d::Identity();
        }
    }

    return true;
}

平面约束

还是参考VINS-Wheel，这里加了个简单的平面约束进来，因为我的实验是在室内做的，所以这里就给了个严格为0的平面，姿态的话偷了下懒，把刚初始化后的roll和pitch作为理想值，后边的残差直接用这个理想值来算了。这里的求导也没什么复杂的

bool PlaneFactor::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    Eigen::Vector3d cur_p(parameters[0][0], parameters[0][1], parameters[0][2]);
    Eigen::Quaterniond cur_q(parameters[0][6], parameters[0][3], parameters[0][4], parameters[0][5]);

    Eigen::Map<Eigen::Matrix<double, 3, 1>> residual(residuals);
    residual.block<1, 1>(0, 0) = Eigen::Matrix<double, 1, 1>::Zero() - e3_.transpose() * cur_p;
    residual.block<2, 1>(1, 0) = init_delta_ - lambda_ * cur_q.toRotationMatrix() * e3_;
    Eigen::Matrix3d sqrt_info = sqrt_info_ * Eigen::Matrix3d::Identity();
    residual = sqrt_info * residual;

    if (jacobians)
    {
        if (jacobians[0])
        {
            Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> jacobian_pose(jacobians[0]);
            jacobian_pose.setZero();
            jacobian_pose.block<1, 3>(0, 0) = -e3_.transpose();
            jacobian_pose.block<2, 3>(1, 3) = lambda_ * cur_q.toRotationMatrix() * skewSymmetric(e3_);
        }
    }

    return true;
}

整体效果

我的实现是在每次轮速数据进来的时候触发的，所以我在类里写了个AddOdom的接口，函数内部触发优化，在optimization里面依次添加变量、残差，最后优化。这里加了四种残差：

  1. 先验残差：参考高博给了个比较高的值，限制每次优化时last_state的变化
  2. IMU预积分：两帧之间的PVQ增量
  3. 轮速速度约束：速度约束，且直接影响位置
  4. 平面约束：roll和pitch，以及Z方向的发散
void IWO::AddWheel(const OdomData &odom)
{
    if (!first_wheel_received_) {
        if (!is_imu_inited_) {
            return;
        }

        last_state_.t_ = odom.time;
        last_odom_ = cur_odom_ = odom;
        first_wheel_received_ = true;
        cur_time_ = odom.time;

        return;
    }

    cur_odom_ = odom;
    is_static_ = odom.v.norm() < 0.001 ? true : false;
    pre_integ_->push_back(odom.time - cur_time_, last_imu_);
    cur_time_ = odom.time;

    cur_state_ = fastPredictWheel();

    // optimize
    optimization();
    ROS_DEBUG_STREAM("cur state v: " << cur_state_.v_.transpose());
    std::cout << std::fixed << "cur time: " << cur_state_.t_ << ",p: " << cur_state_.p_.transpose() << std::endl;
    std::ofstream foutD("/home/zyp/output/VINS/fhqdl/iwo/debug.csv", std::ios::app);
    foutD.setf(std::ios::fixed, std::ios::floatfield);
    foutD << cur_state_.t_ << " "<< cur_state_.v_.transpose() << std::endl;
    foutD.close();
    pre_integ_ = std::make_shared<ImuIntegration>(last_imu_, std::make_pair(cur_state_.ba_, cur_state_.bg_));

    last_state_ = cur_state_;
    last_odom_ = cur_odom_;
}

NavState IWO::fastPredictWheel()
{
    // auto Qj = last_state_.q_ * deltaQ(0.5 * (last_odom_.w + cur_odom_.w) * (cur_odom_.time - last_odom_.time));
    // auto Vj = Qj * (0.5 * (last_odom_.v + cur_odom_.v));
    // auto Pj = last_state_.p_ + Vj * (cur_odom_.time - last_odom_.time);
    Quaterniond Qj = last_state_.q_ * pre_integ_->delta_q_;
    Qj.normalize();
    Vector3d Vj = last_state_.q_.toRotationMatrix() * pre_integ_->delta_v_ + last_state_.v_ - pre_integ_->sum_dt_ * Gv_;

    ROS_DEBUG_STREAM(std::fixed << "CUR TIME: " << std::setprecision(6) << cur_time_);
    ROS_DEBUG_STREAM("imu state v: " << imu_state_.v_.transpose());
    ROS_DEBUG_STREAM("predict v: " << Vj.transpose());
    ROS_DEBUG_STREAM("odom v: " << (Qj * cur_odom_.v).transpose());
    Vector3d Pj = last_state_.p_ + last_state_.q_.toRotationMatrix() * pre_integ_->delta_p_ + last_state_.v_ * pre_integ_->sum_dt_
                     - 0.5 * Gv_ * pre_integ_->sum_dt_ * pre_integ_->sum_dt_;

    return NavState {
        .t_ = last_state_.t_ + pre_integ_->sum_dt_,
        .p_ = Pj,
        .q_ = Qj,
        .v_ = Vj,
        .ba_ = imu_state_.ba_,
        .bg_ = imu_state_.bg_
    };
}

void IWO::optimization()
{   
    auto t_begin = std::chrono::steady_clock::now();
    
    vector2double();

    ceres::Problem problem;
    ceres::LossFunction *loss_function;
    loss_function = new ceres::HuberLoss(2.0);

    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();

    problem.AddParameterBlock(para_last_pose_, SIZE_POSE, local_parameterization);
    problem.AddParameterBlock(para_cur_pose_, SIZE_POSE, local_parameterization);
    problem.AddParameterBlock(para_last_speed_bias_, SIZE_SPEEDBIAS);
    problem.AddParameterBlock(para_cur_speed_bias_, SIZE_SPEEDBIAS);
    
    // add prior constraint
    WheelPriorFactor *wheel_prior_factor = new WheelPriorFactor(last_state_, 1e3);
    problem.AddResidualBlock(wheel_prior_factor, nullptr, para_last_pose_, para_last_speed_bias_);
    
    // add IMU residual
    if (pre_integ_->sum_dt_ < 10.0) {
        IMUFactor *imu_factor = new IMUFactor(pre_integ_.get());
        problem.AddResidualBlock(imu_factor, nullptr,
                                 para_last_pose_, para_last_speed_bias_, 
                                 para_cur_pose_, para_cur_speed_bias_);
    }
    
    // add odom residual
    WheelFactor *wheel_factor = new WheelFactor(cur_odom_, cur_state_, true);
    problem.AddResidualBlock(wheel_factor, loss_function,
                             para_cur_speed_bias_);

    // add plane constraint
    PlaneFactor *plane_factor = new PlaneFactor(Ri0w_.transpose(), 1e2);
    problem.AddResidualBlock(plane_factor, loss_function, para_cur_pose_);
    
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
    options.max_num_iterations = 10;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    double2vector();
    auto t_end = std::chrono::steady_clock::now();
    auto time_cost = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_begin).count();
    ROS_INFO("optimization cost: %ldms.", time_cost);
}

整体实现的效果还可以，可以看到IWO推出来的轨迹相较于轮速自己积分出来的漂移更小，而且和上边的yaw角结果对应起来，感觉也是IWO的更准一些。
在这里插入图片描述

改进方向

其实现在实现的只是非常非常简单的一个框架，像IMU与载体的安装偏差角，轮速自身的尺度误差因子等等都没有考虑在内。因为最近正在尝试把自己之前改了一点的VIO和IWO先简单地融合起来，整体框架还没有写完，所以这部分还没有深入研究。后面也将争取采集几组带有真值的数据(TS60给位置，光纤陀螺给姿态)来整体评估一下定位定姿的精度，如果整理好了的话也会陆续将代码以及数据集开放出来，希望能和更多的人一起交流

什么都不会的小澎友
关注
  • 22
    点赞
  • 踩
  • 11
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
专栏目录
评论 5
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值