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


シャトルの軌道予測

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

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

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

ステレオカメラかKinectか

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

しかし、

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

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

シャトルの運動

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

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

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

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

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

実装

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

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

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

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

動作結果

打点の決定方法

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

座標変換

座標変換

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

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

とすれば以下の式で座標変換できます(テキトーに書いたので間違ってるかもしれません)

20160328_225709

打点

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

打点から目標姿勢の算出

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

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

となります。

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

動作結果

rviz


次回は通信回りや大会当日の話、負けた原因等を書こうかと

カテゴリー: ロボット

3件のコメント

宮脇俊大 · 2019年5月22日 8:15 PM

ロボミントンの日記を読ませていただきました.現役ロボコニストです.
CAN通信に使用されたハードウェアは下記のURLのデバイスでしょうか?
http://www.compass-lab.com/CAN_Tool/CANUSB.htm

    れい · 2019年6月23日 10:56 PM

    返信が遅くなり申し訳ありません。

    CAN-USBですが、宮脇様が書かれましたLAWICEL AB製のものを使用していました。

    正直に申しますと、通信量が多いCANバスやリアルタイム性の高い処理に対し、この製品を使用することはおすすめしません。
    この製品はシリアルデバイスとして動作するのですが、データが16進数表記の文字列でやりとりされます。
    CANを500kbps以上で動作させている場合、460800bpsで動作させたとしても、CANバスよりも通信が遅いため、データの読み落としやエラーが発生します。

    Linuxで使用することが前提であれば多少敷居は高いですが、RaspberryPiやJetson等のSPIが出ているデバイスにMCP2515をつなげて使用することをおすすめします(私はやったことないですが笑

      宮脇 · 2019年8月13日 11:56 PM

      返信していただきありがとうございます.
      CANUSBを使用してCANデバイス3個から100ms周期で出力される8バイトのCANデータを受信しようとしても,CANUSB上のLEDの赤LEDが点灯し,エラーを出力してしまいました.
      ご指摘していただいた通り,モニターを確認したところデータの欠落と遅延(1sから10s)が発生しました.
      この場をお借りしまして,Lawicel社のCANUSBは送受信できるデータの転送速度はCAN通信の転送速度よりもずっと低く,データ欠落と転送遅延をなくすには転送容量の上限はせいぜい8byte/200ms周期が限界かなと思いますので,リアルタイムな制御には不向きです.
      ハードウェアはラズパイ3B,OSはubuntu16.04 mateを使用しています.
      今後はラズパイのGPIO上にCANコントローラを使用して通信状況を確認してみたいと思います.

コメントを残す

メールアドレスが公開されることはありません。 * が付いている欄は必須項目です