Unitree Go2 EDUで障害物を避けてみた

こんにちは。atsuです。

Unitree Go2 EDU のフロア巡回機能を9つに分解した全体像は、別記事で整理しています。本記事では、そのうち 障害物を避ける 機能を扱います。

https://dev.classmethod.jp/articles/go2-patrol/

Nav2 で目的地まで移動できるようになっても、実際のフロア巡回ではまだ足りないことがあります。保存済みの地図には壁や柱のような固定物は入っていますが、巡回中にだけ現れる人、椅子、段ボールなどは入っていません。

この「いま目の前にあるもの」を Nav2 が避けられる形にするために使うのが costmap です。この記事では Nav2 costmap の全機能を網羅するのではなく、Go2 EDU をフロア巡回ロボットとして動かすために、何を costmap に入れ、どこを確認しながら障害物回避を調整したかを扱います。

Go2 EDU（プリインストールのROS2 Foxy）と Nav2、開発PCは M3 Mac という構成です。ROS2 Foxy はすでに公式サポートを終えていますが、Go2実機側の環境に合わせています。マップ作成や自己位置推定などの前提は、本ブログの参考リンクにある過去記事で扱っています。

この記事の中心は、以下の関係です。

この記事でわかること

• Go2 の巡回で costmap がなぜ必要か
• 保存済みマップ、LiDAR、costmap の役割の違い
• local costmap と global costmap の見方
• Go2 の LiDAR（3D 点群）を costmap に入れるときに注意したこと
• 障害物回避が期待通りに動かないときの確認ポイント

先に用語を整理する

用語が多いので、まずは「もともとの地図」と「巡回中に見えている障害物」を分けて読むと追いやすいです。

この記事で主に見る ROS topic は以下です。

Planner（経路計算）や Controller（速度指令）は topic 名ではなく Nav2 の役割です。

Go2 が障害物を避ける流れ

冒頭の図を、Nav2 costmap の内側まで少し分解します。

なお、この図は簡略化しています。実際の Nav2 では global costmap と local costmap は別インスタンスとして動いており、planner_server は global costmap を、controller_server は local costmap を参照します。

Go2 EDU の LiDAR は周囲を3D点群として出します。一方、今回の Nav2 costmap では2Dのスキャンとして扱う構成にしています。そのため、Go2の3D点群を Nav2 が扱いやすい /scan に整えてから costmap に入れています。

costmap 自体は Nav2 の標準機能です。Go2 固有なのは、その手前で Go2 のセンサー情報を Nav2 が読みやすい形に整える部分です。

Go2の巡回で costmap が必要な理由

保存済みマップは、フロアの壁や柱を表す地図です。同じフロアを巡回するなら、この地図は何度も使えます。

ただし、実際のフロアは毎回同じ状態ではありません。椅子の位置が変わることもありますし、人が通路を横切ることもあります。保存済みマップだけを見て経路を引くと、地図上では通れる場所でも、実際には通れないことがあります。

costmap は、保存済みマップに「今見えている障害物」と「近づきすぎないための余白」を重ねた地図です。Nav2 の planner は global costmap を見て経路を考え、controller は local costmap を見ながら速度指令を出します。

ロボットに詳しくない方向けに言うと、costmap はロボット用の「危険度つき床面図」です。ここを分けておくと、「地図は合っているのに Go2 が止まる」「何もないように見える場所を避ける」といった現象を切り分けやすくなります。

local costmap と global costmap

Nav2 では、主に local costmap と global costmap の 2 種類を使います。

巡回中に人が横切るような場面では、local costmap の見え方が特に重要です。global plan は大きな方向を示し、controller が local costmap を見ながら実際の速度を調整します。

一方で、遠回りが必要な障害物や、通路全体が塞がっているような状況では global costmap も効いてきます。

Go2実機で costmap を使うために必要だった3つの対応

Go2 実機で Nav2 costmap を使うために、次の3つを意識しました。

• Go2 の 3D 点群を 2D スキャンとして扱える形に整える
• costmap layer の役割を分けて見る
• マッピング用と巡回用でセンサー処理の設定を分ける

1. Go2 の3D点群を2Dスキャンとして整える

前述のとおり、Go2 の3D点群は2Dの /scan に変換してから costmap に入れています。

このとき、Go2自身の体に当たった反射や、床面近くのノイズをそのまま入れると、Nav2 はそれも障害物として扱います。実際には通れる場所でも、costmap 上では前が塞がっているように見えることがあります。

そのため、3D点群から2Dスキャンへ整える段階で、高さ、距離、孤立点のフィルタを入れました。変換は自作のブリッジノードで行っています（標準の pointcloud_to_laserscan パッケージ相当の変換に、上記のフィルタを加えたもの）。Nav2 の costmap パラメータを調整する前に、まず /scan が妥当かを見るようにしています。

2. costmap layer の役割を分けて見る

costmap は1枚の地図に見えますが、内部では複数の layer を重ねています。

Nav2 公式ドキュメントでも、costmap は planner や controller が衝突や高コスト領域を確認するために使う 2D グリッドとして説明されています。Obstacle Layer は 2D raycasting を使ってセンサー情報を costmap に入れ、Inflation Layer は障害物の周囲にコストを広げます。

実機では、「どの layer の問題か」を分けて見るのが大事でした。たとえば Go2 が何もない場所で止まる場合、inflation が強すぎることもあれば、obstacle layer に古い障害物が残っていることもあります。見た目は同じ「止まる」でも、見る場所が変わります。

3. マッピング用と巡回用で設定を分ける

マップ作成では、壁の形を安定して取りたいのでノイズ除去を強めたくなります。一方、巡回では低い障害物も見たいので、低い点を落としすぎると困ります。

1つの設定で全部を賄おうとすると、マップ作成ではノイズが増え、巡回では低い障害物を見落とす、といったトレードオフが生じます。

そのため、この環境では3D点群から2Dスキャンへ整える設定を、マップ作成用と巡回用で分けました。マップを作るときは地図の安定性を優先し、巡回するときは障害物検知と停止しやすさを優先しています。具体的には、高さフィルタの下限をマッピング時は LiDAR の取り付け高さ、巡回時は地面から約10cm に設定しました。

Go2 の障害物回避を確認するポイント

障害物回避が期待通りに動かないときは、最初から Nav2 全体を疑うより、情報の流れに沿って見る方が切り分けやすいです。

# Foxy の ros2 topic echo には --once オプションが無いため、内容を確認したら Ctrl+C で停止します
# --no-arr は配列フィールドの中身を省略表示するオプションです
ros2 topic echo /scan --no-arr
ros2 topic echo /local_costmap/costmap --no-arr
ros2 topic echo /global_costmap/costmap --no-arr
ros2 topic echo /plan --no-arr
ros2 topic echo /cmd_vel

echo ではまず各 topic が流れているかを確認します。そのうえで、/scan の障害物が local costmap や global costmap に反映されているか、/plan（planner が計算した経路）が障害物を避ける形になっているか、/cmd_vel が減速や停止につながっているかを順番に見ます。なお、/cmd_vel は Nav2 がナビゲーションを実行している間しか流れません。echo しても何も表示されない場合は、ナビゲーション中かどうかを先に確認します。

障害物がどう見えているかは echo だけでは追いにくいので、可視化ツールの Foxglove Studio で /scan, /local_costmap/costmap, /global_costmap/costmap, /plan, /cmd_vel を同時に見ると確認しやすいです。Go2 実機が止まった瞬間に、どこまで情報が伝わっていたかを確認できます。

切り分けの順番は次のようにしました。

Go2実機でつまずいたところ

Go2実機では、costmap の設定だけでは説明しきれない場面がありました。つまずいた点は次の2つです。

何もない場所に障害物が残る

Go2 自身の反射や歩行中の揺れで、実際には何もない場所に障害物が残ることがありました。これが local costmap に残ると、controller は「前が塞がっている」と判断します。

この場合、costmap のパラメータだけを見ても原因をつかみにくいです。私はまず /scan を見て、costmap に入る前のデータに不要な点が含まれていないか確認してから、前述のフィルタ（高さ、距離、孤立点）を調整しました。

障害物が消えた後も避け続ける

障害物が視界から消えたあと、costmap から消えるまでに遅れがあると、Go2は何もない場所を避け続けるように見えます。

このときは、/scan と /local_costmap/costmap_updates（local costmap の差分更新 topic）を並べて確認しました。/scan から障害物が消えているのに costmap 側に残る場合は、clearing や TF、更新タイミングを確認対象にしました。

運用上の見立て

巡回ロボットの障害物回避では、完璧な認識よりも「危ないときに止まれる」「何もない場所では止まりすぎない」のバランスが大事でした。

Go2 のような四足歩行ロボットでは、歩行振動とセンサー反射の影響があります。そのため、最初は保守的に止まれる設定から始め、実機で少しずつ調整する方が安全でした。

もう1つ大事なのは、障害物回避を「最短経路で通れるか」だけで評価しないことです。巡回ロボットは人のいる空間に入るので、経路の短さよりも「見ていて不安にならない距離感」を優先して調整する方が、Go2のフロア巡回には合っていると感じました。

まとめ

Go2 EDU の障害物回避は、Nav2 costmap と、costmap に入る前の /scan を整える処理を組み合わせて実現しました。

• Go2 の障害物回避では、保存済みマップに今見えている障害物と余白を重ねて考える
• local costmap は Go2 周辺の回避、global costmap は大きな迂回経路を見る
• Go2 自身の反射や歩行ノイズは、costmap に入る前の /scan 側でも確認する
• 期待通りに動かないときは、/scan → costmap → /plan → /cmd_vel の順に切り分ける

参考リンク

• Unitree Go2 EDU のフロア巡回機能を 9 つの機能に分解してみた
• Unitree Go2 EDUで巡回用の2Dマップを作ってみた
• Unitree Go2 EDUで現在位置を推定してみた
• Unitree Go2 EDUを目的地まで移動させてみた: Nav2で「目的地まで移動する」
• Nav2 Documentation
• Costmap 2D
• Obstacle Layer Parameters
• Inflation Layer Parameters