imu积分补偿lidar的一些理解

目的是利用lidar形成一帧点云时间段内的imu数据,补偿由于运动造成的lidar的畸变,能够把一帧所有的点云的坐标统一到结束时刻的坐标系下。

一、算法过程

假设已经得到了这一段时间的imu数据,以第一个imu的坐标为基准坐标,不断的递推计算其他时刻的imu相对于这个基准坐标的变化姿态,直到最后一个imu的数据算出最后一个姿态相对于第一个imu的坐标系的变化。
这里涉及到左乘还是右乘的问题:由于imu得到的数值都是基于自身当下的坐标系的值,也就是说,是围绕这动轴在转的,需要用右乘的方式,及下一时刻的坐标姿态等于上一时刻的坐标姿态乘上两时刻间imu计算的变换。
T n + 1 = T n ∗ d e l t a T T_{n+1}=T_{n}*deltaT Tn+1=TndeltaT

比如说:
以第0个imu为基准坐标,计算第一个imu相对于第0时刻的imu坐标系的变换:
在这两个时刻之间,根据imu的角速度、线加速度,以及时间间距,可以得出imu在这两个时刻之间的旋转角度、距离:
ω \omega ω 和 t。
同个角度可以转换为旋转矩阵,那么,1时刻的姿态就是:
T 1 = T 0 ∗ d e l t a R o t 1 + t 1 T_1 = T_0 * deltaRot1 + t1 T1=T0deltaRot1+t1
同样,在2时刻:
T 2 = T 1 ∗ d e l t a R o t 2 + t 2 T_2 = T_1 * deltaRot2 + t2 T2=T1deltaRot2+t2

一直到最后一帧imu。
累加起来就是:
T n = ( ( ( T 0 ∗ d e l t a R o t 1 + t 1 ) ∗ d e l t a R o t 2 + t 2 ) ∗ d e l t a R o t 3 + t 3 ) . . . . T_n = (((T_0*deltaRot1+t1)*deltaRot2+t2)*deltaRot3+t3).... Tn=(((T0deltaRot1+t1)deltaRot2+t2)deltaRot3+t3)....

如果是绕固定轴旋转,那么就是左乘了,即变化矩阵*上一时刻坐标姿态。

二、结合代码

在livox_horizon_loam中,对于利用imu补偿也是这样做的。
这个代码仅仅是对旋转做了补偿,未对位移进行计算,因为位移需要有速度,但是仅仅依靠imu,长时间积分速度会很不准,需要利用反馈信息来纠正,当前没有这样做,所以仅仅是利用了旋转量。

该算法每次都累计若干个imu,对应一帧lidar数据。
为了对这一帧的lidar点进行运动补偿,需要计算出这一帧时间内,旋转了多少。
代码在:

主流程代码:


void ImuProcess::Process(const MeasureGroup &meas) {
  ROS_ASSERT(!meas.imu.empty());
  ROS_ASSERT(meas.lidar != nullptr);
  ROS_DEBUG("Process lidar at time: %.4f, %lu imu msgs from %.4f to %.4f",
            meas.lidar->header.stamp.toSec(), meas.imu.size(),
            meas.imu.front()->header.stamp.toSec(),
            meas.imu.back()->header.stamp.toSec());

  auto pcl_in_msg = meas.lidar;

  if (b_first_frame_) {
    /// The very first lidar frame

    /// Reset
    Reset();

    /// Record first lidar, and first useful imu
    last_lidar_ = pcl_in_msg;
    last_imu_ = meas.imu.back();

    ROS_WARN("The very first lidar frame");

    /// Do nothing more, return
    b_first_frame_ = false;
    return;
  }

  /// Integrate all input imu message
  IntegrateGyr(meas.imu);

  /// Compensate lidar points with IMU rotation
   Initial pose from IMU (with only rotation)
  SE3d T_l_c(gyr_int_.GetRot(), Eigen::Vector3d::Zero());
  dt_l_c_ =
      pcl_in_msg->header.stamp.toSec() - last_lidar_->header.stamp.toSec();
   Get input pcl
  pcl::fromROSMsg(*pcl_in_msg, *cur_pcl_in_);

  /// Undistort points
  //因为积分的时候,是以begin为基准的,得到其他时刻相对于begin时刻的坐标变化
  //而我们要补偿的是补偿到end时刻,所以在执行的时候要注意
  Sophus::SE3d T_l_be = T_i_l.inverse() * T_l_c * T_i_l;//end to begin的姿态变化
  pcl::copyPointCloud(*cur_pcl_in_, *cur_pcl_un_);
  UndistortPcl(cur_pcl_un_, dt_l_c_, T_l_be);
。。。
}

void ImuProcess::IntegrateGyr(
    const std::vector<sensor_msgs::Imu::ConstPtr> &v_imu) {
  /// Reset gyr integrator
  gyr_int_.Reset(last_lidar_->header.stamp.toSec(), last_imu_);
  /// And then integrate all the imu measurements
  for (const auto &imu : v_imu) {
    gyr_int_.Integrate(imu);
  }
  ROS_INFO("integrate rotation angle [x, y, z]: [%.2f, %.2f, %.2f]",
           gyr_int_.GetRot().angleX() * 180.0 / M_PI,
           gyr_int_.GetRot().angleY() * 180.0 / M_PI,
           gyr_int_.GetRot().angleZ() * 180.0 / M_PI);
}
void GyrInt::Integrate(const sensor_msgs::ImuConstPtr &imu) {
  /// Init
  if (v_rot_.empty()) {
    ROS_ASSERT(start_timestamp_ > 0);
    ROS_ASSERT(last_imu_ != nullptr);

    /// Identity rotation
    //处理数据时,总是把过程数据清空,把第一个imu对应的旋转矩阵设置为参考姿态,即无旋转。
    v_rot_.push_back(SO3d());

    /// Interpolate imu in
    sensor_msgs::ImuPtr imu_inter(new sensor_msgs::Imu());
    double dt1 = start_timestamp_ - last_imu_->header.stamp.toSec();
    double dt2 = imu->header.stamp.toSec() - start_timestamp_;
    ROS_ASSERT_MSG(dt1 >= 0 && dt2 >= 0, "%f - %f - %f",
                   last_imu_->header.stamp.toSec(), start_timestamp_,
                   imu->header.stamp.toSec());
    double w1 = dt2 / (dt1 + dt2 + 1e-9);
    double w2 = dt1 / (dt1 + dt2 + 1e-9);

    const auto &gyr1 = last_imu_->angular_velocity;
    const auto &acc1 = last_imu_->linear_acceleration;
    const auto &gyr2 = imu->angular_velocity;
    const auto &acc2 = imu->linear_acceleration;

    imu_inter->header.stamp.fromSec(start_timestamp_);
    imu_inter->angular_velocity.x = w1 * gyr1.x + w2 * gyr2.x;
    imu_inter->angular_velocity.y = w1 * gyr1.y + w2 * gyr2.y;
    imu_inter->angular_velocity.z = w1 * gyr1.z + w2 * gyr2.z;
    imu_inter->linear_acceleration.x = w1 * acc1.x + w2 * acc2.x;
    imu_inter->linear_acceleration.y = w1 * acc1.y + w2 * acc2.y;
    imu_inter->linear_acceleration.z = w1 * acc1.z + w2 * acc2.z;

    v_imu_.push_back(imu_inter);
  }

  ///
  const SO3d &rot_last = v_rot_.back();
  const auto &imumsg_last = v_imu_.back();
  const double &time_last = imumsg_last->header.stamp.toSec();
  Eigen::Vector3d gyr_last(imumsg_last->angular_velocity.x,
                           imumsg_last->angular_velocity.y,
                           imumsg_last->angular_velocity.z);
  double time = imu->header.stamp.toSec();
  Eigen::Vector3d gyr(imu->angular_velocity.x, imu->angular_velocity.y,
                      imu->angular_velocity.z);
  assert(time >= 0);
  double dt = time - time_last;
  auto delta_angle = dt * 0.5 * (gyr + gyr_last);
  auto delta_r = SO3d::exp(delta_angle);

  

  SO3d rot = rot_last * delta_r;

  v_imu_.push_back(imu);
  v_rot_.push_back(rot);
}

注意这里的积分右乘:

SO3d rot = rot_last * delta_r;

imu的积分就是这些了,每次都是相当于从0开始,计算出相对变化姿态。

接下来就是利用得到的姿态对lidar进行补偿了。

对lidar进行补偿

这里需要注意:得到的是结束时刻对于起始时刻的姿态变化,而我们要做的是把所有点云坐标统一到结束时刻的坐标系下。

T b e 表 示 e n d 时 刻 相 对 b e g i n 时 刻 的 姿 态 变 化 , 其 他 下 标 都 是 这 个 含 义 。 T_{be} 表示end时刻相对begin时刻的姿态变化,其他下标都是这个含义。 Tbeendbegin姿


void ImuProcess::UndistortPcl(const pcl::PointCloud<RsPointXYZIRT>::Ptr &pcl_in_out,
                              double dt_be, const Sophus::SE3d &Tbe) {
  const Eigen::Vector3d &tbe = Tbe.translation();
  Eigen::Vector3d rso3_be = Tbe.so3().log();
  double first_time=pcl_in_out->points[0].timestamp;
  for (auto &pt : pcl_in_out->points) {
    // int ring = int(pt.intensity);
    float dt_bi = pt.timestamp-first_time;//pt.intensity - ring;

    if (dt_bi == 0) laserCloudtmp->push_back(pt);
    double ratio_bi = dt_bi / dt_be;
    /// Rotation from i-e
    double ratio_ie = 1 - ratio_bi;

    Eigen::Vector3d rso3_ie = ratio_ie * rso3_be;
    SO3d Rie = SO3d::exp(rso3_ie);

    /// Transform to the 'end' frame, using only the rotation
    /// Note: Compensation direction is INVERSE of Frame's moving direction
    /// So if we want to compensate a point at timestamp-i to the frame-e
    /// P_compensate = R_ei * Pi + t_ei
    Eigen::Vector3d tie = ratio_ie * tbe;
    // Eigen::Vector3d tei = Eigen::Vector3d::Zero();
    Eigen::Vector3d v_pt_i(pt.x, pt.y, pt.z);
    Eigen::Vector3d v_pt_comp_e = Rie.inverse() * (v_pt_i - tie);

    /// Undistorted point
    pt.x = v_pt_comp_e.x();
    pt.y = v_pt_comp_e.y();
    pt.z = v_pt_comp_e.z();
  }
}

对于这句话的说明:

Eigen::Vector3d v_pt_comp_e = Rie.inverse() * (v_pt_i - tie);

p e = R e i ∗ p i + t e i p_e=R_{ei}*p_i+t_{ei} pe=Reipi+tei

R i e ∗ p e + t i e = p i ⇒ p e = R i e − 1 p i − t i e R_{ie}*p_e+t_{ie}=p_i ⇒ p_e=R_{ie}^{-1}p_i-t_{ie} Riepe+tie=pipe=Rie1pitie

  • 3
    点赞
  • 15
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
IMULiDAR是两种不同的传感器技术,通过标定可以增加它们之间的配准准确性,提高它们在定位、导航和感知方面的性能。 IMU(惯性测量单元)是基于陀螺仪和加速度计等组件构成的传感器系统,可以测量和记录物体的加速度、角速度和姿态等状态信息。LiDAR(激光雷达)则是通过发送激光束来测量物体距离和位置的传感器。 IMULiDAR之间的标定过程主要包括两个方面:外部标定和内部标定。 外部标定主要是通过手动或自动方法,将IMULiDAR物理安装在同一个坐标系中,使它们的位置、姿态和对齐得到准确的估计。常见的方式是使用高精度的测量工具(如测距仪)来测量它们之间的距离和角度,进而计算出其准确的相对位置关系。 内部标定主要是通过采集IMULiDAR输出的数据,使用数学方法来估计它们之间的转换矩阵,通过这个矩阵可以将两个传感器的数据在同一个坐标系下进行融合和配准。内部标定的过程通常需要在不同的位置和姿态下进行采集和计算,以获得更准确和鲁棒的标定结果。 IMULiDAR的标定可以提高它们在多传感器融合中的配准精度,从而提高定位、导航和感知等应用中的性能。通过标定,我们可以更准确地得到物体的位置、运动状态和环境信息,为机器人、自动驾驶车辆等智能设备的应用提供更可靠的数据支持。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值