ごきげんよう。あってぃ(@paste_link)です。大学ロボコンやってる人です。
さて大学ロボコンなのですが、今年のルールは”ロボミントン”というものでした。ルールは非常に非情にシンプルで、
「各チームが2台のロボットを持ち込み、バドミントンのダブルスの試合をしろ」
という無茶振りルールでした。
尚、ロボット2台は両方手動でも自動でも構いません。今まで毎年必ず自動機があった大学ロボコンにしては珍しいですよね。
さて、このロボミントンというやつ、ルール自体は みょー↑にシンプルですが、簡単かといわれるとそうでもなく、一言で表すと
研究でやれ！！！！！！！！！
って感じでした。
バドミントンを行うロボットについての先行研究はルール発表時にすでに存在していたのですが、(動画: https://www.youtube.com/watch?v=LSax71cn6A4)
この研究の論文(http://www.fmtc.be/downloads/Modeling&Control/BadmintonRobot-MultidisciplinaryTestCase.pdf)の1ページ目の右下あたりの文章を引用・TOEIC500点代前半という圧倒的英語力にて和訳させていただきます。
“The mechanical design of the robot was challenging due to the extremely high acceleration and velocity requirements. The current configuration is the initial design of a multiyear project. Several alternative mechanical configurations were analyzed during the concept phase. This included wheeled robot with omni-wheels and wheel-chair type robots. Some of them were rejected, some of them may be implemented later. The main reason for discarding such options were the very high accelerations and velocities required in the badminton play, which are impossible to achieve in the wheeled robots due to the physical limitations on the wheel traction.”
――最初はタイヤを使ったバドミントンロボットという案が出たが、それは却下された。タイヤを使ったロボットで、バドミントンで要求されるような高加速度・高速度を実現することは”Impossible”だからだ(読めない単語多いけど多分こんなん)。
impossible
音節im • pos • si • ble発音impɑ'səbl | -pɔ's-
impossibles (複数形)
[形]
1 〈物・事が〉不可能な，無理な
2 〈物・事が〉とてもありえない，信じがたい
3 ((略式))〈人・物・状況などが〉耐えられない，がまんできない，どうしようもない，手に負えない；いやな
━━[名]((the ～))不可能（なこと）
というわけでロボミントンは、いんぽっしぶる？ですよろしく。
ところで、うちのチームはルール発表から1か月くらいの間、自動化はつらそうだから手動機で行こう！という方針で行こうと思っていました。
さて、下の画像はぼくがロボミントンのシミュレートをするために今年の大会のフィールドの設計図を正確に再現したものです。
コートにある白い大きな円柱形が今年のウチのマシン(の大体のサイズ)、そして円柱形の上に微妙に浮いている謎の物体がラケットの当たり判定の大きさです。
人はフィールドの緑色・黄色の部分には入れません。フィールド外の灰色のエリアから操縦します。
というわけで視点を灰色エリアの後ろ側からで、どんな感じになるのか、実際に試してみることにしました。尚、視点の高さは我らがリーダーにして操縦者、みかん(@komepann)くんの身長から設定したので少し低く感じるかもしれませんがご了承ください。あと地味ですがこれもご了承ください。
※シャトルの軌跡は実際に計測したものを利用しているので速度や軌跡の形状は割と正確です。
……お分かりいただけたでしょうか。
ロボミントンとは、
- 奥行き方向がほっとんどわからない地点から
- 浮遊時間が1秒強くらいしかないシャトルに向かって
- 慣性が働き急加速・急停止が難しいタイヤ駆動ロボットを
- 数センチ以内の精度で正確に移動させる
そんな夢とロマンがいっぱいに詰まった全アジア圏を震撼させるサイバーパンクネオロボットスポーツなのだ！！実際難しい！急ブレーキでマシンが激しく上下に動く！！！ほとんど違法行為！！
と、ここまでを踏まえてです。
ぼくたちのチームは練習場所は1～2週間に1度、3時間程度体育館が取れるだけでした。他の練習場所を探すも天井の高さは3.5m程度しかなく、レシーブを行っても天井に当たる始末。しかも肝心の二次ビデオ審査2週間前になると卒業式・入学式でその体育館も使うことができなくなってしまいます。
恐らく練習時間・操縦技術で他校に勝つことはできないでしょう。
そこでぼくらが勝つために手を出す手段と言ったらもう……
というわけで改めてこんにちは。東工大ロボット技研、NHK学生ロボコンチームMaquinista制御班長、あってぃ(@paste_link)です。学生ロボコン2015、「ロボミントン」ではシャトル認識・軌道予測システム担当と、試合時での自動機のコントローラ握る担当でした。ぼくの今年における仕事は、ロボットには脇目もふれず軌道予測を完成させることでした。
さて、結果から言ってしまうと、ぼくたちMaquinistaは二次ビデオ審査で落選しました。
そして先日、5/5に東京農工大学さん、電気通信大学さん、横浜国立大学さんとの交流試合を終え、ぼくたちのロボミントンは終了しました。
そのため、一足先に行ってきたことのアウトプットを行っておきたいと思います。
ここから話していく内容は恐らく長くて本当につまらない遺言です。また、実用的かも怪しい上に恐らく大学ロボコンで同様のことをさせられる日は来ないでしょうので、価値は低いと思います。また、研究でもなんでもないので既存手法との精度比較の類を真っ当に行っていないため、標準偏差が〇〇パーセント改善された、などの具体的数値を持ち出した説明ができません。それに正直なところ今からそれらの比較を行うモチベーションもありません。論理的妥当性などについてはそこそこは検証したつもりですがツッコミどころは多いかもしれません。以上の点、ご了承ください。
以下、読み飛ばしを推奨する部分は灰水色の蛍光を行います。全部読んでもアレなので適宜読み飛ばしてください。この記事の最後に動画置いてあるのでそこだけ見れば十分かもしれません。
コンセプト
ぼくが携わったマシンは下のマシンです(写真は3月中旬くらいのもの。最近写真撮ってませんでした。。。)。正式名称は「黒鷹(コクヨウ)」でしたが通称は「黒」だったり色々でした。適当に呼んでやってください。
コンセプトは『低重心・高速度・高精度な自動機』でした。
今年はルールとしてフィールド外にカメラを設置することが許可されていたため
フィールド外に設置したカメラが落下地点を演算→自動ロボットが地点まで移動
というシンプルな流れを目標にしました。
また、実際の大会ではロボット・そしてカメラのセッティングはすべて1分以内に終えなければなりません。そのため、フィールド外にカメラを置くとしても、正確に位置合わせを行わなくてもよいようにするため、以下のようなシステムを選択しました。
カメラ台は高さ2m程度、そして「目印」としての半径200mm、300mmの円柱形を2つが固定された1つのモジュールです。この台をフィールド外のテキトーな場所に置くことで、ロボットはシャトルの落下地点に自動で移動することができるようになります。設置精度を一切要求していないのがポイントです。
カメラがロボットに送るシャトルの落下地点の座標系は「カメラ台からの相対座標」とし、ロボットはカメラ台に取り付けられた目印を測域センサで観測、与えられた相対座標に移動します。
このシステムのメリットとしては、NHK側が用意したフィールドのものを一切目印にしないため、大会会場でも普段の練習でも同じコンディションで行うことができることが挙げられます。
デメリットとしては測域センサが観測できる目印の点数がさして多くないため、得られる座標の分散が大きくなることが危惧されたことです。結果としては測域センサ担当者やロボットの自動走行担当者の努力もあり、今回の使用の範囲内では問題にはなりませんでした。
シャトル認識デバイス
・シャトル認識デバイスの選定
シャトル認識に用いるデバイスとして、2014年夏に発売された、マイクロソフト社のKinectV2を採用しました。
他にもXtion、初代Kinectなどの選択肢はありましたが、KinectV2はIRレーザの出力がUSBバスからではなく外部電源から供給される(らしい)ためより外乱性能が高いこと、そして他校が同様にKinectなどのIRアクティブカメラを用いた際にもパルス変調ToFを用いている(らしい)ため干渉への対策も十分になされていたこと、一般にIRアクティブカメラは黒い物体を認識するのが苦手な一方で白い物体の認識が得意であり、白色の飛翔体たるシャトルを認識する必要のあるロボミントンに適していたこと、視野角としてもぼくたちが手に入る値段の距離カメラの中ではもっとも広かったことなどが挙げられます。
他の選択肢として高フレームレートを出すことができ、より遠距離の観測が理論上可能であるステレオカメラなどを考えましたが、大会会場は背景が不確定である上にステレオマッチング・キャリブレーションを行うソフト的・ハード的な負担、そして資金的な事情なども考慮して却下としました。
以下の写真は同じ場所に設置した初代KinectとKinectV2で天井から吊るされたシャトルを観測したものです。
↑部屋にシャトルを吊るし、
↑Kinect、KinectV2で観測します。
↑初代Kinect
↑KinectV2
早い話、KinectV2の方が認識精度が良かったです。初代Kinectは4m先のシャトルを観測するのが限界でしたが、KinectV2は6m先のシャトルの認識が可能でした(シャトルはどちらも静止している物で観測を行いました)。
KinectV2が6m先のシャトルを認識した場合の、このカメラ台でシャトルを認識できる範囲は理想的に下図のようになります(フィールドを真上から見た図です。四角形の板がフィールド全体、四角錐ふたつの和集合が2つのKinectV2を合わせた認識範囲に該当します)。
視野の範囲からして相手のサービスなどもある程度は観測できることがロボミントンやってる人には分かっていただけるかもしれません。
・KinectV2の誤差について
次に、KinectV2の距離における分散特性の実験的計測を行いました。
単純に部屋の端にKinectV2を置き、視界中心付近に置いた物体を100mmおきに1000点ずつ観測し、分散・分布を計測しました。
それによって得られた分布は以下のようになりました。
目盛り振ってなくてごめんなさい。。。
1000～6000の軸がKinectV2からの距離(mm)
100～300の軸は1000点観測した中の点数(個)
そして-20～20の軸は平均値からの誤差(mm)でした。
この分布を真上から見たものが以下になります。
また、KinectV2からの距離と分散の関係は以下のようになりました。
結果としては視界中心付近にある限り、
①正規分布に十分近似できる分布が得られる
②距離に拘わらず誤差は非常に小さく、標準偏差σは悪くても5mm程度である
ことが分かりました。6m先の物体を標準偏差5mm以下の精度で観測というのはあまりにも革新的なセンサーですよね。しかも20k程度で購買することができます。
さて、今の計測では「視界中心における」分布を見ましたが、次は距離ではなく、視界のどのあたりを見ているかでどの程度 分散特性が変化を調べるために、KinectV2には3.2mほど離れた壁をただただ眺めてもらったのですが……
ということで今のは見なかったことにしました。
以下、KinectV2は、見ている点(x,y,z)に依らず、i.i.dな正規分布N(0,σ²)のノイズが乗るセンサであると仮定します。オフセットについてはKinect表面から15mm程度内側の点から見ているという方針にしました。また、任意の点についてi.i.d.かつ正規分布と仮定したのは前述のグラフの結果に対する近似というニュアンスのほか、今後の過程で最尤推定を行う都合などがあります。標準偏差は具体的な値を設定する必要が最後までなかったです。
・KinectV2の歪み補正について
下の画像はKinectV2で黒板を見たときの距離画像です。白線はエッジです。
このようにKinectV2から得られる画像は視野の中心から離れるほど歪む傾向がありました。そのため、これに無理矢理補正を掛けました。
これは非線形なタイプの歪みなので、中心ピクセルからの距離の二乗、r²についての関数(a+b*r²+c* r⁴+d*r⁶|a,b,c,dは定数)で強引に補正を掛けました。この補正によって点群を3次元に変換する際の誤差を少しマシにできる気がしたのですが、具体的にどの程度マシになったのかまでは調べていません。しかし、視界に映る直線は曲がることなく座標変換できるようになりました。
シャトル認識
・シャトルの認識アルゴリズム
シャトル認識自体は非常にテキトーに実装しました。どれくらいテキトーかといいますと、昔作りました「部室人多いbot」のソースをコピペして一部の定数を少し書き換えて「Kinectの視界に映る半径70mmの球体よりも小さいくらいのサイズの物体を抽出ーー！！」みたいなノリで実装したら完了しましたし、人などが写っていてもシャトルだけを十分に認識することができました。所要時間30分。下はとりあえず物体のクラスタリングの参考画像です。なんというか、これで察してください。
これだけで済めばなにもかもめでたしめでたし……だったのですが、ウチのKinectV2の設置位置から相手のサービスを観測するためには約5500mm程度先のシャトルを認識する必要がありました。すると、さまざまな外乱と戦わなければなりませんでした。下図は6m程度先のシャトルや人間を見ているときのスクショです。
ノイズと戦おうにも、シャトルが遠くに離れてしまうとシャトルとして観測できる点も多くて1枚の画像あたりで10数ピクセル程度なのでパターンマッチングなどの類はとてもできるものではありません。
つーかパターンマッチング実装めんどいし。そこで行ったのが、時系列データの処理です。各時間 t₁ , t₂ …… tn の点での「シャトルくらいの大きさの浮遊物体の座標」を全て保存しておき、t=nでの観測が得られた時はt=n-5,n-4,……,nでのすべてのシャトルっぽい大きさの物体の群から「シャトルっぽい動きをしている点列」が見つかれば、それを軌跡とする。という感じです。例えば、「軌跡を真上から見たときにちゃんと直線状の形になっているか？(地面への写像を行った平面上の点列に対して主成分分析を用いて分散から切る)」、「各時系列における速度ベクトルの大きさ・角度の変化量が適切か」などの数多くのフィルタリングをハードコーディングすることで実装しています。ちなみにこれでもノイズは乗ってきます。
この認識方法における利点は、有名な画像処理でのノイズ除去アルゴリズム、「収縮→膨張」を用いないために処理がやや早い上により遠くのシャトルを収縮で潰すことなく認識できる点などがある一方で、シャトルの認識に6点以上前の時系列データをシャトルの軌跡の初期認識に用いるため、シャトルの発見に200ms程度の時間が必要になってしまうことなどが挙げられます(KinectV2は30fps)。後述しますがシャトルの認識が遅れてしまうのはロボットの自動化において致命傷の可能性があります。
・シャトルの軌道予測について
・概要
本編です。闇です。まずは下の画像をご覧ください。
画像は(http://www.siip.city.sendai.jp/n/2012/1023/01.html)さまから引用・加工させていただきました。
シャトルの動きはとても放物線と呼べる形をしておりません。そのため、シャトルの軌道を予測したいならば、物理学的なアプローチが必要となります。
まず、シャトルにかかる力は以下の2つが支配的です。
・重力
・空気抵抗
空気抵抗はシャトルの進行方向と真逆に以下の大きさの力が働きます。
$D=\frac{1}{2} \rho V^2 S C_D$
ρは流体の密度、Vはシャトルの速度、Sはシャトルの代表面積、 $ C_D $ はシャトルのレイノルズ数などによって決定される抗力係数らしいですが、ぼくは情報工学科なのでその辺よくわかりません。重要なのは、$\frac{1}{2} \rho S C_D$ を定数cと近似すると、シャトルの軌跡は定数cに依存するシャトルの速度Vの関数で表せますので、適当にシャトルの軌跡に当てはまるようにcを変えてみると、こんな感じになりました。
見た感じ、大体合います(赤が予測軌道、黒がKinectV2で観測した実際の軌跡)。尚、物理モデルの数値計算には4次のRunge-Kutta法を用いました。
しかし、この一変数のモデルだと、シャトルの軌跡と完全一致させることはできません、例えば先ほどの図のように大体合ってそうなパラメータでもシャトルの上昇中のデータに適用すると、下のようになります。
この原因として考えられるものとして、「シャトルの回転によって生じる揚力の類」などが働いているのではないかと考えていますが、これはKinectV2で観測できるものではありません。
また、Kinectは30fpsでしか距離画像の取得ができません。
例えば、下図の場合を考えてみてください。これもKinectV2で実際に認識したシャトルの軌跡の平面データです。
さて、赤い矢印でさされた点での速度はどう推定するのが妥当でしょうか？一個前の点と直線を結ぶ場合、一個後の点と直線を結ぶ場合、など、様々な可能性があってしまい、速度推定が難しいですよね？
ここで速度ベクトルの方向や大きさを間違えてしまうと、予測落下地点は大きくズレます。
もしこれが90fpsで観測したデータならば、理想的にこんな感じになり、速度推定が行いやすくなり、精度も上がるはずです。
また、フレームレートが上がり観測する点の数が増えれば、多少観測誤差があってもローパスなどを通すことで恐らく十分に対応できます。実際に先述した先行研究では100fpsのステレオカメラを用いていました。
このように、フレームレートが低いということは、より少ない情報から正しい値を推測する必要が出てくるために、数多くの闇を伴います。
観測のノイズを対処しながら適切に速度や回転の状態を推定、軌道を予測する手段としてカルマンフィルタや拡張カルマンフィルタ、アンセンテッドカルマンフィルタなどの非線形カルマンフィルタの類が挙げられるますが、学部2年にして情報工学科だった当時のぼくにはカルマンフィルタを理解するだけの力が足りませんでした。ゆえに、11月のぼくは物理モデルを用いた軌道予測については、撤退を余儀なくされました。
・軌跡データを用いた軌道予測
非線形項が渦巻きシャトルのパラメータが、そして数式さえ分からない状態でも、まだ戦う手段はあります。
なぜなら、手元に軌道を予測したい物体そのものと、それを観測する装置があるのですから。（あとついでにそのシステムはカオス力学系でもないのですから。）
数式が分からないなら、数式を一切使わなければいい！
ここで提唱するのは、物理モデルを使わない軌道予測へのアプローチです。
要約すると、軌跡のデータを大量に憶えておき、予測したいデータを受け取ったら、それに近い軌跡を大量に思い出すことで軌道を予測する、という手法です。
こんな感じ
①入力
③出力
これを実現するために『パーティクルフィルタ』という理論を利用します。別名『粒子フィルタ』です。
パーティクルフィルタはベイズフィルタと呼ばれるデジタルフィルタの一種で、ノイズが乗った計測値から「本当の値」を推定する手段の一種です。ベイズフィルタの派生としては、先述したカルマンフィルタなどが挙げられます。
ベイズフィルタについてもここで説明しようと思ったのですが、説明のしにくさとぼくの説明技術の低さが合わさって最強になってしまったので別のページに資料書きました。こちらにお願いします。
・パーティクルフィルタと軌道予測
シャトルの軌道予測では、ベイズフィルタを応用した、改造パーティクルフィルタを用いました。文字で説明するよりはマシだと思うので図で説明します。下図をどうぞ。
まず、フィールド横に置かれたKinectV2に接続されたPCから、シャトルの座標がUDP/IPを用いて軌道予測用PCに送られます。
軌道予測用PCには700個程度のシャトルの二次元(横×高さ)の軌跡データが保存されています。計測されたシャトルを二次元に変換するための手段としては主成分分析を利用しました。
ここで、例えば下図のようにt=0,t=1の二点のシャトルの座標を取得したとします。
得られた速度ベクトルに近いベクトルをデータベースから検索します。赤い丸に囲まれたカラフルな丸がそれに該当し、これらの点の1つ1つを「パーティクル」と呼びます。パーティクルフィルタと呼ばれる所以ですね。これが先ほどのベイズフィルタにおける「初期状態」に該当します。
このとき、なるべく正規分布状になるように抽出を行います。
抽出に正規分布を選んだ理由としては、正規分布を選択しない理由がない、という程度のものです。最も有名かつ対称性のある変数の少ないパラメトリックな分布であり、かつ正規分布自身が自然な共役事前分布であるため扱いやすいという理論的裏付けがあるので想定外の事態が起こりにくいと考えました。
次に、それぞれのパーティクルを『次の時刻の点』に進めます。パーティクルはデータベース上の点であるため、物理モデルに基づいた数式を用いた数値計算を行うことなく次のデータをたどるだけで「次の観測するシャトルの位置の予想」が行えます。
全てのパーティクルを進めたものが「KinectV2によるシャトルの座標を得る前の予想の分布」であり、ベイズフィルタにおける事前分布に該当します。
ここでt=2におけるKinectV2が観測したシャトルの座標を取得します。この後、得られた観測点を基に各パーティクルがどの程度妥当か(尤度)を計算します。尤度は大きいほど「そのパーティクル付近に本当にシャトルが存在する可能性が高い」ことを意味しています。
ここで念のため述べておくと、今やろうとしていることは「KinectV2で認識した座標は様々な誤差が乗っているので、この誤差を軽減して『本当はシャトルはどの位置にあったのか』を確率的に考える」ことです。
シャトルの真の位置の推定は、先程求めた尤度を用いた重み付き平均を求めることで行います。これは理想的には最小分散推定量に該当します。
ここで、推定位置を始点としてそれぞれの先の軌跡を最後まで描画し、それぞれの重み付きの平均を求めたものを予測軌道とします。
また、t=1、t=2のシャトルの観測点を基にパーティクルの抽出・そして尤度の低かったパーティクルを淘汰させます。これが事後分布となり、次の初期状態となります。
この工程を逐次的に繰り返していくことでシャトルの落下地点を予測していきます。
ちなみに、パーティクルフィルタそのものは必ずしもデータベースを用いるアルゴリズムではなく、むしろ今回の使い方が少しメジャーじゃない気がしますのでパーティクルフィルタ=データベース！では断じてありません。よろしくお願いします。
・いいからはよ動画だせや
この軌道予測を用いてロボットを全自動走行させた動画が以下の二つになります。
こちらは3月中旬の動画です。レシーブではなく、150mm×200mm程度の大きさの段ボールにシャトルを入れる試験走行です。足回りは完全自動で、マシンが「ﾋﾟﾋﾟﾋﾟﾋﾟﾋﾟ…」と音を発しているとき、軌道予測PCから落下地点を受信しています。
3月後半での動画です。移動先を段ボール箱からラケットに変えたものです。シャトルの落下地点が近い場合についてはラケット一本でも場所が合わなくはないくらいの精度が出なくもないようです。あと、背景に映っているのがカメラ台です。
・ビデオ落ちとその後の改善
ここでビデオ落ちします。恐らくこちらの自動機については「多種多様な球筋に対応できる」ものと判断されなかったのだと考えています。あと、純粋に他校のクオリティが高かったのだと思います。
今回ぼくが用いたシャトル認識・軌道予測の手法は以下のようなメリット・デメリットがありました。
シャトル認識
メリット
・遠くまで認識することができ、自フィールドのサービスエリア側にコントローラを置いたにも拘わらず、相手のサービスまで十分認識できる。
・マシンなどが写っていてもシャトルの高精度な認識ができる。
デメリット
・最初に軌跡を認識するために6点一気に必要とするので5点分の時間遅れが生じる。
軌道予測
メリット
・30fpsで得られたシャトルのデータを用いてマッチングを行ったため、速度推定を一切行う必要がなかった。
・シャトルの物理モデルや、それに伴う非線形項を考えずに済む。
・そこそこ高精度。
デメリット
・パーティクルが少ないうちは認識精度はそれほど高くないので、パーティクル数がある規定値を超えるまで落下予測地点をロボットに送信しなかった。これによってシャトル5点分程度の時間遅れが生じたうえ、UDP/IPの遅延なども合わせると予測がワンテンポ遅いものになってしまった。
つまり、今回のシステムは精度や認識距離と引き換えに時間を犠牲にしていました。素早く落下地点に移動する必要が不可欠なロボミントンではこれは痛手です。そこで、次に行った改良は「全自動化を諦める」ことでした。
操縦者はフィールドの真横に立ち、マシンの前後方向だけを操縦します。一方で軌道予測でマシンの左右方向を自動で移動します。このマシンは右半分のコートしか動き回らない設定なので、自動化部分の移動範囲を小さくすることで遅延をカバーする、という方針です。
操縦者は上画像の視点のように、ロボットを真横から見る視点での操縦を行います。
これによって、ロボミントンの操縦の一番の難関であった「奥行き方向が全くわからない」という問題を回避することができます。
これを実装したものが以下の動画です。
ロボットは横方向の移動を完全に自動で行っています。操縦者は見えていませんがフィールドの側面側の、サービスエリア付近から操縦しています。 操縦範囲が2次元から1次元になり、しかも最も把握し辛い奥行き成分を全く考えないで済むようになったことで、かなり操縦がしやすく、そして全手動の場合と比べで命中精度が大きく上がりました(当社比)
以上がぼくの今年行ったことの大体全てです。ビデオ審査で落ちてからは焦点の定まらない目で壁を眺める生活を続けていますが、ぼくは今B3なのであと1回ロボコンをリトライできるので、それまでにもうちょっとマシな制御屋さんになれるように頑張っていきたいと思います。