Self Driving and ROS 2 - Learn by Doing! Odometry & Control: 回転行列 (セクション6-2/13)

2024年6月3日 22:00

ロボットの姿勢を2D空間で表現するために、回転行列と変換行列の概念を学習します。
PythonとC++でTurtlesimを用いて、2つのタートル間の回転行列と変換行列を計算するROS 2ノードを実装します。
これらの行列を使って、ロボットの位置と向きを正確に制御し、障害物の位置を世界座標系で特定します。

ロボット運動学の魅力的な世界をさらに探求する中で、『Self Driving and ROS 2 - Learn by Doing! Odometry & Control』コースのセクション6の後半は、回転行列と変換行列の詳細に深入りします。これらの概念は、ロボットの姿勢を2D空間で定義するために不可欠であり、その位置と方向の両方を含みます。これらの高度なトピックと、ROS 2およびTurtlesimを使用した実際の応用について見ていきましょう。

回転行列：方向の理解

回転行列は、ロボットの方向を記述するために不可欠です。これは、回転による変位を考慮して、ある参照フレームから別の参照フレームへの座標を変換します。

ロボットの方向：

参照フレーム: ロボットのフレーム（R）と世界のフレーム（W）を定義します。
座標: これらのフレーム内のポイントは、世界のフレームではPx、Py、ロボットのフレームではPx'、Py'で表されます。
回転角（θ）： 2つのフレーム間の変位角です。

投影と計算：

投影の式:
Px = Px' * cos(θ) - Py' * sin(θ)
Py = Px' * sin(θ) + Py' * cos(θ)
回転行列:

| cos(θ) -sin(θ) |
| sin(θ)  cos(θ) |

PythonでのTurtlesimを使用した実際の応用

次に、Pythonを使用して2つのタートル間の回転行列を計算する実際の例を見てみましょう。

Turtlesimの設定:

Turtlesimノードを起動します:

ros2 run turtlesim turtlesim_node

2つ目のタートルをスポーンします:

ros2 service call /spawn turtlesim/srv/Spawn "x: 1.0 y: 4.0 theta: 0.0 name: 'turtle2'"

回転行列を計算するPythonスクリプト:

import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose
import math

class SimpleTurtlesimKinematics(Node):
       
       def __init__(self):
           super().__init__("simple_turtlesim_kinematics")
           self.turtle1_pose_sub_ = self.create_subscription(Pose, "/turtle1/pose", self.turtle1PoseCallback, 10) 
           self.turtle2_pose_sub_ = self.create_subscription(Pose, "/turtle2/pose", self.turtle2PoseCallback, 10)
           self.last_turtle1_pose_ = Pose()
           self.last_turtle2_pose_ = Pose()
       
       def turtle1PoseCallback(self, pose):
           self.last_turtle1_pose_ = pose
       
       def turtle2PoseCallback(self, pose):
           self.last_turtle2_pose_ = pose
           Tx = self.last_turtle2_pose_.x - self.last_turtle1_pose_.x
           Ty = self.last_turtle2_pose_.y - self.last_turtle1_pose_.y
           theta_rad = self.last_turtle2_pose_.theta - self.last_turtle1_pose_.theta
           theta_deg = 180 * theta_rad / math.pi
           R11 = math.cos(theta_rad)
           R12 = -math.sin(theta_rad)
           R21 = math.sin(theta_rad)
           R22 = math.cos(theta_rad)
           self.get_logger().info(f"""
           Translation Vector turtle1 -> turtle2
           Tx: {Tx}
           Ty: {Ty}
           Rotation Matrix turtle1 -> turtle2
           theta (rad): {theta_rad}
           theta (deg): {theta_deg}
           |R11   R12|:  |{R11} {R12}|
           |R21   R22|   |{R21} {R22}|
           """)

def main():
       rclpy.init()
       simple_turtlesim_kinematics = SimpleTurtlesimKinematics()
       rclpy.spin(simple_turtlesim_kinematics)
       simple_turtlesim_kinematics.destroy_node()
       rclpy.shutdown()

if __name__ == '__main__':
       main()

Pythonスクリプトの実行:

ワークスペースをビルドします:

colcon build

セットアップファイルをソースします:

. install/setup.bash

スクリプトを実行します:

ros2 run bumperbot_py_examples simple_turtlesim_kinematics

C++でのTurtlesimを使用した実際の応用

次に、同じ機能をC++で実装します。

回転行列を計算するC++コード:

#include "bumperbot_cpp_examples/simple_turtlesim_kinematics.hpp"

using std::placeholders::_1;

SimpleTurtlesimKinematics::SimpleTurtlesimKinematics(const std::string& name)
       : Node(name)
{
       turtle1_pose_sub_ = create_subscription<turtlesim::msg::Pose>(
           "/turtle1/pose", 10, std::bind(&SimpleTurtlesimKinematics::turtle1PoseCallback, this, _1));
       turtle2_pose_sub_ = create_subscription<turtlesim::msg::Pose>(
           "/turtle2/pose", 10, std::bind(&SimpleTurtlesimKinematics::turtle2PoseCallback, this, _1));
}

void SimpleTurtlesimKinematics::turtle1PoseCallback(const turtlesim::msg::Pose& pose)
{
       last_turtle1_pose_ = pose;
}

void SimpleTurtlesimKinematics::turtle2PoseCallback(const turtlesim::msg::Pose& pose)
{
       last_turtle2_pose_ = pose;
       float Tx = last_turtle2_pose_.x - last_turtle1_pose_.x;
       float Ty = last_turtle2_pose_.y - last_turtle1_pose_.y;
       float theta_rad = last_turtle2_pose_.theta - last_turtle1_pose_.theta;
       float theta_deg = 180 * theta_rad / 3.14;
       RCLCPP_INFO_STREAM(get_logger(), "\nTranslation Vector turtle1 -> turtle2 \n" <<
                           "Tx: " << Tx << "\n" <<
                           "Ty: " << Ty << "\n" <<
                           "Rotation Matrix turtle1 -> turtle2 \n" << 
                           "theta (rad): " << theta_rad << "\n" <<
                           "theta (deg): " << theta_deg << "\n" <<
                           "|R11   R12|:  |" << std::cos(theta_rad) << "\t" << -std::sin(theta_rad) << "|\n" <<
                           "|R21   R22|   |" << std::sin(theta_rad) << "\t\t" << std::cos(theta_rad) << "|\n");
}

int main(int argc, char* argv[])
{
       rclcpp::init(argc, argv);
       auto node = std::make_shared<SimpleTurtlesimKinematics>("simple_turtlesim_kinematics");
       rclcpp::spin(node);
       rclcpp::shutdown();
       return 0;
}

C++スクリプトの実行:

ワークスペースをビルドします:

colcon build

セットアップファイルをソースします:

. install/setup.bash

スクリプトを実行します:

ros2 run bumperbot_cpp_examples simple_turtlesim_kinematics

変換行列：平行移動と回転の組み合わせ

変換行列は、平行移動と回転の両方を組み合わせ、2D空間におけるロボットの姿勢を完全に記述します。

変換行列の構成要素:

平行移動ベクトル (T): ロボットの位置を記述します。
回転行列 (R): ロボットの方向を記述します。
組み合わせ行列:

| R  T |
| 0  1 |

**ワールドフレーム

での位置の計算:**

ロボットフレームでの座標 (xP, yP) と変換行列を考慮して、ワールドフレームでの位置を計算します:

| xwP | = | cos(θ) -sin(θ) Tx | | xP |
| ywP |   | sin(θ)  cos(θ) Ty | | yP |
|  1  |   |   0       0      1 | |  1 |

実際の応用:

変換行列に値を代入することで、ロボットのセンサーで検出された障害物のグローバル位置を決定できます。

結論

回転行列と変換行列を理解し実装することで、2D空間でロボットを正確に制御しナビゲートする能力が向上します。これらの概念は、高度な自律システムを開発するための基本であり、ROS 2とTurtlesimを使用した実際の応用により、理論と現実世界のロボット工学のギャップを埋めます。この包括的なアプローチにより、技術的なスキルが向上するだけでなく、ロボット工学分野の複雑な課題に取り組む準備が整います。

「超本当にドラゴン」へ