Self Driving and ROS 2 - Learn by Doing! Map & Localization: オドメトリ (セクション5-2/11)
オドメトリモーションモデルの理論と実装を学び、ロボットの動きを3つの基本動作に分解し、ノイズの影響を考慮した。
PythonでROS2ノードを作成し、ガウスノイズを追加してロボットの位置推定を行う粒子フィルタを実装した。
RVizを用いて、粒子分布の視覚化を行い、ロボットの動きにおける不確実性の伝播を理解した。
Self Driving and ROS 2 - Learn by Doing! Map & Localizationコースを進める中で、最近、セクション5のレッスン49から51に取り組みました。これらのレッスンは、ロボットの動きと位置を正確に推定するための基本コンポーネントであるオドメトリモーションモデルの複雑さに焦点を当てています。
レッスン49: オドメトリモーションモデル
理論的基礎
このレッスンは、オドメトリモーションモデルの理論的基礎について詳細に説明します。モデルの核心は、ロボットの動きを3つの基本的な動きに分解することです：
初回回転（$${(\delta_{\text{rot1}})}$$）：ある角度でのその場での回転。
平行移動（$${(\delta_{\text{trans}})}$$）：ある距離の直線移動。
最終回転（$${(\delta_{\text{rot2}})}$$）：最終的な向きを達成するためのもう一つのその場での回転。
これらの動きは数学的に次のように表現できます：
$$
[
\delta_{\text{trans}} = \sqrt{(X' - X)^2 + (Y' - Y)^2}
]
$$
$$
[
\delta_{\text{rot1}} = \text{atan2}(Y' - Y, X' - X) - \theta
]
$$
$$
[
\delta_{\text{rot2}} = \theta' - \theta - \delta_{\text{rot1}}
]
$$
ノイズの考慮
実際のシナリオでは、オドメトリデータは測定誤差によるノイズの影響を受けます。このレッスンでは、このノイズを考慮したモデルを拡張し、次のようになります：
$$
[
\hat{\delta}{\text{rot1}} = \delta{\text{rot1}} - \text{prob}(q_1 \delta_{\text{rot1}} + q_2 \delta_{\text{trans}})
]
$$
$$
[
\hat{\delta}{\text{trans}} = \delta{\text{trans}} - \text{prob}(q_3 \delta_{\text{trans}} + q_4 (\delta_{\text{rot1}} + \delta_{\text{rot2}}))
]
$$
$$
[
\hat{\delta}{\text{rot2}} = \delta{\text{rot2}} - \text{prob}(q_1 \delta_{\text{rot2}} + q_2 \delta_{\text{trans}})
]
$$
このノイズは、特定の係数（$${(\alpha_1, \alpha_2, \alpha_3, \alpha_4)}$$）に依存するゼロ平均のガウス分布によって特徴付けられます。
行列表現
ノイズを組み込んだロボットの新しい位置は次のように計算されます：
$$
[
\begin{bmatrix}
\hat{X}' \
\hat{Y}' \
\hat{\theta}'
\end{bmatrix}=\begin{bmatrix}
X \
Y \
\theta
\end{bmatrix}
+
\begin{bmatrix}
\hat{\delta}{\text{trans}} \cos(\theta + \hat{\delta}{\text{rot1}}) \
\hat{\delta}{\text{trans}} \sin(\theta + \hat{\delta}{\text{rot1}}) \
\hat{\delta}{\text{rot1}} + \hat{\delta}{\text{rot2}}
\end{bmatrix}
]
$$
レッスン50: Pythonでオドメトリモーションモデルを実装
ノードのセットアップ
このレッスンでは、Pythonを使用したオドメトリモーションモデルの実際の実装に焦点を当てます。新しいROS2ノード `odometry_motion_model.py` を作成し、ロボットのオドメトリメッセージにサブスクライブし、ガウスノイズを含むオドメトリモーションモデルを適用し、可能なロボットの姿勢を表す粒子のセットを発行します。
主要なステップ
ノードの作成：以前のカルマンフィルターノードの実装内容をコピーし、オドメトリモーションモデル用に修正。
クラスの修正：クラス名を変更し、ROS2のサブスクライバとパブリッシャを設定。
パラメータの宣言： `alpha1`, `alpha2`, `alpha3`, `alpha4`, `number_of_samples` といったパラメータを宣言。
オドメトリ増分の計算：クォータニオンのオリエンテーションデータをオイラー角に変換し、位置とオリエンテーションの増分を計算。
基本動作の計算： $${(\delta_{\text{rot1}}, \delta_{\text{trans}}, \delta_{\text{rot2}})}$$ を計算。
ガウスノイズの追加：モーションモデルのコンポーネントにガウスノイズを組み込む。
粒子の発行：粒子を生成し、それらの分布をRVizで視覚化するために発行。
レッスン51: オドメトリモーションモデルにノイズを追加
詳細な実装
このレッスンでは、モーションコンポーネントにガウスノイズを追加することに焦点を当てます。詳細なステップは次のとおりです：
分散の計算：モーションコンポーネントに影響を与えるガウスノイズの分散を決定。
乱数生成： `random` ライブラリを使用して、各粒子に対してガウス分布ノイズを生成。
粒子位置の更新：ノイズのないモーションコンポーネントと現在のオリエンテーションに基づいて各粒子の位置とオリエンテーションを更新。
主要な方程式
このレッスンでは、ノイズを正確にモデル化する重要性を強調しています：
$$
[
\text{Var}(\delta_{\text{rot1}}) = \alpha_1 \cdot \delta_{\text{rot1}} + \alpha_2 \cdot \delta_{\text{trans}}
]
[
\text{Var}(\delta_{\text{trans}}) = \alpha_3 \cdot \delta_{\text{trans}} + \alpha_4 \cdot (\delta_{\text{rot1}} + \delta_{\text{rot2}})
]
[
\text{Var}(\delta_{\text{rot2}}) = \alpha_1 \cdot \delta_{\text{rot2}} + \alpha_2 \cdot \delta_{\text{trans}}
]
$$
結論
これらのレッスンの終わりには、オドメトリモーションモデルの理論的側面を理解するだけでなく、このモデルを組み込んだ実用的なROS2ノードを実装してテストすることができました。RVizでの粒子分布を視覚化することで、ロボットの動きにおける不確実性がどのように伝播し、姿勢の推定にどのように影響するかを深く理解することができました。
これらのレッスンは、Self Driving and ROS 2 - Learn by Doing! Map & Localization コースの中で重要なステップであり、ロボティクスや自律システムにおけるより複雑な課題に取り組むための重要なスキルと知識を私に提供してくれました。