こんにちは。まえけんです。
今回は同時変換行列を用いて手首と手先座標を計算してみたいと思います。
また、今回求める手先の座標はCRANE-X7のエンドエフェクタが閉じている場合に限定します。
手首と手先の座標を求める
求める座標
今回はCRANE-X7の手首と手先の座標を求めます。
以前の記事では2自由度だったので順運動学の式も簡単でした。
今回は7軸全てを考慮した時の手首、手先座標を求めます。
しかし前回と同じように解析的に求めると非常に大変なので今回は同時変換行列というものを使用します。
同次変換行列とは
回転行列を拡張した、回転と平行移動を1つの行列で扱えるようなものです。
以下の6つの行列を使ってリンクの位置を計算します。
回転や平行移動を順番にかけて手先の座標を計算します。
同次変換行列の詳細は実践ロボット制御の第3章4部(p44~p60)にあります。
並行移動の無い同次変換行列の作り方
まず、並行移動の無い同次変換行列の作り方です。
並行移動の無い場合は以前の記事で作った回転行列を拡張して同次変換行列にすることが出来ます。
3×3の回転行列に4行目と4列目を加えて4×4行列にします。
この時、4行4列の要素は1、それ以外は0となります。
Eigenという行列演算用の標準ライブラリを使って上記作業を関数として実装します。
/**
* @brief 回転行列を同次変換行列に拡張したあと値を初期化する関数
*/
Eigen::MatrixXd init_3dto4dmat(Eigen::Matrix3d mat){
Eigen::MatrixXd mat_4d(4, 4);
Eigen::MatrixXd temp_mat(3,3);
temp_mat = mat;
temp_mat.conservativeResize(4,4);
mat_4d = temp_mat;
for(int cnt=0;cnt<3;cnt++){
mat_4d(3,cnt) = 0;
mat_4d(cnt,3) = 0;
}
mat_4d(3,3)=1;
return mat_4d;
}この関数に3×3の回転行列を引数として与えると、以下のような4×4の行列に拡張が可能です。
これは平行移動の無い場合の同次変換行列と一致します。
並行移動のある同次変換行列の作り方(次回へつづく)
並行移動がある場合は、ただの回転行列とは形が異なります。
したがって、素直に回転行列と平行移動用の行列をかけて同次変換行列を作ります。
並行移動があるのは以前の記事のリンク番号の付け方だと2linkから3link(根本から肘)への同次変換行列と3linkから4link(肘から手首)への同次変換行列の2箇所です。
エンドエフェクタは最後にオフセットとしてかけるので同次変換行列を作る必要がありません。
並行移動がある場合は軸がわかりにくく、長くなるため詳細は次回につづきます。
(記事訂正：求める座標を「肘、手先」から「手首、手先」に変更)
