WORKS 開発事例

ロボット開発 2026.09.25

【ヒューマノイド Unitree G1】通信途絶時にホームへ自律帰還する機能を開発【ROS2・Nav2による実機検証】

(2026年8月18日時点の記事です)

はじめに

フィジカルAIロボットの現場課題の１つに、「通信状態が途切れても安全帰還する」という要件があります。
例えば、プラント施設の場合、Wi-Fiが途切れてロボットが誤作動した場合、周囲の機器を破損するリスクがあります。
廃炉作業の現場においては、万が一、遠隔ロボットが通信途絶状態になった場合、機体を回収するために、人が被ばくリスクを負うことになります。
「自律帰還機能」は、現場導入にあたり優先的な課題の1つです。

クフウシヤでは、ヒューマノイド「Unitree G1」に、ネットワーク切断を検知したら自律的にホームポジションへ帰還する機能を開発しています。
本記事では、SLAMの精度改善から自律移動の実装、そして帰還動作の実現までの様子をご紹介します。

1. 自己位置推定の安定化

１－１. ロボットモデルの修正

まず初めに、SLAMと自己位置推定の精度が思うように出ないという問題がありました。
RVizの点群を確認したところ、実機では足裏が確かに地面に接地しているにもかかわらず、RViz上では足裏と地面の点群が平行になっていないことに気が付きました。

これは、ロボットモデル上のTF（各リンクの位置・姿勢を表す座標変換）が、実機の状態とずれていたということです。
LiDARから得られた点群を正しい位置・姿勢で扱うには、実機上のLiDAR位置とロボットモデル上のTFが一致している必要があります。
このずれがあると、点群自体が正しくても地図生成や自己位置推定に誤差が生じます。

使用しているG1の機体バージョンに対応したURDFを使用していましたが、実機と比較すると特に頭部周辺の形状やLiDARの取付位置・角度に差があることが分かりました。
G1の機体のヘッドにはLiDARのMID-360が搭載されているため、この差がセンサフレームのずれとして効いていました。

そこで、実機の見た目に合わせてMID-360のフレーム位置・角度を目視で調整しました。
その結果、修正前に見られていた自己位置推定の不安定さが軽減され、より安定して動作することを確認しました。

１－２. LiDARの視野設計

当初は、MID-360の後方90度を見ないように設定していました。
理由は、吊り下げ治具や、背後に立つオペレータを点群として拾うことを避けるためです。
しかし、運用してみると、後方の視界を切り落とすことで環境から得られる特徴量が不足し、自己位置推定が不安定になっていました。
特に、壁や床など似た形状が連続する廊下では、自己位置を拘束するための幾何的な情報が少なくなり、この傾向が顕著でした。
360度すべてを見るように変更して検証した結果は次のとおりです。

■ 吊り下げ治具で吊った状態：自己位置を見失わない
■ 歩行させた場合：すぐ後ろに人が立つと視界が狭くなり見失うことがあるが、人が離れていれば安定して保持できる

視野を絞って不要な点群を減らすか、広げて特徴量を稼ぐか、環境に応じた見極めを行いました。

２. 自律移動のためのG1歩行制御機能の実装

自律移動のオープンソースパッケージであるNav2から出力される並進・旋回速度指令を受け取り、G1の歩行コマンドへ変換して機体を動かすための制御ノードを実装しました。実装にあたっては、Unitreeが公開しているROS 2のコードを改良しています。

■ loco_controller：
Unitreeの既存実装はMultiThreadedExecutorでの利用を前提とした構成でした。
そのまま利用すると複数のコールバック間で排他制御を考慮する必要があるため、今回のシステムではSingleThreadedExecutor上でも扱えるよう制御処理を見直しました。

■ loco_client：
既存実装ではメッセージ受信のたびにSubscriptionの生成・待機処理が発生しており、速度指令の反映に遅延が生じていました。そこで、必要なSubscriptionを初期化時に生成し、再利用する構成へ変更しました。

■ 旋回速度：
G1では、小さすぎる速度指令を与えても実際には歩行・旋回を開始しない領域があることが分かりました。
そこで、実機を用いて並進・旋回それぞれについて動作可能な最低速度を確認しました。この値は、後述するNav2の速度指令補正にも利用しています。

３. 自律移動の安定化

navigation2による自律移動に進むと、目標地点にたどり着けず、途中で止まってしまう問題が起きました。
対策として、①処理負荷の低減、②TF更新の安定化、③最低速度を考慮した速度指令補正を行いました。

３－１. 処理負荷の低減

ロボットが途中停止する際、Nav2実行中は計算負荷が高まり、TFの更新・参照時刻の差が大きくなるケースが確認されました。
対策として、経路追従に用いるMPPI（Model Predictive Path Integral：多数の制御候補をサンプリングして評価し最適な指令を選ぶ手法）のサンプリング数を減らして処理負荷を下げ、TFの時刻許容差 transform_tolerance を大きめに設定しました。

３－２. TF更新の安定化

lidar_localization_ros2 では、自己位置推定結果を受信したタイミングでのみTFを配信していました。
そのため、推定結果の更新間隔が一時的に長くなると、その間TFも更新されず、Nav2が必要とする時刻のTFを取得できない場合がありました。
対策として、lidar_localization_ros2 を最新版に更新し、タイマー関数でTFをパブリッシュするようパラメータを調整（その他のパラメータも調整）しました。
ROS 2のナビゲーションは、「この時刻のロボットはこの位置にいた」という時系列のTFを前提に動きます。
センサデータの時刻に対応するTFが存在しない、あるいは古すぎれば、変換に失敗して制御が破綻する恐れがあります。

３－３．最低速度を考慮した速度指令補正

Nav2から出力される速度指令がG1の最低動作速度を下回ると、指令は出ているものの機体が動かず、スタックするケースがありました。
対策として、一定以上の速度指令が出ている場合は、G1が実際に動作可能な最低速度を下回らないよう補正する処理を追加しました。
前章で測定した最低動作速度を使用し、自律移動検証時に微調整しています。

４. 通信途絶検知と自律帰還機能の実装

自律移動が安定して行えるようになったため、最後に遠隔PCとの通信途絶を検知して自律帰還へ移行する機能を実装しました。

４－１. ネットワーク構成

自律帰還の検証では、G1背面に外付けルータを搭載し、Jetson PCなどG1内部の機器と遠隔操作PCを同一ネットワークに接続しています。

４－２．　通信途絶検知と自律帰還機能について

G1側では遠隔PCとの通信状態を監視し、一定条件で通信途絶と判断すると、現在実行中のナビゲーションをキャンセルします。
その後、音声で通信途絶と帰還開始を通知し、あらかじめ設定したホームポジションをNav2へ目標地点として与えることで、自律的に帰還します。

ただし、RVizの画面だけでは切断が検出されたことが外から分かりにくいため、G1に音声で状況を知らせる機能を実装しました。実際の動作は次のとおりです。

１．自律移動を開始
２．移動中にネットワークを切断
３．切断を検出したロボットが停止
４．「ネットワークが切断されたのでホームに戻ります」と英語で発話
５．ホームポジションまで自律移動