はじめに

これまで,オドメトリとGPSでロボットの自己位置推定を行ってきましたが,屋内環境で長距離を走行する場合に限界を感じました.そこで今回はSLAMに挑戦してみました.

動作環境

環境は以下の通りです.

項目	バージョン
ubuntu	22.04
ROS	ROS2 humble
Lidar	RPLidar A1 M8

↓RPLidar A1 M8

cartograpaherのインストール

下記のコマンドでcartograpaherをgithubからインストールします.

cd ~/ros2_ws/src
sudo apt install ros-humble-cartographer 
sudo apt install ros-humble-cartographer-rviz

Lidarの環境構築

RPLidarA1の起動できるように,環境構築を行います.下記のサイトが参考になります.

launchファイルの作成

cartographerを起動するためのlaunchファイルを作成します.

cd ~/ros2_ws/src/sllidar_ros2/launch
touch rplidar_cartographer.launch.py

launchファイルで起動するノードを指定します.ここでは,可視化用のrvizとcartographerを起動させます.

import launch
import launch.actions
import launch.substitutions
import launch_ros.actions

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
from launch.actions import DeclareLaunchArgument

import os

def generate_launch_description():
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    share_dir = get_package_share_directory('sllidar_ros2')
    rviz_config_file = os.path.join(
            get_package_share_directory('sllidar_ros2'),
            'rviz',
            'cartographer.rviz')

    cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', 
                                                    default=os.path.join(share_dir, 'config'))
    configuration_basename = LaunchConfiguration('configuration_basename', default='rplidar_cartographer.lua')

    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    
    return LaunchDescription([

        Node(package='rviz2',
                    executable='rviz2',
                    name='rviz2',
                    arguments=['-d', rviz_config_file],
            ),

        Node(
            package='tf2_ros',
            executable='static_transform_publisher',
            output='screen',
            arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'base_link', 'laser_frame']
            ),

        Node(
            package='cartographer_ros',
            executable='cartographer_occupancy_grid_node',
            name='cartographer_occupancy_grid_node',
            output='screen',
            parameters=[{'resolution': 0.05}],
            ),

            
        Node(
            package='cartographer_ros',
            executable='cartographer_node',
            name='cartographer_node',
            output='screen',
            parameters=[{'use_sim_time': False}],
            arguments=['-configuration_directory', cartographer_config_dir, '-configuration_basename', configuration_basename],
            ),
    ])

static_transform_publisher：静的な変換を公開するノードで,base_linkとlaser_frameフレーム間の位置関係を設定します.「laser_frame」は, lidar側で設定したframe_idに対応する名前を適宜設定してください.

Node(
    package='tf2_ros',
    executable='static_transform_publisher',
    output='screen',
    arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'base_link', 'laser_frame']
    ),

cartographer_occupancy_grid_node：Occupancy Grid（占有格子）マップを生成するためのノードで,指定した解像度で地図を作成します.

Node(
    package='cartographer_ros',
    executable='cartographer_occupancy_grid_node',
    name='cartographer_occupancy_grid_node',
    output='screen',
    parameters=[{'resolution': 0.05}],
    ),

cartographer_node：地図の構築およびロボットの自己位置推定を実行します.

Node(
    package='cartographer_ros',
    executable='cartographer_node',
    name='cartographer_node',
    output='screen',
    parameters=[{'use_sim_time': False}],
    arguments=['-configuration_directory', cartographer_config_dir, '-configuration_basename', configuration_basename],
    ),

luaファイルの作成

次にCartographerの設定を記述するluaファイルを作成します.
説明はコメントアウトで乗せておきます.パラメータは環境に応じて変更してください.

-- "map_builder.lua"と"trajectory_builder.lua"の設定ファイルをインクルードする
include "map_builder.lua"
include "trajectory_builder.lua"

-- オプション設定の開始
options = {
  map_builder = MAP_BUILDER,  -- 地図生成の設定オブジェクト
  trajectory_builder = TRAJECTORY_BUILDER,  -- 軌跡生成の設定オブジェクト
  map_frame = "map",  -- 地図の基準フレームの名前
  tracking_frame = "base_link",  -- ロボットの位置を追跡するためのフレーム
  published_frame = "base_link",  -- 発行するフレームの名前
  odom_frame = "odom",  -- オドメトリの基準フレームの名前
  provide_odom_frame = false,  -- 自動的にオドメトリフレームを生成するかどうか
  publish_frame_projected_to_2d = false,  -- 発行フレームを2次元に投影するか
  use_odometry = false,  -- オドメトリデータを使用するか
  use_nav_sat = false,  -- GPSデータを使用するか
  use_landmarks = false,  -- ランドマークデータを使用するか
  num_laser_scans = 1,  -- 使用するレーザースキャンの数
  num_multi_echo_laser_scans = 0,  -- マルチエコーレーザースキャンの数
  num_subdivisions_per_laser_scan = 1,  -- レーザースキャンごとに分割する数
  num_point_clouds = 0,  -- 使用するポイントクラウドの数
  lookup_transform_timeout_sec = 0.2,  -- トランスフォームのタイムアウト時間（秒）
  submap_publish_period_sec = 0.05,  -- サブマップの発行周期（秒）
  pose_publish_period_sec = 5e-3,  -- ポーズの発行周期（秒）
  trajectory_publish_period_sec = 30e-3,  -- 軌跡の発行周期（秒）
  rangefinder_sampling_ratio = 1.,  -- 距離センサーのサンプリング割合
  odometry_sampling_ratio = 1.,  -- オドメトリのサンプリング割合
  fixed_frame_pose_sampling_ratio = 1.,  -- 固定フレームのポーズのサンプリング割合
  imu_sampling_ratio = 1.,  -- IMUのサンプリング割合
  landmarks_sampling_ratio = 1.,  -- ランドマークのサンプリング割合
}

-- 2次元の軌跡生成を使用する設定
MAP_BUILDER.use_trajectory_builder_2d = true

-- 2D軌跡生成の設定
TRAJECTORY_BUILDER_2D.min_range = 0.  -- 距離測定の最小範囲（メートル）
TRAJECTORY_BUILDER_2D.max_range = 10.  -- 距離測定の最大範囲（メートル）
TRAJECTORY_BUILDER_2D.missing_data_ray_length = 5.  -- 欠損データの代わりに使用する距離（メートル）
TRAJECTORY_BUILDER_2D.use_imu_data = false  -- IMUデータを使用するか
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false  -- オンラインスキャンマッチングを使用するか

-- POSE_GRAPHの制約ビルダーのスコア設定
POSE_GRAPH.constraint_builder.min_score = 0.65  -- スキャンマッチングの最小スコア
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7  -- グローバル位置推定の最小スコア

-- POSE_GRAPHの最適化問題の重み設定
POSE_GRAPH.optimization_problem.local_slam_pose_translation_weight = 1e5  -- ローカルSLAMの平行移動に対する重み
POSE_GRAPH.optimization_problem.local_slam_pose_rotation_weight = 1e5  -- ローカルSLAMの回転に対する重み
POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e5  -- オドメトリの平行移動に対する重み
POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e5  -- オドメトリの回転に対する重み
POSE_GRAPH.optimization_problem.huber_scale = 1e3  -- Huber損失関数のスケール

-- スキャンマッチングの重み設定
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.occupied_space_weight = 10  -- 占有空間に対する重み
TRAJECTORY_BUILDER_2D.ceres_scan_matcher.rotation_weight = 40  -- 回転に対する重み

-- サブマップとモーションフィルターの設定
TRAJECTORY_BUILDER_2D.submaps.num_range_data = 120  -- サブマップあたりの距離データの数
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.1  -- 最大距離のフィルタ（メートル）
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.2)  -- 最大角度のフィルタ（ラジアン）

-- 最終的なオプション設定を返す
return options

Rvizの設定ファイルを作成

rvizのファイルを作成します.

cd ~/ros2_ws/sllidar_ros2/rviz
touch cartographer.rviz

SLAMを可視化するためのRvizを設定します.

Panels:
  - Class: rviz_common/Displays
    Help Height: 78
    Name: Displays
    Property Tree Widget:
      Expanded:
        - /Global Options1
        - /Status1
        - /Grid1
        - /LaserScan1
        - /LaserScan1/Topic1
        - /TF1
      Splitter Ratio: 0.5
    Tree Height: 787
  - Class: rviz_common/Selection
    Name: Selection
  - Class: rviz_common/Tool Properties
    Expanded:
      - /2D Goal Pose1
      - /Publish Point1
    Name: Tool Properties
    Splitter Ratio: 0.5886790156364441
  - Class: rviz_common/Views
    Expanded:
      - /Current View1
    Name: Views
    Splitter Ratio: 0.5
Visualization Manager:
  Class: ""
  Displays:
    - Alpha: 0.5
      Cell Size: 1
      Class: rviz_default_plugins/Grid
      Color: 160; 160; 164
      Enabled: true
      Line Style:
        Line Width: 0.029999999329447746
        Value: Lines
      Name: Grid
      Normal Cell Count: 0
      Offset:
        X: 0
        Y: 0
        Z: 0
      Plane: XY
      Plane Cell Count: 10
      Reference Frame: <Fixed Frame>
      Value: true
    - Alpha: 1
      Autocompute Intensity Bounds: true
      Autocompute Value Bounds:
        Max Value: 0
        Min Value: 0
        Value: true
      Axis: Z
      Channel Name: intensity
      Class: rviz_default_plugins/LaserScan
      Color: 255; 255; 255
      Color Transformer: Intensity
      Decay Time: 0
      Enabled: true
      Invert Rainbow: false
      Max Color: 255; 255; 255
      Max Intensity: 47
      Min Color: 0; 0; 0
      Min Intensity: 47
      Name: LaserScan
      Position Transformer: XYZ
      Selectable: true
      Size (Pixels): 3
      Size (m): 0.05000000074505806
      Style: Flat Squares
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: System Default
        Value: /scan
      Use Fixed Frame: true
      Use rainbow: true
      Value: true
    - Class: rviz_default_plugins/Marker
      Enabled: false
      Name: Marker
      Namespaces:
        {}
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /filtered_point_marker
      Value: false
    - Class: rviz_default_plugins/TF
      Enabled: true
      Frame Timeout: 15
      Frames:
        All Enabled: true
        base_link:
          Value: true
        laser_frame:
          Value: true
        map:
          Value: true
      Marker Scale: 1
      Name: TF
      Show Arrows: true
      Show Axes: true
      Show Names: true
      Tree:
        map:
          base_link:
            laser_frame:
              {}
      Update Interval: 0
      Value: true
    - Alpha: 0.699999988079071
      Class: rviz_default_plugins/Map
      Color Scheme: map
      Draw Behind: false
      Enabled: true
      Name: Map
      Topic:
        Depth: 5
        Durability Policy: Volatile
        Filter size: 10
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /map
      Update Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /map_updates
      Use Timestamp: false
      Value: true
  Enabled: true
  Global Options:
    Background Color: 48; 48; 48
    Fixed Frame: map
    Frame Rate: 30
  Name: root
  Tools:
    - Class: rviz_default_plugins/Interact
      Hide Inactive Objects: true
    - Class: rviz_default_plugins/MoveCamera
    - Class: rviz_default_plugins/Select
    - Class: rviz_default_plugins/FocusCamera
    - Class: rviz_default_plugins/Measure
      Line color: 128; 128; 0
    - Class: rviz_default_plugins/SetInitialPose
      Covariance x: 0.25
      Covariance y: 0.25
      Covariance yaw: 0.06853891909122467
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /initialpose
    - Class: rviz_default_plugins/SetGoal
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /goal_pose
    - Class: rviz_default_plugins/PublishPoint
      Single click: true
      Topic:
        Depth: 5
        Durability Policy: Volatile
        History Policy: Keep Last
        Reliability Policy: Reliable
        Value: /clicked_point
  Transformation:
    Current:
      Class: rviz_default_plugins/TF
  Value: true
  Views:
    Current:
      Class: rviz_default_plugins/Orbit
      Distance: 13.607483863830566
      Enable Stereo Rendering:
        Stereo Eye Separation: 0.05999999865889549
        Stereo Focal Distance: 1
        Swap Stereo Eyes: false
        Value: false
      Focal Point:
        X: 0
        Y: 0
        Z: 0
      Focal Shape Fixed Size: true
      Focal Shape Size: 0.05000000074505806
      Invert Z Axis: false
      Name: Current View
      Near Clip Distance: 0.009999999776482582
      Pitch: 0.9847959280014038
      Target Frame: <Fixed Frame>
      Value: Orbit (rviz)
      Yaw: 3.175386905670166
    Saved:
      - Class: rviz_default_plugins/Orbit
        Distance: 12.960694313049316
        Enable Stereo Rendering:
          Stereo Eye Separation: 0.05999999865889549
          Stereo Focal Distance: 1
          Swap Stereo Eyes: false
          Value: false
        Focal Point:
          X: 0
          Y: 0
          Z: 0
        Focal Shape Fixed Size: true
        Focal Shape Size: 0.05000000074505806
        Invert Z Axis: false
        Name: Orbit
        Near Clip Distance: 0.009999999776482582
        Pitch: 1.5697963237762451
        Target Frame: <Fixed Frame>
        Value: Orbit (rviz)
        Yaw: 1.2103947401046753
Window Geometry:
  Displays:
    collapsed: false
  Height: 1016
  Hide Left Dock: false
  Hide Right Dock: false
  QMainWindow State: 000000ff00000000fd0000000400000000000001c40000039efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000045b0000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
  Selection:
    collapsed: false
  Tool Properties:
    collapsed: false
  Views:
    collapsed: false
  Width: 1850
  X: 70
  Y: 27

地図を作成

Cartographerを使用してLiDARのデータをリアルタイムで地図に変換し,環境のマッピングを行います.以下のコマンドでLiDARとCartographerを順に起動して,ROS 2でリアルタイムに地図を生成することが可能です.

Lidarを起動

LiDARデバイスを起動します.これにより,LiDARセンサーのデータが取得され,周囲の物体を検出できるようになります.

source ~/ros2_ws/install/setup.bash
ros2 launch sllidar_ros2 view_sllidar_a1_launch.py

Cartographerを起動

続いて,Cartographerを起動して,LiDARデータをもとに地図を構築します.Cartographerは,センサーのスキャンデータを解析し,2Dマッピングを行います.

source ~/ros2_ws/install/setup.bash
ros2 launch sllidar_ros2 rplidar_cartographer.launch.py

mapを保存

下記のコマンドで,作成した地図を保存できます.

ros2 run nav2_map_server map_saver_cli -f ~/map

これで地図の作成が完了しました.CartographerがLiDARからのデータを収集し,実際の環境の2Dマップがリアルタイムで生成されているのが確認できます.この地図を使用することで,ロボットの自己位置推定や経路計画に活用できます.

参考文献

謝辞

この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.