フィードフォワード制御

このレッスンでは、ROS2 Humble環境において、Pythonでフィードフォワード制御を実装し、移動ロボットを制御するプログラムの作り方を学びます。ノードは /kachaka/manual_control/cmd_vel トピックに対して geometry_msgs/Twist 型のメッセージを発行します。

1. プログラムの概要

  • 移動ロボットをフィードフォワード制御で制御します
  • 事前に定義された速度と時間に基づいて動作を実行します
  • 直進、回転、停止などの基本的な動作を組み合わせて複雑な動作を実現します

2. フィードフォワード制御とは

フィードフォワード制御（Feedforward Control）は、制御対象の出力を測定せずに、入力のみに基づいて制御を行う方式です。事前に定義された指令を実行し、制御対象の応答を予測して動作を決定します。

フィードフォワード制御の特徴

  • 事前計画: 動作を事前に計画し、時間と速度を指定して実行します
  • フィードバック不要: センサーデータを使用せず、事前に決められた動作を実行します
  • シンプルな実装: 比較的シンプルなコードで実装できます
  • 予測可能性: 動作の結果を予測しやすいです

フィードバック制御との違い

フィードバック制御は、制御対象の出力を測定し、目標値との差（誤差）に基づいて制御を行います。一方、フィードフォワード制御は出力を測定せず、入力のみに基づいて制御を行います。

3. 前提条件

  • ROS2 Humble がインストール済み
  • Python3 が利用可能
  • ROS2ワークスペース（例: ~/ros2_ws）がセットアップ済み

必要なパッケージ

  • rclpy: ROS2のPythonクライアントライブラリ
  • geometry_msgs: 位置や速度などの幾何学メッセージを扱うためのパッケージ

4. パッケージの作成

まず、ROS2ワークスペースの src ディレクトリ内でパッケージを作成します。

cd ~/ros2_ws/src
ros2 pkg create --build-type ament_python kachaka_feedforward_control --dependencies rclpy geometry_msgs --node-name kachaka_feedforward_control

このコマンドにより、以下の構造のパッケージが生成されます：

kachaka_feedforward_control/
├── resource/
│   └── kachaka_feedforward_control
├── kachaka_feedforward_control/
│   ├── __init__.py
│   └── kachaka_feedforward_control.py
├── test/
├── package.xml
└── setup.py

5. ノードの実装

kachaka_feedforward_control パッケージ内の kachaka_feedforward_control ディレクトリに、以下の内容の kachaka_feedforward_control.py ファイルを作成します。

kachaka_feedforward_control.py

#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from rclpy.duration import Duration

class KachakaFeedforwardControl(Node):
    def __init__(self):
        super().__init__('kachaka_feedforward_control')
        # Create a Publisher that sends Twist type messages to the /kachaka/manual_control/cmd_vel topic
        self.publisher = self.create_publisher(Twist, '/kachaka/manual_control/cmd_vel', 10)

    def time_control(self, velocity, yawrate, time_seconds):
        """
        Control the robot at specified speed and time
        
        Parameters:
            velocity: Forward speed [m/s]
            yawrate: Rotation speed [rad/s]
            time_seconds: Movement duration [seconds]
        """
        # Create velocity command message
        vel = Twist()
        vel.linear.x = velocity
        vel.angular.z = yawrate
        
        # Calculate end time
        end_time = self.get_clock().now() + Duration(seconds=time_seconds)
        
        # Continue publishing velocity commands until the specified time
        while self.get_clock().now() < end_time:
            self.publisher.publish(vel)
            rclpy.spin_once(self, timeout_sec=0.1)  # Wait for 0.1 seconds

def main(args=None):
    rclpy.init(args=args)
    kachaka_feedforward_control = KachakaFeedforwardControl()

    try:
        # Execute the following movement sequence
        kachaka_feedforward_control.time_control(0.0, 0.0, 0.5)  # Stop
        kachaka_feedforward_control.time_control(0.1, 0.0, 2.0)  # Forward
        kachaka_feedforward_control.time_control(0.0, 0.0, 0.5)  # Stop
        kachaka_feedforward_control.time_control(-0.1, 0.0, 2.0)  # Backward
        kachaka_feedforward_control.time_control(0.0, 0.0, 0.5)  # Stop
        kachaka_feedforward_control.time_control(0.0, 0.5, 2.0)  # Rotate left
        kachaka_feedforward_control.time_control(0.0, 0.0, 0.5)  # Stop
        kachaka_feedforward_control.time_control(0.0, -0.5, 2.0)  # Rotate right
    except KeyboardInterrupt:
        pass
    finally:
        kachaka_feedforward_control.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

6. setup.py の編集

–node-nameを使用していない場合はパッケージルートにある setup.py のエントリーポイントを設定します。

entry_points={
    'console_scripts': [
        'kachaka_feedforward_control = kachaka_feedforward_control.kachaka_feedforward_control:main'
    ],
},

7. パッケージのビルドと実行

パッケージのビルド

cd ~/ros2_ws
colcon build --packages-select kachaka_feedforward_control

環境設定の読み込み

source install/setup.bash

ノードの実行

ros2 run kachaka_feedforward_control kachaka_feedforward_control

8. コードの詳細解説

クラスとメソッド

  • KachakaFeedforwardControl: メインのノードクラス
    • __init__: ノードの初期化とパブリッシャーの設定
    • time_control: 指定された速度と時間でロボットを制御するメソッド

制御パラメータ

  • velocity: 直進速度 [m/s]
    • 正の値: 前進
    • 負の値: 後退
    • 0: 停止
  • yawrate: 回転速度 [rad/s]
    • 正の値: 左回転
    • 負の値: 右回転
    • 0: 回転なし
  • time_seconds: 動作を継続する時間 [秒]

動作シーケンス

プログラムは以下の動作シーケンスを実行します：

  1. 停止 (0.5秒)
  2. 前進 (2.0秒)
  3. 停止 (0.5秒)
  4. 後退 (2.0秒)
  5. 停止 (0.5秒)
  6. 左回転 (2.0秒)
  7. 停止 (0.5秒)
  8. 右回転 (2.0秒)

フィードフォワード制御の実装ポイント

  1. 時間ベースの制御:

    • time_control メソッドは、指定された時間（time_seconds）の間、一定の速度（velocity, yawrate）でロボットを動かします
    • 終了時間（end_time）を計算し、その時間まで速度指令を発行し続けます

  2. 速度指令の継続的な発行:

    • ROS2では、速度指令は継続的に発行する必要があります
    • while ループ内で publish メソッドを呼び出し、一定間隔（0.1秒）で速度指令を発行します

  3. 動作の組み合わせ:

    • 直進、回転、停止などの基本的な動作を組み合わせて、複雑な動作シーケンスを実現します
    • 各動作の間に停止を入れることで、動作の切り替えを明確にします

演習

軌跡生成

フィードフォワード制御は、事前に計画された軌跡に沿ってロボットを動かすのに適しています。例えば、以下のような軌跡を生成できます：

  • 正方形の軌跡
  • 円形の軌跡
  • 8の字の軌跡

パラメータ調整

ロボットの特性に合わせて、以下のパラメータを調整することで、より正確な制御が可能になります：

  • 速度（velocity, yawrate）
  • 動作時間（time_seconds）
  • 動作間の停止時間

三角形の軌跡

フィードフォワード制御を使用して、ロボットに三角形の軌跡を描かせる機能を実装してみましょう。

応用

好きな軌道を作成してみよう！