この記事はrogy Advent Calender 2020 14日目の記事です。

こんにちは、15 の nomo ( @nomotech )です。

この記事は昨日 Loop 君が書いてくれた 【Gano剣】第11回 ROBO-剣(アーム型) 参加報告 – 機構編 – の続きです。
今回は画像処理について解説していきます。

はじめに

私は動きの速いものに対する画像処理は高速である必要があると思っています。しかし大抵のロボコンではロボットは高速に動くのに対して画像処理の高速性にはあまり注目されていないような気がします。
そこで私はこのROBO剣で高速に画像処理をして相手の技をすべて見切ってやろうと考えていました。
が、残念ながら今年のROBO-剣はオンライン開催になり、ロボット同士の剣道の試合ではなく制限時間内にダミーロボットにどれだけ技を決めることができたかを競うルールになってしまいました。

残念ながら認識する相手は動かないダミーロボットですが、来年のことも見据えて高速なリアルタイム画像処理を実装しました。

概要

高速カメラ1台を用いて面と小手の3次元位置を100fpsで推定していきます。

今回使用したカメラは basler acA1440-220uc です。
解像度は1440×1080で100fpsで使用しました

処理はPC上で python で行い、ロボットの制御とは別プロセスで行いました。画像処理の結果はROS topicで制御プロセスに送信します。

画像の撮像、面の推定、小手の推定、制御との通信をそれぞれ別のスレッドで処理をすることによって高速に実行することができます。処理のスループットは100fps、レイテンシは撮像に10ms 画像認識に10ms の合計 20ms です。

キャリブレーション

画像処理をするときにはまず初めにカメラのキャリブレーションが必要です。
カメラキャリブレーションとは、カメラの内部パラメータ、外部パラメータなどを調べる作業です。内部パラメータはカメラの焦点位置や画像中心を表すパラメータであり、外部パラメータはカメラの位置・方向を表しています。

内部パラメータの推定についてはggるとたくさん出てくるので端折ります。
外部パラメータの推定はチェスボードを用いて行います。
下の画像のように、あらかじめ大きさが分かっている場合、PNP問題 (Perspective N Point Problem) によってチェスボードとカメラの相対位置を計算することができます。
フィールドの基準にチェスボードを置くことで、フィールド座標系でのカメラの位置と姿勢を推定することができます。

このキャリブレーションは試合前に一度行い、カメラを動かさない限り再度する必要はありません。

物体の検知

いよいよダミーロボットの画像認識をしていきます。
ダミーロボットは以下のようなものです。面が赤、胴が青、小手が黄色の色で、色も寸法もルールで決まっています。
そこで、色による検知で面や小手を検知していきます。

ここでは小手の検知で説明していきます。

初めに入力画像をRGBからHSVに変換し、色相(H)でフィルタリングをしてマスク画像を作成します。
黄色の場合は色相(H)が 50 ~ 65 くらいでフィルタリングするときれいに抽出できました。
その後、抽出した領域の重心を求めます。

下の真ん中の画像がマスク画像で、黄色い領域を識別できていることがわかります。また、下の右の画像は黄色い部分の重心のUV座標を表示したものです。小手が検知できていることがわかります。
この段階ではまだ画面内の位置なので2次元の情報です、また自ロボットの小手と区別ができていません。
(画像処理やCGの分野では画像上のxy座標のことをuv座標と呼ぶことがあります、3次元座標と混乱しないように2次元の座標はuv座標で表します)

面についても同様に、色相(H) 0 ~ 30 くらいでフィルタリングすればきれいに取れました。

3次元推定

次は、画面上での重心の座標(u, v)から3次元座標(x, y, z)を推定していきます。
どうして2次元座標から3次元の情報を推定できるのか？と思うでしょうが、今回の大会ではダミーロボットの面と小手の高さ z が変わらないため、実は x, y だけを推測すればいいのです。

カメラの位置の向き、画角などがわかっていれば、以下の図のように3次元上のある直線上に存在することがわかります。
この直線と z = (面の高さ) の平面との交点を求めれば面の3次元座標を得ることができます。

ここで活躍するのが透視投影変換です。
透視投影変換は以下の式で表されます。

\begin{aligned} s\left(\begin{matrix}u\\v\\1\\1/s\end{matrix}\right) &= \left(\begin{matrix} f_x & 0 & c_x & 0\\ 0 & f_y & c_y & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}\right) \left(\begin{matrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \\ 0 & 0 & 0 & 1 \end{matrix}\right) \left(\begin{matrix}x\\y\\z\\1 \end{matrix}\right) \\\\ s{m^{\prime}} &= A\left(\begin{matrix} R & t \\ 0 & 1 \end{matrix}\right){X^{\prime}} \\\\ \Leftrightarrow X^{\prime} &= s \left(\begin{matrix} R & t \\ 0 & 1 \end{matrix}\right)^{-1}A^{-1} m^{\prime} \end{aligned}

ここで A は前述したカメラの内部パラメータであり、 (R|t) はカメラの外部パラメータです。
計算しやすいように 4\times4 の正方行列にしてあります。

この式を解いて、z座標が面の高さである490mmになるようにsの値を決定してあげれば面の3次元座標 X^{\prime} が求まります。

以下の図は面のuv座標から3次元座標を計算したものです。
小手も同様に高さ360mmの高速条件で計算すれば求まります。

敵味方識別

実際に試合をしてみると、下の図のようにカメラにはダミーロボットだけでなく、自分のロボットも映り込んでしまいます。
色で面、小手を検出しているので、自ロボットが映り込んでしまった場合どちらが敵なのかがわからなくなります。

ここで、自ロボットは各関節についたエンコーダによって位置姿勢がわかっているため、この情報を使ってどちらがダミーロボットなのかを判別します。
この時に活躍するのがまたも透視投影変換です。

先ほどは2次元を3次元に変換しましたが、今度は3次元を2次元に変換します。
ロボットの姿勢から求めた自ロボットの面と小手の位置を透視投影変換し、カメラ画像ではどこに映るはずなのかを計算します。
そしてこの計算されたuv座標に近い場合にはダミーロボットの面や小手ではないと判定します。

わかりにくいですが、下の動画で紫の点で示したのが推測された自ロボットの面と小手のuv座標です。
自ロボットを映り込ませても識別できていることがわかります。

以上の処理によって100fpsでダミーロボットの位置と姿勢を計算することができました。

最後に

実は実装は大会本番の1週間前からはじめ、前日に完成したので制御のfirefly君は試合練習がほとんどできなかったと思います。
迷惑かけて申し訳ない。
まにあってよかったです。

今回は100fpsで画像処理をしましたが、まだまだ高速化をすることができます。
今年はオンライン開催で相手が動かないダミーロボットでしたが、来年はロボット同士の試合ができる信じています。
超高速な画像処理で相手の竹刀の軌道を完璧に見切って、避けたりいなしたりできたら面白いと思っています。
最後まで読んでいただきありがとうございました。

明日はfirefly君による制御編の記事です。
私の画像処理の結果が実際に使われている様子も紹介されると思います。
ぜひご覧ください。