【姿勢推定】回転行列vsクォータニオン

姿勢推定を計算するときに回転行列とクォータニオンのどっちを使えばいいんでしょうか。

この記事では、実際にシミュレーションで検証していきます。

ROS実装

まずは、回転行列を用いた実装と、クォータニオンを用いた実装の一部をそれぞれご紹介します。この記事では、ROSをベースに実装しました。

実装内容

IMUで計測した角速度を積算することで姿勢角（ロール、ピッチ、ヨー）を算出します。

回転行列

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Core>
#include <Eigen/LU>

void attitudeEstimationRotationMatrix(
	double& r,
	double& p,
	double& y,
	sensor_msgs::Imu imu,
	double dt
){
	Eigen::Vector3d x(r, p, y);
	Eigen::Matrix3d Rot;
	Rot <<
		1,	sin(r)*tan(p),	cos(r)*tan(p),
		0,	cos(r),		-sin(r),
		0,	sin(r)/cos(p),	cos(r)/cos(p);

	Eigen::Vector3d u(
		imu.angular_velocity.x*dt,
		imu.angular_velocity.y*dt,
		imu.angular_velocity.z*dt
	);

	x = x + Rot*u;
	for(int i=0; i<x.size(); ++i)	atan2(sin(x(i)), cos(x(i)));	//-pi to pi

	r = x(0);
	p = x(1);
	y = x(2);
}

クォータニオン

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>

void attitudeEstimationQuaternion(
	double& r,
	double& p,
	double& y,
	sensor_msgs::Imu imu,
	double dt
){
	tf::Quaternion q_orientation = tf::createQuaternionFromRPY(r, p, y);

	dr = imu.angular_velocity.x*dt;
	dp = imu.angular_velocity.y*dt;
	dy = imu.angular_velocity.z*dt;
	tf::Quaternion q_rel_rot = tf::createQuaternionFromRPY(dr, dp, dy);

	q_orientation = q_orientation*q_rel_rot;
	q_orientation.normalize();

	tf::Matrix3x3(q_orientation).getRPY(r, p, y);
}

検証

シミュレーションを使って実際に姿勢推定してみました。IMUデータには仮想的にノイズを付与しています。

ちなみに今回使用したシミュレーションはMicrosoftのAirSimです。
▶UbuntuでAirSimを使う【C++, ROS】

結果

回転行列の処理に比べ、クォータニオンの処理の誤差が小さくなりました。

考察

コンピュータ内の数値計算では、計算すればするほど誤差が大きくなりやすいと知られています。なぜなら、ビット数によって表現できる桁数が限らているからです。

これをふまえて、回転行列のパラメータ数はクォータニオンより多い分、数値誤差が大きくなっているのだと考察します。

• 回転行列：9パラメータ
• クォータニオン：4パラメータ

さいごに

姿勢推定における回転行列とクォータニオンについてまとめてみました。シミュレーションを用いた検証では、回転行列の処理に比べ、クォータニオンの処理の誤差が小さくなるという結果になりました。

参考になれば幸いです。

以上です。