【ROS】ジャイロスコープのバイアス計測の実装
ジャイロスコープは角速度を計測できるセンサーであり、移動体の姿勢を推定するためによく使われます。
この際、センサーのバイアスを考慮することでより高精度な推定ができます。
この記事では、静止状態のジャイロスコープの計測値からバイアスを推定する実装をご紹介します。センサが静止しているのだから、計測値もゼロであるはずだということを利用し、一定時間の計測値の平均値をバイアスとして算出します。センサの「ゼロ点合わせ」と言い換えても問題ないと思います。
実装の概要
- ジャイロスコープのバイアスを推定
- 「静止状態の計測値の平均値」=「バイアス」
- sub:IMUの計測値
※IMU:ジャイロスコープ+加速度センサ(この記事では加速度は使いません) - pub:センサのバイアス
ソースコード
#include <ros/ros.h> #include <Eigen/Core> #include <Eigen/LU> #include <sensor_msgs/Imu.h> class GyroBiasEstimation{ private: /*node hangle*/ ros::NodeHandle nh; /*subscribe*/ ros::Subscriber sub_imu; /*publish*/ ros::Publisher pub_bias; /*const*/ const int data_size = 6; //wx, wy, wx, vx, vy, vz const double time_enough = 60.0; //[s] const double threshold_diff_imu_angular = 0.03; // const double threshold_diff_imu_linear = 0.2; const int min_record_size = 10; /*objects*/ std::vector<sensor_msgs::Imu> record; sensor_msgs::Imu average; /*flags*/ bool imu_is_moving = false; bool estimation_is_done = false; public: GyroBiasEstimation(); void CallbackIMU(const sensor_msgs::ImuConstPtr& msg); Eigen::VectorXd ImuMsgToVector(sensor_msgs::Imu imu); sensor_msgs::Imu VectorToImuMsg(Eigen::VectorXd Vec); void ComputeAverage(void); bool JudgeMovingIMU(sensor_msgs::Imu imu); void InputZero(void); void Publication(void); }; GyroBiasEstimation::GyroBiasEstimation() { sub_imu = nh.subscribe("/imu/data", 1, &GyroBiasEstimation::CallbackIMU, this); pub_bias = nh.advertise<sensor_msgs::Imu>("/imu/bias", 1); } void GyroBiasEstimation::CallbackIMU(const sensor_msgs::ImuConstPtr& msg) { if(!estimation_is_done){ double duration = 0; bool imu_is_moving = false; if(record.size()>0){ duration = (msg->header.stamp - record[0].header.stamp).toSec(); imu_is_moving = JudgeMovingIMU(*msg); } if(duration>time_enough){ estimation_is_done = true; std::cout << "Duration is now enough!(" << duration << " [s])" << std::endl; } else if(imu_is_moving){ estimation_is_done = true; std::cout << "record.size() = " << record.size() << std::endl; if(record.size()<min_record_size){ std::cout << ">> Estimation is FAILED" << std::endl; InputZero(); } else std::cout << ">> Estimation is SUCCEEDED" << std::endl; } else{ record.push_back(*msg); ComputeAverage(); } } else Publication(); } void GyroBiasEstimation::ComputeAverage(void) { Eigen::VectorXd Ave = Eigen::VectorXd::Zero(data_size); for(size_t i=0;i<record.size();i++) Ave += ImuMsgToVector(record[i]); Ave /= (double)record.size(); average = VectorToImuMsg(Ave); } bool GyroBiasEstimation::JudgeMovingIMU(sensor_msgs::Imu imu) { Eigen::VectorXd Ave = ImuMsgToVector(average); Eigen::VectorXd New = ImuMsgToVector(imu); for(int i=0;i<3;i++){ double diff = fabs(Ave(i) - New(i)); if(diff > threshold_diff_imu_angular){ std::cout << "Started moving!(imu_angular) index#: " << i << std::endl; return true; } } /* for(int i=3;i<6;i++){ double diff = fabs(Ave(i) - New(i)); if(diff > threshold_diff_imu_linear){ std::cout << "Started moving!(imu_linear) index#: " << i << std::endl; return true; } } */ return false; } Eigen::VectorXd GyroBiasEstimation::ImuMsgToVector(sensor_msgs::Imu imu) { Eigen::VectorXd Vec(data_size); Vec << imu.angular_velocity.x, imu.angular_velocity.y, imu.angular_velocity.z, imu.linear_acceleration.x, imu.linear_acceleration.y, imu.linear_acceleration.z; return Vec; } sensor_msgs::Imu GyroBiasEstimation::VectorToImuMsg(Eigen::VectorXd Vec) { sensor_msgs::Imu imu; imu.angular_velocity.x = Vec(0); imu.angular_velocity.y = Vec(1); imu.angular_velocity.z = Vec(2); imu.linear_acceleration.x = Vec(3); imu.linear_acceleration.y = Vec(4); imu.linear_acceleration.z = Vec(5); return imu; } void GyroBiasEstimation::InputZero(void) { average = VectorToImuMsg(Eigen::VectorXd::Zero(data_size)); initial_orientation.x = 0.0; initial_orientation.y = 0.0; initial_orientation.z = 0.0; initial_orientation.w = 1.0; } void GyroBiasEstimation::Publication(void) { pub_bias.publish(average); } int main(int argc, char**argv) { ros::init(argc, argv, "gyro_bias_estimation"); std::cout << "Estimation START!" << std::endl; GyroBiasEstimation gyro_bias_estimation; ros::spin(); }
解説
- 計算終了の条件
- 成功:計測時間が十分(duration>time_enough)
- IMUが静止状態→走行(imu_is_moving == true)
- 成功:計測のサンプル数が十分(record.size()>=min_record_size)
- 失敗:計測のサンプル数が不十分(record.size()<min_record_size)
- IMUの静/動判定:計測値の平均値とリアルタイムの計測値との差が閾値を超えたかどうか(diff > threshold_diff_imu_angular)
- 上記の終了条件の通り、計測時間が一定以上の場合と、センサが動き始めた場合に推定を終了する実装になっています
- まとめて計算するために、sensor_msgs::Imu型をわざわざベクトルに変換しています
補足
この記事の実装はセンサが静止しているときにしか使えませんが、EKFを用いて、移動(走行)中にもセンサのバイアス(ドリフト)を推定する手法もあります。
さいごに
ジャイロスコープのバイアスを推定する単純な手法として、「静止状態の計測値の平均値」=「バイアス」とする実装を公開しました。
参考になったら幸いです。
以上です。
Ad.
コメント