論文

J. Borenstein and L. Feng, Gyrodometry: A New Method for Combining Data from Gyros and Odometry in Mobile Robots, Proceedings of the 1996 IEEE International Conference on Robotics and Automation, pp.423--428, 1996.

おそらくこの論文が”Gyrodometry”の大元の論文だと思います。

概要

• ROSでジャイロオドメトリを実装
• sub1：ホイールオドメトリ（並進速度）
• sub2：ジャイロスコープ（角速度）
• pub：ジャイロオドメトリ

ソースコード

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>

class Gyrodometry{
	private:
		/*node handle*/
		ros::NodeHandle nh;
		/*subscribe*/
		// ros::Subscriber sub_inipose;
		// ros::Subscriber sub_bias;
		ros::Subscriber sub_odom;
		ros::Subscriber sub_imu;
		/*publish*/
		ros::Publisher pub_odom;
		tf::TransformBroadcaster tf_broadcaster;
		/*odom*/
		nav_msgs::Odometry odom2d_now;
		nav_msgs::Odometry odom2d_last;
		nav_msgs::Odometry odom3d_now;
		nav_msgs::Odometry odom3d_last;
		/*objects*/
		// sensor_msgs::Imu bias;
		sensor_msgs::Imu imu_last;
		/*flags*/
		// bool inipose_is_available = false;
		// bool bias_is_available = false;
		bool first_callback_odom = true;
		bool first_callback_imu = true;
	public:
		Gyrodometry();
		void InitializeOdom(nav_msgs::Odometry& odom);
		// void CallbackInipose(const geometry_msgs::QuaternionConstPtr& msg);
		// void CallbackBias(const sensor_msgs::ImuConstPtr& msg);
		void CallbackOdom(const nav_msgs::OdometryConstPtr& msg);
		void CallbackIMU(const sensor_msgs::ImuConstPtr& msg);
		void Publication(void);
};

Gyrodometry::Gyrodometry()
{
	// sub_inipose = nh.subscribe("/initial_orientation", 1, &Gyrodometry::CallbackInipose, this);
	// sub_bias = nh.subscribe("/imu/bias", 1, &Gyrodometry::CallbackBias, this);
	sub_odom = nh.subscribe("/odom", 1, &Gyrodometry::CallbackOdom, this);
	sub_imu = nh.subscribe("/imu/data", 1, &Gyrodometry::CallbackIMU, this);
	pub_odom = nh.advertise<nav_msgs::Odometry>("/gyrodometry", 1);
	InitializeOdom(odom3d_now);
	InitializeOdom(odom3d_last);
}

void Gyrodometry::InitializeOdom(nav_msgs::Odometry& odom)
{
	odom.header.frame_id = "/odom";
	odom.child_frame_id = "/gyrodometry";
	odom.pose.pose.position.x = 0.0;
	odom.pose.pose.position.y = 0.0;
	odom.pose.pose.position.z = 0.0;
	odom.pose.pose.orientation.x = 0.0;
	odom.pose.pose.orientation.y = 0.0;
	odom.pose.pose.orientation.z = 0.0;
	odom.pose.pose.orientation.w = 1.0;
}
/*
void Gyrodometry::CallbackInipose(const geometry_msgs::QuaternionConstPtr& msg)
{
	if(!inipose_is_available){
		odom3d_now.pose.pose.orientation = *msg;
		inipose_is_available = true;
	} 
}

void Gyrodometry::CallbackBias(const sensor_msgs::ImuConstPtr& msg)
{
	if(!bias_is_available){
		bias = *msg;
		bias_is_available = true;
	}
}
*/
void Gyrodometry::CallbackOdom(const nav_msgs::OdometryConstPtr& msg)
{
	odom2d_now = *msg;

	if(!first_callback_odom){
		/*2Dto3D*/
		tf::Quaternion q_pose2d_last;
		tf::Quaternion q_pose3d_last;
		quaternionMsgToTF(odom2d_last.pose.pose.orientation, q_pose2d_last);
		quaternionMsgToTF(odom3d_last.pose.pose.orientation, q_pose3d_last);

		tf::Quaternion q_global_move2d(
			odom2d_now.pose.pose.position.x - odom2d_last.pose.pose.position.x,
			odom2d_now.pose.pose.position.y - odom2d_last.pose.pose.position.y,
			odom2d_now.pose.pose.position.z - odom2d_last.pose.pose.position.z,
			0.0
		);
		tf::Quaternion q_local_move2d = q_pose2d_last.inverse()*q_global_move2d*q_pose2d_last;
		tf::Quaternion q_global_move3d = q_pose3d_last*q_local_move2d*q_pose3d_last.inverse();

		/*integration*/
		odom3d_now.pose.pose.position.x = odom3d_last.pose.pose.position.x + q_global_move3d.x();
		odom3d_now.pose.pose.position.y = odom3d_last.pose.pose.position.y + q_global_move3d.y();
		odom3d_now.pose.pose.position.z = odom3d_last.pose.pose.position.z + q_global_move3d.z();
	}

	odom2d_last = odom2d_now;
	odom3d_last = odom3d_now;
	first_callback_odom = false;

	Publication();
}

void Gyrodometry::CallbackIMU(const sensor_msgs::ImuConstPtr& msg)
{
	/*Get dt*/
	ros::Time time_imu_now = msg->header.stamp;	//or use ros::Time::now()
	ros::Time time_imu_last = imu_last.header.stamp;
	double dt;
	try{
		dt = (time_imu_now - time_imu_last).toSec();
	}
	catch(std::runtime_error& ex) {
		ROS_ERROR("Exception: [%s]", ex.what());
	}

	/*Orientation Estimation*/
	if(first_callback_imu)	dt = 0.0;
	else{	// else if(inipose_is_available){

		double delta_r = (msg->angular_velocity.x + imu_last.angular_velocity.x)*dt/2.0;
		double delta_p = (msg->angular_velocity.y + imu_last.angular_velocity.y)*dt/2.0;
		double delta_y = (msg->angular_velocity.z + imu_last.angular_velocity.z)*dt/2.0;
		/*
		if(bias_is_available){
			delta_r -= bias.angular_velocity.x*dt;
			delta_p -= bias.angular_velocity.y*dt;
			delta_y -= bias.angular_velocity.z*dt;
		}
		*/
		tf::Quaternion q_relative_rotation = tf::createQuaternionFromRPY(delta_r, delta_p, delta_y);
		tf::Quaternion q_pose3d_now;
		quaternionMsgToTF(odom3d_now.pose.pose.orientation, q_pose3d_now);
		q_pose3d_now = q_pose3d_now*q_relative_rotation;
		q_pose3d_now.normalize();
		quaternionTFToMsg(q_pose3d_now, odom3d_now.pose.pose.orientation);
	}

	Publication();

	imu_last = *msg;
	first_callback_imu = false;
}

void Gyrodometry::Publication(void)
{
	/*publish*/
	odom3d_now.header.stamp = odom2d_now.header.stamp;
	pub_odom.publish(odom3d_now);
	/*tf broadcast*/
	geometry_msgs::TransformStamped transform;
	transform.header.stamp = odom2d_now.header.stamp;
	transform.header.frame_id = "/odom";
	transform.child_frame_id = "/gyrodometry";
	transform.transform.translation.x = odom3d_now.pose.pose.position.x;
	transform.transform.translation.y = odom3d_now.pose.pose.position.y;
	transform.transform.translation.z = odom3d_now.pose.pose.position.z;
	transform.transform.rotation = odom3d_now.pose.pose.orientation;
	tf_broadcaster.sendTransform(transform);
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "gyrodometry");

	Gyrodometry gyrodometry;

	ros::spin();
}

解説

• 姿勢（orientation）推定と並進速度を統合
• 姿勢：角速度の時間積分
• 並進速度：ホイールオドメトリから抽出
• クォータニオン（quaternion）ベースでの実装
• tfも生成する実装

補足

• 時刻t-1と時刻tでのホイールオドメトリの座標（x, y, z）から並進速度を計算する実装にしましたが、既にodom.twist.linearに値が入っているならば、単純に（速度）x（時間）でokです。
  dx = v*dt
• 一般的に、角速度センサを用いる場合はバイアスを考慮するべきです。バイアスを推定する簡単な方法として、センサが静止状態の時の出力の平均値をバイアスとする方法があります。
  ※この記事ではバイアスを考慮する実装はしておらず、コメントアウトしておきました。

さいごに

もっとも単純なジャイロオドメトリをROSで実装してみました。
参考になったら幸いです。

以上です。

↓ジャイロスコープのバイアス推定の実装はコチラ↓