差動２輪移動ロボットの作り方

  作動型の２輪移動ロボットを１から作る方法を解説する．

具体的には，後でビジュアルを見ればわかるが，薄い円筒のボディの左右に動力付きの車輪を２つとりつけ，前後にはキャスタボールキャスタを取り付けた移動ロボットを作成する．

作り方は，別ページで紹介している４輪の移動ロボットのテンプレートファイルのパラメータを修正するだけで作成可能となる．

利用するのは「wheel.urdf.xacro」．４つの駆動輪を設定可能であるが，今回は２輪だけを設定すれば良い．

まずは，いつもどおりノード作成のためにディレクトリを作成．

$ cd ~/catkin_ws/src

$ catkin_create_pkg diff_mobile_robot urdf xacro

$ cd diff_mobile_robot

$ mkdir urdf

$ cd urdf

$ gedit diff_mobile_robot.xacro

なお，あとでもいいので，別ページで紹介している４輪の移動ロボットのwheel.urdf.xacroをurdfディレクトリにコピーして入れておくこと．

定義ファイルwheel.urdf.xacroが入っていれば，下記の文言があれば２輪の駆動輪が生成される．

  <wheel fb="front" lr="right" parent="base_link" translateX="0" translateY="0.5" flipY="1"/>

  <wheel fb="front" lr="left" parent="base_link" translateX="0" translateY="-0.5" flipY="1"/>

円筒のベース部分もマクロで定義されているため下記のように記載するとベースの大きさを設定可能になる

  <property name="base_height" value="0.02" /> 

  <property name="base_radius" value="0.15" /> 

  <property name="base_mass" value="5" />

base_massはkg単位で定義．

マクロを見てみると，円筒の場合の公式で慣性モーメントも定義して設定してくれる．

最後にキャスタ部分は定義がないので，自分で下記のように記載して完成．

    <link name="caster_front_link">

    <visual>

      <origin xyz="0 0.02 0" rpy="${M_PI/2} 0 0" />

      <geometry>

<sphere radius="${caster_f_radius}" />

      </geometry>

      <material name="Black" />

    </visual>  

上記は，前輪のみ，後輪はfrontをbackに書き換える．

完成したdiff_mobile_robot.urdf.xacroは下記の通り．

ちなみに面倒なのでgazeboのための設定とレーザーレンジファインダも備えた結果になっています．

diff_mobile_roobt.xacro

<?xml version="1.0"?>

<robot name="differential_wheeled_robot" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find diff_mobile_robot)/urdf/wheel.urdf.xacro" /> 

<!-- Defining the colors used in this robot -->

  <material name="Black">

    <color rgba="0.0 0.0 0.0 1.0"/>

  </material>

  <material name="Red">

    <color rgba="0.8 0.0 0.0 1.0"/>

  </material>

  <material name="White">

    <color rgba="1.0 1.0 1.0 1.0"/>

  </material>

  <material name="Blue">

    <color rgba="0.0 0.0 0.8 1.0"/>

  </material>

  <!-- PROPERTY LIST -->

  <!--All units in m-kg-s-radians unit system -->

  <property name="M_PI" value="3.1415926535897931" />

  <property name="M_PI_2" value="1.570796327" />

  <property name="DEG_TO_RAD" value="0.017453293" />

  <!-- Main body radius and height -->

  <!-- Main Body Cylinder base   -->

  <property name="base_height" value="0.02" /> 

  <property name="base_radius" value="0.15" /> 

  <property name="base_mass" value="5" /> <!-- in kg-->

  <!-- caster wheel radius and height -->

  <!-- caster wheel mass -->

  <property name="caster_f_height" value="0.04" /> 

  <property name="caster_f_radius" value="0.025" /> 

  <property name="caster_f_mass" value="0.5" /> <!-- in kg-->

  <!-- caster wheel radius and height -->

  <!-- caster wheel mass -->

  <property name="caster_b_height" value="0.04" /> 

  <property name="caster_b_radius" value="0.025" /> 

  <property name="caster_b_mass" value="0.5" /> <!-- in kg-->

  <!-- Wheels -->

  <property name="wheel_mass" value="2.5" /> --> <!-- in kg-->

  <property name="base_x_origin_to_wheel_origin" value="0.25" />

  <property name="base_y_origin_to_wheel_origin" value="0.3" />

  <property name="base_z_origin_to_wheel_origin" value="0.0" />

  <!-- Hokuyo Laser scanner -->

  <property name="hokuyo_size" value="0.05" />

  <!-- Macro for calculating inertia of cylinder -->

  <macro name="cylinder_inertia" params="m r h">

    <inertia  ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"

              iyy="${m*(3*r*r+h*h)/12}" iyz = "0"

              izz="${m*r*r/2}" /> 

  </macro>

  <!-- BASE-FOOTPRINT -->

  <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin -->

  <link name="base_footprint">

    <inertial>

      <mass value="0.0001" />

      <origin xyz="0 0 0" />

      <inertia ixx="0.0001" ixy="0.0" ixz="0.0"

          iyy="0.0001" iyz="0.0" 

          izz="0.0001" />

    </inertial>

    <visual>

        <origin xyz="0 0 0" rpy="0 0 0" />

        <geometry>

            <box size="0.001 0.001 0.001" />

        </geometry>

    </visual>

  </link>

  <gazebo reference="base_footprint">

    <turnGravityOff>false</turnGravityOff>

  </gazebo>

  <joint name="base_footprint_joint" type="fixed">

    <origin xyz="0 0 ${wheel_radius - base_z_origin_to_wheel_origin}" rpy="0 0 0" />

    <parent link="base_footprint"/>

    <child link="base_link" />

  </joint>

  <!-- BASE-LINK -->

  <!--Actual body/chassis of the robot-->

  <link name="base_link">

    <inertial>

      <mass value="${base_mass}" />

      <origin xyz="0 0 0" />

      <!--The 3x3 rotational inertia matrix. -->

        <cylinder_inertia  m="${base_mass}" r="${base_radius}" h="${base_height}" />

    </inertial>    

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

          <cylinder length="${base_height}" radius="${base_radius}" />

      </geometry>

      <material name="White" />

    </visual>  

    <collision>

      <origin xyz="0 0 0" rpy="0 0 0 " />

      <geometry>

          <cylinder length="${base_height}" radius="${base_radius}" />

      </geometry>

    </collision>     

  </link>

  <gazebo reference="base_link">

    <material>Gazebo/White</material>

    <turnGravityOff>false</turnGravityOff>

  </gazebo>

<!--Caster front  -->

    <link name="caster_front_link">

    <visual>

      <origin xyz="0 0.02 0" rpy="${M_PI/2} 0 0" />

      <geometry>

<sphere radius="${caster_f_radius}" />

      </geometry>

      <material name="Black" />

    </visual>  

      <collision>

        <geometry>

<sphere radius="${caster_f_radius}" />

        </geometry>

      <origin xyz="0 0.02 0" rpy="${M_PI/2} 0 0" />

      </collision>      

      <inertial>

        <mass value="${caster_f_mass}" />

        <origin xyz="0 0 0" />

        <inertia ixx="0.001" ixy="0.0" ixz="0.0"

                 iyy="0.001" iyz="0.0" 

                 izz="0.001" />

      </inertial>

    </link>

 <joint name="caster_front_joint" type="fixed">

      <parent link="base_link"/>

      <child link="caster_front_link"/>

      <origin xyz="0.115 0.0 0.007" rpy="${-M_PI/2} 0 0"/>

    </joint>

  <gazebo reference="caster_front_link">

    <turnGravityOff>false</turnGravityOff>

  </gazebo>

<!--Caster back  -->

    <link name="caster_back_link">

    <visual>

      <origin xyz="0.02 0.02 0 " rpy="${M_PI/2} 0 0" /> 

      <geometry>

<sphere radius="${caster_b_radius}" />

      </geometry>

      <material name="Black" />

    </visual>  

      <collision>

        <geometry>

<sphere radius="${caster_b_radius}" />

        </geometry>

      <origin xyz="0 0.02 0 " rpy="${M_PI/2} 0 0" /> 

      </collision>      

      <inertial>

        <mass value="${caster_b_mass}" />

        <origin xyz="0 0 0" />        

        <inertia ixx="0.001" ixy="0.0" ixz="0.0"

                 iyy="0.001" iyz="0.0" 

                 izz="0.001" />

      </inertial>

    </link>

    <joint name="caster_back_joint" type="fixed">

      <parent link="base_link"/>

      <child link="caster_back_link"/>

      <origin xyz="-0.135 0.0 0.009" rpy="${-M_PI/2} 0 0"/>

    </joint>

  <gazebo reference="caster_back_link">

    <turnGravityOff>false</turnGravityOff>

  </gazebo>

<!-- Wheel Definitions -->

  <wheel fb="front" lr="left" parent="base_link" translateX="0" translateY="0.5" flipY="1"/>

  <wheel fb="front" lr="right" parent="base_link" translateX="0" translateY="-0.5" flipY="1"/>

  <!-- SENSORS -->

  <!-- hokuyo -->

  <link name="hokuyo_link">

    <visual>

      <origin xyz="0 0 0" rpy="0 0 0" />

      <geometry>

        <box size="${hokuyo_size} ${hokuyo_size} ${hokuyo_size}"/>

      </geometry>

      <material name="Blue" />

    </visual>

  </link>

  <joint name="hokuyo_joint" type="fixed">

    <origin xyz="${base_radius - hokuyo_size/2} 0 ${base_height+hokuyo_size/4}" rpy="0 0 0" />

    <parent link="base_link"/>

    <child link="hokuyo_link" />

  </joint>

  <gazebo reference="hokuyo_link">

    <material>Gazebo/Blue</material>

    <turnGravityOff>false</turnGravityOff>

    <sensor type="ray" name="head_hokuyo_sensor">

      <pose>${hokuyo_size/2} 0 0 0 0 0</pose>

      <visualize>false</visualize>

      <update_rate>40</update_rate>

      <ray>

        <scan>

          <horizontal>

            <samples>720</samples>

            <resolution>1</resolution>

            <min_angle>-1.570796</min_angle>

            <max_angle>1.570796</max_angle>

          </horizontal>

        </scan>

        <range>

          <min>0.10</min>

          <max>10.0</max>

          <resolution>0.001</resolution>

        </range>

      </ray>

      <plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_laser.so">

        <topicName>/scan</topicName>

        <frameName>hokuyo_link</frameName>

      </plugin>

    </sensor>

  </gazebo>

  <!-- Differential drive controller  -->

  <gazebo>

    <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">

      <rosDebugLevel>Debug</rosDebugLevel>

      <publishWheelTF>false</publishWheelTF>

      <robotNamespace>/</robotNamespace>

      <publishTf>1</publishTf>

      <publishWheelJointState>true</publishWheelJointState>

      <alwaysOn>true</alwaysOn>

      <updateRate>100.0</updateRate>

      <leftJoint>front_left_wheel_joint</leftJoint>

      <rightJoint>front_right_wheel_joint</rightJoint>

      <wheelSeparation>${2*base_radius}</wheelSeparation>

      <wheelDiameter>${2*wheel_radius}</wheelDiameter>

      <broadcastTF>1</broadcastTF>

      <wheelTorque>30</wheelTorque>

      <wheelAcceleration>1.8</wheelAcceleration>

      <commandTopic>cmd_vel</commandTopic>

      <odometryFrame>odom</odometryFrame> 

      <odometryTopic>odom</odometryTopic> 

      <robotBaseFrame>base_footprint</robotBaseFrame>

    </plugin>

  </gazebo> 

</robot>

どのように生成されたか確認するためにlaunchファイルを作成する．

$ cd ~/catkin_ws/src/diff_mobile_robot

$ mkdir launch

$ cd launch

$ gedit diff_mobile_robot.launch

diff_mobile_robot.launch

<launch>

<arg name="model" />

<!-- Parsing xacro and setting robot_description parameter -->

<param name="robot_description" command="$(find xacro)/xacro.py $(find diff_mobile_robot)/urdf/diff_mobile_robot.xacro" />

<!-- Setting gui parameter to true for display joint slider -->

<param name="use_gui" value="true"/>

<!-- Starting Joint state publisher node which will publish the joint values -->

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

<!-- Starting robot state publish which will publish tf -->

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

</launch>

下記のコマンドでノード立ち上げ．

$ roslaunch diff_mobile_robot.launch

別ウィンドウで rvizを起動して，ロボットモデルを「Add」して表示を確認．

別ページで紹介している４輪の移動ロボットのwheel.urdf.xacroのままだと車輪が大きすぎるので

  <!-- Wheels -->

  <property name="wheel_radius" value="0.04" /> 

  <property name="wheel_height" value="0.02" />

  <property name="wheel_mass" value="0.25" /> <!-- in kg-->

に修正しておくこと．

Gazeboで表示させるには下記のlaunchファイルを作成する．

$ cd ~/catkin_ws/src/diff_mobile_robot/launch

$ gedit diff_mobile_gazebo.launch

diff_mobile_gazebo.launch

<launch>

  <!-- these are the arguments you can pass this launch file, for example paused:=true -->

  <arg name="paused" default="false"/>

  <arg name="use_sim_time" default="true"/>

  <arg name="gui" default="true"/>

  <arg name="headless" default="false"/>

  <arg name="debug" default="false"/>

  <!-- We resume the logic in empty_world.launch -->

  <include file="$(find gazebo_ros)/launch/empty_world.launch">

    <arg name="debug" value="$(arg debug)" />

    <arg name="gui" value="$(arg gui)" />

    <arg name="paused" value="$(arg paused)"/>

    <arg name="use_sim_time" value="$(arg use_sim_time)"/>

    <arg name="headless" value="$(arg headless)"/>

  </include>

  <!-- urdf xml robot description loaded on the Parameter Server-->

  <param name="robot_description" command="$(find xacro)/xacro.py $(find diff_mobile_robot)/urdf/diff_mobile_robot.xacro" /> 

 <!-- Setting gui parameter to true for display joint slider -->

 <param name="use_gui" value="true"/>

 <!-- Starting Joint state publisher node which will publish the joint values -->

 <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />

 <!-- Starting robot state publish which will publish tf -->

 <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

  <!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->

  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"

args="-urdf -model diff_mobile_robot -param robot_description"/> 

</launch>

早速実行

$ roslaunch diff_mobile_robot diff_mobile_gazebo.launch

やってみよう！

１．gazebo中に障害物をおいてみる．

２．rvizを立ち上げてレーザーの情報が反映されているか確かめてみる．

３．gazebo内の移動ロボットをコントロールできるか確かめてみる．

        移動ロボットの操作方法は，Gazebo上での移動ロボットモデルの構築で記載した「Pythonでキーボードを使った操作方法」などで動かすことができる．