Self Driving and ROS 2 - Learn by Doing! Odometry & Control: 差動運動学 (セクション7-2/13)
差動運動学の基本概念を学び、ロボットのホイールの回転速度と全体の速度（線速度と角速度）の関係を理解することが重要である。
ROS 2を使った簡単な速度コントローラーの実装方法を、PythonとC++のコード例を通じて解説した。
この知識を使ってジョイスティックからの速度コマンドをロボットのホイール速度に変換し、ロボットを効果的に制御する方法を学んだ。
自動運転技術とロボット制御の世界で、微分運動学を理解することは非常に重要です。この記事では、「Self-Driving and ROS 2 - Learn by Doing! Odometry & Control」コースの講義60から68に焦点を当て、微分運動学の基本概念を探求します。これらの講義では、ロボットの車輪の回転速度と全体の速度を関連付ける基本的な方程式や、これらの概念がROS 2でどのように実装されるかを説明します。
講義60：微分運動学の導入
微分運動学は、ロボットの車輪の回転速度とその全体の速度（直線速度と角速度の両方）との関係を定義するために不可欠です。この関係により、車輪に直接速度コマンドを送信してロボットの動きを制御することが可能になります。
講義61：移動ロボットの速度
ロボットの速度を説明するには、最初にその姿勢を三つの成分で表現します：$${x}$$、$${y}$$、および$${\theta}$$。ここで、$${x}$$と$${y}$$は位置を示し、$${\theta}$$は方向を示します。時間という変数を導入して、速度を位置の時間に対する変化として表現します。主な成分は次の通りです：
$${x\dot}$$ および $${y\dot}$$： $${x}$$軸および$${y}$$軸に沿った線形速度。
$${\theta\dot}$$：角速度。
独立して制御される二つの車輪を持つ差動駆動ロボットの場合、問題は車輪の速度とロボット全体の速度を関連付ける関数を見つけることになります。
講義62：線形速度
ロボットの線形速度を分解するには、ロボットの中心点と左車輪および右車輪の中心点における速度を理解する必要があります。ロボットの線形速度$${V}$$は、次のように車輪の速度の平均として表現できます：
$${ V = \frac{r}{2} (\phi_R\dot + \phi_L\dot) }$$
ここで、$${r}$$は車輪の半径、$${\phi_R\dot}$$および$${\phi_L\dot}$$はそれぞれ右車輪および左車輪の回転速度です。
講義63：角速度
ロボットの角速度$${W}$$は、次のように二つの車輪の速度の差によって決定されます：
$${ W = \frac{r}{w} (\phi_R\dot - \phi_L\dot) }$$
ここで、$${w}$$は車輪間の距離です。この方程式は、差動駆動システムを定義します。
講義64：世界座標系における速度
ロボットの速度を世界座標系に関連付けるには、ロボットの方向$${\theta}$$を考慮した回転行列$${R(\theta)}$$を使用します。世界座標系での速度$${P\dot}$$は次のように表現できます：
$${ P\dot = R(\theta)(V, W) }$$
講義65：微分順運動学
順運動学は、線形速度と角速度を組み合わせて、世界座標系におけるロボットの全体の速度を提供します。ヤコビ行列$${J}$$は、この変換において重要な役割を果たします：
$$
\begin{bmatrix}
\dot{x} \
\dot{y} \
\dot{\theta}
\end{bmatrix}
\begin{bmatrix}
\frac{r}{2} \cos(\theta) & \frac{r}{2} \cos(\theta) \
\frac{r}{2} \sin(\theta) & \frac{r}{2} \sin(\theta) \
\frac{r}{w} & -\frac{r}{w}
\end{bmatrix}
\begin{bmatrix}
\dot{\phi}_R \
\dot{\phi}_L
\end{bmatrix}
$$
講義66-68：簡易速度コントローラの実装
簡易速度コントローラの実装には、ロボットの基準座標系でのジョイスティックコマンドを、微分運動学の方程式を使用して車輪コマンドに変換することが含まれます。PythonとC++の両方で開発されたROS 2ノードは、速度コマンドを購読し、車輪コマンドを発行してロボットの動きを制御します。
Python実装
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
from geometry_msgs.msg import TwistStamped
import numpy as np
class SimpleController(Node):
def __init__(self):
super().__init__("simple_controller")
self.declare_parameter("wheel_radius", 0.033)
self.declare_parameter("wheel_separation", 0.17)
self.wheel_radius_ = self.get_parameter("wheel_radius").get_parameter_value().double_value
self.wheel_separation_ = self.get_parameter("wheel_separation").get_parameter_value().double_value
self.get_logger().info(f"Using wheel radius {self.wheel_radius_}")
self.get_logger().info(f"Using wheel separation {self.wheel_separation_}")
self.wheel_cmd_pub_ = self.create_publisher(Float64MultiArray, "simple_velocity_controller/commands", 10)
self.vel_sub_ = self.create_subscription(TwistStamped, "bumperbot_controller/cmd_vel", self.velCallback, 10)
self.speed_conversion_ = np.array([
[self.wheel_radius_/2, self.wheel_radius_/2],
[self.wheel_radius_/self.wheel_separation_, -self.wheel_radius_/self.wheel_separation_]
])
self.get_logger().info(f"The conversion matrix is \n{self.speed_conversion_}")
def velCallback(self, msg):
robot_speed = np.array([[msg.twist.linear.x], [msg.twist.angular.z]])
wheel_speed = np.linalg.inv(self.speed_conversion_) @ robot_speed
wheel_speed_msg = Float64MultiArray()
wheel_speed_msg.data = [wheel_speed[1, 0], wheel_speed[0, 0]]
self.wheel_cmd_pub_.publish(wheel_speed_msg)
def main():
rclpy.init()
simple_controller = SimpleController()
rclpy.spin(simple_controller)
simple_controller.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()C++実装
#include "bumperbot_controller/simple_controller.hpp"
#include <Eigen/Geometry>
using std::placeholders::_1;
SimpleController::SimpleController(const std::string& name) : Node(name)
{
declare_parameter("wheel_radius", 0.033);
declare_parameter("wheel_separation", 0.17);
wheel_radius_ = get_parameter("wheel_radius").as_double();
wheel_separation_ = get_parameter("wheel_separation").as_double();
RCLCPP_INFO_STREAM(get_logger(), "Using wheel radius " << wheel_radius_);
RCLCPP_INFO_STREAM(get_logger(), "Using wheel separation " << wheel_separation_);
wheel_cmd_pub_ = create_publisher<std_msgs::msg::Float64MultiArray>("/simple_velocity_controller/commands", 10);
vel_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("/bumperbot_controller/cmd_vel", 10, std::bind(&SimpleController::velCallback, this, _1));
speed_conversion_ << wheel_radius_/2, wheel_radius_/2, wheel_radius_/wheel_separation_, -wheel_radius_/wheel_separation_;
RCLCPP_INFO_STREAM(get_logger(), "The conversion matrix is \n" << speed_conversion_);
}
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;
std_msgs::msg::Float64MultiArray wheel_speed_msg;
wheel_speed_msg.data.push_back(wheel_speed.coeff(1));
wheel_speed_msg.data.push_back(wheel_speed.coeff(0));
wheel_cmd_pub_->publish(wheel_speed_msg);
}
int main(int argc
, char* argv[])
{
rclcpp::init(argc, argv);
auto node = std::make_shared<SimpleController>("simple_controller");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}結論
微分運動学の旅は、差動駆動ロボットを制御するために必要な数学的基盤と実際の実装を提供します。これらの方程式を理解し適用することで、速度コマンドを使用してロボットの動きを効果的に制御することができます。この知識は、ロボティクスに関心のある人々や、自律システムと制御の分野に興味を持つすべての人にとって重要です。