LoginSignup
8
7

More than 1 year has passed since last update.

@srs(project_srs)

ROS講座51 オムニホイールのシミュレーション3（コントローラーを作る）

Last updated at Posted at 2018-09-02

環境

この記事は以下の環境で動いています。

項目 値
CPU Core i5-8250U
Ubuntu 20.04
ROS Noetic
Gazebo 11.9.0

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

前回作ったシミュレーションでは各々のモーターの軸の回転速度を指定して動かしました。これをtwist入力で動かすためのコントローラーをつくります。また高度な移動機能を持つためにはodomの取得が必須です。この2つの機能を持つc++ノードを製作します。

変換数式

twist(並進X・Y、回転Z）から3つの軸の回転速度に変換する数式を紹介します。

##値の定義

  • 変数（入力側）
V_x: 車体速度（前方+）(m/s)\\
V_y: 車体速度（左側+）(m/s)\\
\Omega: 車体角速度（反時計回り+）(rad/s)\\
  • 変数（出力側）
\omega_0: 0番オムニホイールのドライブ回転数(rad/s) \\
\omega_0: 1番オムニホイールのドライブ回転数(rad/s) \\
\omega_0: 2番オムニホイールのドライブ回転数(rad/s) \\
(軸側=外側から見て右回りが正) \\
  • 定数
r: タイヤの半径(m) \\
l: 車体中心とタイヤの距離(m) \\
\theta_0: 0番オムニホイールの位置(rad) \\
\theta_1: 1番オムニホイールの位置(rad) \\
\theta_2: 2番オムニホイールの位置(rad) \\

数式

  • ロボットが並進するときのことを考えると
並進の場合
r\omega_i=

\begin{pmatrix}
cos(\theta_i-\pi/2) & sin(\theta_i-\pi/2)
\end{pmatrix}

\begin{pmatrix}
V_x \\
V_y \\
\end{pmatrix}
  • ロボットがその場で回転するときを考えると
回転の場合
-r\omega_i=l\Omega
  • この2つを合わせると
並進と回転
\begin{pmatrix}
\omega_0 \\
\omega_1 \\
\omega_2 \\
\end{pmatrix}
=
\frac{1}{r}
\begin{pmatrix}
cos(\theta_0 - \pi/2) & sin(\theta_0 - \pi/2) \\
cos(\theta_1 - \pi/2) & sin(\theta_1 - \pi/2) \\
cos(\theta_2 - \pi/2) & sin(\theta_2 - \pi/2)
\end{pmatrix}

\begin{pmatrix}
V_x \\
V_y \\
\end{pmatrix}
-\frac{l}{r}
\begin{pmatrix}
1 \\
1 \\
1 \\
\end{pmatrix}
\Omega
\\
　\\
=
\frac{1}{r}
\begin{pmatrix}
cos(\theta_0 - \pi/2) & sin(\theta_0 - \pi/2) & -l\\
cos(\theta_1 - \pi/2) & sin(\theta_1 - \pi/2) & -l\\
cos(\theta_2 - \pi/2) & sin(\theta_2 - \pi/2) & -l
\end{pmatrix}

\begin{pmatrix}
V_x \\
V_y \\
\Omega
\end{pmatrix}
  • （参考）逆にオドメトリの計算は
オドメトリ
\begin{pmatrix}
V_x \\
V_y \\
\omega_0
\end{pmatrix}
=
r
\begin{pmatrix}
cos(\theta_0 - \pi/2) & sin(\theta_0 - \pi/2) & -l\\
cos(\theta_1 - \pi/2) & sin(\theta_1 - \pi/2) & -l\\
cos(\theta_2 - \pi/2) & sin(\theta_2 - \pi/2) & -l
\end{pmatrix}
^{-1}
\begin{pmatrix}
\omega_0 \\
\omega_1 \\
\omega_2 \\
\end{pmatrix}

ソースコード中のcalculation()が上記の数式を解いているところです。

ソースコード

twist変換

sim2_lecture/src/sim2_omni_commander.cpp
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Float64.h>
#include <geometry_msgs/Twist.h>
#include <string>
#include <math.h>

geometry_msgs::Twist twist_last;
bool twist_enable;
void twist_stamped_callback(const geometry_msgs::Twist& twist_msg)
{
  twist_last = twist_msg;
  twist_enable = true;
}

float wheel_base = 0.100;
float wheel_radius = 0.20;
float wheel[3] = { M_PI / 3, M_PI, 5 * M_PI / 3 };
float wheel_normal[3];

void calculation(float* out, float* in)
{
  for (int i = 0; i < 3; i++)
  {
    out[i] = (cos(wheel_normal[i]) * in[0] + sin(wheel_normal[i]) * in[1]) / wheel_radius;
    out[i] += -in[2] * wheel_base / wheel_radius;
  }
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "omni_driver");
  ros::NodeHandle n;
  ros::NodeHandle pn("~");
  // rosparam
  pn.getParam("wheel_base", wheel_base);
  pn.getParam("wheel_radius", wheel_radius);
  pn.getParam("wheel0", wheel[0]);
  pn.getParam("wheel1", wheel[1]);
  pn.getParam("wheel2", wheel[2]);

  // publish
  ros::Publisher wheel0_pub = n.advertise<std_msgs::Float64>("wheel0", 10);
  ros::Publisher wheel1_pub = n.advertise<std_msgs::Float64>("wheel1", 10);
  ros::Publisher wheel2_pub = n.advertise<std_msgs::Float64>("wheel2", 10);
  // Subscribe
  ros::Subscriber joy_sub = n.subscribe("cmd_vel", 10, twist_stamped_callback);

  for (int i = 0; i < 3; i++)
    wheel_normal[i] = wheel[i] - M_PI / 2;

  ros::Rate loop_rate(20);
  while (ros::ok())
  {
    if (twist_enable)
    {
      float in[3] = { 0 };
      float out[3] = { 0 };
      in[0] = twist_last.linear.x;
      in[1] = twist_last.linear.y;
      in[2] = twist_last.angular.z;
      calculation(out, in);
      std_msgs::Float64 data[3];
      data[0].data = out[0];
      data[1].data = out[1];
      data[2].data = out[2];
      wheel0_pub.publish(data[0]);
      wheel1_pub.publish(data[1]);
      wheel2_pub.publish(data[2]);
    }
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

odom変換

sim2_lecture/src/sim2_omni_odometry.cpp
#include <ros/ros.h>
#include <control_msgs/JointControllerState.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <string>
#include <math.h>

float wheel_speed[3] = { 0 };
void state0_callback(const control_msgs::JointControllerState& state_msg)
{
  wheel_speed[0] = state_msg.process_value;
}
void state1_callback(const control_msgs::JointControllerState& state_msg)
{
  wheel_speed[1] = state_msg.process_value;
}
void state2_callback(const control_msgs::JointControllerState& state_msg)
{
  wheel_speed[2] = state_msg.process_value;
}

float wheel_base = 0.100;
float wheel_radius = 0.20;
float publish_rate = 20;
std::string frame_id = "odom";
geometry_msgs::Twist wheel_invert(float* in)
{
  geometry_msgs::Twist out;
  float lv = 1.0 / wheel_radius;
  float av = wheel_base / wheel_radius;
  float r3 = sqrt(3);
  out.linear.x = +(1.0 / r3 / lv) * in[0] + 0.0 * in[1] - (1.0 / r3 / lv) * in[2];
  out.linear.y = -(1.0 / 3 / lv) * in[0] + (2.0 / 3 / lv) * in[1] - (1.0 / 3 / lv) * in[2];
  out.angular.z = -(1.0 / 3 / av) * in[0] - (1.0 / 3 / av) * in[1] - (1.0 / 3 / av) * in[2];
  return out;
}
geometry_msgs::Pose update_pose(geometry_msgs::Pose last_pose, geometry_msgs::Twist speed, float dt)
{
  geometry_msgs::Pose next_pose;

  tf::Quaternion quat_tmp;
  double roll, pitch, last_yaw = 0;
  // get yaw
  quaternionMsgToTF(last_pose.orientation, quat_tmp);
  tf::Matrix3x3(quat_tmp).getRPY(roll, pitch, last_yaw);
  // update
  next_pose.position.x = last_pose.position.x + (cos(last_yaw + speed.angular.z * dt / 2) * speed.linear.x -
                                                 sin(last_yaw + speed.angular.z * dt / 2) * speed.linear.y) *
                                                    dt;
  next_pose.position.y = last_pose.position.y + (sin(last_yaw + speed.angular.z * dt / 2) * speed.linear.x +
                                                 cos(last_yaw + speed.angular.z * dt / 2) * speed.linear.y) *
                                                    dt;
  float next_yaw = last_yaw + speed.angular.z * dt;
  // sey yaw
  quat_tmp = tf::createQuaternionFromRPY(0, 0, next_yaw);
  quaternionTFToMsg(quat_tmp, next_pose.orientation);
  return next_pose;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "s4_omni_odom");
  ros::NodeHandle n;
  ros::NodeHandle pn("~");
  // publish
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
  // Subscribe
  ros::Subscriber odometry0 = n.subscribe("wheel0/state", 10, state0_callback);
  ros::Subscriber odometry1 = n.subscribe("wheel1/state", 10, state1_callback);
  ros::Subscriber odometry2 = n.subscribe("wheel2/state", 10, state2_callback);

  pn.getParam("wheel_base", wheel_base);
  pn.getParam("wheel_radius", wheel_radius);
  pn.getParam("publish_rate", publish_rate);
  pn.getParam("frame_id", frame_id);

  float dt = 1.0 / publish_rate;
  ros::Rate loop_rate(publish_rate);
  geometry_msgs::Pose body_position;
  body_position.orientation.w = 1.0;
  while (ros::ok())
  {
    geometry_msgs::Twist body_speed;
    body_speed = wheel_invert(wheel_speed);

    body_position = update_pose(body_position, body_speed, dt);

    nav_msgs::Odometry odom_msg;
    odom_msg.header.stamp = ros::Time::now();
    odom_msg.header.frame_id = frame_id;
    odom_msg.twist.twist = body_speed;
    odom_msg.pose.pose = body_position;
    odom_pub.publish(odom_msg);

    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}

CMakeList

sim_lecture/CMakeLists.txtに追加
add_executable(sim2_omni_commander src/sim2_omni_commander.cpp)
add_executable(sim2_omni_odometry src/sim2_omni_odometry.cpp)
target_link_libraries(sim2_omni_commander
  ${catkin_LIBRARIES}
)
target_link_libraries(sim2_omni_odometry
  ${catkin_LIBRARIES}
)

ビルド

cd ~/catkin_ws
catkin_make

実行

以下のコマンドで実行します。各ターミナルごとに実行前にsource ~/catkin_ws/devel/setup.bashを実行する必要があります。

実行
roslaunch sim2_lecture odm_gazebo3.launch

また別のターミナルを開いて以下を打つとロボットが動き出します。

cmd_velの送信
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 0.2
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.0"

sim2_omni_controller.gif

参考

オムニホイールの運動

目次ページへのリンク

ROS講座の目次へのリンク

8
7
2

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
8
7