今回は軌道予測と打点の決定方法についてです

---

シャトルの軌道予測

まず、以下の文献を参考に、軌道予測の方法を考えました。

• パーティクルフィルタとカルマンフィルタを補完的に利用したバドミントン映像からのシャトル軌道推定
• モーションブラーを活用したバドミントンシャトルの追跡手法
• Badminton playing robot – a multidisciplinary test case in Mechatronics

これらの文献では以下の共通点がありました。

• ステレオカメラでシャトルの位置を検出
• シャトルは空気抵抗を考慮した斜方投射運動としている。
• 拡張カルマンフィルタで軌道を予測している

ステレオカメラかKinectか

ステレオカメラによる座標の計算については以下の文献に詳しく記載されており、実装比較的容易にできそうです。

• ２視点からの映像を利用したサッカーボール追跡手法

しかし、

• シャトルという高速に移動する小さな物体を正確に捕捉するには、同期信号の出ているCマウントカメラ等、高額な機材が必要
• 奥行を正確に検出するにはカメラの間隔・輻輳角を十分に稼ぐ必要があると思われ、ロボットの本体に搭載するのは難しい
• 会場の壁の色・照明環境・振動に対応できる保証がない

等、いくつかの問題が存在したため、ステレオカメラは使用せずKinectV2で検出を行うことにしました。

シャトルの運動

シャトルの空気抵抗の係数などは以下のサイトのデータを参考にしました。

• バドミントン練習用ロボットをつくる
• シャトルの飛行シミュレーション(Excel)

拡張カルマンフィルタ(EKF)

カルマンフィルタは離散的な誤差のある観測から、時々刻々と時間変化する量を推定するために用いられます。
例えば、カーナビではGPSによる位置情報、電子コンパスによる方位情報、及び加速度センサ・ジャイロセンサによる加速度と角速度の情報をカルマンフィルタによって統合し、車の位置・速度を取得しています。

拡張カルマンフィルタは、非線形モデルを現在の推定値の回りで線形化微分することで、カルマンフィルタを適用する手法です。

原理などは確率ロボティクスや以下のサイトを読んでください。

• カルマンフィルター – Wikipedia
• 拡張カルマンフィルタを使用した自己位置推定MATLABサンプルプログラム – MyEnigma

実装

EKFの実装に行列を用いるため、C++でEigenか、PythonでNumPyを用いる必要があります。
EKFの計算コストが少ないという利点を活かし、プログラミングが楽なPythonを選択しました。

また、実装に当たって以下のサイトのコードをを参考にしました。

• Pythonで拡張カルマンフィルタを実装してみる

公開しているプロジェクトの、shuttle_kalmanフォルダのshuttle_kalman_node.pyとshuttle.pyを用いて軌道予測を行っています。

軌道予測(EKF)に関するプログラムはshuttle.pyでクラス化、shuttle_kalman_node.pyでROSで他のノードとの橋渡しをしています。

動作結果

打点の決定方法

予測したシャトルの軌道から目標の打点を計算します。

座標変換

元のXYZの座標系から、ラケット旋回機構の回転軸に平行な平面をX’Y’軸とする座標系に座標変換します。
同次座標でアフィン変換すればいいだけなので、

• 旋回機構は地面に対し50度傾いている
• 旋回中心のロボット中心からの奥行をAz
• 旋回中心の地面からの高さをAz
• ロボットのyaw方向の回転をθ
• ロボットの位置をRx, Ry

とすれば以下の式で座標変換できます(ﾃｷﾄｰに書いたので間違ってるかもしれません)

打点

予測された軌道を座標変換し、z’=0平面との交点を求めます。
この交点が目標の打点となります。

打点から目標姿勢の算出

打点の座標を[ x’ y’ 0 ]、ラケットの長さがrの時、

-旋回機構の目標角度φはAsin(y’/r)
-横移動ユニットの目標位置はx’-sinφ

となります。

abs(y)が1より大きい場合や、横移動ユニットの稼働限界を超える場合は、足回りを移動させる必要があります。

動作結果