Self Driving and ROS 2 - Learn by Doing! Odometry & Control: 角度表現 (セクション8-4/13)

• TF2ライブラリを使ったオイラー角とクォータニオンによる3D空間でのオリエンテーション表現方法を学習。

• PythonとC++でクォータニオンを使用した回転の実装方法を実践し、オイラー角との変換方法を理解。

• TF2ツールを用いて、トランスフォームの視覚化や管理を行う実践的な方法を習得。

Self Driving and ROS 2 - Learn by Doing! Odometry & Control

「Self Driving and ROS 2 - Learn by Doing! Odometry & Control」コースのセクション8を締めくくるにあたり、TF2ライブラリについてさらに詳しく学びます。最終講義（86～91）では、角度表現、オイラー角とクォータニオンの使用、および変換を管理するためのTF2の実用ツールについて説明します。これらの概念とその実装について詳しく見ていきましょう。

講義86：角度表現

オイラー角とクォータニオン

オイラー角とクォータニオンは、3D空間での姿勢を表すための2つの方法です。オイラー角は直感的で、主軸（X、Y、Z）周りの回転を表します。クォータニオンは直感的ではありませんが、その効率性と堅牢性のために計算環境で好まれます。

重要なポイント

• オイラー角:

• Z、Y、X軸周りの回転（ヨー、ピッチ、ロール）。

• 3つの角度とそれに対応する回転行列で表される。

• 簡単ですが、オイラー角はギンバルロック（姿勢表現が自由度を失う現象）に悩まされることがあります。

• クォータニオン:

• 4つの成分：1つの実数部と3つの虚数部。

• ギンバルロックを回避し、滑らかな補間を提供。

• 計算用途に効率的だが、人間には直感的ではない。

講義87：オイラー角の詳細

オイラー角は、主軸周りの3つの連続した回転を通じて3D空間での回転を表します。これらの回転は次の行列で表されます：

1. ヨー（ψ）回転：

$$
R_z(\psi) = \begin{pmatrix}
\cos(\psi) & -\sin(\psi) & 0 \
\sin(\psi) & \cos(\psi) & 0 \
0 & 0 & 1
\end{pmatrix}
$$

1. ピッチ（θ）回転：

$$
R_y(\theta) = \begin{pmatrix}
\cos(\theta) & 0 & \sin(\theta) \
0 & 1 & 0 \
-\sin(\theta) & 0 & \cos(\theta)
\end{pmatrix}
$$

1. ロール（φ）回転：

$$
R_x(\phi) = \begin{pmatrix}
1 & 0 & 0 \
0 & \cos(\phi) & -\sin(\phi) \
0 & \sin(\phi) & \cos(\phi)
\end{pmatrix}
$$

最終的な回転行列は、これらの行列を掛け合わせることで得られます：

$$
R = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi)
$$

講義88：クォータニオンの理解

クォータニオンは、3D回転を表すための複素数の拡張です。クォータニオンは4つの成分で構成され、1つのスカラー（実数）部分と3つのベクトル（虚数）部分があります。

主要な概念

• クォータニオンの成分:
  $${ q = a + bi + cj + dk }$$
  ここで、$${ a }$$ はスカラー部分、$${ bi, cj, dk }$$ はベクトル部分です。

• 単位クォータニオン: 回転に使用されるクォータニオンの大きさは1でなければなりません：
  $${ a^2 + b^2 + c^2 + d^2 = 1 }$$

• クォータニオンの乗算: 回転を組み合わせるためにクォータニオンの乗算を使用します。これは16の積と9の和を含みます。

• クォータニオンの逆: クォータニオンの逆はベクトル部分の符号を変更することで求められます：
  $${ q^{-1} = a - bi - cj - dk }$$

講義89：Pythonでのオイラー角からクォータニオンへの変換

この講義では、クォータニオンベースの回転を実装するためのPythonスクリプトを修正します。このスクリプトでは、TF2ライブラリを使用して変換と回転を処理します。

主要な実装

• 初期化：

self.last_orientation_ = quaternion_from_euler(0, 0, 0)
self.orientation_increment_ = quaternion_from_euler(0, 0, 0.05)

• 回転の更新：

q = quaternion_multiply(self.last_orientation_, self.orientation_increment_)
self.dynamic_transform_stamped_.transform.rotation.x = q[0]
self.dynamic_transform_stamped_.transform.rotation.y = q[1]
self.dynamic_transform_stamped_.transform.rotation.z = q[2]
self.dynamic_transform_stamped_.transform.rotation.w = q[3]

• 逆回転：

self.orientation_increment_ = quaternion_inverse(self.orientation_increment_)

講義90：C++でのオイラー角からクォータニオンへの変換

この講義では、同様のクォータニオンベースの回転機能をC++で実装します。

主要な実装

• 初期化：

last_orientation_.setRPY(0, 0, 0);
orientation_increment_.setRPY(0, 0, 0.05);

• 回転の更新：

tf2::Quaternion q;
q = last_orientation_ * orientation_increment_;
q.normalize();
dynamic_transform_stamped_.transform.rotation.x = q.x();
dynamic_transform_stamped_.transform.rotation.y = q.y();
dynamic_transform_stamped_.transform.rotation.z = q.z();
dynamic_transform_stamped_.transform.rotation.w = q.w();

• 逆回転：

orientation_increment_ = orientation_increment_.inverse();

講義91：TF2ツールの使用

TF2は、変換を管理および視覚化するためのいくつかの便利なツールを提供します。

実用ツール

• フレームの表示：

ros2 run tf2_tools view_frames

このコマンドは、変換ツリーのPDFビジュアライゼーションを生成します。

• TF2エコー：

ros2 run tf2_ros tf2_echo <source_frame> <target_frame>

このツールは、2つのフレーム間の現在の変換行列を表示し、変換と回転の値を表示します。

結論

「Self Driving and ROS 2 - Learn by Doing! Odometry & Control」コースのセクション8では、TF2ライブラリの包括的な理解が提供され、3D姿勢を表すためのオイラー角とクォータニオンに焦点が当てられました。PythonとC++の実践的な例を通じて、変換を効果的に実装および管理する方法を学びました。TF2ツールは、変換関係を視覚化およびデバッグする能力をさらに向上させ、ROS 2での堅牢で効率的な開発を確保します。

「超本当にドラゴン」へ