PoseExtrapolator

PoseExtrapolator

功能:解决IMU数据, 里程计和位姿信息的融合问题, 计算速度.
输入 : IMU, Odometry, Pose
输出 : 位姿计算的线速度和角速度 , 里程计计算的线速度和角速度,以及某一个时刻的位姿 (ExtrapolatePose(const common::Time time))
旋转：ImuTracker的方向角的变化量
平移：里程计或者位姿线速度计算的移动量

输入

1, 位姿信息以及通过位姿计算线速度和角速度

struct TimePose
{
	common::Time time;
	transform::Rigid3d pose;
}
std::dequetimed_pose_queue_;
Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_From_poses_ = Eigen::Vector3d::Zero();

void AddPose(common::Time time, const transform::Rigid3d& pose);

2, IMU信息

const double gravity_time_constant_;
std::deque imu_data_;
std::unique_ptr imu_tracker_;
std::unique_ptr odometry_imu_tracker_;
std::unique_ptrextrapolation_imu_tracker_;
TimePose cached_extrapolated_pose_;

void AddImuData(cosnt sensor::ImuData& imu_data);

3, 里程计信息以及计算线速度和角速度

std::deque odometry_data_;
Eigen::Vector3d linear_velocity_from_odometry_ =  Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_odometry =  Eigen::Vector3d::Zero();

void AddOdometryData(const sensor::OdometryData& odometray_data);

依次根据数据采集频率处理IMU, Odometry, Pose的数据对应

void AddImuData(const sensor::ImuData& imu_data);
void AddOdometryData(const sensor::OdometryData& odometry_data);
void AddPose(common::Time time, const transform::Rigid3d& pose);

然后在需要的时候进行融合,作为激光scan match时候的初始值, 具体:

输出

1 如何获取速度

根据位姿获取速度

void PoseExtrapolator::UpdateVelocitiesFromPoses() 
{
	  if (timed_pose_queue_.size() < 2)
	   {
		    // We need two poses to estimate velocities.
		    return;
	  }
	  CHECK(!timed_pose_queue_.empty());
	  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
	  const auto newest_time = newest_timed_pose.time;
	  const TimedPose& oldest_timed_pose = timed_pose_queue_.front();
	  const auto oldest_time = oldest_timed_pose.time;
	  const double queue_delta = common::ToSeconds(newest_time - oldest_time);
	  if (queue_delta < 0.001)
	   {  // 1 ms
	    LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
	                 << queue_delta << " ms";
	    return;
	  }
	  //速度 = 位置/时间
	  // 线速度 = (newest_pose.translation() - oldest_pose.translation() )/ queue_delta;
	  // 角速度 =   oldest_pose.rotation().inverse() * newest_pose.rotation()
	  const transform::Rigid3d& newest_pose = newest_timed_pose.pose;
	  const transform::Rigid3d& oldest_pose = oldest_timed_pose.pose;
	  linear_velocity_from_poses_ =  (newest_pose.translation() - oldest_pose.translation()) / queue_delta;
	  angular_velocity_from_poses_ = transform::RotationQuaternionToAngleAxisVector(      oldest_pose.rotation().inverse() * newest_pose.rotation()) /queue_delta;
}

同理还可根据里程计获取速度

void PoseExtrapolator::AddOdometryData(const sensor::OdometryData& odometry_data)
 {
	  CHECK(timed_pose_queue_.empty() || odometry_data.time >= timed_pose_queue_.back().time);
	  odometry_data_.push_back(odometry_data);
	  TrimOdometryData();
	  if (odometry_data_.size() < 2) // odometry_data_大于两个才能计算速度
	   {
	    	return;
	  }
	  // TODO(whess): Improve by using more than just the last two odometry poses.
	  // Compute extrapolation in the tracking frame.
	  const sensor::OdometryData& odometry_data_oldest = odometry_data_.front();
	  const sensor::OdometryData& odometry_data_newest = odometry_data_.back();
  	  const double odometry_time_delta = common::ToSeconds(odometry_data_oldest.time - odometry_data_newest.time);
      const transform::Rigid3d odometry_pose_delta = odometry_data_newest.pose.inverse() * odometry_data_oldest.pose;
      angular_velocity_from_odometry_ = transform::RotationQuaternionToAngleAxisVector(odometry_pose_delta.rotation()) /odometry_time_delta;
	  if (timed_pose_queue_.empty()) 
	  {
	    	return;
	  }
    const Eigen::Vector3d linear_velocity_in_tracking_frame_at_newest_odometry_time = odometry_pose_delta.translation() / odometry_time_delta;
    const Eigen::Quaterniond orientation_at_newest_odometry_time = timed_pose_queue_.back().pose.rotation() *ExtrapolateRotation(odometry_data_newest.time, odometry_imu_tracker_.get());
    linear_velocity_from_odometry_ = orientation_at_newest_odometry_time *linear_velocity_in_tracking_frame_at_newest_odometry_time;
}

2 推算某一个时刻的位姿

ExtrapolatePose: ExtrapolateTranslation + ExtrapolateRotation 推算位姿包括平移和旋转, 内部维护的就是TimedPose cached_extrapolated_pose_
基本思路: 1, 在有pose的时候

void PoseExtrapolator::AddPose(const common::Time time, const transform::Rigid3d& pose) 
{

	//初始化一个ImuTracker 分有和没有imu_data时候
	  if (imu_tracker_ == nullptr)
	   {
		    common::Time tracker_start = time; //没有imu_data的时候就用当前pose的时刻进行初始化
		    if (!imu_data_.empty()) //有imu_data的时候用imu_data的起始时间初始化
		     {
		      tracker_start = std::min(tracker_start, imu_data_.front().time);
		    }
		    imu_tracker_ = common::make_unique(gravity_time_constant_, tracker_start);
	  }
	  
	  // 处理姿态队列timed_pose_queue 里的数据 根据pose_queue_duration_ 
	  timed_pose_queue_.push_back(TimedPose{time, pose});
	  while (timed_pose_queue_.size() > 2 && timed_pose_queue_[1].time <= time - pose_queue_duration_) 
	  {
	    	 timed_pose_queue_.pop_front();
	  }
	  //更新姿态速度
	  UpdateVelocitiesFromPoses();
	  //推算ImuTracker
	  AdvanceImuTracker(time, imu_tracker_.get());
	  //对齐IMU和里程计数据
	  TrimImuData();
	  TrimOdometryData();
	  //重新初始化两个ImuTracker
	  odometry_imu_tracker_ = common::make_unique(*imu_tracker_);
	  extrapolation_imu_tracker_ = common::make_unique(*imu_tracker_);
}

注意这里有三个ImuTracker的变量 分别是:

std::unique_ptr imu_tracker_;
std::unique_ptr odometry_imu_tracker_;
std::unique_ptr extrapolation_imu_tracker_;

其中imu_tracker_在AddPose中使用, odometry_imu_tracker_在AddOdometryData中计算旋转的时候使用,最后一个则是在ExtrapolatePose中推算旋转时使用

transform::Rigid3d PoseExtrapolator::ExtrapolatePose(const common::Time time) 
{
  const TimedPose& newest_timed_pose = timed_pose_queue_.back();
  CHECK_GE(time, newest_timed_pose.time);
  if (cached_extrapolated_pose_.time != time)  //如果这个位姿不是最新的, 则根据这个时间段内采集到的IMU, Odometry Pose进行推算平移和旋转位姿
  {
	    const Eigen::Vector3d translation =  ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
	    const Eigen::Quaterniond rotation = newest_timed_pose.pose.rotation() * ExtrapolateRotation(time, extrapolation_imu_tracker_.get());
	    cached_extrapolated_pose_ = TimedPose{time, transform::Rigid3d{translation, rotation}};
  }
  return cached_extrapolated_pose_.pose;
}

总结

PoseExtrapolator的主要功能就是利用IMU ,Odometry惯导加激光SLAM得出的Pose进行融合得到姿态(平移, 旋转), 因为IMU, Odometry的速率比激光有高, 在一定时间段内可能没有Pose数据,则通过IMU, Odometry推断,同时因为他们存在累积误差, 在有Pose的时候则对他们进行一些更新和初始化消除累积误差. 同时,
在估计旋转的时候, 可以在没有IMU的时候可以利用之前的Pose 或者 Odometry估算出的速度进行模拟在一定程度上可以保持位姿数据的稳定性.

void PoseExtrapolator::AdvanceImuTracker(const common::Time time, ImuTracker* const imu_tracker) const
 {
  CHECK_GE(time, imu_tracker->time());
  if (imu_data_.empty() || time < imu_data_.front().time) 
  {
    // There is no IMU data until 'time', so we advance the ImuTracker and use
    // the angular velocities from poses and fake gravity to help 2D stability.
    imu_tracker->Advance(time);
    
    imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
    //没有IMU数据的时候, 或者里程计的时候利用之前Pose估算的角速度模拟, 有里程计的时候则根据里程计估算的
    imu_tracker->AddImuAngularVelocityObservation(
    							odometry_data_.size() < 2 ? angular_velocity_from_poses_
                                  : angular_velocity_from_odometry_);
    return;
  }
  if (imu_tracker->time() < imu_data_.front().time) {
    // Advance to the beginning of 'imu_data_'.
    imu_tracker->Advance(imu_data_.front().time);
  }
  auto it = std::lower_bound(
      imu_data_.begin(), imu_data_.end(), imu_tracker->time(),
      [](const sensor::ImuData& imu_data, const common::Time& time) {
        return imu_data.time < time;
      });
  while (it != imu_data_.end() && it->time < time) {
    imu_tracker->Advance(it->time);
    imu_tracker->AddImuLinearAccelerationObservation(it->linear_acceleration);
    imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
    ++it;
  }
  imu_tracker->Advance(time);
}