Unitree Go2 EDUを目的地まで移動させてみた: Nav2で「目的地まで歩く」

こんにちは。atsuです。

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

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

巡回用の地図を作り、AMCL でその地図上の現在位置が分かるようになると、巡回アプリはロボットに「この waypoint まで行く」と指示できます。この「目的地までの経路を考え、そこへ向かう速度指令を出す」部分を担うのが ROS 2 の Nav2 です。Nav2 は、経路計算、経路追従、回復動作などの役割を組み合わせたナビゲーションの仕組みです。

前提になる地図作成と自己位置推定は、以下の記事で扱っています。

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

https://dev.classmethod.jp/articles/unitree-go2-patrol-amcl-localization/

この記事では Nav2 の機能を網羅するのではなく、Go2 EDU を waypoint 巡回ロボットとして動かすために、Nav2 が経路を考える部分、Nav2 が速度を出す部分、その速度指令を Unitree SDK の Move() / StopMove() 呼び出しにつなぐ部分をどう構成したかを扱います。

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

Nav2 が直接 Go2 を動かすのではなく、Nav2 は ROS でよく使われる速度指令 /cmd_vel を出します。その /cmd_vel を Unitree SDK の Move() / StopMove() 呼び出しにつなぐことで、Go2 実機が動きます。

この記事でわかること

• Nav2 を使って Go2 が目的地まで移動する処理の全体像
• ROS topic、ROS 2 Action、Nav2 の役割の違い
• waypoint 巡回で NavigateToPose から始めた理由と、他の選択肢との使い分け
• /cmd_vel を Unitree SDK の Move() / StopMove() 呼び出しにつなぐ考え方
• Go2 実機で安全に試すための確認ポイントと、つまずいたところ

先に用語を整理する

Nav2 の記事では、Nav2、Action、topic、waypoint、Planner、Controller、/cmd_vel といった言葉が出てきます。最初から個別の名前を追うと混乱しやすいので、先に大きな枠を整理します。

ROS 2 は、ロボットの中で複数のプログラムをつなぐための仕組みです。Nav2 は、その ROS 2 上で動くナビゲーション用のソフトウェア群です。この記事では Nav2 を「地図上の目的地まで移動するために、経路計算や速度指令を分担する仕組み」として扱います。

ROS topic は、ロボット内の情報チャンネルです。この記事では、/ から始まる名前を topic として扱っています。一方で、Planner や Controller は topic 名ではなく Nav2 の役割、NavigateToPose は ROS 2 Action、Move() / StopMove() は Unitree SDK 側の関数です。

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

Go2 が目的地まで移動する流れ

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

図の中には planner_server など ROS 2 / Nav2 の実際のノード名も出てきます。初見では名前を覚えなくても読めます。planner_server や controller_server はノード、/plan や /cmd_vel は topic、Move() / StopMove() は Unitree SDK 側の関数として見てください。

Nav2 を初めて触る場合は、Planner は経路、Controller は速度、Behavior Tree Navigator は目的地移動の流れを扱う、と分けると追いやすいです。役割を分けておくと、問題が起きたときに Nav2 側を見るべきか、速度指令変換処理を見るべきかを切り分けやすくなります。

環境

ロボットは Unitree Go2 EDU、開発 PC は M3 Mac です。ROS 2 は Go2 EDU のプリインストール環境に合わせて Foxy を使い、Navigation には Nav2 を使いました。経路計算と経路追従は Nav2 の planner / controller を使い、Go2 実機制御は /cmd_vel を Unitree SDK の Move() / StopMove() 呼び出しにつなぐ構成にしています。

Foxy はすでに公式サポート期間を終えたディストリビューションですが、今回は実機側の環境に合わせることを優先しました。新しく環境を選べる場合は、サポート中の ROS 2 ディストリビューションの方が扱いやすいです。

Nav2 では、経路計算や経路追従の方式をプラグインとして差し替えられます。ここでは各アルゴリズムの詳細には踏み込まず、Go2 巡回の中で「どう役割を分け、どう安全に実機へ反映したか」に絞ります。参考リンクには Nav2 の公式ドキュメントも載せていますが、最新版の内容と Foxy で使える機能や設定が異なる場合があります。

Go2 巡回で使う waypoint の考え方

Go2 の巡回では、フロア上に複数の WP を置きます。各 WP は x, y, yaw を持つ pose です。

{"name": "entrance", "x": 1.0, "y": 0.0, "yaw": 1.57}

x, y は地図上のどこへ行くか、yaw は到着時にロボットが向く方向です。巡回アプリでは、カメラで監視したい方向があるため、WP に位置だけでなく向きも持たせています。

配送ロボットなら「棚の前に到着すればよい」場面もありますが、見回りでは「到着後にどちらを向いているか」が意味を持ちます。カメラでドアを見るのか、通路奥を見るのか、設備を見るのかで yaw を指定します。

初期実装では、巡回アプリが WP ごとに NavigateToPose を呼びました。

ここでの SUCCEEDED は、Nav2 が「巡回アプリから受け取った goal の処理に成功した」と返している状態です。

巡回アプリが WP ごとに NavigateToPose を呼ぶ方式は単純で追いやすい一方、ロボットは WP ごとに停止しやすくなります。Nav2 には複数の WP を順番に扱う waypoint follower や、複数 pose をまとめて扱う NavigateThroughPoses という選択肢もあります。ただし今回の Go2 + Foxy 環境では、巡回アプリがまず NavigateToPose で 1 点ずつ確実に動かす方針にしました。

ここは「Nav2 にどの機能があるか」よりも、「今回の Go2 巡回では何を優先するか」で選びました。

初期段階では、WP ごとの結果、到着誤差、Go2 実機の動きの関係を見やすくすることを優先しました。まず 1 点ずつ行ける状態を作り、その後で Go2 の巡回動作をスムーズにする順番で進めました。

Go2 実機で Nav2 巡回を動かすために必要だった 3 つの対応

ここからは実装寄りの話です。冒頭の図に沿って、次の 3 つを整理します。

• Nav2 側の役割を分けて起動する
• /cmd_vel を Unitree SDK の Move() / StopMove() 呼び出しにつなぐ
• Go2 実機で安全に試すための対策を入れる

1. Nav2 側の役割を分けて起動する

Nav2 は 1 つの大きなプログラムではなく、複数の役割に分かれています。今回も、保存済みマップの配信、地図上の自己位置推定、WP までの経路計算、経路に沿うための速度指令、移動がうまく進まないときの回復動作、目的地へ行く流れの管理を組み合わせて動かしています。

実機では、Nav2 の各役割をどの順番で起動し、どの設定でつなぐかが効いてきます。今回の構成では、Go2 側で動いている ROS 2 Foxy 環境と Docker 運用に合わせて、巡回に必要な役割を明示的に起動する方針にしました。

2. 標準の速度指令を Unitree SDK の呼び出しにつなぐ

Nav2 の controller は geometry_msgs/Twist 形式の /cmd_vel を出します。Twist は、前後・左右・回転の速度をまとめた ROS の標準メッセージです。/cmd_vel は Unitree SDK の呼び出し形式とは異なるため、速度指令変換処理で Move() / StopMove() へつなぎます。

ここでの Move() / StopMove() は ROS topic ではなく、Unitree SDK の Go2 向け移動 API です。Move(vx, vy, vyaw) は前後・左右・回転の速度を Go2 に渡し、StopMove() は移動停止を指示します。速度指令変換処理では、Nav2 から出る /cmd_vel を Unitree SDK 側の呼び出しに合わせています。

3. Go2 実機向けの安全対策を入れる

速度指令変換処理では、速度クランプと無通信時停止を入れています。速度クランプは、Nav2 から大きな速度指令が出ても、Unitree SDK の Move() を呼ぶ前に上限内へ収める処理です。前進速度と横移動速度の上限を設けることで、Go2 実機が急に速く動きすぎたり、横方向の動きが大きくなりすぎたりしないようにしました。

無通信時停止は、一定時間 /cmd_vel が来ない場合に StopMove() を呼ぶ処理です。実際の上限値や停止までの時間は、フロアの広さ、周囲の人、Go2 実機の挙動を見て調整します。速度指令変換処理が終了したり通信が途切れたりしたときは、Go2 側の障害物回避設定を元の状態へ戻すようにしました。

巡回ロボットでは「移動できる」ことに加えて「停止できる」ことも大切です。速度指令変換処理を入れると、Unitree SDK を呼ぶ手前で速度上限や停止判断を挟めます。Go2 側にも障害物回避に関する設定があるため、処理終了時はその設定を元の状態へ戻すようにしました。

Go2 の巡回動作をどうスムーズにしたか

初期の Go2 巡回では、Go2 実機が WP ごとに止まる、到着時の向き合わせで小刻みに回転する、controller から出る速度指令が細かく変化する、といった挙動がありました。そこで私は、既存の安定構成を残したまま、Go2 の巡回動作のスムーズさを重視した構成も別に用意して比較しました。

ここで言う「Go2 の巡回動作のスムーズさ」は、単に速く移動することではありません。見回りロボットとして、次のような違和感を減らすことです。

Go2 の巡回動作をスムーズにする構成は、既存動作を保ったまま、起動時の設定で切り替えられるようにしました。まずは既存安定構成で WP 成功数、停止できること、再現性を見ます。そのうえで、WP 間の停止や小刻みな向き合わせを減らしたい場面では、スムーズさ重視の構成を試しました。

向き許容は、到着時の向きのズレをどこまで許すかです。見回りでは向きも大事ですが、Nav2 側の向き合わせを厳しくしすぎると、Go2 実機が WP 付近で細かく回転し続けることがあります。WP 間の待ち時間は、巡回アプリが次の goal を送る前に入れる短い余白です。

数値では、次のメトリクスを見ました。

Go2 の目的地移動が期待通り進まないときに見る場所

ここは、実機で目的地移動が期待通りに進まないときに、どこを見るかを決めるための確認です。目的地移動では、「Nav2 が経路を作れているか」「controller が速度指令を出しているか」「Go2 実機がその指令に沿って動いているか」を分けて見ると、次の調整箇所を決めやすくなります。

ros2 action list | grep -i navigate
ros2 topic echo /plan --once
ros2 topic hz /cmd_vel
ros2 topic echo /cmd_vel --once

最初からすべての topic を理解する必要はありません。まず ros2 action list | grep -i navigate で Nav2 の目的地移動 Action が起動しているかを見ます。そのうえで、Nav2 に goal を送っている間に /plan が出ているか、/cmd_vel が継続して出ているか、実際にどんな速度指令が出ているかを確認すると、大きな流れを追えます。

Foxglove で /plan, /cmd_vel, /amcl_pose, /global_costmap/costmap, /local_costmap/costmap を同時に見ると、経路計算、速度指令、自己位置、障害物認識のどこを調整すべきかを追いやすいです。最後は topic だけでなく、Go2 実機が指令通りに動くか、止まれるかも合わせて見ます。この見方が、次の「つまずいたところ」の切り分けにもつながります。

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

Go2 実機では、Nav2 の結果だけでは巡回品質を判断しきれない場面がありました。つまずいた点は次の 3 つです。

Nav2 の「成功」と Go2 巡回として十分な状態は分けて考える

NavigateToPose の結果が SUCCEEDED の場合、Nav2 は「受け取った goal の処理は成功した」と判断しています。ただし見回り用途では、カメラで見たい方向や場所に対して Go2 が十分近い位置にいるかも確認したくなります。

そのため、巡回アプリ側でも到着誤差を確認するようにしました。ここでの到着誤差は、設定した WP と /amcl_pose で推定された現在位置の距離です。Nav2 の結果と到着誤差を分けて見ると、「Nav2 としては成功」「巡回品質としてはもう少し調整が必要」という状態を説明しやすくなります。

巡回アプリが次の WP を送るタイミングにも余裕が必要だった

巡回アプリは、WP1 の移動が終わったら WP2 の goal を Nav2 に送ります。実機では WP1 の完了直後にすぐ WP2 を送ると、次の移動が安定しない場面がありました。

原因を 1 つに断定せず、「前の goal の完了処理」「次の経路計算」「controller の状態更新」が短い時間に重なっていると考えました。そこで WP 間に短い待ち時間を入れ、次の goal を送る前に状態が落ち着くようにしています。

この待ち時間は、Go2 実機を遅くするためではありません。実機では、ソフトウェア上の完了と、Go2 の姿勢や速度が落ち着くタイミングに少し差が出ることがあります。その差を吸収するための余白です。

Go2 実機の挙動は Nav2 の設定だけでは説明できないこともあった

もう 1 つ分かりにくかったのは、巡回アプリから送った goal が、Nav2 側で期待通り扱われない場面があったことです。

この場合、最初は Nav2 の planner や controller の設定を疑いたくなります。しかし、経路計算の設定だけを見ても説明しきれない挙動でした。最終的には、開発環境と実機環境で ROS 2 の通信まわりの設定差分も確認対象に入れました。

ROS 2 では、ノード同士の通信設定や実装差分が、表からは見えにくい形で挙動に影響することがあります。Nav2 の設定だけ、Unitree SDK 側の呼び出しだけ、と範囲を狭めず、goal、topic、Go2 実機への API 呼び出しの経路を分けて確認しました。

まとめ

Go2 EDU を目的地まで移動させる機能は、Nav2 の速度指令を Unitree SDK の Move() / StopMove() 呼び出しにつなぐ構成で実現しています。

• Go2 の目的地移動は、巡回アプリ、Nav2、速度指令変換処理、Unitree SDK の役割を分けると理解しやすい
• ROS topic はデータの流れ、ROS 2 Action は依頼と結果、Planner や Controller は Nav2 の役割として見る
• WP 巡回では NavigateToPose から始め、1 点ずつ結果や到着誤差を見ながら進めた
• /cmd_vel はそのまま Go2 を動かす指令ではなく、Unitree SDK の Move() / StopMove() 呼び出しにつなぐ必要がある
• Go2 実機では、速度クランプ、無通信時停止、到着誤差、WP 間の待ち時間、通信設定差分を確認対象にした

目的地まで安全に移動するには、動的な障害物を costmap に取り込む設計も重要になります。

参考リンク

• Unitree Go2 EDU のフロア巡回機能を 9 つの機能に分解してみた
• Nav2 Documentation
• NavigateToPose
• Waypoint Follower
• NavigateThroughPoses
• unitree_ros2
• unitree_sdk2_python
• unitree_sdk2 Go2 SportClient
• ROS 2 Foxy Fitzroy