工程化的cartographer框架为了实现实时可靠的位姿推断,在算法中实现了位姿推测器(pose_extrapolator), 其作用是通过现有的有关机器人位姿的数据, 给前端匹配一个可靠的目标值进行ceres优化. 也就是以位姿推测器推出来的位姿为前端扫描匹配的目标位姿. 所以前端得到的位姿结果相当依赖于位姿推测器. 前端和位姿推测器的交集如下, 在local_trajectory_builder_2d.cc中:
range_data_poses.push_back(
extrapolator_->ExtrapolatePose(time_point).cast<float>());
......
const transform::Rigid3d non_gravity_aligned_pose_prediction =
extrapolator_->ExtrapolatePose(time);
......
extrapolator_->AddPose(time, pose_estimate);
......
extrapolator_->AddPose(time, transform::Rigid3d::Identity());
......
extrapolator_->AddOdometryData(odometry_data);
cartographer为位姿推测器写了个接口, 文件为pose_extrapolator_Interface.cc/h,都是虚函数与结构体定义, 其接口的实现在pose_extrapolator.cc/h中.
1. PoseExtrapolator类
这个类构造函数如下
PoseExtrapolator::PoseExtrapolator(const common::Duration pose_queue_duration,
double imu_gravity_time_constant)
: pose_queue_duration_(pose_queue_duration),
gravity_time_constant_(imu_gravity_time_constant),
cached_extrapolated_pose_{common::Time::min(),
transform::Rigid3d::Identity()} {}
pose_queue_duration_与gravity_time_constant_都是从lua配置文件中读取的参数, 意思是两次预测位姿需要最小间隔多长时间 与 重力常数.
定义在头文件中的私有变量:
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_;
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. 通过odometer进行位姿推测
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;
}
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;
}
上面判断是不是有两个有效的odometer数据来判断是不是用了odometer. 在odom数据修剪后的队列中得到最新与最老的数据, 计算时间差, 然后通过odometer的位移和角度与时间差微分得到速度和角速度,然后在通过积分得到从上一时刻的位姿(这个位姿是优化后的位姿, 不是odom的位姿)到现在时刻的位姿的转换矩阵,从而推出现在的估计位姿.
修剪odomdata
void PoseExtrapolator::TrimOdometryData() {
while (odometry_data_.size() > 2 && !timed_pose_queue_.empty() &&
odometry_data_[1].time <= timed_pose_queue_.back().time) {
odometry_data_.pop_front();
}
}
3. 通过IMU进行位姿预测
实际用到的IMU位姿预测没有在pose_extrapolator中实现, 而是调用的imu_tracker. 其核心的预测代码在Advance中, 如下:
void ImuTracker::Advance(const common::Time time) {
CHECK_LE(time_, time);
const double delta_t = common::ToSeconds(time - time_);
const Eigen::Quaterniond rotation =
transform::AngleAxisVectorToRotationQuaternion(
Eigen::Vector3d(imu_angular_velocity_ * delta_t));
orientation_ = (orientation_ * rotation).normalized();
gravity_vector_ =
rotation.conjugate() *
gravity_vector_;
time_ = time;
}
可以看到, Advance只用到了IMU的角速度信息(陀螺仪), 并没有用到加速度信息(加速度计). 只用陀螺仪积分得到了角度变化, 用角度的变化更新重力的方向.
在PoseExtrapolator类中, 与 ImuTracker类进行交互的方法是AdvanceImuTracker:
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) {
imu_tracker->Advance(time);
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) {
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);
}
上面代码说明了有或者没有IMU的情况下, 怎么预测旋转. 大家可能会疑惑, 为啥没有IMU还能调用imu_tracker进行Advance? 这要看一下ImuTracker的构造函数
ImuTracker::ImuTracker(const double imu_gravity_time_constant,
const common::Time time)
: imu_gravity_time_constant_(imu_gravity_time_constant),
time_(time),
last_linear_acceleration_time_(common::Time::min()),
orientation_(Eigen::Quaterniond::Identity()),
gravity_vector_(Eigen::Vector3d::UnitZ()),
imu_angular_velocity_(Eigen::Vector3d::Zero()) {}
可以看到构造函数里面都有初值, 那么初值是在哪更新的呢?
如果有IMU的话, 直接通过IMU得到的角速度更新:
imu_tracker->AddImuAngularVelocityObservation(it->angular_velocity);
如果没有IMU的话, 就只能通过其他方式获得估计的角速度, 即有odometer数据就用odom推测的角速度, 没有的话就用pose推测的角速度.
imu_tracker->AddImuAngularVelocityObservation(
odometry_data_.size() < 2 ? angular_velocity_from_poses_
: angular_velocity_from_odometry_);
4. 通过Pose进行位姿预测
由于cartographer中只有激光雷达是必须的, 如果没有odom和IMU, 只用激光雷达推测出的Pose也是可以用位姿推测器的.
void PoseExtrapolator::UpdateVelocitiesFromPoses() {
if (timed_pose_queue_.size() < 2) {
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;
}
方法和odom的预测方式大同小异, 就不介绍了.
5. 插入历史位姿AddPose
AddPose是位姿推测器中最重要的函数, 作用是通过插入上一帧前端匹配后的位姿来进行当前时间点的位姿预测. 这个函数调用了上面提到的函数, 预测出角速度与线速度.
void PoseExtrapolator::AddPose(const common::Time time,
const transform::Rigid3d& pose) {
if (imu_tracker_ == nullptr) {
common::Time tracker_start = time;
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();
AdvanceImuTracker(time, imu_tracker_.get());
TrimImuData();
TrimOdometryData();
odometry_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
extrapolation_imu_tracker_ = absl::make_unique<ImuTracker>(*imu_tracker_);
}
AddPose只用了pose和imu进行预测, 为啥没见到odom呢?这需要结合local_trajectory_builder_2d.cc这个文件看, 不同传感器运用的方案有不同位姿推测器的方案.
6. 位姿推测器的总结
有3中位姿推测的方法, 但是不同情况下用哪一个呢? 当然是哪个准用哪个, 具体哪个准是工程经验得到的. 预测位姿分为4中情况, 共同点是都是用匀速模型进行预测:
- 使用imu, 使用里程计
平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过imu的角速度乘以时间 - 使用imu, 不使用里程计
平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过imu的角速度乘以时间 - 不使用imu, 使用里程计
平移的预测: 通过里程计数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过里程计数据队列开始和末尾的2个数据计算出的角速度乘以时间 - 不使用imu, 不用里程计
平移的预测: 通过pose数据队列开始和末尾的2个数据计算出的线速度乘以时间
姿态的预测: 通过pose数据队列开始和末尾的2个数据计算出的角速度乘以时间
总结:
- 预测平移时: 有里程计就用里程计的线速度, 没有里程计就用pose计算的线速度进行预测
- 预测姿态时: 有IMU就用IMU的角速度, 没有IMU时, 如果有里程计就用里程计计算出的角速度, 没有里程计就用pose计算的角速度进行预测
- 预测的都是相对值, 要加上最后一个pose的位姿
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
