前回は、KHR-3HVをPCから制御する基本的な仕組みを説明し、バッテリ電圧を調べるサンプルプログラムを紹介した。今回は、定番の距離センサーであるPSDセンサーを実際に使ってみることにしよう。

PSDセンサーで距離を計測

今回、ロボットに組み込んだPSDセンサーはシャープ製の「GP2Y0A21YK」「GP2Y0A02YK」である。これは電源電圧が4.5~5.5Vのため、KHR-3HVのコントロールボード「RCB-4」では、アナログ入力端子にそのまま繋げることができる。接続ケーブルを自作するなり購入するなりして用意するだけでいいので簡単だ。

RCB-4には10ポートのアナログ入力端子がある。外側から、GND、Vdd(+5V)、AD端子となる

筆者はこのようにケーブルを自作した。PSD側のケーブルの色は合っていないので注意

このPSDセンサーは、電源電圧が与えられると測距を開始。測定結果に応じた電圧を出力する。出力特性はデータシートで公開されているので、RCB-4側で電圧を調べれば、そこから測定した距離が計算できるわけだ。

PSDセンサーの出力特性の例

以下、GP2Y0A21YKを例に説明しよう。このセンサーの計測範囲は10cm~80cmで、このときの出力電圧は約2.3V~約0.4Vとなる。基本的に、遠距離になるほど電圧は下がるのだが、電圧から距離を計算する際に注意が必要なのは、特性が非線形であることと、10cm以下のところにピークがあることだ。

非線形の場合、折れ線グラフで近似して、そこから計算する方法もあるのだが、今回はExcelの機能を利用して、近似曲線を求める方法を試した。出力特性のグラフから10点ほどデータを入力し、Excelが算出した近似式は以下の通りだ。

距離(cm)=16067×電圧値-1.202

入力したデータ(青点)と、算出された近似曲線。グラフは、Y軸が距離(cm)、X軸が電圧(5V=1024)

この式にAD変換後の電圧値(0~5Vが0~1024)を与えると、簡単に距離を計算することができる。今回用意したサンプルプログラム「psd_test.py」のget_psd_s関数には、この近似式がそのまま入っているので確認して欲しい。近似の誤差は少しあるものの、もともとPSDセンサーはそれほど精度が高いものでもないので、これで十分だろう。

またこのPSDセンサーの場合、5cm付近に出力電圧のピークがあるため、ある電圧に対し、一致する点が5cm以下と5cm以上にそれぞれ1カ所ずつ存在する。原理上、このどちらが実際の距離か区別はできないため、5cm以下の方については無視している。ロボットが密着したとき、距離が5cm以下になる可能性もあるが、これは仕方ない。

psd_test.pyの実行結果。何かキーを押すと終了する

なおこのプログラムでは、測定結果を画面上のウィンドウに表示するために、画像処理ライブラリ「OpenCV」と数値計算ライブラリ「numpy」を利用している。インストール方法などに関する説明はここでは省略するので、各自で調べて欲しい。

周囲をスキャンしてみよう

ではこのPSDセンサーを使って、対戦相手を発見できるようにしよう。

PSDセンサーは正面方向の距離しか分からないのだが、少しずつ方角を変えて計測することを繰り返せば、周囲をスキャンすることができそうだ。連載の第1回で説明したように、ロボットには腰ヨー軸があるので、このサーボモーターを使えば、上半身ごとPSDセンサーを旋回することができる。

これを実装したのがサンプルプログラム「psd_scan.py」である。このプログラムでは、

rcb4.search_psd(-50,50)

という命令で、RCB4_comクラス(後述)のsearch_psd関数(メソッド)を呼び出し、左50°~右50°の範囲をスキャンしている(スキャン範囲は引数で変更可能)。

ただし、このpsd_scan.pyは、ほぼインタフェース周りの処理しかやっていない。実際にスキャン処理を実行しているのは、RCB4_comというクラスを定義した「rcb4.py」だ。

RCB4_comクラスは、KHR-3HVのコントロールに必要な機能を一通りまとめたものである。詳しい動作はファイル中のコメントを参照して欲しいが、search_psd関数が行っているのは、

  1. 腰ヨー軸をスキャン開始位置まで回転
  2. GP2Y0A21YKとGP2Y0A02YKで距離を計測(近距離ならGP2Y0A21YK、遠距離ならGP2Y0A02YKの値を採用)
  3. 腰ヨー軸を10°だけ回転して0.2秒待つ→2を繰り返す
  4. スキャン終了位置を超えたら腰ヨー軸を0°に戻して終了

といった処理だ。計測結果は、RCB4_comクラスの配列(リスト)dist_listに格納されているので、サンプルプログラムで可視化を行っている。なお、最短距離を検出した方角はRCB4_comクラスのdetect_ang、そのときの距離はmin_distでそれぞれ参照可能だ。

psd_scan.pyの実行結果(円は1mと2mの位置)。部屋の角で実行したので、壁の平面が現れている

次回紹介する自律バトル用プログラムの都合により、search_psd関数では、旋回したついでに頭部のPSDセンサーを使って地面までの距離を測り、崖があるかどうかの判定も行っている。この結果を使って、転落を防止しているわけだ。

PSDセンサーで周囲をスキャンする動作

ところでこのスキャン方法だが、動画を見ると分かるように、測定に結構時間がかかる。PSDセンサーの数を増やせばもっと早くすることも可能なのだが、今回はなるべくシンプルな構成にしたかったため、このまま行くことにした。まだ第1回大会のため、いきなり突撃してくるような相手もいないだろう、という読みもあった。

結果的に、これが完全に読み間違いであったのだが……。それについては、また次回に述べることにしたい。


今回使用したPythonのサンプルプログラム

(zipで圧縮されているので、解凍して使用してください)

psd_test.py

psd_scan.py

rcb4.py