当前位置：首页 > article >正文

VINS-MONO学习笔记-手动添加ZUPT

article 2025/1/19 23:13:15

近日吗喽，leader让用vslam算法辅助机器人现有的里程计定位算法，意在消除轮速计在部分环境下打滑导致的定位偏移，苦于自己之前一直做的是激光SLAM，花了很长一段时间学习VSLAM，前一个月要我主攻VINS-MONO算法，本文主要针对自己在知乎看到的一篇关于VINS工程落地经验贴，其中提及到在对VINS实现工程落地中，常用到的一个葵花宝典—ZUPT（零速修正），自己也借着前人的肩膀自己手写复现了一下，最后效果在开源数据集euroc的MH_easy_01这个数据包下，精度提升了约10%，效果一般，可能限于我自己本身能力有限，且VSLAM这块我也是第一次接触，对VINS-MONO的源码理解的不深刻，所以请各位读者大佬如果发现有问题或者有更好的方案，能够不吝赐教！

ZUPT–零速修正

其实在笔者读研期间，就已经接触过这方面的知识，只是当时使用的是另一个版本，即车身非完整性约束(NHC)，与零速修正不同的是，NHC假设的是载体在运动的过程中侧向和垂向的速度为0，零速修正则是需要判断当前载体的运动状态，如果检测到为静止状态，那么直接构造一个三维的零速向量，进而修正当前载体的速度；

VINS算法

零速修正的理论其实不难，关键在与两点：

1. 载体的运动状态判断

2. 加在哪里

对于问题1，知乎那篇文章已经说的很明白了，我自己的理解就是，计算一个时间段两头的IMU数据的欧式距离，根据这个欧式距离去判断当前载体的运动状态是否处于静止。什么意思呢，比如VINS源码中，有一个imu_callback函数，在这个函数中主要做的是对IMU数据的处理，那么可以在这个位置判断当前载体的运动状态，我的处理方法就是每次获取到imu_buf最新的一个IMU数据，然后将其与前第10帧的IMU数据作运算，得到两帧IMU的欧式距离，当然，计算的是IMU的线加速度信息的欧式距离。根据这个欧式距离，可以判断当前载体的运动状态，至于这个状态阈值的选取，可以根据你自己的机器人数据的情况来定，开源数据集我选的0.05；

对于问题2，我其实也纠结了很久，倒不是因为要选择一个很好的位置，而是我一直不太明白应该加在哪里，最后在阅读了一些关于VINS源码解析的博客后，最后选择加在estimator.cpp中的processIMU()函数处，至于为什么，我自己的想法是因为每一次计算得到的IMU预计分信息都会当作后续图像帧对应IMU的PVQ信息，以用于后续的后端优化；

代码

完成上述的一些准备工作后，直接就加入工程代码看效果：
首先是针对状态的判断，此部分功能函数添加在imu_callback函数中：

imu_buf.push(imu_msg);
    
if(imu_buf.size() > 10){
        sensor_msgs::ImuConstPtr imu_now = imu_buf.back();
        sensor_msgs::ImuConstPtr imu_old;
        int target_index = imu_buf.size() - 10;
        int current_index = 0;

        std::queue<sensor_msgs::ImuConstPtr> temp_buf = imu_buf; // Create a temporary copy of imu_buf
        while (!temp_buf.empty()) {
            if (current_index == target_index) {
                imu_old = temp_buf.front();
                break;
            }
            temp_buf.pop();
            current_index++;
        }
        Eigen::Vector3d accel_now(imu_now->linear_acceleration.x, imu_now->linear_acceleration.y, imu_now->linear_acceleration.z);
        Eigen::Vector3d accel_old(imu_old->linear_acceleration.x, imu_old->linear_acceleration.y, imu_old->linear_acceleration.z);
        
        // Calculate the Euclidean distance between accel_now and accel_old
        double distance = (accel_old - accel_now).norm();

        // ROS_INFO("THE DISTANS IS %f\n",distance);
        if(distance <= 0.05){
            // ROS_INFO("the robot is static\n");
            is_static = true;
        }else{
            is_static = false;
        }
    }

    m_buf.unlock();

可以对应源码查看所加代码块的位置，其中is_static是一个全局变量；

ZUPT所加位置：

class VelocityFusionCost : public ceres::SizedCostFunction<3, 3> {
public:
    // 构造函数，初始化观测的速度
    VelocityFusionCost(const Eigen::Vector3d& observed_velocity)
        : observed_velocity_(observed_velocity) {}

    // 残差计算
    virtual bool Evaluate(const double* const* parameters, double* residuals, double** jacobians) const override {
        const double* predicted_velocity = parameters[0];

        // 计算残差：预测速度与观测速度之间的差值
        residuals[0] = predicted_velocity[0] - observed_velocity_[0];
        residuals[1] = predicted_velocity[1] - observed_velocity_[1];
        residuals[2] = predicted_velocity[2] - observed_velocity_[2];

        // 计算雅可比矩阵
        if (jacobians != nullptr && jacobians[0] != nullptr) {
            Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> jacobian_v(jacobians[0]);
            jacobian_v.setIdentity();
        }

        return true;
    }

private:
    const Eigen::Vector3d observed_velocity_;  // 观测速度
};

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity, const bool is_static)
{
    if (!first_imu)// 第一帧IMU数据，如果是第一帧，那么直接把当前IMU数据放在acc_0和gyr_0
    {
        //如果是系统的第一帧imu数据，则保存数据为 acc_0、gyr_0，由于dt=0 ，在计算预积分以及中值积分时都为0，
        //不过重点是将本次IMU的测量值保存，这样下一次IMU数据到时就可以进行中值积分。
        first_imu = true;
        acc_0 = linear_acceleration;
        gyr_0 = angular_velocity;
    }

    if (!pre_integrations[frame_count])//如果滑动窗口中当前帧frame_count图像预积分对象空，则为其创建一个预积分对象。
    {
        pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
    }
    if (frame_count != 0)
    {
        //如果当前图像帧 frame_count = 0 ，则说明此时处理的是第一帧图像数据，则不需要进行预积分，
        //因为预积分是表示两帧图像之间的约束， frame_count != 0时 ，用imu数据更新该图像帧的预积分
        //pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity)
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //if(solver_flag != NON_LINEAR)
            tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

        dt_buf[frame_count].push_back(dt);
        linear_acceleration_buf[frame_count].push_back(linear_acceleration);
        angular_velocity_buf[frame_count].push_back(angular_velocity);

        int j = frame_count;         
        //用中值预积分更新当前图像帧对应的IMU PVQ数组 Ps、Vs、Rs，Ps、Vs、Rs
        //数组保存滑动窗口中每一帧在世界坐标系中的位姿，在初始化完成之前这个数组保存的姿态是相对于参考帧的
        Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
        Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
        Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
        Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
        Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
        Vs[j] += dt * un_acc;

        // 24-10-18 by xinyi ZUPT
        if (is_static) {
            ceres::Problem problem;

            ceres::CostFunction* cost_function = new VelocityFusionCost(Eigen::Vector3d::Zero());
            problem.AddResidualBlock(cost_function, nullptr, Vs[j].data());

            // 配置求解器选项
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
            options.minimizer_progress_to_stdout = true;

            // 输出优化摘要
            ceres::Solver::Summary summary;

            // 求解问题
            ceres::Solve(options, &problem, &summary);
        }
    }
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
}