2022年1月1日土曜日

知能ロボコン2022計画再始動

飛行型ロボットで知能ロボコンに出場したいと考えています。およそ1年前に構想・計画したものでしたが、昨年もコンテストが中止となったためハードデッドラインがなくなり、プロジェクトも事実上一旦凍結していました。




とはいえ、昨年1月ごろに子機ハードウェアは飛行可能なものができ、2月に外部からの制御の実験を始め、その後Maker Faire Kyoto / Tokyo に向けてハードウェア・ソフトウェアの改善を進めたので現状それなりに信頼できる移動機構ができた状態にあります。

2022年6月の知能ロボコン出場に向けて、大まかな計画を再作成します。

  • 1月
    • 親機: カメラ視野確認(カメラに可動軸が必要か?)
    • 親機: 子機との接続部分メカニズム検討
    • 競技台設計・木工加工など発注
    • 競技台設置場所確保(≒作業部屋の片付け)
  • 2月
    • 子機: 競技台上空飛行の実験
    • 親機: ボール誘導部等の展開機構検討
  • 3--4月
    • 親機製作
    • その他
  • 5月
    • Maker Faire Kyoto出展(開催未定)(本年開催されず)
    • 余裕があれば親機にボール分別機構を付ける
  • 6月
    • 知能ロボコン参加

2021年12月19日日曜日

パラレルリンク機構と視覚情報処理を使ったロボット

パラレルリンク機構を使ったテーブルホッケーロボットを作りました。

元々は大きなホワイトボードに字や絵を描くロボットを構想していたのですが、その前段階として要素技術の検証のために小型のものを作りました。要素技術とは、

  • パラレルリンク機構
  • 画像センサーを用いたキャリブレーション
  • ある程度高速な動作
  • ロボット用サーボモーターの速度制御機能を使う
このうちメカと画像センサーの統合までできました。

メカニズム




ABSで滑り軸受を作りました。FDM方式3Dプリンタによる成形です。



根本の2個のモーターで駆動します。KONDOのロボット用サーボモーター5000シリーズを使ってみました。これはラジコン用サーボモーターと違い、出力軸がセレーションではなくIカット形状になっていて、フレームを直接取り付けることができます。


エンドエフェクタ部分はある程度簡単に取り替えられるようにしました。なお、真ん中部分の白い点に見えるのはキャリブレーション時にマーカーを貼り付けるためのソフト粘着剤(商品名: ひっつき虫)です。

電装系


ROBO-剣用ロボットと同じく、PCからKONDO USB DUALアダプターを使用して直接ロボット用サーボモーターにコマンドを送ります。電源は12V ACアダプタまたは10.8(NiMH)または11.1V(LiPo)のバッテリーを使用。

ソフトウェア

カメラキャリブレーション

まずこれがなくては始まりません。
平面パターンを使ってカメラのレンズパラメータと地面平面に対するカメラ位置姿勢の推定を行います。
opencv.jpのサンプルプログラムをもとにしたキャリブレーションツールを自作して使っています。GitHubに、このサンプルプログラムをOpenCV2のAPIで書き換えたものを公開しています。

ロボット用サーボモーターとの通信

KONDOのUSB-DUALアダプターのICSモードを使い、仮想シリアルポートから直接サーボモーターと通信します。このライブラリはGitHubで公開しています。

物体検出

オレンジ色のピンポン玉を使ったので、次のような処理としました。
  1. 歪補正
  2. 処理時間短縮のため画像を1/2に縮小
  3. 色相と彩度で2値化
  4. 連続領域ごとにわけ、輪郭抽出
  5. 周長^2と面積の比が最も真円に近いものを選択
  6. 楕円のあてはめにより中心を求める
動画作成時の環境では0.015秒程度で処理を完了できています。
また、この後地面平面との三角測量により3次元の物理座標を求めています。今回実験ではたまたまほぼ真上から見た映像のため紛らわしいのですが、カメラで得た画像内での座標を直接処理に使っているわけではありません。遠近法の効果が出るような斜め向きにカメラを設置してもプログラムに変更は必要ありません。カメラ位置を変更した際にはカメラキャリブレーションの部分で地面平面の位置と座標軸の原点+方向を与え、この情報を使います。

ハンドアイ較正(hand-eye calibration)

カメラで計測したボールの位置に合わせてアームを動かすには、アームがどのような形状をしているのか、カメラとの相対的な位置関係はどうなっているのかの情報が必要です。事前に計測・推定しておく方法を取りました。
パラレルリンク機構の形状、リンクの繋がり方は予め与えますが、関節間の距離などは自動的に推定するようにします。
  • 根本の固定関節の位置(2×2自由度)
  • 各リンクの長さ(4)
  • エンドエフェクタのリンク内相対位置(2)
  • サーボトリム(2)
以上の12変数を推定する非線形最適化問題となります。トルク0の状態にしたサーボモーターのポテンショメータを使い、複数の点で(2つの関節角度)と(実際のエンドエフェクタ位置)の組を取得します。上記12パラメータを仮に与えれば、関節角度から順運動学計算によりエンドエフェクタ位置が計算できます。この2つの誤差を最小にするような解を求めます。cminpackによるLM法の実装を用いました。
なお、12自由度としていますが、エンドエフェクタの位置を決めるものとしては冗長で複数の解が等価となるため、これらをあてはめた表示は必ずしも実際のロボットの形状と一致しません。



移動予測 


USBカメラからの画像取得について、OpenCVのcv::VideoCpatureを使用して通常のビデオデバイスとしてキャプチャしています。使用環境においてこの方法ではおよそ100ms程度の遅延があることがわかりました。仮に機械の動作が十分速いとしても、認識処理に最低これだけの遅延が発生することになります。転がってくるボールをうまく打ち返せるようにするため、等速直線運動を仮定して未来の位置を予測し、これをもとに動かすようにしました。
白色い丸印が現在(ここでは、今取得できた画像の意味)のボール位置を、黄色い丸印が予測位置を示しています。
移動予測のために、まず直前0.2秒間程度、時間と位置のペアを記憶しておきます(上の画像上で米印で表示)。これらの点に最小二乗法による時間関数の当てはめを行います(x,y,z各座標について独立)。そして関数の外挿(要するに、未来時刻を代入して関数の値を計算)を行い、予測位置とします。※この図では時間方向の情報が入っていないため不親切ですが、この軌道(x-y平面)に対して直線を当てはめているわけではありません。
ここで使用する関数を1次関数とすれば等速直線運動、2次関数とすれば等加速度運動を仮定することになります。平面上を転がるボールの運動で、しかも0.1秒程度の短時間の予測なので、1次関数としました。(なお、こちらは上記のハンドアイ系と異なり、線形の最適化問題なので疑似逆行列の計算1回でできます。)
しかしこれだけでは、ボールが壁で跳ね返ることを反映できません。予測位置が容易にフィールド外になってしまいます。そのためまず、予測位置が範囲外となる場合は予め設定した壁のところでY座標を折り返した位置を予測位置とします。

更に、関数当てはめに使用する過去データが壁での反発の前後両方を含む場合は、そのままでは最小二乗法によるフィッティングができません。これについてはまだ対処を行っておらず、妥当ではない予測値が出ています。但し、この状態は一瞬だけなのですぐ復帰します。

動作アルゴリズム

ボールを打ち返す動作実現のために、最低限単純なルールを設定しました。X軸は前後方向、Y軸は左右方向です。
  • a) ボールが見つかった時
    • 上記の方法により0.1秒後の位置を予測する。できた場合は以降「ボール位置」をそれで置き換える。
    • a1) X座標(手前→奥の軸)が一定値より遠い側であれば、X座標を待機位置、Y座標はボール位置と同じ地点に最速で移動する。
    • a2) 近い側であれば、ボール位置からラケット半径分手前側の位置へ最速で移動する。
  • b) ボールが見つからない時
    • 待機位置にゆっくり移動する
a2になるのは通常a1の直後を想定しています。a1によりボールを追いかけ、a2によりラケットを前へ急激に出して接触することで、ボールを打ち返す動作となります。インパクト時のラケットの速度は制御しておらず、これはまだ誤魔化しているところになります。
次は速度の制御をもう少し緻密にしてみたいと思います。

2021年5月9日日曜日

子機の新規ハードウェア(機械・電子回路)

子機となるクワッドコプターの新しいハードウェアを作成しました。制御ソフトウェアを移植し、旧型子機と同等の機能を一応実現しました。

但し、使用するセンサーの違いにより、IMUによる機体姿勢推定の処理が全く変わることとなりました。旧型ではICで加速度と角速度センサーの両方を使って姿勢を推定する処理までを行っているのに対し、新型ではそれをCPUのファームウェアで行わなければならないためです。現状、一応動いてはいるものの未だ不十分な状態です。

モーターとプロペラは同じものを使い、フレームと電子回路をオリジナル設計しました。また、Telloの純正品や、Tello用としてサードパーティーから販売されているプロペラガードが使えるようにしました。
Tello純正品のプロペラガード
サードパーティー製の球状のガード

コントローラ(制御装置)にはM5ATOM Matrixを使用しました。ATOM MatrixはATOM Liteと比べてマトリクスLEDが付いていることが特徴ですが、それだけでなくIMU用センサーICのMPU6886も搭載されています。無線通信とIMU、4本以上のI/Oポートピンがあるということから、かねてより注目していたものでした。

なおATOM FlyというAtom Liteを使ったキットが既にあったのですが、これは見つけた当時既に売り切れていて、その後入荷する様子もなかったので諦めて自作に至りました。現在では廃番(end of life)となっています。


制御基板はKiCADで設計して基盤製造サービスで製造しました。昇圧DCDCコンバータ、モーター駆動用パワーMOSFET、電池電圧の分圧抵抗、リセットIC等が載っています。


I/Oポートと回路の接続にはM5ATOM裏面のピンソケット(コネクタ)を使用しますが、それだけでは足りなかったので側面のGrooveコネクタからも配線を引き出しています。


Makerfabs基板はモーターを保持するフレームを兼ねていましたが、本機では気流を邪魔する形を避けるために基板面積は最低限とし、フレームは別の部品としました。モーター同士の間隔をTelloに合わせて設計し、3Dプリンタで作成しました。全部を一体で成形するのは難しかったため、いくつかに分けて出力した上でABS用接着剤で組み立てました。



バッテリーは新たに600mAh 50Cのものを採用しました。(コネクタは付け替えてあります)


結果的には、40%~50%ぐらいの出力でホバリングできるようになりました。ハードウェア改善としては成功ではないかと思います。
制御ソフトウェアがまだ不完全なため正確な航続時間はまだ計測していませんが、全体を覆うドーム状のプロペラガードをつけた状態において、少なくとも2分半は飛行を続けることができました。
次は制御部分の完成度を上げていきます。

2021年5月2日日曜日

Maker Faire Kyoto 2021 (オンライン)

 Maker Faire Kyoto 2021に出展予定でしたが、現地でのイベントが中止になったためオンラインイベントとなりました。これまでの製作状況をまとめた動画を作成しました。

2021年2月28日日曜日

モーター駆動方法の再検討

<追記> この件についてstrv様に教えていただきました。今回使用している部品の組み合わせでは、電流不連続モードでおそらく問題ないということがわかりました。https://twitter.com/qzy13700/status/1365943112128819202

以下に元の記事はそのまま残しておきます。

2021年2月9日火曜日

外部からの位置計測によるホバリング成功

バッテリ残量低下検出と緊急降下機能[1]、ラジコン操縦およびトリム調整UI、半自動の離陸、位置検出と可視化等を整備して、実験環境が整ってきました。それらを活用して、本機の動作の基本となる自動ホバリング機能を達成しました。 
空間の1点を目標として設定し、機体の重心位置をそこへ近づけるようなフィードバック制御を行います。これによりホバリングができるわけですが、それだけではありません。この目標地点を徐々に動かすと、空間を自由に移動することが出来ます。(但し、ゆっくりで良い場合に限る)

位置計測のためのセンサーとしては、Intel RealSense D435を用いました。ラップトップPCで点群を処理して位置を計測し、無線LANでロボット(本体)に操縦指令を送ります。
この操縦指令と子機側での制御は、以前書いたラジコンとして飛行させるために作った機能そのものです。ロボットは搭載したIMUにより機体姿勢(回転角)を把握しており、操縦指令のうちのロール角とピッチ角を保つようなフィードバック制御をローカルで行っています。

位置計測のための点群処理は、仮に単純なものを実装したところです。飛行想定範囲内にある点群の重心位置を子機の位置としています。クラスタリング等の処理を入れると処理の遅延が大きくなるため、まずは最もパフォーマンスが出るように、単純なものから試しています。現状では物体検出部分は16fps程度で行われています。カメラと競技台の位置関係といった概念を扱うプログラムもまだ作成していないため、実験ではカメラの座標系(光軸がZ軸、等)で目標地点を与え、そのまま制御に使っています。

なお初期の検討時問題と考えていた機体の回転方向の検出については、おそらく不要であろうということになりました。離陸時の姿勢を基準としてヨー回転を抑えながら飛ぶため、数分程度であればIMUの情報だけで最初の向きを維持できるのではないかと予想しています。

位置フィードバック制御の方も、まずは最も単純な方法ということで、とりあえず3軸方向独立に比例微分制御を実装したものです。X、Y、Z軸それぞれの方向について独立に、ロール軸目標角度、ピッチ軸目標角度、スロットル、という操作量にそれぞれ反映するというものです。フィードバックゲインは実験しながら手動で調整しました。
実際はこの3つの操作は独立ではありません。しかし、原点近く[2]ではお互いへの影響は十分小さい[3]から独立と思って制御してもある程度うまくいく、という理屈ですが、実際は必ずしも微小量とはいえないため、相当なごまかしを含んでいます。

回転をロールとピッチで表している時点でもすでに互いへの影響がありますし、機体を傾けるなら鉛直方向の分力が減るわけなのでその分推力を上げる必要があります。このような特性を反映しての制御もそのうち試します。
更に、スムーズに移動するためには位置ではなく速度の方を制御対処とする必要があるかもしれません。

なお、強調しておきたい点としては、このように外界から信頼できる観測が行えると、逆にロボットの特性の方を校正することができます。この場合で言えば、PID制御などでホバリングを維持したときの操作量から逆に、ラジコンで言うところのトリム調整を自動で行えるはずです。実験の再現性や作業性に大きくプラスになると思われるので、これも近いうちに追加します。

2021年1月11日月曜日

昇圧回路搭載

電源回路の見直しを行いました。

今まで500mAhのバッテリーで4つのモーターはduty比100%まで駆動可能、飛行も可能でした。但し、モーターを急に始動した時(例:静止状態から短時間でフル・スロットルにする)マイコンが正常動作できなくなり、暴走するという問題が起こっていました。これは2つの問題が関係しています。

1.正常動作を続けられない状況でマイコンを停止させていない

ESP32 IMU基板には低電圧検出(リセットIC)が付いていません。これはおそらく、USBケーブルでPCに接続しての使用を前提としているからではないかと思います。

ESP32のICおよびWROVERモジュールの方にもその機能はありません。バッテリによる電源を使う場合は、(他の多くのマイコンと同じように)電源電圧監視をして2.3Vを下回ったらENピンにlowレベルを入力して動作停止させることがデータシート(Figure 9 Reset Circuitの下)で推奨されています。

動作停止ではなくモーター出力ONのままマイコンが機能を停止(ロボットが暴走)することがあるのはおそらくこのためです。これは明らかな問題ですので、後日改造します。秋月で購入できるものとしてはTCM809R(2.63V)あるいはR3111N211A(2.1V)が使えそうです。

2.電池を一時的に限界近くの領域で使用している

現在使用している500mAhのバッテリは、顕著な瞬間的な負荷以外は電圧を大きく低下させずに電流を供給可能です。一般的なロボットの設計上、バッテリー電圧が顕著に低下しないような負荷の範囲で使用するべきと考えています。しかし、今回は重量との兼ね合いもあり、妥協点を探しているところです。

あまり良い方法ではないかもしれませんが、マイコン用電源に昇圧回路を追加しました。基板上のシリーズレギュレータ(3.3V)に入力する前に一旦、より高い電圧(5V)まで昇圧します。これにより、モーターへの電流供給で電池電圧が(一時的に)3V以下まで低下しても、昇圧回路に必要な電流が供給できている限りマイコン側は正常動作を続けられます。

一時的な高負荷以外でも電池電圧が低い場合は早急に着陸して停止すべきですので、マイコンによる電池電圧監視も同時に行います。また、バッテリの過放電を防ぐ必要があるのでICによる電圧監視も併用します。(後で追加予定)


ESP32 IMU基板は動力系とマイコン電源が分かれていないので、まず基板上のパターンをカットしました。次いで、秋月の昇圧モジュール(AE-XCL101とAE-XCL102)を基板上に接着して配線しました。また、以前述べたように、基板上の電源スイッチは当ロボットの使い方では定格を超えるので、使わないようにバイパスしてあります。

知能ロボコン2022計画再始動

飛行型ロボットで知能ロボコンに出場したいと考えています。 およそ1年前 に構想・計画したものでしたが、昨年もコンテストが中止となったためハードデッドラインがなくなり、プロジェクトも事実上一旦凍結していました。 とはいえ、 昨年1月ごろに子機ハードウェアは飛行可能 なものができ、 ...