网站的内容有哪些内容,唐山网站托管,爱站seo排名可以做哪些网站,做网站app需多少钱目录
基于 ESKF 的松耦合 LIO 系统 1 坐标系说明 2 松耦合 LIO 系统的运动和观测方程 3 松耦合 LIO 系统的数据准备 3.1 CloudConvert 类 3.2 MessageSync 类 4 松耦合 LIO 系统的主要流程 4.1 IMU 静止初始化 4.2 ESKF 之 运动过程——使用 IMU 预测 4.3 使用 IMU 预测位姿进…目录
基于 ESKF 的松耦合 LIO 系统 1 坐标系说明 2 松耦合 LIO 系统的运动和观测方程 3 松耦合 LIO 系统的数据准备 3.1 CloudConvert 类 3.2 MessageSync 类 4 松耦合 LIO 系统的主要流程 4.1 IMU 静止初始化 4.2 ESKF 之 运动过程——使用 IMU 预测 4.3 使用 IMU 预测位姿进行运动补偿 4.4 松耦合系统的配准部分 基于 ESKF 的松耦合 LIO 系统 这里我们实现一个相对简单的案例使用第 3 章介绍的 ESKF配合 7.3.2 中的增量 NDT 里程计实现松耦合的 LIO 系统。 整个系统的流程如下图所示可以观察到其中的滤波器部分和点云配准部分是相对解耦的 1 坐标系说明 2 松耦合 LIO 系统的运动和观测方程 由于整个 LIO 运行在 IMU 坐标系中状态变量的运动方程与《自动驾驶与机器人中的SLAM技术》ch3惯性导航与组合导航 中介绍的 ESKF 的运动方程保持一致我们不再展开叙述。同时雷达里程计LO的输出位姿可直接视为对状态变量 , 的观测。这个过程实际和第 3 章的 ESKF 中谈到的 GNSS 观测是一样的。 LO 对 的观测可以直接写成对误差状态 的观测从而省去前面的链式法则推导简化整个线性化过程。LO 的旋转观测方程为类似于 把误差状态归入名义状态 的方程 其中 为该时刻的名义状态 为误差状态 为观测位姿。由于在观测过程中名义状态 是确定的。我们 不妨将 直接视为对 的观测。我们对该方程稍作变换可以写为 此时 是对 的直接观测即 所以 关于 的雅可比矩阵即旋转部分的雅可比矩阵为单位阵 LO 的平移观测方程为类似于 把误差状态归入名义状态 的方程 类似的我们对该方程稍作变换可以写为 因此平移部分的雅可比矩阵也为单位阵 由于 LO 观测数据为 6 维 的 故 矩阵的维度为 具体形式如下 这样就避免了再从名义状态到误差状态进行转换的过程可以直接得到对误差状态的雅可比矩阵。注意当我们这样做时原本 ESKF 中 部分的更新量innovation 也应该写成流形的形式 即得到 该式表明了从直观上来看LO 的 主要是在观测阶段通过卡尔曼增益作用于误差状态变量中。 3 松耦合 LIO 系统的数据准备 松耦合的代码实现主要分为三个部分
我们需要将 IMU 数据与激光数据进行同步。激光通常使用 10Hz 的频率而 IMU 通常是更高的 100Hz。我们希望能够统一处理两个激光数据之间的那 10 个 IMU 数据。我们需要处理激光的运动补偿而运动补偿需要有激光测量时间内的位姿数据来源正好可以用 ESKF 对每个 IMU 数据的预测值。我们应该从 ESKF 中拿到预测的位姿数据交给里程计算法再将里程计配准之后的位姿放入 ESKF 中。 3.1 CloudConvert 类 CloudConvert 类负责将各种格式的点云转化为 PCL 格式的点云。以 livox_ros_driver::CustomMsg 类型点云为例输入 msg 输出时间单位为毫秒(ms)、跳点处理后的 PCL 格式点云 pcl_out。代码如下
/// 带ring, range等其他信息的全量信息点云
struct FullPointType {PCL_ADD_POINT4D; //宏定义来自 PCL为 FullPointType 添加了四维坐标点x,y,z,w。其中前三个是空间坐标而 w 是填充位通常为 1.0用于齐次坐标。float range 0; //点的距离通常是到传感器的距离float radius 0; //点的半径有时可以表示与传感器的水平距离具体视应用而定uint8_t intensity 0; //点的强度值反射强度uint8_t ring 0; //点的扫描线编号uint8_t angle 0; //点的角度值double time 0; //点的时间戳float height 0; //点的高度信息inline FullPointType() {}EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
void CloudConvert::Process(const livox_ros_driver::CustomMsg::ConstPtr msg, FullCloudPtr pcl_out) {AviaHandler(msg);*pcl_out cloud_out_;
}void CloudConvert::AviaHandler(const livox_ros_driver::CustomMsg::ConstPtr msg) {cloud_out_.clear();cloud_full_.clear();int plsize msg-point_num;cloud_out_.reserve(plsize);cloud_full_.resize(plsize);std::vectorbool is_valid_pt(plsize, false); // 标记哪些点是有效的std::vectoruint index(plsize - 1);for (uint i 0; i plsize - 1; i) {index[i] i 1; // 从1开始}std::for_each(std::execution::par_unseq, index.begin(), index.end(), [](const uint i) {// 0x30: 00110000// 0x10: 00010000、0x00: 00000000// 只保留置信度优和中的点if ((msg-points[i].line num_scans_) ((msg-points[i].tag 0x30) 0x10 || (msg-points[i].tag 0x30) 0x00)) {// 跳点比例if (i % point_filter_num_ 0) {cloud_full_[i].x msg-points[i].x;cloud_full_[i].y msg-points[i].y;cloud_full_[i].z msg-points[i].z;cloud_full_[i].intensity msg-points[i].reflectivity;cloud_full_[i].time msg-points[i].offset_time / float(1000000); // mid360 时间戳单位为 ns转换为 msif ((abs(cloud_full_[i].x - cloud_full_[i - 1].x) 1e-7) ||(abs(cloud_full_[i].y - cloud_full_[i - 1].y) 1e-7) ||(abs(cloud_full_[i].z - cloud_full_[i - 1].z) 1e-7)) {is_valid_pt[i] true;}}}});for (uint i 1; i plsize; i) {if (is_valid_pt[i]) {cloud_out_.points.push_back(cloud_full_[i]);}}
} 3.2 MessageSync 类 MessageSync 类负责将 IMU 数据与点云进行同步。该类接收 ROS 数据包中原始的 IMU 消息与激光雷达消息通过 Sync 函数将它们组装成一个 MeasureGroup 实例对象再将它传递给回调函数。我们后续的松耦合、紧耦合算法就只需要考虑如何处理 MeasureGroup 实例对象而不必再操心数据准备、同步的实现代码了。 这里需要注意 ① bag 包中雷达 msg 的时间戳为一帧雷达数据中首个点的时间戳 ② 每个 MeasureGroup 实例对象中存储的 IMU 消息的时间戳均小于其存储的 LIDAR 消息的 lidar_end_time_。MeasureGroup 实例对象中一般存储 1 帧雷达消息和 10 帧 IMU 消息。 /// IMU 数据与雷达同步
struct MeasureGroup {MeasureGroup() { this-lidar_.reset(new FullPointCloudType()); };double lidar_begin_time_ 0; // 雷达包的起始时间double lidar_end_time_ 0; // 雷达的终止时间FullCloudPtr lidar_ nullptr; // 雷达点云std::dequeIMUPtr imu_; // 上一时时刻到现在的IMU读数
};
bool MessageSync::Sync() {if (lidar_buffer_.empty() || imu_buffer_.empty()) {return false;}// MeasureGroup 中是否存在 单帧 LiDAR 数据。若不存在进入该 ifif (!lidar_pushed_) {measures_.lidar_ lidar_buffer_.front();measures_.lidar_begin_time_ time_buffer_.front(); // lidar 数据中的时间戳是 lidar_begin_time_lidar_end_time_ measures_.lidar_begin_time_ measures_.lidar_-points.back().time / double(1000); // 以 s 为单位measures_.lidar_end_time_ lidar_end_time_;lidar_pushed_ true;}// imu_buffer_ 最后一个 imu 数据的时间戳要大于等于 lidar_end_time_确保 imu 数据覆盖当前的 lidar 时间范围(lidar_begin_time_ ~ lidar_end_time_)if (last_timestamp_imu_ lidar_end_time_) {return false;}double imu_time imu_buffer_.front()-timestamp_;measures_.imu_.clear();while ((!imu_buffer_.empty()) (imu_time lidar_end_time_)) {imu_time imu_buffer_.front()-timestamp_;if (imu_time lidar_end_time_) {break;}measures_.imu_.push_back(imu_buffer_.front());imu_buffer_.pop_front();}// 将已处理的 LiDAR 数据从 lidar_buffer_ 和时间缓存 time_buffer_ 中移除为下一轮同步准备lidar_buffer_.pop_front();time_buffer_.pop_front();lidar_pushed_ false;if (callback_) {callback_(measures_);}return true;
} 4 松耦合 LIO 系统的主要流程 松耦合 LooselyLIO 类持有一个 IncrementalNDTLO增量 NDT 里程计对象一个 ESKF 对象一个 MessageSync 对象 处理同步之后的点云和 IMU。该类处理流程非常简单当 MeasureGroup 到达后在 IMU 未初始化时使用第 3 章的静止初始化来估计 IMU 零偏。初始化完毕后先使用 IMU 数据进行预测再用预测数据对点云去畸变最后对去畸变的点云做配准。
void LooselyLIO::ProcessMeasurements(const MeasureGroup meas) {LOG(INFO) call meas, imu: meas.imu_.size() , lidar pts: meas.lidar_-size();measures_ meas;if (imu_need_init_) {// 初始化IMU系统TryInitIMU();return;}// 利用IMU数据进行状态预测Predict();// 对点云去畸变Undistort();// 配准Align();
} 4.1 IMU 静止初始化 IMU 的静止初始化与《自动驾驶与机器人中的SLAM技术》ch3惯性导航与组合导航 中介绍的大体一致。当 MeasureGroup 到达后在 IMU 未初始化时调用 StaticIMUInit::AddIMU() 函数进行 IMU的静止初始化。当 IMU 初始化成功时在当前 MeasureGroup 中完成 ESKF 中 Q, V, b_g, b_a, g_w, P 的初始化。
void LooselyLIO::TryInitIMU() {for (auto imu : measures_.imu_) {imu_init_.AddIMU(*imu);}if (imu_init_.InitSuccess()) {// !!! 下面 4 行代码即完成了 Q, V, b_g, b_a, g_w, P 的初始化// 读取初始零偏设置ESKFsad::ESKFD::Options options;// 噪声由初始化器估计options.gyro_var_ sqrt(imu_init_.GetCovGyro()[0]);options.acce_var_ sqrt(imu_init_.GetCovAcce()[0]);eskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());imu_need_init_ false;LOG(INFO) IMU初始化成功;}
} IMU 静止初始化结果如下 I0112 15:59:51.430646 354274 loosely_lio.cc:54] call meas, imu: 10, lidar pts: 3601
I0112 15:59:51.430662 354274 static_imu_init.cc:86] mean acce: -0.00215149 00.00016898 000.0978879
I0112 15:59:51.430694 354274 static_imu_init.cc:109] IMU 初始化成功初始化时间 9.99018, bg -0.00259592 00.00176906 0.000707638, ba 000.213411 -0.0167615 00-9.70973, gyro sq 5.96793e-05 4.42613e-05 3.58264e-05, acce sq 9.71749e-07 1.85436e-06 2.14871e-07, grav 000.215562 -0.0169305 00-9.80762, norm: 9.81
I0112 15:59:51.430707 354274 static_imu_init.cc:113] mean gyro: -0.00259592 00.00176906 0.000707638 acce: 000.213411 -0.0167615 00-9.70973
imu try init true time:1547714610.30704498
I0112 15:59:51.430723 354274 loosely_lio.cc:100] IMU初始化成功4.2 ESKF 之 运动过程——使用 IMU 预测 IMU 预测部分与先前介绍的 GINS 中预测部分类似。上一个 MeasureGroup 中完成了 IMU 的静止初始化当前 MeasureGroup 中的 IMU 数据就开始参与 ESKF 的运动过程了。std::vectorNavStated 类型的 imu_states_ 的大小为 MeasureGroup 中的IMU 数量 1其存储上一 IMU 时刻 ESKF 的名义转态变量和当前 MeasureGroup 中每一个 IMU 数据预测后的 ESKF 的名义转态变量用来插值进行点云的去畸变。
void LooselyLIO::Predict() {imu_states_.clear();imu_states_.emplace_back(eskf_.GetNominalState());std::cout current_time_: eskf_.GetTime() std::endl;/// 对IMU状态进行预测for (auto imu : measures_.imu_) {eskf_.Predict(*imu);imu_states_.emplace_back(eskf_.GetNominalState());std::cout current_time_: eskf_.GetTime() std::endl;}
} 4.3 使用 IMU 预测位姿进行运动补偿 其原理简单来说就是通过固定的世界坐标系结合每个时刻的插值结果 将一帧雷达中所有时刻的点全部转移到雷达扫描结束时刻。 假设比例 计算公式如下其中 为待插值的时刻 为起始时刻 为结束时刻 旋转部分插值四元数球面线性插值 (SLERP)确保旋转路径在旋转空间中的弧长最短。 平移部分插值平移向量线性插值 (LERP) 注意这种去畸变的方法前提是滤波器本身有效。如果滤波器失效或位姿发散去畸变算法也就随之发散了。 SE3 pose_first;
SE3 pose_next;// 计算旋转插值结果使用球面线性插值 - SLERP
Eigen::Quaterniond result_R pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion());// 计算平移插值结果使用线性插值 - LERP
Eigen::Vector3d result_p pose_first.translation() * (1 - s) pose_next.translation() * s;// 检查结果仅用于调试
std::cout Interpolated Rotation (Quaternion): result_R.coeffs().transpose() std::endl;
std::cout Interpolated Translation: result_p.transpose() std::endl;void LooselyLIO::Undistort() {auto cloud measures_.lidar_;auto imu_state eskf_.GetNominalState(); // 最后时刻的状态SE3 T_end SE3(imu_state.R_, imu_state.p_);if (options_.save_motion_undistortion_pcd_) {sad::SaveCloudToFile(/home/wu/slam_in_autonomous_driving/data/ch7/undist/before_undist.pcd, *cloud);}/// 将所有点转到最后时刻状态上std::for_each(std::execution::par_unseq, cloud-points.begin(), cloud-points.end(), [](auto pt) {SE3 Ti T_end; // 只是为了初始化NavStated match;// 根据pt.time查找时间pt.time是该点打到的时间与雷达开始时间之差单位为毫秒// 插值结果为 Timath::PoseInterpNavStated(measures_.lidar_begin_time_ pt.time * 1e-3, imu_states_, [](const NavStated s) { return s.timestamp_; },[](const NavStated s) { return s.GetSE3(); }, Ti, match);Vec3d pi ToVec3d(pt);Vec3d p_compensate TIL_.inverse() * T_end.inverse() * Ti * TIL_ * pi;pt.x p_compensate(0);pt.y p_compensate(1);pt.z p_compensate(2);});scan_undistort_ cloud;if (options_.save_motion_undistortion_pcd_) {sad::SaveCloudToFile(/home/wu/slam_in_autonomous_driving/data/ch7/undist/after_undist.pcd, *cloud);}
}
/*** pose 插值算法* tparam T 数据类型* tparam C 数据容器类型* tparam FT 获取时间函数* tparam FP 获取pose函数* param query_time 查找时间* param data 数据容器* param take_pose_func 从数据中取pose的谓词接受一个数据返回一个SE3* param result 查询结果* param best_match_iter 查找到的最近匹配** NOTE 要求query_time必须在data最大时间和最小时间之间(容许0.5s内误差)* data的map按时间排序* return*/
template typename T, typename C, typename FT, typename FP
inline bool PoseInterp(double query_time, C data, FT take_time_func, FP take_pose_func, SE3 result,T best_match, float time_th 0.5) {if (data.empty()) {LOG(INFO) cannot interp because data is empty. ;return false;}// 如果 query_time 超过最大时间但在容许阈值 time_th 范围内此时插值的结果直接使用最后一条数据的位姿。// rbegin() 返回的是反向迭代器指向容器的最后一个元素从后往前遍历的起点double last_time take_time_func(*data.rbegin());if (query_time last_time) {if (query_time (last_time time_th)) {// 尚可接受result take_pose_func(*data.rbegin());best_match *data.rbegin();return true;}return false;}auto match_iter data.begin();for (auto iter data.begin(); iter ! data.end(); iter) {auto next_iter iter;next_iter;if (take_time_func(*iter) query_time take_time_func(*next_iter) query_time) {match_iter iter;break;}}auto match_iter_n match_iter;match_iter_n;double dt take_time_func(*match_iter_n) - take_time_func(*match_iter);double s (query_time - take_time_func(*match_iter)) / dt; // s0 时为第一帧s1时为next// 出现了 dt为0的bugif (fabs(dt) 1e-6) {best_match *match_iter;result take_pose_func(*match_iter);return true;}SE3 pose_first take_pose_func(*match_iter);SE3 pose_next take_pose_func(*match_iter_n);// 旋转部分使用了四元数的球面线性插值Slerp。SlerpSpherical Linear Interpolation 是一种在两四元数之间进行插值的方式。与普通的线性插值不同Slerp 能够保持旋转的路径最短给出的旋转角度总是通过球面路径最优化。// s 是插值的参数。当 s 在 0 和 1 之间时结果是 pose_first 和 pose_next 之间的旋转的插值。// 平移部分使用线性插值y (1-s)*y_0 s*y_1result {pose_first.unit_quaternion().slerp(s, pose_next.unit_quaternion()),pose_first.translation() * (1 - s) pose_next.translation() * s};best_match s 0.5 ? *match_iter : *match_iter_n;return true;
} 4.4 松耦合系统的配准部分 前文已经得到了去畸变的点云这里只需将其传递给增量 NDT 里程计并使用滤波器预测得到的先验位姿作为增量 NDT 里程计的初始位姿经过迭代计算后得到优化后的位姿后再返回给滤波器滤波器进行观测过程。在这个过程中滤波器部分和点云配准部分是解耦的。 void LooselyLIO::Align() {FullCloudPtr scan_undistort_trans(new FullPointCloudType);pcl::transformPointCloud(*scan_undistort_, *scan_undistort_trans, TIL_.matrix()); // 将 lidar 坐标系下的点云转换到 imu 坐标系下// scan_undistort_ 为 imu 坐标系下 无畸变的点云scan_undistort_ scan_undistort_trans;auto current_scan ConvertToCloudFullPointType(scan_undistort_);// voxel 降采样pcl::VoxelGridPointType voxel;voxel.setLeafSize(0.5, 0.5, 0.5);voxel.setInputCloud(current_scan);CloudPtr current_scan_filter(new PointCloudType);voxel.filter(*current_scan_filter);/// 处理首帧雷达数据if (flg_first_scan_) {SE3 pose;inc_ndt_lo_-AddCloud(current_scan_filter, pose);flg_first_scan_ false;return;}/// 从EKF中获取预测pose放入LO获取LO位姿最后合入EKFSE3 pose_predict eskf_.GetNominalSE3();inc_ndt_lo_-AddCloud(current_scan_filter, pose_predict, true); // 第 3 个参数为 true, 即不使用匀速运动假设推测位姿pose_of_lo_ pose_predict;eskf_.ObserveSE3(pose_of_lo_, 1e-2, 1e-2);if (options_.with_ui_) {// 放入UIui_-UpdateScan(current_scan, eskf_.GetNominalSE3()); // 转成Lidar Pose传给UIui_-UpdateNavState(eskf_.GetNominalState());}frame_num_;
} I0112 21:54:14.812438 383071 ndt_inc.cc:124] aligning with inc ndt, pts: 1532, grids: 819
I0112 21:54:14.812675 383071 ndt_inc.cc:222] iter 0 total res: 2003.41, eff: 960, mean res: 2.08689, dxn: 0.00164213, dx: -0.000169117 00.000230697 00.000262647 00.000125452 0-0.00158076 00.000176706
I0112 21:54:14.812845 383071 ndt_inc.cc:222] iter 1 total res: 1981.36, eff: 954, mean res: 2.0769, dxn: 0.000790319, dx: 07.83699e-06 02.29818e-05 09.93558e-05 -0.000364279 -0.000640747 00.000266245
I0112 21:54:14.812858 383071 ndt_inc.cc:227] converged, dx 07.83699e-06 02.29818e-05 09.93558e-05 -0.000364279 -0.000640747 00.000266245
