前言
在进行GPS加IMU的组合导航或者Lidar加IMU的组合导航时，用EKF或者ESKF的滤波方法时，需要提前知道惯导的测量噪声、初始零偏、重力方向等信息。
此时就需要对惯导进行一个初始化，来获取以上信息，常见的初始化方法为静止初始化法。例如无人机在上电后要进行自检，此时需要无人机静止一段时间，通过指示灯来提示自检是否完毕，在静止的过程中，则对惯导进行了初始化的方法。
静止初始化方法
在传统组合导航系统中，最常见的是使用静止初始化方法。
静止初始化就是把IMU放在某个地方静止一段时间。在静止时间内，由于物体本身没有任何运动，可以简单地认为IMU的陀螺仪只测到零偏，而加速度计则测到零偏与重力之和。
可以设置一个静止初始化流程来获取这些变量：
1、将IMU静止一段时间，比如10s。静止状态检测可以由轮速计进行判断，当两轮的轮速均小于阈值时，认为机器人静止。在没有轮速计的机器人上，也可以直接认为机器人静止，来测定相关变量。
2、统计静止时间内的陀螺仪与加速度计读数均值，记为$\bar{d} _{gyr},\bar{d} _{acc}$
3、由于机器人并未发生转动，这段时间的陀螺仪均值可以取$b_{g}=\bar{d} _{gyr}$
4、加速度的测量方程为
当机器人的实际加速度为零，旋转视为R=I时，加速度实际测到$b_{a}-g$,其中$b_{a}$为小量，g的长度可视为固定值。
在这些前提下，取方向为$-\bar{d} _{acc}$，大小为9.8的矢量作为重力矢量，这一步确定了重力的朝向。
5、将这段时间的加速度计读数去掉重力，重新计算$\bar{d} _{acc}$。
6、取$b_{a}=\bar{d} _{acc}$
7、认为零偏不动，估计陀螺仪和加速度计的测量方差。该方差可用于ESKF的噪声参数
惯导静止初始化实现代码
声明静态惯导初始化的类
IMU水平静止状态下初始化器
使用方法：调用AddIMU, AddOdom添加数据，使用InitSuccess获取初始化是否成功
成功后，使用各Get函数获取内部参数
class StaticIMUInit {
下面为类的具体内容
++++++++++++++++++++++++++++++++++++++++++++++++++++
// 可配置参数结构
struct Options {
Options() {}
double init_time_seconds_ = 10.0; // 静止时间
int init_imu_queue_max_size_ = 2000; // 初始化IMU队列最大长度
int static_odom_pulse_ = 5; // 静止时轮速计输出噪声
double max_static_gyro_var = 0.5; // 静态下陀螺测量方差
double max_static_acce_var = 0.05; // 静态下加计测量方差
double gravity_norm_ = 9.81; // 重力大小
bool use_speed_for_static_checking_ = true; // 是否使用odom来判断车辆静止（部分数据集没有odom选项）
};
公共变量区，设置可配置参数结构体
具体参数：
- 静止时间（采集这么长时间数据后进行初始化）
- 初始化IMU队列最大长度（如在设置的静止时间内，队列满，再来数据则逐步省略掉最早的数据）
- 静止时轮速计输出噪声（轮速计输出小于此值认为静止）
- 静止下陀螺和加计测量方差最大值（计算结果大于此值则认为计算失败）
- 重力大小
- 是否使用odom来判断车辆静止
++++++++++++++++++++++++++++++++++++++++++++++++++++
/// 判定初始化是否成功
bool InitSuccess() const { return init_success_; }
/// 获取各Cov, bias, gravity
Vec3d GetCovGyro() const { return cov_gyro_; }
Vec3d GetCovAcce() const { return cov_acce_; }
Vec3d GetInitBg() const { return init_bg_; }
Vec3d GetInitBa() const { return init_ba_; }
Vec3d GetGravity() const { return gravity_; }
获得计算的 方差、零偏、重力等
++++++++++++++++++++++++++++++++++++++++++++++++++++
私有变量区声明，要用到的变量
Options options_; // 选项信息
bool init_success_ = false; // 初始化是否成功
Vec3d cov_gyro_ = Vec3d::Zero(); // 陀螺测量噪声协方差（初始化时评估）
Vec3d cov_acce_ = Vec3d::Zero(); // 加计测量噪声协方差（初始化时评估）
Vec3d init_bg_ = Vec3d::Zero(); // 陀螺初始零偏
Vec3d init_ba_ = Vec3d::Zero(); // 加计初始零偏
Vec3d gravity_ = Vec3d::Zero(); // 重力
bool is_static_ = false; // 标志车辆是否静止
std::deque<IMU> init_imu_deque_; // 初始化用的数据
double current_time_ = 0.0; // 当前时间
double init_start_time_ = 0.0; // 静止的初始时间
下面来看各实现函数
++++++++++++++++++++++++++++++++++++++++++++++++++++
当惯导数据来时，添加惯导数据
如果已经初始化成功则直接返回
bool StaticIMUInit::AddIMU(const IMU& imu) {
if (init_success_) {
return true;
}
通过轮速计判断机器人 没有静止 ，则将用于初始化的惯导数据队列清空
if (options_.use_speed_for_static_checking_ && !is_static_) {
LOG(WARNING) << "等待静止";
init_imu_deque_.clear();
return false;
}
如果惯导数据队列为空，那么则刚进入静止状态，记录初始静止时间
if (init_imu_deque_.empty()) {
// 记录初始静止时间
init_start_time_ = imu.timestamp_;
}
将数据加如用于初始化的imu队列数据
// 记入初始化队列
init_imu_deque_.push_back(imu);
计算静止时间数据有了多久
double init_time = imu.timestamp_ - init_start_time_; // 初始化经过时间
如果时间大于了设定值，那么则用采集好的数据进入初始化函数
if (init_time > options_.init_time_seconds_) {
// 尝试初始化逻辑
TryInit();
}
时间没够，但是数据大于了设定队列长度，则删除掉队列最前数据
// 维持初始化队列长度
while (init_imu_deque_.size() > options_.init_imu_queue_max_size_) {
init_imu_deque_.pop_front();
}
运行到这则说明，静止的时间还没够，更新当前时间，整体函数返回false
current_time_ = imu.timestamp_;
return false;
++++++++++++++++++++++++++++++++++++++++++++++++++++
下面是轮速计数据来时的操作函数
bool StaticIMUInit::AddOdom(const Odom& odom) {
// 判断车辆是否静止
if (init_success_) {
return true;
}
if (odom.left_pulse_ < options_.static_odom_pulse_ && odom.right_pulse_ < options_.static_odom_pulse_) {
is_static_ = true;
} else {
is_static_ = false;
}
current_time_ = odom.timestamp_;
return true;
}
主要就是根据数据，判断是否静止，将标志位进行设置
++++++++++++++++++++++++++++++++++++++++++++++++++++
然后就是当静止时间够后，进入的初始化函数
如果队列数据太短，则无法进行初始化
bool StaticIMUInit::TryInit() {
if (init_imu_deque_.size() < 10) {
return false;
}
声明 陀螺和加计的均值
Vec3d mean_gyro, mean_acce;
下面则计算均值和协方差
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });
这里主要调用了数学库中的均值与协方差计算函数
++++++++++++++++++++++++++++++++++++++++++++++++++++
LOG(INFO) << "mean acce: " << mean_acce.transpose();
gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;
以acce均值为方向，取9.8长度为重力
将加计读数减去重力，重新计算协方差
// 重新计算加计的协方差
math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
[this](const IMU& imu) { return imu.acce_ + gravity_; });
检测 计算的 IMU 噪声是否过大，过大则 直接返回 false
// 检查IMU噪声
if (cov_gyro_.norm() > options_.max_static_gyro_var) {
LOG(ERROR) << "陀螺仪测量噪声太大" << cov_gyro_.norm() << " > " << options_.max_static_gyro_var;
return false;
}
if (cov_acce_.norm() > options_.max_static_acce_var) {
LOG(ERROR) << "加计测量噪声太大" << cov_acce_.norm() << " > " << options_.max_static_acce_var;
return false;
}
赋值IMU零偏
init_bg_ = mean_gyro;
init_ba_ = mean_acce;
至此初始成功，函数返回true
init_success_ = true;
return true;
}
在gazebo中进行测试
为了在gazebo中测试上面的代码，在无人机的测试环境中进行，用无人机的IMU，进行测试，所以需要在类的定义中，加入对无人机IMU的订阅话题，然后在回调函数中调用上面的代码函数。
// 将ros的imu格式的数据 转为 设置的imu格式的数据
sad::IMU imu;
imu.timestamp_ = Imu_msg->header.stamp.toSec();
imu.gyro_ << Imu_msg->angular_velocity.x,Imu_msg->angular_velocity.y,Imu_msg->angular_velocity.z;
imu.acce_ << Imu_msg->linear_acceleration.x,Imu_msg->linear_acceleration.y,Imu_msg->linear_acceleration.z;
//添加IMU数据，时间够了则自动进行计算
AddIMU(imu);
由于上面方法函数已经写好，所以测试函数就比较简单，上面就是将ros的imu格式的数据 转为 设置的imu格式的数据
下面就是添加IMU数据，时间够了则自动进行计算。
计算的结果在上面的实现函数中，直接打印在终端，可以查看
编译完代码，则可以进行测试。
启动无人机仿真环境：
无人机在地面静止
启动测试代码
终端输出
I0307 10:13:05.245363 49670 static_imu_init.cc:73] mean acce: -0.220884 -0.193247 009.92608
I0307 10:13:05.245481 49670 static_imu_init.cc:95] IMU 初始化成功，初始化时间= 10, bg = 0-0.00141685 000.00568429 -1.93852e-05, ba = -0.00267846 -0.00234334 0000.120365, gyro sq = 1.13541e-05 1.16579e-05 1.17825e-05, acce sq = 0.00150067 0.00157587 0.00172446, grav = 0.218205 0.190904 -9.80571, norm: 9.81
I0307 10:13:05.245513 49670 static_imu_init.cc:99] mean gyro: 0-0.00141685 000.00568429 -1.93852e-05 acce: -0.00267846 -0.00234334 0000.120365
通过打印信息可以看到得到的初始化测量结果：
- 陀螺仪零偏:0.00141 0.00568 -1.93852e-05
- 加速度计零偏：-0.00267846 -0.00234334 0.120365
- 陀螺仪测量噪声：1.13541e-05 1.16579e-05 1.17825e-05
- 加速度计测量噪声：0.00150067 0.00157587 0.00172446
- 重力矢量：0.218205 0.190904 -9.80571
- 重力模长：9.81
评论（0）
您还未登录，请登录后发表或查看评论