【連載第3回】つくばで500m完走！CartographerとNav2による高安定な自律走行システム

Last updated at 2026-03-14Posted at 2026-02-12

はじめに

TERAKATA Roboticsの連載第3回です。
第1回でハードウェア、第2回で開発環境を整えました。今回はいよいよ真打ち登場、ロボットを動かすための 自律走行アルゴリズム（SLAM & Navigation） について解説します。

つくばチャレンジ2025、本走行後の実験走行。
私たちの開発したロボット TERABO は確認走行区間の 約500mを自律走行 で走り切りました。スタックなし、介入なし。
この成功を支えたのは、奇抜な新技術ではなく、標準的な技術の 徹底的なチューニング でした。

1. SLAM: Google Cartographer (3D-to-2D)

TERABOは3D LiDAR (VLP-16) を搭載していますが、移動計画（Path Planning）は2D平面で行うため、生成する地図は2Dグリッドマップ（Occupancy Grid Map）です。
私たちはGoogle製のSLAMライブラリ Cartographer を採用しました。

なぜCartographerか？

安定性: 長時間の計測でも破綻しにくい。
3D-to-2D: VLP-16の豊富な3D点群情報を使いつつ、2D地図を出力するモードが標準でサポートされている。
IMU統合: 3DスキャンマッチングにおいてIMU情報を強く信頼させることで、激しい動きでも自己位置を見失いません。

実環境チューニングのキモ (Lua設定)

デフォルト設定では屋外の広大な環境には対応しきれません。以下のような変更を行いました。

cartographer_config_scan_with_imu.lua:

-- VLP-16の実用レンジに合わせて調整
TRAJECTORY_BUILDER_2D.min_range = 0.3
TRAJECTORY_BUILDER_2D.max_range = 50.0

-- IMUを使用し、リアルタイムスキャンマッチング（Correlative）は重いのでOFF
TRAJECTORY_BUILDER_2D.use_imu_data = true
TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = false

-- Motion Filter（「動いていない」判定を厳しくして、停止時の地図太り防止）
TRAJECTORY_BUILDER_2D.motion_filter.max_time_seconds = 1.0
TRAJECTORY_BUILDER_2D.motion_filter.max_distance_meters = 0.03
TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.3)

また、自己位置推定（Localization）やループクロージャーの安定化のため、POSE_GRAPH も調整しています。

-- ループクロージャーの誤マッチング抑制
POSE_GRAPH.constraint_builder.min_score = 0.57
POSE_GRAPH.constraint_builder.global_localization_min_score = 0.60

```lua
-- 最適化の頻度と重み付け
POSE_GRAPH.optimize_every_n_nodes = 60
POSE_GRAPH.constraint_builder.loop_closure_rotation_weight = 8e4 -- 回転ズレを許さない

作成された地図

実走行ログから作成した、つくばチャレンジコース（約500m）の全体地図です。ループクロージャーが効いており、スタート地点とゴール地点が見事に整合しています。

2. Navigation: Nav2 Stack

ROS 2標準のNavigation Stackである Nav2 を使用しました。
ここでのポイントは、Planner（大域的経路計画） と Controller（局所的動作制御） の選定です。

Planner: Smac Planner 2D (A*)

デフォルトの NavFn Planner (Dijkstra/A*) は優秀ですが、私たちはグリッドベースの探索である Smac Planner 2D を採用しました。
Smac Plannerは非常に効率的で、複雑な迷路のような環境でも最適なパスを高速に見つけ出します。Hybrid A*版（SmacPlannerHybrid）もありますが、TERABOは差動二輪であり、その場旋回が可能であるため、純粋な2Dグリッド探索で十分と判断しました。

nav2_params_scan2d.yaml:

PlannerServer:
  ros__parameters:
    planner_plugins: ["GridBased"]
    GridBased:
      plugin: "nav2_smac_planner/SmacPlanner2D"
      tolerance: 0.5
      use_astar: false
      allow_unknown: true

Controller: DWB Controller

障害物回避を行うローカルプランナーには、定番のDynamic Window Approach (DWA) である DWB Controller を採用。
つくばの街中は人が多いため、対人回避の挙動が重要になります。

Critics（評価関数）の調整:
DWBは複数のスコア（Critic）の重み付けで挙動が決まります。
- PathAlign（パスへの整列）: 32.0 と非常に高く設定。グローバルプランに忠実に追従させることを最優先しています。
- GoalAlign（ゴールへの整列）: 16.0。ゴールの方向を向くことも重要ですが、パスから外れないことの方が優先です。
- PathDist（パスとの距離）: 10.0。パスから離れすぎないように制御します。
- BaseObstacle（障害物回避）: 3.0。低めに見えますが、コストマップで膨張させているため、これで十分回避します。
Costmap Inflationの魔法:
コストマップの inflation_radius（障害物の周りの危険地帯）は、ロボットの挙動に直結します。
- Global Costmap: inflation_radius: 0.45 / cost_scaling_factor: 3.0。コストの減衰を急にして、通れる場所を広く認識させます。
- Local Costmap: inflation_radius: 0.55 / cost_scaling_factor: 1.0。コストの減衰を緩やかにし、障害物に近づくにつれて早めに回避行動を取らせるようにしています。
  この「大小の使い分け」が、安全かつスムーズな走行の鍵でした。