【5】lightning_lm项目-LIO 前端 -深入学习 IMU 处理模块

文章目录阶段二LIO 前端 - 深入学习 IMU 处理模块一、ImuProcess 类的功能二、核心函数 1Process() - 主处理函数三、核心函数 2IMUInit() - IMU 初始化四、核心函数 3UndistortPcl() - 点云去畸变五、去畸变的数学推导六、总结IMU 处理模块的关键技巧七、整体流程回顾LIO 前端重点总结下一步阶段二LIO 前端 - 深入学习 IMU 处理模块学习 IMU 处理模块这是 LIO 系统的另一个核心部分一、ImuProcess 类的功能看 imu_processing.hpp这个类主要做两件事IMU 初始化 IMUInit() 点云去畸变 UndistortPcl() 先看类的成员变量std::dequelightning::IMUPtrimu_queue_;// IMU 数据队列std::vectorPose6Dimu_pose_;// 每个 IMU 时刻的位姿Vec3d mean_acc_;// 平均加速度用于初始化重力Vec3d mean_gyr_;// 平均角速度用于初始化陀螺零偏Mat3d R_lidar_imu_Mat3d::Identity();// 雷达-IMU 外参旋转Vec3d t_lidar_mu_Vec3d::Zero();// 雷达-IMU 外参平移boolimu_need_init_true;// 是否需要初始化二、核心函数 1Process() - 主处理函数看 imu_processing.hppvoidImuProcess::Process(constMeasureGroupmeas,ESKFkf_state,CloudPtrscan)流程 如果 IMU 还没初始化 → 调用 IMUInit()如果已经初始化 → 调用 UndistortPcl() 给点云去畸变三、核心函数 2IMUInit() - IMU 初始化看 imu_processing.hpp这一步做什么 初始化 IMU 的几个重要参数步骤详解计算平均加速度和角速度for(constautoimu:meas.imu_){constautoimu_accimu-linear_acceleration;constautogyr_accimu-angular_velocity;// 递推平均mean_acc_(cur_acc-mean_acc_)/N;mean_gyr_(cur_gyr-mean_gyr_)/N;// 递推协方差cov_acc_cov_acc_*(N-1)/N(cur_acc-mean_acc_).cwiseProduct(cur_acc-mean_acc_)*(N-1)/(N*N);cov_gyr_cov_gyr_*(N-1)/N(cur_gyr-mean_gyr_).cwiseProduct(cur_gyr-mean_gyr_)*(N-1)/(N*N);N;}为什么这样做 静止时加速度计测量的是重力静止时陀螺仪的测量值就是零偏用前几帧数据计算平均值作为初始值初始化 ESKF 状态autoinit_statekf_state.GetX();init_state.grav_-mean_acc_/mean_acc_.norm()*G_m_s2;// 重力向量init_state.bg_mean_gyr_;// 陀螺零偏kf_state.ChangeX(init_state);重力方向怎么确定 加速度计测量的是 -g 因为重力向下而加速度计测量的是支撑力向上所以 grav_ -mean_acc_然后归一化并乘以 G_m_s2 9.81 m/s²初始化协方差矩阵autoinit_Pkf_state.GetP();init_P.setIdentity();init_P.block3,3(NavState::kBgIdx,NavState::kBgIdx)0.0001*Mat3d::Identity();kf_state.ChangeP(init_P);四、核心函数 3UndistortPcl() - 点云去畸变看 imu_processing.hpp这是最关键的函数 因为雷达扫描一帧需要时间比如 0.1 秒这期间机器人在动所以点云会畸变点云畸变的原因 第一个点和最后一个点的时间差 0.1 秒这 0.1 秒内机器人移动了所以点云是扫出来的不是同一时刻的去畸变的原理 用 IMU 数据预测每个时刻的位姿把每个点都变换到同一时刻通常是帧结束时刻UndistortPcl() 步骤详解准备数据autov_imumeas.imu_;v_imu.push_front(last_imu_);// 把上一帧最后一个 IMU 加进来可选IMU 滤波if(use_imu_filter_){for(autoimu:v_imu){autoimu_ffilter_.Filter(*imu);// 低通滤波*imuimu_f;}}正向传播预测每个 IMU 时刻的位姿这是一个循环用 IMU 数据积分for(autoit_imuv_imu.begin();it_imu(v_imu.end()-1);it_imu){autohead*(it_imu);autotail*(it_imu1);// 平均角速度和加速度angvel_avr0.5*(head-angular_velocitytail-angular_velocity);acc_avr0.5*(head-linear_accelerationtail-linear_acceleration);// 调用 ESKF 的 Predict() 函数kf_state.Predict(dt,Q_,gyro,acc);// 保存这个时刻的位姿imu_pose_.emplace_back(Pose6D(offs_t,acc_s_last_,angvel_last_,imu_state.vel_,imu_state.pos_,imu_state.rot_.matrix()));}预测到帧结束时刻dtnote*(pcl_end_time-imu_end_time);kf_state.Predict(dt,Q_,gyro,acc);对点云按时间排序pcl_outmeas.scan_;std::sort(pcl_out-points.begin(),pcl_out-points.end(),[](constPointTypep1,constPointTypep2){returnp1.timep2.time;});反向传播去畸变重点 从最后一个点开始往回处理autoit_pclpcl_out-points.end()-1;for(autoit_kpimu_pose_.end()-1;it_kp!imu_pose_.begin();it_kp--){autoheadit_kp-1;autotailit_kp;R_imuhead-rot;vel_imuhead-vel;pos_imuhead-pos;// 处理属于这个区间的点for(;it_pcl-time/1000.0head-offset_timeit_pcl!pcl_out-points.begin();it_pcl--){dtit_pcl-time/1000.0-head-offset_time;// 数学公式这里是核心Mat3dR_i(R_imu*math::exp(angvel_avr,dt).matrix());Vec3dP_i(it_pcl-x,it_pcl-y,it_pcl-z);Vec3dT_ei(pos_imuvel_imu*dt0.5*acc_imu*dt*dt-imu_state.pos_);// 去畸变后的点Vec3d p_compensateR_lidar_imu_.transpose()*(imu_state.rot_.inverse()*(R_i*(R_lidar_imu_*P_it_lidar_mu_)T_ei)-t_lidar_mu_);// 保存it_pcl-xp_compensate(0);it_pcl-yp_compensate(1);it_pcl-zp_compensate(2);}}五、去畸变的数学推导把点云里的点都变换到帧结束时刻的坐标系已知点 P 在i 时刻的雷达坐标系里 P_i把它变换到e 时刻的雷达坐标系里 P_e步骤 把 P_i 从 i 时刻雷达坐标系 → i 时刻 IMU 坐标系P_imu_iR_lidar_imu*P_it_lidar_mu把 P_imu_i 从 i 时刻 IMU 坐标系 → 世界坐标系P_worldR_i*P_imu_iT_i其中R_i i 时刻 IMU 的旋转T_i i 时刻 IMU 的平移把 P_world 从世界坐标系 → e 时刻 IMU 坐标系P_imu_eR_e.inverse()*(P_world-T_e)把 P_imu_e 从 e 时刻 IMU 坐标系 → e 时刻雷达坐标系P_eR_lidar_imu.inverse()*(P_imu_e-t_lidar_mu)合并起来 Vec3d p_compensateR_lidar_imu_.transpose()*(imu_state.rot_.inverse()*(R_i*(R_lidar_imu_*P_it_lidar_mu_)T_ei)-t_lidar_mu_);其中 T_ei T_i - T_e六、总结IMU 处理模块的关键技巧七、整体流程回顾LIO 前端现在把之前学的串联起来1. 接收 IMU 数据 → 存入队列 2. 接收点云 → 预处理 3. 同步数据 → 找到这帧点云对应的 IMU 数据 4. IMU 处理 a. 初始化如果需要 b. 正向传播预测每个时刻位姿 c. 反向去畸变把点云都变换到同一时刻 5. ESKF 更新 a. 迭代优化匹配点云 b. 更新状态和协方差 6. 检查是否需要创建关键帧 7. 如果是关键帧 → 更新局部地图 IVox重点总结IMU 初始化 估计重力方向和陀螺零偏点云去畸变原理 用 IMU 预测位姿把点都变换到同一时刻去畸变的数学推导 坐标变换的链式法则正向传播 反向去畸变 实现方法下一步下次可以学习点云预处理模块 pointcloud_preprocess.h/cc IVox 局部地图 ivox3d.h