hero画像
技術コラム
Livoxコラム

3D-LiDAR（Livox：MID-360）とTurtlebot2を使った自動走行

目次

Livox MID-360 をTurtlebot2に搭載して、自動走行を実現しました。

  • MID-360内蔵IMU（慣性計測ユニット）とTurtlebot2からのホイールオドメトリを使用します。
  • 取得した点群（地図）とスキャンしている点群から自己位置推定を行い、ゴールまで障害物を避けながら自動で走行します。

  • Turtlebot2＋MID-360

  • ゴールまで自動走行

特徴

MID-360をTurtlebot2（kobuki）に搭載し、地図作成と自己位置推定、障害物を避け、ルートを探索してロボットを自動走行することができます。
2D-LiDARでは傾斜や振動の影響を受け、自己位置をロストする場合がありますが、MID-360では多くの点群を使用するため、ロストせずに走行が可能です。
さらに、垂直方向の障害物を検出することができるため、段差や出っ張り等空間での認識が可能となります。

環境

ROS（Noetic）

ソフトウェアとしては、以下を使用します。

  • 地図作成：gmapping
  • 自己位置推定：amcl（adaptive monte carlo localization）
  • 障害物検知、経路探索：move_base

ただし、MID-360 の出力は 3D のPointCloudフォーマットで出力されますが、move_baseが、2Dの地図データしか扱えないため、 PointCloud　→　LaserScan　に変換するため、pointcloud_to_laserscanを使用します。
また、3Dから2Dに切り出すため、今回は以下の範囲を設定しています。
水平：180°（前方方向）、垂直：-0.15（LiDAR取付位置から地面までの距離）～1.5m、距離：0.1～50m

Navigation Stack Setup構成図

地図の作成

まず最初に、Turtlebot2をマニュアルで動作させて、地図を作成します。（gmapping）
キーボード入力を使って、turtlebotを移動させます。（PS4コントローラーを使って移動させることも可能です。）

Terminal-1：turtlebot の起動

$ roslaunch turtlebot_bringup miniml.launch

Terminal-2：Livox_ros_driver の起動 

$ roslaunch livox_ros_driver2 pcd_MID360_p2s.launch

xfer_format　”０” を設定し、pointcloud_to_laserscanにより、点群のエリアを限定しています。
水平：180°（前方方向）、垂直：-0.15（LiDAR取付位置から地面までの距離）～1.5m、距離：0.1～50m 

<launch>

	<!--user configure parameters for ros start-->
	<arg name="lvx_file_path" default="livox_test.lvx"/>
	<arg name="bd_list" default="100000000000000"/>
	<arg name="xfer_format" default="0"/>
	<arg name="multi_topic" default="0"/>
	<arg name="data_src" default="0"/>
	<arg name="publish_freq" default="10.0"/>
	<arg name="output_type" default="0"/>
	<arg name="rviz_enable" default="false"/>
	<arg name="rosbag_enable" default="false"/>
	<arg name="cmdline_arg" default="$(arg bd_list)"/>
	<arg name="msg_frame_id" default="livox_frame"/>
	<arg name="lidar_bag" default="true"/>
	<arg name="imu_bag" default="true"/>
	<!--user configure parameters for ros end--> 

	<param name="xfer_format" value="$(arg xfer_format)"/>
	<param name="multi_topic" value="$(arg multi_topic)"/>
	<param name="data_src" value="$(arg data_src)"/>
	<param name="publish_freq" type="double" value="$(arg publish_freq)"/>
	<param name="output_data_type" value="$(arg output_type)"/>
	<param name="cmdline_str" type="string" value="$(arg bd_list)"/>
	<param name="cmdline_file_path" type="string" value="$(arg lvx_file_path)"/>
	<param name="user_config_path" type="string" value="$(find livox_ros_driver2)/config/MID360_config.json"/>
	<param name="frame_id" type="string" value="$(arg msg_frame_id)"/>
	<param name="enable_lidar_bag" type="bool" value="$(arg lidar_bag)"/>
	<param name="enable_imu_bag" type="bool" value="$(arg imu_bag)"/>

	<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
	      type="livox_ros_driver2_node" required="true"
	      output="screen" args="$(arg cmdline_arg)"/>

	<group if="$(arg rviz_enable)">
		<node name="livox_rviz" pkg="rviz" type="rviz" respawn="true"
				args="-d $(find livox_ros_driver2)/config/display_point_cloud_ROS1.rviz"/>
    </group>

	<group if="$(arg rosbag_enable)">
    	<node pkg="rosbag" type="record" name="record" output="screen"
          		args="-a"/>
    </group>

    <!-- Run pointcloud_to_laserscan -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan_node" output="screen" >
        <remap from="cloud_in" to="/livox/lidar" />
        <remap from="scan" to="/pointcloud_scan" />
        <param name="target_frame" value="livox_frame" />
        <param name="min_height" value="-0.15" />
        <param name="max_height" value="1.5" />
        <param name="angle_min"  value="-1.5708" />
        <param name="angle_max"  value="1.5708" />
        <param name="angle_increment"  value="0.0026" />
        <param name="range_min"  value="0.1" />
        <param name="range_max"  value="50" />
        <param name="use_inf" value="true" />
    </node>

</launch>

Terminal-3：gmapping の起動

$ roslaunch turtlebot_navigation gmappig_livox.launch

<launch>
  <arg name="3d_sensor" default="livox"/>  <!-- r200, kinect, asus_xtion_pro -->

  <!-- Gmapping -->
  <arg name="custom_gmapping_launch_file" default="$(find turtlebot_navigation)/launch/includes/gmapping/$(arg 3d_sensor)_gmapping.launch.xml"/>
  <include file="$(arg custom_gmapping_launch_file)"/>

  <!-- Move base -->
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>

</launch>

livox_gmapping.launch.xml と move_base.launch.xlm にて、入力される点群の topic名を変更します。 

・livox_gnmapping.launch.xml
<!-- arg name="scan_topic"  default="scan" / -->
<arg name="scan_topic"  default="/pointcloud_scan" />

・move_base.launch.xml
<!-- arg name="laser_topic" default="scan" / -->
<arg name="laser_topic" default="/pointcloud_scan" />

Terminal-4：rviz の起動

$ roslaunch turtlebot_rviz_launchers view_navigation.launch

Terminal-5：キーボード入力の起動 

$ roslaunch turtlebot_teleop keyboard_teleop.launch

以下のようなテキストが表示されるので、移動させる方向に合わせてキーボードの文字を押して移動させてください。
ロボットを少しずつ移動させることにより、地図データが出来ていきます。

地図データ作成画面

Terminal-6：地図の保存

地図の作成が終わったら、データを保存します。

$ rosrun map_server map_server -f [file_name]

実行したディレクトリに、[file_name].yaml [file_name].pgm の2つのファイルができます。
これで、地図作製は終了です。

gmappingによる地図作成

自己位置推定＆自動走行

保存した地図を読み込んで、自己位置推定（AMCL）を実行し、ゴールを指定して自動走行を行います。

Terminal-1：turtlebot の起動

$ roslaunch turtlebot_bringup miniml.launch

Terminal-2：Livox_ros_driver の起動 

$ roslaunch livox_ros_driver2 pcd_MID360_p2s.launch

Terminal-3：amcl の起動

$ roslaunch turtlebot_navigation amcl_livox.launch

保存した地図データの場所を指定します。（赤字部分）
初期位置は rviz の　"2D Pose Estimate" を使用するため、初期位置はすべて 0.0 となっています。 

<launch>
  <arg name="3d_sensor" default="livox"/>  <!-- r200, kinect, asus_xtion_pro -->

  <!-- Map server -->
  <arg name="map_file" default="$(find turtlebot_navigation)/maps/[file_name].yaml"/>
  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

  <!-- AMCL -->
  <arg name="custom_amcl_launch_file" default="$(find turtlebot_navigation)/launch/includes/amcl/$(arg 3d_sensor)_amcl.launch.xml"/>
  <arg name="initial_pose_x" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
  <arg name="initial_pose_y" default="0.0"/> <!-- Use 17.0 for willow's map in simulation -->
  <arg name="initial_pose_a" default="0.0"/>
  <include file="$(arg custom_amcl_launch_file)">
    <arg name="initial_pose_x" value="$(arg initial_pose_x)"/>
    <arg name="initial_pose_y" value="$(arg initial_pose_y)"/>
    <arg name="initial_pose_a" value="$(arg initial_pose_a)"/>
  </include>

  <!-- Move base -->
  <arg name="custom_param_file" default="$(find turtlebot_navigation)/param/$(arg 3d_sensor)_costmap_params.yaml"/>
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml">
    <arg name="custom_param_file" value="$(arg custom_param_file)"/>
  </include>

</launch>

livox_amcl.launch.xml にて、入力される点群の topic名を変更します。 

・livox_amcl.launch.xml
<!-- arg name="scan_topic"  default="scan" / -->
<arg name="scan_topic"  default="/pointcloud_scan" />

Terminal-4：rviz の起動

$ roslaunch turtlebot_rviz_launchers view_navigation.launch

Terminal-5：キーボード入力の起動 

$ roslaunch turtlebot_teleop keyboard_teleop.launch

"2D Pose Estimate" を実行後、ロボットを回転させて自己位置推定を行うために起動しておきます。

自己位置推定の実行

rviz上に地図が表示され、TurtleBot が表示されます。
緑色の矢印の"2d Pose Estimate" を使って、初期位置と向きをラフに指定します。

自己位置推定の実行の工程イメージ1

その後、キーボードを使って、Turtlebot を回転させ、自己位置を推定させます。（地図が移動していきます）
Turtlebot の位置が地図上と一致したら、Terminal-5 の turtlebot_teleop を Ctrl＋c を使って止めます。（これを止めないと、自動走行できない）
最後に、"2D Nav Goal" を使って、ゴール位置と到着時の向きを指定します。

自己位置推定の実行の工程イメージ2

Turtlebot が自動的に移動し始め、途中に障害物があると回避しながらゴールまで移動していきます。

AMCLおよび自動走行