LOG46HRI2026/1/25

Unity × ROS 2 実装：Raycastを用いた全方位衝突回避システムと、進行方向判定による自動ブレーキ解除ロジック

自動ブレーキが正常に動作するようになった一方で、一度停止すると壁の前で固まってしまい、身動きが取れなくなるという課題が見つかりました。これは、「前方に壁があれば、どんな命令が来ても速度を0にする」というルールが厳格すぎたことが原因です。そこで、「前進（プラスの速度）のみを禁止し、後退（マイナスの速度）は許可する」というルールに変更し、壁際から自力で脱出できる機能を実装しました。

※薬剤搬送ロボットハコブくんは技術検証用の仮想設定です。実用化目的ではなく、制御・UIデザイン・HRI（人間とロボットの相互作用）の本質を探求するコンセプトとして定義しています。

バック走行による脱出

自動ブレーキは確実に作動していましたが、一歩も動けなくなってしまったハコブくんに、「前がダメなら後ろへ下がる」という柔軟なロジックを教え込むことにしました。

速度の「向き」を正しく解釈する

これまでは速度の絶対値（大きさ）のみを制御の対象にしていましたが、ROS 2から届く速度指令（msg.linear.x）には、前進（＋）と後退（－）の重要な情報が含まれています。この「符号」を維持したまま物理エンジンに伝えるよう、スクリプトの構造を根本から見直しました。

「前進のみ禁止」の条件分岐を導入

センサーが壁を検知した際、一律に停止させるのではなく、「今届いている命令が前進（速度 > 0）であるか」をチェックする条件式を追加しました。これにより、壁が目の前にあっても「後ろに下がる」という選択肢をハコブくんに残すことができました。

壁際からのスムーズな移動

ハコブくんを壁に向かわせるテストを行いました。壁に近づき、警告音が出て停止したところで、操作用ターミナルの k キー（後退）を叩きます。ハコブくんはセンサーの赤い線を前方に出しながらも、静かに、そしてスムーズに後退を開始しました。壁から十分に離れると、センサーは再び緑色に変わり、また自由に廊下を駆け巡る準備を整えることができました。

バック走行を可能にするコードの修正内容

HakobuMove.cs を以下のように修正しました。速度の絶対値（magnitude）ではなく、生の数値（rawLinearSpeed）を保持して判定に利用するのがポイントです。

// --- クラス変数の定義に追加 ---
private float rawLinearSpeed; 

void MoveHakobu(TwistMsg msg)
{
    // ROS 2からの速度指令をそのまま保存（正なら前進、負なら後退）
    rawLinearSpeed = (float)msg.linear.x; 
    lastAngularVelocity = -(float)msg.angular.z * Mathf.Rad2Deg;
}

void FixedUpdate()
{
    float powerMultiplier = 3.0f;
    // currentSpeedに生の指令値を代入
    float currentSpeed = rawLinearSpeed; 

    Vector3 rayStart = transform.position + transform.forward * 0.4f;
    RaycastHit hit;
    bool isObstacleAhead = Physics.Raycast(rayStart, transform.forward, out hit, sensorRange);

    if (isObstacleAhead)
    {
        if (hit.collider.gameObject != this.gameObject)
        {
            // 【重要】「前方に壁がある」かつ「前進（>0）しようとしている」時だけブレーキ
            if (currentSpeed > 0)
            {
                currentSpeed = 0;
                if (!isWarningPlayed && warningSound != null)
                {
                    audioSource.PlayOneShot(warningSound);
                    isWarningPlayed = true;
                    if (brakeUI != null) brakeUI.SetActive(true);
                }
            }
        }
    }
    else
    {
        // 障害物がなくなれば警告表示を解除
        isWarningPlayed = false;
        if (brakeUI != null) brakeUI.SetActive(false);
    }

    // currentSpeedが負（後退命令）であれば、そのまま後方へ動きます
    rb.linearVelocity = transform.forward * (currentSpeed * powerMultiplier);
    rb.angularVelocity = new Vector3(0, lastAngularVelocity * Mathf.Deg2Rad, 0);

    // デバッグ用の線をScene画面に描画
    Debug.DrawRay(rayStart, transform.forward * sensorRange, isObstacleAhead ? Color.red : Color.green);
}

動作確認の手順

実装後、以下の手順で正しく「脱出」できるかを確認しました。

Unityを再生し、i キーでハコブくんを壁に近づけて停止させます。
画面に「⚠ BRAKE」が表示され、音が鳴ることを確認します。
そのまま、操作用ターミナルの k キー（後退） を押します。
ハコブくんがバックして壁から離れ、UI表示が消えることを確認しました。

壁から離れれば、再び旋回（j や l）して新しい進路へ進むことができます。これで「動くワイヤーフレーム」としての移動・安全機能が、より実用的になりました。

ハコブくん、方位センサーの構築

バック走行ができるようになったことで、「後退中に後ろの壁に衝突してしまう」という新たな課題が見つかりました。ロボットを安全に運用するためには、全方位の安全確保が不可欠です。そこで、ハコブくんの前後左右に4本のレーザー（Raycast）を飛ばし、どの方向に進んでいても衝突を検知して停止できるシステムへアップグレードしました。

4本のレーザーによるパーソナルスペースの確保

従来のセンサーを拡張し、前後（Forward/Back）に加えて左右（Right/Left）にもRaycastを照射するようにしました。これにより、直進時だけでなく旋回中や狭い場所での安全も担保されます。それぞれのレーザーが捉えた情報は、ハコブくんの現在の進行方向とリアルタイムに照合されます。

進行方向のみを制限する条件ロジックの導入

「前方に壁があれば前進禁止」「後方に壁があれば後退禁止」という、進行方向に応じた独立した判定処理を導入しました。これにより、後ろの壁にぶつかる直前でピタリと止まりつつ、前方が空いていれば前進してスムーズに回避できるという、実用的な回避行動が可能になりました。

全方位シールドの完成

実際にテスト走行を行ったところ、後ろの壁に向かってバックすると衝突寸前で「⚠ BRAKE」と共に停止しました。その後、そのまま i キーで前進命令を送ると、問題なく壁から離れることができました。これで、2.0mの廊下という限られた空間の中でも、ハコブくんを安全に走らせることができます。

全方位センサーに対応した「HakobuMove.cs」の改修内容

既存のコードをアップデートし、後方の判定とそれに応じた条件分岐を追加しました。

void FixedUpdate()
{
    float powerMultiplier = 3.0f;
    float currentSpeed = rawLinearSpeed; 

    // センサーの開始位置（中心）を設定
    Vector3 center = transform.position;
    RaycastHit hit;

    // --- 1. 前方センサーの配置 ---
    Vector3 frontStart = center + transform.forward * 0.4f;
    bool frontObstacle = Physics.Raycast(frontStart, transform.forward, out hit, sensorRange);
    
    // --- 2. 後方センサーの配置 (新規追加) ---
    Vector3 backStart = center - transform.forward * 0.4f;
    bool backObstacle = Physics.Raycast(backStart, -transform.forward, out hit, sensorRange);

    // --- 衝突回避ロジックの更新 ---
    bool isBraking = false;

    // 前方に壁があり、かつ前進(>0)しようとしている場合のみブレーキ
    if (frontObstacle && currentSpeed > 0)
    {
        currentSpeed = 0;
        isBraking = true;
    }
    // 後方に壁があり、かつ後退(<0)しようとしている場合のみブレーキ (新規追加)
    else if (backObstacle && currentSpeed < 0)
    {
        currentSpeed = 0;
        isBraking = true;
    }

    // ブレーキ時の演出処理（警告音・UI表示）
    if (isBraking)
    {
        if (!isWarningPlayed && warningSound != null)
        {
            audioSource.PlayOneShot(warningSound);
            isWarningPlayed = true;
            if (brakeUI != null) brakeUI.SetActive(true);
        }
    }
    else
    {
        isWarningPlayed = false;
        if (brakeUI != null) brakeUI.SetActive(false);
    }

    // 物理移動の適用（バック時はマイナスの値が渡されます）
    rb.linearVelocity = transform.forward * (currentSpeed * powerMultiplier);
    rb.angularVelocity = new Vector3(0, lastAngularVelocity * Mathf.Deg2Rad, 0);

    // Scene画面での視覚化（デバッグ用に4方向の線を描画）
    Debug.DrawRay(frontStart, transform.forward * sensorRange, frontObstacle ? Color.red : Color.green);
    Debug.DrawRay(backStart, -transform.forward * sensorRange, backObstacle ? Color.red : Color.green);
    
    // 左右のセンサーも視覚化（緑色の線でガイドを表示）
    Debug.DrawRay(center + transform.right * 0.4f, transform.right * 0.5f, Color.green);
    Debug.DrawRay(center - transform.right * 0.4f, -transform.right * 0.5f, Color.green);
}

作業のポイントと確認結果

判定の構造: 前進と後退の判定を個別に分けることで、どちらか一方が塞がっていても反対方向への移動は妨げないように工夫しました。
Sceneビューでのデバッグ: 再生中にSceneビューを確認し、ハコブくんの前後から伸びるレーザーが、壁との距離に応じて正しく「緑」から「赤」に変化することを確認しました。