Self Driving and ROS 2 - Learn by Doing! Odometry & Control: コントローラー (セクション9-2/13)

• 車輪オドメトリの計算方法を導入し、ロボットの位置と向きを線形速度と角速度から求める手法を説明。

• PythonおよびC++でのシンプルコントローラの実装例を示し、各コードの詳細とその動作を解説。

• Gazeboシミュレーションを用いて、実装したコントローラの動作を確認し、ロボットの正確な位置と向きを推定する方法を検証。

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

はじめに

ロボット工学の世界では、オドメトリの理解と実装は自律走行車の正確なナビゲーションと制御にとって非常に重要です。このブログ記事では、「自律走行とROS 2 - 実践で学ぶ！オドメトリと制御」コースのセクション9の中盤3分の1、特に講義98〜103に焦点を当てています。このセグメントでは、車輪オドメトリの基礎、車輪速度から位置と方向を導出する方法、これらの概念をPythonとC++で実装する方法に焦点を当てています。

講義98：車輪オドメトリ - 位置

オドメトリには、車輪の動きに基づいてロボットの位置と方向を計算することが含まれます。位置は、時間に対する線速度を積分することによって求められます。手順は以下の通りです。

1. 線速度の積分：

• 位置 $${pos}$$ は、時間 $${t}$$ に対する線速度 $${v}$$ を積分することによって得られます：
  $${pos = \int_{t_0}^{t} v , dt}$$

1. 速度の分割：

• 速度 $${v}$$ は、右と左の車輪速度（$${v_R}$$ と $${v_L}$$）の寄与に分割されます：
  $${v = \frac{w_2}{2} \dot{\phi}_R + \frac{w_2}{2} \dot{\phi}_L}$$

1. 各成分の積分：

• 各成分を個別に積分します：
  $${pos = \frac{w_2}{2} \int_{t_0}^{t} \dot{\phi}R , dt + \frac{w_2}{2} \int{t_0}^{t} \dot{\phi}_L , dt}$$

1. 微分形式への変換：

• 時間に対する導関数の積分は、変数の変化量を与えます：
  $${∫_{t_0}^{t} \dot{\phi}R , dt = \phi_R(t) - \phi_R(t_0)}$$
  $${∫{t_0}^{t} \dot{\phi}_L , dt = \phi_L(t) - \phi_L(t_0)}$$

1. 簡略化された位置方程式：

• 最終的な位置方程式：
  $${pos = \frac{w_2}{2} \Delta \phi_R + \frac{w_2}{2} \Delta \phi_L}$$

これは、位置が右と左の車輪の角変位の変化の和であり、それぞれが車輪トラック幅 $${w_2}$$ の半分で重み付けされていることを示しています。

講義99：車輪オドメトリ - 方向

方向は角速度から導出されます。手順は以下の通りです：

1. 角速度方程式：
   $${\omega = \frac{W_2}{W_S} \dot{\theta}_R - \frac{W_2}{W_S} \dot{\theta}_L}$$

2. 角速度の積分：
   $${\text{Orientation} = \int \omega , dt = \int \left( \frac{W_2}{W_S} \dot{\theta}_R - \frac{W_2}{W_S} \dot{\theta}_L \right) dt}$$

3. 積分の分離：
   $${\int \left( \frac{W_2}{W_S} \dot{\theta}_R - \frac{W_2}{W_S} \dot{\theta}_L \right) dt = \frac{W_2}{W_S} \int \dot{\theta}_R , dt - \frac{W_2}{W_S} \int \dot{\theta}_L , dt}$$

4. 積分の結果：
   $${\frac{W_2}{W_S} \left( \theta_R(t) - \theta_R(0) \right) - \frac{W_2}{W_S} \left( \theta_L(t) - \theta_L(0) \right)}$$

5. 最終方向方程式：
   $${\text{Orientation} = \frac{W_2}{W_S} \Delta \theta_R - \frac{W_2}{W_S} \Delta \theta_L}$$

講義100 & 102：Pythonでの実装

Python実装には、車輪速度をサブスクライブしてオドメトリデータをパブリッシュするコントローラーノードの作成が含まれます。コア機能は以下の通りです。

• `velCallback`: ロボットの線速度と角速度を車輪速度に変換します。

• `jointCallback`: 車輪の位置を線速度と角速度に変換し、ロボットの位置と方向を計算します。

class SimpleController(Node):
    def __init__(self):
        super().__init__("simple_controller")
        # パラメータとpublisher/subscriberの初期化

    def velCallback(self, msg):
        # 差動キネマティックモデルの実装

    def jointCallback(self, msg):
        # 逆差動キネマティックモデルの実装
        # 線速度と角速度の計算
        # 位置増分の計算
        # オドメトリメッセージのパブリッシュ

講義101 & 103：C++での実装

C++実装はPythonバージョンと似ていますが、生産環境で典型的なパフォーマンスと堅牢性を重視しています。コアメソッドは以下の通りです。

• `velCallback`: 指令速度を車輪速度に変換する処理を行います。

• `jointCallback`: 車輪の位置をロボットの速度に変換し、ロボットのオドメトリを更新する処理を行います。

class SimpleController : public rclcpp::Node {
public:
    SimpleController(const std::string& name);
private:
    void velCallback(const geometry_msgs::msg::TwistStamped &msg);
    void jointCallback(const sensor_msgs::msg::JointState &msg);
    // その他のメンバー変数とメソッド
};

まとめ

「自律走行とROS 2 - 実践で学ぶ！オドメトリと制御」コースのセクション9の中盤3分の1は、オドメトリの包括的な理解を提供します。車輪速度から位置と方向を理論的に導出する方法と、PythonとC++での実践的な実装方法を説明しています。これらの概念と実装方法を習得することは、自律走行車で信頼性と精度の高いナビゲーションシステムを開発するために不可欠です。

「超本当にドラゴン」へ