2024年度CRANE-X7開発研修
CRANE-X7M5StackRealsenseROS 2ROS2研修

2024年度CRANE-X7 開発研修　RealSense による物体検出・最終調整・まとめ（山本）

2024年度CRANE-X7開発研修

こんにちは、山本です。

現在、新人研修の一環として、チーム開発研修を行っています。この新人研修のリレーブログは３回目を迎え、いよいよ最終週となりました。

2024年度CRANE-X7開発研修

2024年新卒の研修ブログです

rt-net.jp

今回の内容は、私が開発を担当している、RealSense を用いた「箱」の検出と、前回制作した M5Stack のアップデート、そして本研修のまとめです。

RealSense を用いた物体検出の実装

概要

RealSense D435 には「ステレオの RGB カメラ」と「深度カメラ」が備わっています。アールティの CRANE-X7 のリポジトリでは、「色」、深度カメラを利用した「点群」、「ArUcoマーカー」を利用した、物体把持のサンプルが提供されています。

本研修の最初の方は、立方体に ArUco マーカーを貼り付けて、テストを行っていました。

クアンさんの記事から引用

ですが、四角錐の上面には ArUco マーカーを貼り付けることは出来ません。また、斜めから物体を見ると、物体の奥行きや、姿勢の推定が難しいです。

そこで、物体の検出時の姿勢の変更を行いました。物体の位置や姿勢を正しく認識するため、フィールドの上から見降ろすような姿勢にしました。

物体検出アルゴリズム

続いて、物体検出アルゴリズムです。以下の2つの方法を採用し、切り替えて使えるようにしました。

  1. RGBカメラを用いて、黒い物体を抽出する
  2. 深度画像を用いて、高さのある物体を抽出する

深度画像の処理は、アールティのサンプルとほとんど同じなので割愛し、RGBカメラの方について少し述べたいと思います。と言っても、こちらもアールティのサンプルをベースにしていたりします。

処理の流れとしては以下のとおりです。

  1. 画像を HSV に変換し、物体の色（今回は黒）を抽出する
    • カメラで特定の色の物体を検出するときは、RGB より HSV のほうが人間の感覚に近いため、狙った結果を出しやすいとされています。
  2. OpenCV を用いて、四角形の領域を検出する：参考にしたコード
    • OpenCV の画像から四角形を検出するサンプルを参考にしました。
      これで、画像中の四角形の角の座標を取得することができました。
  3. 四角の領域の四隅の座標を取得し、中心座標を計算する
    • 先ほどの四角形の角の座標を用いて、中心座標を計算します。
      本来は、複数の四角形を検出する場合がありますが、今回の研修ではフィールド上に箱は１つしか存在しないという前提があるため、この処理で成立します。
  4. 中心座標をカメラから見た座標系に変換する
    • 深度画像を利用して、カメラから見た物体の位置を推定します。

これらの処理で、カメラから見た箱の位置を推定することができました。ですが、これだけではロボットが箱をつかむには情報が不十分です。そこで、ロボットの原点座標からの位置に変換しています。

物体の回転検出アルゴリズム

また、掴むためには、物体の回転方向も必要になります。そこで、画像処理で物体の姿勢を推定しています。

まず、前節のRGB カメラの画像処理（1,2）と同様の処理を行い、四角形の四隅の座標を取得します。その後、隣接する2点の座標から傾きを計算します。更に、つかみやすくするために、±45度 に収めるという制約もつけます。コードにすると、以下のようになりました。

// squares は四角形のオブジェクトの 四隅の点を格納する配列

double theta =
          M_PI / 4 - std::fmod(std::atan2(squares[1].y - squares[0].y,
                                          squares[1].x - squares[0].x) +
                                   2.25 * M_PI,
                               M_PI / 2);

ここまでで実際に動いている様子は、こんな感じです。左の入力された画像に対し、中央の画像は「傾きを検出し、表示」、右の画像は「物体の位置を検出し、表示」をそれぞれ行っています。結構きれいに抽出できたかと思います。

物体の位置情報の送信

これらの座標・回転情報をまとめて transformations（tf）という形式で送信します。tf を利用することで、ロボットは物体の位置を認識できるようになります。先程取得した、物体の位置と回転角度を合成し、tf を得ます。ノード構造は以下の図のようになりました。

flowchart LR
    Rotation[傾きを検出]
    subgraph strategy[検出アルゴリズム]
    	direction LR
    	RGBカメラで検出
    	深度カメラで検出
    end
    ObjectDetection[座標と回転の情報を統合]
    Robot
    Rotation -- RPY（回転） --> ObjectDetection
    strategy -- XYZ（座標） --> ObjectDetection
    ObjectDetection -- tf（transformations）で出力 --> Robot

そして、これらの動作を確認します。左が現実の、右が RViz での画像です。位置と回転を正しく認識できていることがわかりました。

M5Stack の改良

電池残量の通知

前回のブログでチラ見せした、バッテリーのステータス表示機能です。

写真左から、「正常」「バッテリが少ない」「充電中」に合わせて、バーの色が変わります。

また、バッテリーが少なくなると、ブザーで通知する機能をもついています。（この動画は音が出ます）

M5Unified には、M5Stack のバッテリの状況を取得する機能が存在します。これを、FreeRTOS のタスクとして動作させ、バッテリーを監視しています。

void t_BatteryNotify(void * pvParameters)
{
  while (true) {
    M5.Speaker.tone(1000, 1000);
    vTaskDelay(1000 / portTICK_PERIOD_MS);
    M5.Speaker.stop();
    vTaskDelay(1000 / portTICK_PERIOD_MS);
  }
  vTaskDelete(NULL);
}

void t_BatteryChecker(void * pvParameters)
{
  battery_level_t battery_level = 0;
  bool charge_required = false;

  while (true) {
    if (battery_level != M5.Power.getBatteryLevel()) {
      battery_level = M5.Power.getBatteryLevel();
      xQueueSend(battery_queue, &battery_level, 0);
    }
    if (battery_level < BATTERY_WARNING_LEVEL) {
      if (!charge_required) {
        xTaskCreatePinnedToCore(
          t_BatteryNotify, "battery_notify_task", 4096, NULL, 10, &batteryNotifyTaskHandle, 1);
        charge_required = true;
      }
    } else {
      if (charge_required) {
        vTaskDelete(batteryNotifyTaskHandle);
        charge_required = false;
      }
    }

    vTaskDelay(1);
  }
}

激しい運動の通知

「箱」の評価をどのようにするか話し合った結果、箱にかかった加速度を利用するという結論に至りました。以下、加速度を評価する仕組みについて説明します。

IMU で取得できる値は、XYZ 方向の加速度と RPY 方向の”角速度”です。そう、RPY 方向は角加速度ではありません。そこで、角速度から角加速度を求めていきます。

時刻 \(t\) から 時刻 \(\Delta t\) に、角速度が \(\Delta \omega\) 変化したときの角加速度 \(a\) は、式

\begin{align}
a = \frac{\Delta t}{\Delta \omega}
\end{align}

で求められます。

これで角加速度を求めることができましたが、どうやらノイズが乗ってしまうようです。そこで、ローパスフィルタを適用して、ノイズを除去します。実装はリアルタイム処理に適した、一次遅れ系です。

まず、カットオフ周波数 \(f\) を用いて、フィルタの時定数 \(\tau\) は、

\begin{align}
\tau = \frac{1}{2 \pi f}
\end{align}

定義できます。そして、時刻 \(t\) について、サンプリング周波数を \(T\)、前回の角加速度を \(\omega_{t-1}\)、入力値を \(x\)とそれぞれ定義すると、現在の角加速度 \(\omega_{t}\) は

\begin{align}
\alpha &= \frac{\tau}{T+\tau} \\
\omega_{t} &= \alpha \cdot \omega_{t-1}+(1-\alpha) \cdot x
\end{align}

で求められます。

これで直線方向と回転方向の加速度が揃いました。これらの加速度がしきい値を越えた際に、通知音を鳴らすようにしました。（この動画は音が出ます）

タスクベースでの管理

これらの処理は一定の時間ごとに動作させたり、状態によってはスキップしたりなど、複雑な起動条件が必要になります。例えば、バッテリーが少ないときは IMU の処理を停止する、等です。

そこで、ESP32 のデュアルコアと FreeRTOS を使って、これら複数のタスクと状態を管理しました。概ね、以下のような構成です。

sequenceDiagram
    box Core0
    participant app_main as main
    participant button as button
    participant display
    end

    box Core1
    participant t_battery as Batteryタスク
    participant t_imu as IMUタスク
    end

    app_main ->> t_battery: start
    app_main ->> t_imu: start

    loop main loop
        button ->> app_main: ボタンからの入力
        t_battery ->> app_main: バッテリーの情報
        t_imu ->> app_main: IMU の情報
        app_main ->> display: 画面に表示する情報
    end

これにより、状態管理や利用リソースの把握が行いやすくなり、拡張性も高まりました。

発表会

この研修のゴールを覚えていますか？

開発したシステムを先輩社員に「納品」したのち「評価」が行われます。つまり、動かすのは先輩社員であって私たちではありません。すなわち、納品時にシステムと一緒に「取扱説明書」的な資料が必要になります。

2024年度CRANE-X7開発研修　チーム開発研修について（山本）

そう、作って終わりではありません。

本研修の最終日、発表会が行われました。開発物とドキュメント一式を評価担当の先輩社員に渡し、動かしてもらいました。

結果として、当初想定した動作をさせることが出来ました。ですが、課題の残る結果となりました。

まず、CRANE-X7 や M5Stack に想定していないトラブルが発生しました。CRANE-X7 は直前に調整のため酷使したところ、一部ねじが緩んで動作不良を起こしていました。M5Stack では会場のネットワークとの相性が悪く、M5Stack から mros2 の Topic 送信に失敗し、フリーズしてしまいました。

また、説明書に不備やわかりにくい点があり、作業者に手間をかけさせたりしました。例えば、順序だてが良くなく、説明が前後したり、トラブルでさかのぼって対処する手順が残っており、何度もやり直してもらいました。

これらの経験から、様々な環境下でテストを実施したり、開発者以外の方にチェックを依頼することが大切だと感じました。

まとめ

今回は、RealSense を用いた「箱」の検出と、M5Stack の更新内容、研修のまとめについて書きました。

本研修を通じて、ROS 2 の使い方や画像認識、ESP32・FreeRTOS を利用した処理の実装などについて詳しくなりました。また、アールティでの開発方法などについて学び、取り入れることができました。

そして、ドキュメントの作成や本番環境でのトラブルの対処など、学生の間意識しなかった箇所について、今後学んで行きたいと思いました。

そして、同期のメンバーの技術や考え方などを知り、深める、とても良い時間になりました。

2024年度CRANE-X7開発研修

2024年新卒の研修ブログです

rt-net.jp
タイトルとURLをコピーしました