imu_tracker
PoseExtrapolator 调用 imu_trakcer 来利用imu数据对角速度进行计算估计,并通过求解加速度来估计重力向量。
文件在 Cartographer/mapping/imu-tracker.cc
1.什么是imu?
惯性测量单元(英文:Inertial measurement unit,简称IMU)是测量物体三轴姿态角(或角速率)以及加速度的装置。陀螺仪及加速度计是IMU的主要元件,其精度直接影响到惯性系统的精度。
在实际工作中,由于不可避免的各种干扰因素,而导致陀螺仪及加速度计产生误差,从初始对准开始,其导航误差就随时间而增长,尤其是位置误差,这是惯导系统的主要缺点。所以需要利用外部信息进行辅助,实现组合导航,使其有效地减小误差随时间积累的问题。为了提高可靠性,还可以为每个轴配备更多的传感器。一般而言IMU要安装在被测物体的重心上。imu的角速度测量精度往往比线速度好的多,因此,在工程上,经常采用imu的角速度数据。
一般情况,一个IMU包含了三个单轴的加速度计和三个单轴的陀螺仪,加速度计检测物体在载体坐标系统独立三轴的加速度信号,而陀螺仪检测载体相对于导航坐标系的角速度信号,测量物体在三维空间中的角速度和加速度,并以此解算出物体的姿态。在导航中有着很重要的应用价值。
利用三轴地磁解耦和三轴加速度计,受外力加速度影响很大,在运动/振动等环境中,输出方向角误差较大,此外地磁传感器有缺点,它的绝对参照物是地磁场的磁力线,地磁的特点是使用范围大,但强度较低,约零点几高斯,非常容易受到其它磁体的干扰, 如果融合了Z轴陀螺仪的瞬时角度,就可以使系统数据更加稳定。
**加速度测量的是重力方向,**在无外力加速度的情况下,能准确输出ROLL/PITCH两轴姿态角度 并且此角度不会有累积误差,在更长的时间尺度内都是准确的。但是加速度传感器测角度的缺点是加速度传感器实际上是用MEMS技术检测惯性力造成的微小形变,而惯性力与重力本质是一样的,所以加速度计就不会区分重力加速度与外力加速度,当系统在三维空间做变速运动时,它的输出就不正确了。
陀螺仪输出角速度是瞬时量,角速度在姿态平衡上不能直接使用, 需要角速度与时间积分计算角度,得到的角度变化量与初始角度相加,就得到目标角度,其中积分时间Dt越小输出的角度越精确。但陀螺仪的原理决定了它的测量基准是自身,并没有系统外的绝对参照物,加上Dt是不可能无限小的,所以积分的累积误差会随着时间的流逝迅速增加,最终导致输出角度与实际不符,所以陀螺仪只能工作在相对较短的时间尺度内。
所以在没有其它参照物的基础上,要得到较为真实的姿态角,就要**利用加权算法(一阶低通滤波)**扬长避短,结合两者的优点,摈弃其各自缺点,设计算法在短时间尺度内增加陀螺仪的权值,在更长时间尺度内增加加速度权值,这样系统输出角度就接近真实值了。
2.imu-tracker.h
用来计算重力加速度的方向,相当于一个姿态解算器.从IMU数据中计算重力加
速度的向量.
class ImuTracker {
public:
// 构造函数,指数滤波器的常数 和 当前时间.
ImuTracker(double imu_gravity_time_constant, common::Time time);
// Advances to the given 'time' and updates the orientation to reflect this.
// 把ImuTracker更新到指定时刻time
//得到time时刻的重力向量信息,相当于进行外插.
void Advance(common::Time time);
// Updates from an IMU reading (in the IMU frame).
// 加入IMU坐标系的数据,用来更新重力向量和角速度信息.
void AddImuLinearAccelerationObservation(
const Eigen::Vector3d& imu_linear_acceleration);
void AddImuAngularVelocityObservation(
const Eigen::Vector3d& imu_angular_velocity);
// Query the current time.
// 得到当前的时间.
common::Time time() const { return time_; }
// Query the current orientation estimate.
// 得到当前的估计.
Eigen::Quaterniond orientation() const { return orientation_; }
private:
const double imu_gravity_time_constant_;
common::Time time_;
common::Time last_linear_acceleration_time_;
Eigen::Quaterniond orientation_;
Eigen::Vector3d gravity_vector_;
Eigen::Vector3d imu_angular_velocity_;
};
3.imu-tracker.cc
3.1Advance
按照时间利用角速度进行推测–外插
求得方位角,和重力向量
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;
}
3.2跟新重力向量-:AddImuLinearAccelerationObservation
增加加速度信息的函数–用来跟新重力向量.使用了一阶低通融合算法防止加速度跳动剧烈。
void ImuTracker::AddImuLinearAccelerationObservation(
const Eigen::Vector3d& imu_linear_acceleration)
{
// Update the 'gravity_vector_' with an exponential moving average using the
// 'imu_gravity_time_constant'.
// 当前时刻离上一次时刻的差.
const double delta_t =
last_linear_acceleration_time_ > common::Time::min()
? common::ToSeconds(time_ - last_linear_acceleration_time_)
: std::numeric_limits<double>::infinity();
last_linear_acceleration_time_ = time_;
//计算权重,时间差越大,当前的权重越大.
const double alpha = 1. - std::exp(-delta_t / imu_gravity_time_constant_);
//一阶低通融合.
gravity_vector_ =
(1. - alpha) * gravity_vector_ + alpha * imu_linear_acceleration;
// Change the 'orientation_' so that it agrees with the current
// 'gravity_vector_'.
// 根据新的重力向量,更新对应的旋转矩阵.
const Eigen::Quaterniond rotation = Eigen::Quaterniond::FromTwoVectors(
gravity_vector_, orientation_.conjugate() * Eigen::Vector3d::UnitZ());
//更新方位角
orientation_ = (orientation_ * rotation).normalized();
CHECK_GT((orientation_ * gravity_vector_).z(), 0.);
CHECK_GT((orientation_ * gravity_vector_).normalized().z(), 0.99);
}
一阶低通融合
一阶滤波,又叫一阶惯性滤波,或一阶低通滤波。是使用软件编程实现普通硬件RC低通滤波器的功能。
一阶低通滤波的算法公式为:
式中:α=滤波系数;X(n)=本次采样值;Y(n-1)=上次滤波输出值;Y(n)=本次滤波输出值。 一阶低通滤波法采用本次采样值与上次滤波输出值进行加权,得到有效滤波值,使得输出对输入有反馈作用。
其中权重系数a:
从公式可以看出,时间差越大,当前的权重越大.其中d为滤波器的常数
当数据快速变化时,滤波结果能及时跟进(灵敏度优先);当数据趋于稳定,在一个固定的点上下振荡时,滤波结果能趋于平稳(平稳度优先)
3.3增加角速度信息
因为imu直接测得角速度,所以直接更新即可
//更新角速度信息.
void ImuTracker::AddImuAngularVelocityObservation(
const Eigen::Vector3d& imu_angular_velocity)
{
imu_angular_velocity_ = imu_angular_velocity;
}
小节
imu_tracker主要作用是利用角速度对旋转进行了插值,求得重力向量。
到目前为止,前端匹配还剩CSM和ceres就基本上结束了,接下来将对扫描匹配进行介绍。