アームロボット研修ロボットアーム研修(まえけん編)

ロボットアーム研修(まえけん編) その8 ~回転行列を用いた姿勢計算~

CRANE-X7紅白 アームロボット研修

こんにちは、まえけんです。 今回は肘関節までの姿勢を回転行列を用いて計算してみます。
手先三軸に関しては今回姿勢を計算していません。

姿勢を求める手順

今回はロール・ピッチ・ヨー(ZYX-オイラー)を用いて手首の姿勢を求めます。
ざっくりと以下の手順で手首の姿勢を求めることが出来ます。

  1. 回転行列を関節数分用意する
  2. 原点座標に戻すための回転行列を用意する
  3. 関節角度を代入した回転行列を根本の関節から順番にかける
  4. ロール・ピッチ・ヨーの値を計算した行列から求める

回転行列の準備

使用するライブラリ

EigenというC++の標準ライブラリを用いて行列、ベクトルの操作をすると楽です。
ライブラリは以下のようにインクルードしました。

#include <eigen3/Eigen/Dense>

今回はX,Y,Zの単位行列を用意したあとに、その単位行列を使って回転行列を用意します。

各軸の回転行列の準備

Eigen::Matrix3d CX7::rot_X(double rad){
	Eigen::Matrix3d rot_x;
	rot_x = Eigen::AngleAxisd(rad, Eigen::Vector3d::UnitX())
			* Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
			* Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
	rot_x(0,1) = rot_x(0,2) = rot_x(1,0) = rot_x(2,0) = 0; 
	return rot_x;
}
Eigen::Matrix3d CX7::rot_Y(double rad){
	Eigen::Matrix3d rot_y;
	rot_y = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
			* Eigen::AngleAxisd(rad, Eigen::Vector3d::UnitY())
			* Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
	rot_y(0,1) = rot_y(1,0) = rot_y(1,2) = rot_y(2,1) = 0; 
	return rot_y;
}
Eigen::Matrix3d CX7::rot_Z(double rad){
	Eigen::Matrix3d rot_z;
	rot_z = Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX())
			* Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
			* Eigen::AngleAxisd(rad, Eigen::Vector3d::UnitZ());
	rot_z(0,2) = rot_z(1,2) = rot_z(2,0) = rot_z(2,1) = 0; 
	return rot_z;
}	

上記のようにAngleAxisdを使って回転行列を作るとピッタリ0にならない要素が発生します。
おそらくVector3d::Unit○()でベクトルを正規化する際に微小な値のずれが発生するようです。
(例:0にしたい値が1.19e-14とか-1.19e-14のようになる)
参照:https://eigen.tuxfamily.org/dox/classEigen_1_1AngleAxis.html
0にしたい要素の値が微小な負の値になる可能性があり、
計算に影響があるので最後に明示的に0を代入する必要があります。

回転行列の定義

先程定義した回転行列を使って関節間の回転行列を定義します。
引数の”rad”は各関節の角度とします。

Eigen::Matrix3d CR7::rot_R01(double rad){
//Z軸にrad
Eigen::Matrix3d R01;
R01 = rot_Z(rad);
return R01;
}

Eigen::Matrix3d CR7::rot_R12(double rad){
//Xに-90度、Z軸にrad
Eigen::Matrix3d R12;
R12 = rot_X(-M_PI/2.0) * rot_Z(rad);
return R12;
}

Eigen::Matrix3d CR7::rot_R23(double rad){
//Yに90度、Z軸にrad
Eigen::Matrix3d R23;
R23 = rot_Y(M_PI/2.0) * rot_Z(rad);
return R23;
}

Eigen::Matrix3d CR7::rot_R34(double rad){
//Yに-90度、Z軸にrad
Eigen::Matrix3d R34;
R34 = rot_Y(-M_PI/2.0) * rot_Z(rad);
return R34;
}

原点座標と合わせるための回転行列を定義

回転行列を、根本~求めたい関節まで作りました。
この回転行列を順番にかけた後、原点の座標系に合わせるための回転行列をかける必要があります。
ここまで用意してやっと姿勢を求めるための回転行列が求められます。
今回はX軸に90度、Z軸に90度回転する行列をかけると原点の座標系に戻ります。

Eigen::Matrix3d offset = rot_X(M_PI/2.0)*rot_Z(M_PI/2.0);

回転行列をかける

ここまで求めたら後は順番に回転行列をかけます。

Eigen::Matrix3d R_elbow;
R_elbow = rot_R01(rad[0]) * rot_R12(rad[1])*rot_R23(rad[2])*rot_R34(rad[3])*offset;

ロールピッチヨーを求める

先程計算した行列の要素を計算することでロールピッチヨーを求めることが出来ます。
計算式は以下の通りです。

#include <math.h>
(中略)
double cos_theata;
cos_theata = sqrt(R_elbow(2, 1) * R_elbow(2, 1) + R_elbow(2, 2) * R_elbow(2, 2));
//yow
yow = atan2(R_elbow(1, 0), R_elbow(0, 0));
//roll
roll = atan2(R_elbow(2, 1), R_elbow(2, 2));
//pitch
pitch = atan2(-R_elbow(2, 0), cos_theata);

これで手首の姿勢を求めることが出来ます。

実際の動作

実装して動作確認をしてみました。

終わりに

回転行列を作る上で大事なこととして、前回の記事のように予めサーボの原点と回転方向を計算軸の原点と回転方向になおしておくことが大事です。これが出来ていると、ロボット内のサーボの配置を意識しないで回転行列を作ることが出来ます。

次回は同次変換行列を用いた手先位置の取得を行います。

タイトルとURLをコピーしました