本文介绍了一种鲁棒的激光雷达惯性系统初始化方法LI-Init，无需目标或额外传感器，能校准固态和机械式雷达与IMU之间的关系，并详细描述了关键参数设置，如时间偏移、滤波器大小等。
LI-Init是一种鲁棒、实时的激光雷达惯性系统初始化方法。该方法可校准激光雷达与IMU之间的时间偏移量和外部参数，以及重力矢量和IMU偏差。我们的方法不需要任何目标或额外的传感器，特定的结构化环境，先前的环境点图或初始值的外在和时间偏移。
重点参数
参数在config文件夹下的xxx.yaml文件里
里面有几种雷达的，有几款livox的固态雷达
- avia
- horizon
- mid360
还有禾赛的雷达
- pandar
还有几种常见的机械式激光雷达 - ouster
- robosense
- velodyne
可以看出 Lidar_IMU_Init 这套标定算法不但可以用于固态激光雷达同样也可以用于机械式激光雷达
下面来看重点参数
1、lid_topic ： 雷达点云的消息名称
2、imu_topic： IMU的消息名称
3、cut_frame_num： 将一帧分割成子帧，提高里程计频率。必须是正整数。
4、orig_odom_freq： 原始雷达帧输入频率，对于大部分雷达，输入频率是10HZ,机械旋转激光雷达建议cut_frame_num * orig_odom_freq = 30， Livox 激光雷达建议 cut_frame_num * orig_odom_freq = 50。
5、mean_acc_norm : IMU 静止时的加速度范数。通常，普通 IMU 为 9.805，Livox 内置 IMU 为 1。
6、data_accum_length ： 评估数据是否足以初始化的阈值。太小可能会导致质量差的结果。
7、online_refine_time（秒）：FAST-LIO2 的外在精炼时间。建议约15~30秒的细化。
8 、 filter_size_surf（米）：室内场景建议filter_size_surf=0.05~0.15，室外场景filter_size_surf=0.5。
9 、 filter_size_map（米）：室内场景建议filter_size_map=0.15~0.25，室外场景filter_size_map=0.5。
10、 blind （米）： 激光雷达距离有效最小距离
核心代码
相关配置参数
nh.param<int>("max_iteration", NUM_MAX_ITERATIONS, 4);
nh.param<int>("point_filter_num", p_pre->point_filter_num, 2);
nh.param<string>("map_file_path", map_file_path, "");
nh.param<string>("common/lid_topic", lid_topic, "/livox/lidar");
nh.param<string>("common/imu_topic", imu_topic, "/livox/imu");
nh.param<double>("mapping/filter_size_surf", filter_size_surf_min, 0.5);
nh.param<double>("mapping/filter_size_map", filter_size_map_min, 0.5);
nh.param<double>("cube_side_length", cube_len, 200);
nh.param<float>("mapping/det_range", DET_RANGE, 300.f);
nh.param<double>("mapping/gyr_cov", gyr_cov, 0.1);
nh.param<double>("mapping/acc_cov", acc_cov, 0.1);
nh.param<double>("mapping/grav_cov", grav_cov, 0.001);
nh.param<double>("mapping/b_gyr_cov", b_gyr_cov, 0.0001);
nh.param<double>("mapping/b_acc_cov", b_acc_cov, 0.0001);
nh.param<double>("preprocess/blind", p_pre->blind, 1.0);
nh.param<int>("preprocess/lidar_type", lidar_type, AVIA);
nh.param<int>("preprocess/scan_line", p_pre->N_SCANS, 16);
nh.param<bool>("preprocess/feature_extract_en", p_pre->feature_enabled, 0);
nh.param<bool>("initialization/cut_frame", cut_frame, true);
nh.param<int>("initialization/cut_frame_num", cut_frame_num, 1);
nh.param<int>("initialization/orig_odom_freq", orig_odom_freq, 10);
nh.param<double>("initialization/online_refine_time", online_refine_time, 20.0);
nh.param<double>("initialization/mean_acc_norm", mean_acc_norm, 9.81);
nh.param<double>("initialization/data_accum_length", Init_LI->data_accum_length, 300);
nh.param<vector<double>>("initialization/Rot_LI_cov", Rot_LI_cov, vector<double>());
nh.param<vector<double>>("initialization/Trans_LI_cov", Trans_LI_cov, vector<double>());
nh.param<bool>("publish/path_en", path_en, true);
nh.param<bool>("publish/scan_publish_en", scan_pub_en, 1);
nh.param<bool>("publish/dense_publish_en", dense_pub_en, 1);
nh.param<bool>("publish/scan_bodyframe_pub_en", scan_body_pub_en, 1);
nh.param<bool>("runtime_pos_log_enable", runtime_pos_log, 0);
nh.param<bool>("pcd_save/pcd_save_en", pcd_save_en, false);
nh.param<int>("pcd_save/interval", pcd_save_interval, -1);
订阅雷达消息
ros::Subscriber sub_pcl = p_pre->lidar_type == AVIA ? \
nh.subscribe(lid_topic, 200000, livox_pcl_cbk) : \
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
订阅imu消息
ros::Subscriber sub_imu = nh.subscribe<sensor_msgs::Imu>
(imu_topic, 200000, boost::bind(&imu_cbk, _1, pubIMU_sync));
发布的消息
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered", 100000);
ros::Publisher pubLaserCloudFullRes_body = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_registered_body", 100000);
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
("/cloud_effected", 100000);
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
("/Laser_map", 100000);
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
("/aft_mapped_to_init", 100000);
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
("/path", 100000);
同步测量值
if (sync_packages(Measures)) {
处理imu数据
p_imu->Process(Measures, state, feats_undistort);
在激光雷达 FOV 中分割地图
lasermap_fov_segment();
对于一帧中的特征点进行下采样
downSizeFilterSurf.setInputCloud(feats_undistort);
downSizeFilterSurf.filter(*feats_down_body);
t1 = omp_get_wtime();
feats_down_size = feats_down_body->points.size();
初始化地图kdtree
if (ikdtree.Root_Node == nullptr) {
if (feats_down_size > 5) {
ikdtree.set_downsample_param(filter_size_map_min);
feats_down_world->resize(feats_down_size);
for (int i = 0; i < feats_down_size; i++) {
pointBodyToWorld(&(feats_down_body->points[i]), &(feats_down_world->points[i]));
}
ikdtree.Build(feats_down_world->points);
}
continue;
}
int featsFromMapNum = ikdtree.validnum();
kdtree_size_st = ikdtree.size();
icp和迭代卡尔曼滤波更新
normvec->resize(feats_down_size);
feats_down_world->resize(feats_down_size);
euler_cur = RotMtoEuler(state.rot_end);
pointSearchInd_surf.resize(feats_down_size);
Nearest_Points.resize(feats_down_size);
int rematch_num = 0;
bool nearest_search_en = true;
t2 = omp_get_wtime();
迭代状态估计
std::vector<M3D> body_var;
std::vector<M3D> crossmat_list;
body_var.reserve(feats_down_size);
crossmat_list.reserve(feats_down_size);
double t_update_start = omp_get_wtime();
for (iterCount = 0; iterCount < NUM_MAX_ITERATIONS; iterCount++) {
match_start = omp_get_wtime();
laserCloudOri->clear();
corr_normvect->clear();
total_residual = 0.0;
计算测量值的雅克比矩阵和H矩阵和测量向量
MatrixXd Hsub(effect_feat_num, 12);
MatrixXd Hsub_T_R_inv(12, effect_feat_num);
VectorXd R_inv(effect_feat_num);
VectorXd meas_vec(effect_feat_num);
Hsub.setZero();
Hsub_T_R_inv.setZero();
meas_vec.setZero();
for (int i = 0; i < effect_feat_num; i++) {
const PointType &laser_p = laserCloudOri->points[i];
V3D point_this_L(laser_p.x, laser_p.y, laser_p.z);
V3D point_this = state.offset_R_L_I * point_this_L + state.offset_T_L_I;
M3D var;
calcBodyVar(point_this, 0.02, 0.05, var);
var = state.rot_end * var * state.rot_end.transpose();
M3D point_crossmat;
point_crossmat << SKEW_SYM_MATRX(point_this);
/*** get the normal vector of closest surface/corner ***/
const PointType &norm_p = corr_normvect->points[i];
V3D norm_vec(norm_p.x, norm_p.y, norm_p.z);
R_inv(i) = 1000;
laserCloudOri->points[i].intensity = sqrt(R_inv(i));
/*** calculate the Measurement Jacobian matrix H ***/
if (imu_en) {
M3D point_this_L_cross;
point_this_L_cross << SKEW_SYM_MATRX(point_this_L);
V3D H_R_LI = point_this_L_cross * state.offset_R_L_I.transpose() * state.rot_end.transpose() *
norm_vec;
V3D H_T_LI = state.rot_end.transpose() * norm_vec;
V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, VEC_FROM_ARRAY(
H_R_LI), VEC_FROM_ARRAY(H_T_LI);
} else {
V3D A(point_crossmat * state.rot_end.transpose() * norm_vec);
Hsub.row(i) << VEC_FROM_ARRAY(A), norm_p.x, norm_p.y, norm_p.z, 0, 0, 0, 0, 0, 0;
}
Hsub_T_R_inv.col(i) = Hsub.row(i).transpose() * 1000;
/*** Measurement: distance to the closest surface/corner ***/
meas_vec(i) = -norm_p.intensity;
}
迭代卡尔曼更新
H_T_H.block<12, 12>(0, 0) = Hsub_T_R_inv * Hsub;
MD(DIM_STATE, DIM_STATE) &&K_1 = (H_T_H + state.cov.inverse()).inverse();
K = K_1.block<DIM_STATE, 12>(0, 0) * Hsub_T_R_inv;
auto vec = state_propagat - state;
solution = K * meas_vec + vec - K * Hsub * vec.block<12, 1>(0, 0);
状态更新
state += solution;
rot_add = solution.block<3, 1>(0, 0);
T_add = solution.block<3, 1>(3, 0);
if ((rot_add.norm() * 57.3 < 0.01) && (T_add.norm() * 100 < 0.015))
flg_EKF_converged = true;
deltaR = rot_add.norm() * 57.3;
deltaT = T_add.norm() * 100;
euler_cur = RotMtoEuler(state.rot_end);
收敛判断和协方差更新
if (!EKF_stop_flg && (rematch_num >= 2 || (iterCount == NUM_MAX_ITERATIONS - 1))) {
if (flg_EKF_inited) {
/*** Covariance Update ***/
G.setZero();
G.block<DIM_STATE, 12>(0, 0) = K * Hsub;
state.cov = (I_STATE - G) * state.cov;
total_distance += (state.pos_end - position_last).norm();
position_last = state.pos_end;
if (!imu_en) {
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(euler_cur(0), euler_cur(1), euler_cur(2));
} else {
//Publish LiDAR's pose, instead of IMU's pose
M3D rot_cur_lidar = state.rot_end * state.offset_R_L_I;
V3D euler_cur_lidar = RotMtoEuler(rot_cur_lidar);
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(euler_cur_lidar(0), euler_cur_lidar(1), euler_cur_lidar(2));
}
VD(DIM_STATE) K_sum = K.rowwise().sum();
VD(DIM_STATE) P_diag = state.cov.diagonal();
}
EKF_stop_flg = true;
}
solve_time += omp_get_wtime() - solve_start;
if (EKF_stop_flg) break;
发布里程计
publish_odometry(pubOdomAftMapped);
扫一扫
1706
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
