pose_extrapolator 主要用于融合里程、IMU和历史位置的输出,目前我用的版本是 2021.05.07 master branch,在旧的tag 0.2.0上是有ukf的,后来的版本没有ufk了。
cartographer 算法中提供了一个位置和姿态估计器,其主要作用是在前端匹配时作为预测位置的输入,即可认为是匹配的初始位置和姿态。在前面已经详细讲解了前端scan math的三种方法,其中前端的优化匹配和相关匹配一定程度上依赖于初始位姿,因此获取预测位置十分重要。
获取预测的位置方式有许多种,比如其他slam算法中常见的odometry转换、IMU积分、上刻位置近似等。而cartographer提供了一个3d预测估计器实现(包含2d实现),可自动融合历史位置、odometer和IMU数据。其调用接口目录为:
cartographer/mapping/pose_extrapolator_interface.h
接口内部为虚函数,具体有两种实现方式,目前2d使用一种PoseExtrapolator,其位姿估计器具体实现代码目录为:
cartographer/mapping/pose_extrapolator.h
一、代码讲解
1.pose_extrapolator类定义
//时间参数,两次预测估计最小时间差
const common::Duration pose_queue_duration_;
// 带时间戳的位置结构体
struct TimedPose {
common::Time time;
transform::Rigid3d pose;
};
// 带时间戳的位置队列
std::deque<TimedPose> timed_pose_queue_;
// 从位置队列估计的线速度和角速度
Eigen::Vector3d linear_velocity_from_poses_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_poses_ = Eigen::Vector3d::Zero();
// 重力加速度常数
const double gravity_time_constant_;
// Imu 原始data 队列,一般仅保留两个或最新预测时间之后的所有序列
std::deque<sensor::ImuData> imu_data_;
// 全局的航向推算器
std::unique_ptr<ImuTracker> imu_tracker_;
// 共享临时航向角推算器
std::unique_ptr<ImuTracker> odometry_imu_tracker_;
std::unique_ptr<ImuTracker> extrapolation_imu_tracker_;
// 推算新的位置缓存
TimedPose cached_extrapolated_pose_;
// 里程计队列信息,一般仅保留两个或最新预测时间之后的所有序列
std::deque<sensor::OdometryData> odometry_data_;
// 通过里程计估计线速度和角速度
Eigen::Vector3d linear_velocity_from_odometry_ = Eigen::Vector3d::Zero();
Eigen::Vector3d angular_velocity_from_odometry_ = Eigen::Vector3d::Zero();
2.插入IMU数据 AddImuData()
// 添加imu data
void PoseExtrapolator::AddImuData(const sensor::ImuData& imu_data) {
CHECK(timed_pose_queue_.empty() ||
imu_data.time >= timed_pose_queue_.back().time);
imu_data_.push_back(imu_data);
TrimImuData();
}
由于IMU可直接提供角速度信息,因此无需进行一定预处理,即已获取imu的角速度信息;
3.插入里程计数据 AddOdometryData()
这里根据最新和最旧odom获取线速度和角速度
// 添加 odometrydata
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) {
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;
// 提取里程计和当前真实位置的旋转转移矩阵
// 其中odometry_imu_tracker_ 与 与刚添加历史位置时的imu_tracker_信息一致
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;
}
4.根据位姿获取速度 UpdateVelocitiesFromPose()
// 从历史位置估计当前的速度
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 < common::ToSeconds(pose_queue_duration_)) {
LOG(WARNING) << "Queue too short for velocity estimation. Queue duration: "
<< queue_delta << " s";
return;
}
// 获取最近两次的位置
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;
}
5.插入历史位置 AddPose()
// 添加历史位置
void PoseExtrapolator::AddPose(const common::Time time,
const transform::Rigid3d& pose) {
// 2d slam imu_tracker_ 没有初始化
// 第一次加入历史位置时,需初始化
if (imu_tracker_ == nullptr) {
// 当前时间为初始时间
common::Time tracker_start = time;
// 如果有imu数据的话, 取imu和当前时间最早时间
if (!imu_data_.empty()) {
tracker_start = std::min(tracker_start, imu_data_.front().time);
}
// 初始化 角度推算器
imu_tracker_ =
absl::make_unique<ImuTracker>(gravity_time_constant_, tracker_start);
}
// 记录对应时间和位置,放入一个队列存储
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();
//将全局imu推算器执行推算,主要推算估计的航向信息
AdvanceImuTracker(time, imu_tracker_.get());
TrimImuData();
TrimOdometryData();
// 智能指针创建,并共享内容,无需delete
// 即每次添加新的pose时同时开辟两个tracker
odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}
插入历史位置进行更新估计为主要和必要方法,即使没有odometer和imu插入数据也可正常工作,因此AddPose()可为主要函数,即目的按照时间顺序插入历史真实位置(实际上为scan-match后较为准确的位置)。然后进行估计线速度和角速度。通过角速度再次进行积分获取航向变换信息。
6.航向角积分 AdvanceImuTracker()
//imu 估计器更新,即迭代更新当前估计位置的航向信息, 估计器采用指针可调用不同具体估计器
//
void PoseExtrapolator::AdvanceImuTracker(const common::Time time,
ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
// 当没有imu数据或者,更新的当前时间比imu数据时间早
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,则无线性加速度信息
imu_tracker->AddImuLinearAccelerationObservation(Eigen::Vector3d::UnitZ());
// 更新推算器中目前采用的角速度
// 如果有里程计信息,显然更相信里程计信息
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;
});
// 将imudata中每一时刻进行估计更新
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);
}
由于已估计出目前的线速度和角速度,为获取估计姿态则需要对角速度进行积分,然后获取角度旋转矩阵。由于航向积分并非像平移一样线性加减即可,因此cartographer专门提供了一个用于角度积分的ImuTracker类。
7.获取预测位置 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) {
// 获取估计的平移矩阵
// 然后对上次的位置进行平移,获取估计位置
const Eigen::Vector3d translation =
ExtrapolateTranslation(time) + newest_timed_pose.pose.translation();
// 获取估计旋转矩阵
// 然后对上次的航向进行旋转,获取估计的航向
// extrapolation_imu_tracker_ 与最新加入历史位置时的imu_tracker_一致
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;
}
以上最终输出预测值接口,在上次真实位置基础上进行积分,即获取平移矩阵和旋转矩阵,进行转换获取最终估计位置和姿态;
其中位置平移矩阵积分较为简单,可参考代码即可。
8.获取运动平移ExtrapolateTranslation()
//获取当前时间与插入最新位置的平移矩阵
Eigen::Vector3d PoseExtrapolator::ExtrapolateTranslation(common::Time time) {
// 最新插入的位置信息
const TimedPose& newest_timed_pose = timed_pose_queue_.back();
// 计算时间差
const double extrapolation_delta =
common::ToSeconds(time - newest_timed_pose.time);
// 根据线速度直接计算
if (odometry_data_.size() < 2) {
return extrapolation_delta * linear_velocity_from_poses_;
}
return extrapolation_delta * linear_velocity_from_odometry_;
}
9.获取运动旋转ExtrapolateRotation()
通过角度积分器获取角度
// 获取当前时间与插入最新位置的旋转变换矩阵
Eigen::Quaterniond PoseExtrapolator::ExtrapolateRotation(
const common::Time time, ImuTracker* const imu_tracker) const {
CHECK_GE(time, imu_tracker->time());
// 更新当前的航向信息
AdvanceImuTracker(time, imu_tracker);
// 获取全局imu_tracker_ 朝向,由于未进行更新,即为上一时刻的朝向
const Eigen::Quaterniond last_orientation = imu_tracker_->orientation();
// 求出上次到最新航向矩阵的转换矩阵
return last_orientation.inverse() * imu_tracker->orientation();
}
二、思想总结
位姿估计器解决的是在实现前端scan-match之前的预测值,即激光新的一帧匹配前获取当前预测定位信息。同时假设传感器数据之间,slam本体以恒定线速度和恒定角速度移动。根据上次的scan-match的准确位置后进行积分,获取预测位置和姿态。
其中估计的恒定速度主要两种方式组成:采用两次scan-match位置进行微分获得;另一种通过里程计进行微分获得;由于里程计的更新速度更快,故里程计可计算最新结果时,优先使用;
估计的角速度则有三种方式组成:1.采用两次scan-match的航向进行微分获得,频率最低;2.采用里程计进行微分;3.采用IMU直接获取,优先级最高;
cartographer针对imu和odometer是否使用可进行配置,根据配置信息和时间序列进行选择具体方式,最后根据估计的线速度和角速度进行实时积分,获取预测估计位置。
最基本的配置odometry和imu均无,则估计器实际上仅是根据历史位置进行推算出线速度和角速度,进行估计位置。
说到这里也可以通过后端优化node数设置为0,单独用odom、imu来检测前端的效果。