はじめに

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。コストの減衰を緩やかにし、障害物に近づくにつれて早めに回避行動を取らせるようにしています。
  この「大小の使い分け」が、安全かつスムーズな走行の鍵でした。

3. リカバリー動作 (Recovery Behaviors)

500mも走っていれば、人混みに囲まれて立ち往生することもあります。
Nav2のBehavior Tree（BT）をカスタマイズし、単純なスピンやバックだけでなく、「一度待つ（Wait）」を追加しました。

Wait: 5秒停止。歩行者が通り過ぎるのを待つ。
Spin: その場で回転し、最新の地図情報を取得。
BackUp: 少し後退してスペースを作る。

焦って動き回るよりも、「まず止まる」方が、ロボットも周囲の人間もお互いに安全だということを学びました。

4. 実際の走行結果

つくばチャレンジ本走行後の実験走行での様子です。Nav2による自律走行で安定して走行しているのがわかります。

まとめ

つくばでの500m完走は、決して魔法ではありません。

Cartographer で歪みのない地図を作る。
Smac Planner で幾何学的に正しいパスを引く。
DWB Controller と Costmap の調整で、現実世界の「狭さ」に適応する。

一つ一つのパラメータには、何度も現地で走ってはログを見て修正した、泥臭い試行錯誤が詰まっています。
これからNav2に触れる皆さんも、ぜひデフォルト設定で満足せず、「自分のロボットに合った数値」を探求してみてください。

次回は、そんな試行錯誤を支えた 秘密兵器（自作ツール） をご紹介します。
Rvizでぽちぽちウェイポイントを修正するツール、欲しくないですか？

リンク

最新の活動状況や、今回紹介した技術のソースコードは以下で公開しています。
興味を持っていただけた方は、ぜひフォロー＆スターをお願いします！

Website: https://sites.google.com/view/terakata-robotics
X (Twitter): https://x.com/terakata_robot
Instagram: https://www.instagram.com/terakata_robotics/
GitHub: https://github.com/TERAKATA-Robotics/terabo-navigation