Self Driving and ROS 2 - Learn by Doing! Odometry & Control: Gazebo (セクション9-3/13)

• ロボットの位置と方向を車輪の回転速度から計算し、ROS 2トピックにオドメトリメッセージとして公開する方法を解説しています。

• PythonとC++の両方の実装で、ロボットの線形および角速度、位置、および方向を正確に推定するための手順が詳細に説明されています。

• GazeboシミュレーションとROS 2を用いて、ロボットの動きに応じた正確なオドメトリデータの計算と公開の実演を行っています。

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

Udemyコース「自律走行とROS 2 - 実践で学ぶ！オドメトリと制御」のセクション9の最終3分の1は、自律ロボットのオドメトリ計算の実践的な実装に焦点を当てています。この部分は、講座104と105とともに、PythonとC++の両方を使用した車輪オドメトリの理論的基礎と実践的応用を強調しています。

車輪オドメトリの重要な概念

車輪オドメトリ は、車輪の回転速度を分析することでロボットの位置と向きを理解するために重要です。以下は、カバーされている主要トピックの内訳です：

1. 位置計算：

• 位置 $${pos}$$ は、線速度 $${v}$$ を時間 $${t}$$ で積分することで求められます：
  $${pos = \int_{t_0}^{t} v , dt}$$

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

• 最終的な位置方程式は次のようになります：
  $${pos = \frac{w_2}{2} \Delta \phi_R + \frac{w_2}{2} \Delta \phi_L}$$

2. 向き計算：

• 角速度 $${ω}$$ は次のように表されます：
  $${\omega = \frac{W_2}{W_S} \dot{\theta}_R - \frac{W_2}{W_S} \dot{\theta}_L}$$

• 角速度を積分することで、ロボットの向きが得られます：
  $${\text{Orientation} = \frac{W_2}{W_S} \Delta \theta_R - \frac{W_2}{W_S} \Delta \theta_L}$$

実践的な実装

Python実装

実践的な側面では、これらの方程式をPythonノードに実装してロボットの位置と向きを計算します。以下はPython実装の概要です：

• 初期化：

self.x_ = 0.0
self.y_ = 0.0 
self.theta_ = 0.0

• 速度コールバック：
  この関数は、ロボットの線速度と角速度から車輪速度を計算します。

def velCallback(self, msg):
    robot_speed = np.array([[msg.twist.linear.x], [msg.twist.angular.z]])
    wheel_speed = np.matmul(np.linalg.inv(self.speed_conversion_), robot_speed)
    wheel_speed_msg.data = [wheel_speed[1, 0], wheel_speed[0, 0]]
    self.wheel_cmd_pub_.publish(wheel_speed_msg)

• ジョイントコールバック：
  この関数は、車輪エンコーダデータからロボットの新しい位置と向きを計算します。

def jointCallback(self, msg):
    dp_left = msg.position[1] - self.left_wheel_prev_pos_ 
    dp_right = msg.position[0] - self.right_wheel_prev_pos_
    dt = Time.from_msg(msg.header.stamp) - self.prev_time_

    fi_left = dp_left / (dt.nanoseconds / S_TO_NS) 
    fi_right = dp_right / (dt.nanoseconds / S_TO_NS)

    linear = (self.wheel_radius_ * fi_right + self.wheel_radius_ * fi_left) / 2
    angular = (self.wheel_radius_ * fi_right - self.wheel_radius_ * fi_left) / self.wheel_separation_
    
    d_s = (self.wheel_radius_ * dp_right + self.wheel_radius_ * dp_left) / 2
    d_theta = (self.wheel_radius_ * dp_right - self.wheel_radius_ * dp_left) / self.wheel_separation_
    
    self.theta_ += d_theta 
    self.x_ += d_s * math.cos(self.theta_)
    self.y_ += d_s * math.sin(self.theta_)
         
    self.publish_odometry(linear, angular)

C++実装

C++実装はPythonコードと同様の機能を確保しつつ、これを反映しています：

• 初期化：

x_(0.0), y_(0.0), theta_(0.0)  

• 速度コールバック：
  この関数は、ロボット速度から車輪速度を計算します。

void SimpleController::velCallback(const geometry_msgs::msg::TwistStamped &msg) {
    Eigen::Vector2d robot_speed(msg.twist.linear.x, msg.twist.angular.z);
    Eigen::Vector2d wheel_speed = speed_conversion_.inverse() * robot_speed;
    wheel_cmd_pub_->publish(wheel_speed_msg);
}

• ジョイントコールバック：
  この関数は、車輪エンコーダから位置と向きを計算します。

void SimpleController::jointCallback(const sensor_msgs::msg::JointState &state) {
    double dp_left = state.position.at(1) - left_wheel_prev_pos_;
    double dp_right = state.position.at(0) - right_wheel_prev_pos_;
    rclcpp::Duration dt = msg_time - prev_time_;
    
    double fi_left = dp_left / dt.seconds();
    double fi_right = dp_right / dt.seconds();

    double linear = (wheel_radius_ * fi_right + wheel_radius_ * fi_left) / 2;
    double angular = (wheel_radius_ * fi_right - wheel_radius_ * fi_left) / wheel_separation_;

    double d_s = (wheel_radius_ * dp_right + wheel_radius_ * dp_left) / 2;
    double d_theta = (wheel_radius_ * dp_right - wheel_radius_ * dp_left) / wheel_separation_;

    theta_ += d_theta;
    x_ += d_s * cos(theta_);
    y_ += d_s * sin(theta_);

    publish_odometry(linear, angular);
}

最終ステップ：テストと検証

オドメトリ計算の実装後、次のステップではGazeboを使用したシミュレーション環境で結果を検証することが含まれます。速度コマンドを発行し、ロボットの動作を観察することで、オドメトリ計算が正確であることを確認します。オドメトリメッセージは、ロボットの位置と向きに関するリアルタイムデータを提供し、それを監視して正確性を検証できます。

結論

セクション9の最終3分の1は、講座104と105とともに、自律ロボットの車輪オドメトリを実装するための包括的なガイドを提供しています。理論的理解とPythonとC++での実践的なコーディングを組み合わせることで、このセクションは学生にロボットの位置と向きを正確に追跡するために必要なスキルを身につけさせます。これは自律ナビゲーションの重要な側面です。

「超本当にドラゴン」へ