沼津高専 電子制御工学科
MIRS0602 ポスト探索プログラム仕様書
MIRS0602-SOFT-0002
改訂記録
版数 作成日 作成者 承認 改訂内容
A01 2007.2.27 吉田 尾崎 初版


目次

  1. はじめに
  2. ポストの判別原理
  3. プログラムのアルゴリズム
  4. デバッグ文解説
  5. 既知のバグ
  6. おわりに

はじめに

本ドキュメントは、MIRS0602で使用したポスト探索プログラム、「post_search_t」についての詳細を記すものである。このプログラムは従来の標準プログラム「post_search」とは全く異なり、その弱点をいくつか克服しているので、今後のMIRS開発に役立ててもらえれば幸いである。

ポストの判定原理

まず、標準プログラム「post_search」をもとにテストプログラムを作成し、MIRSからポストがどのように見えているのかを調べた。

ソースファイル(t_test4.c)のダウンロード
※コンパイル時は標準関数の次のファイルをリンクすること。functions.o thread_func.o both.o pwm.o encoder.o initialize.o

次のようなポスト配置でプログラムを走らせ、得られた結果について考察する。なお、超音波センサの位置は左で、ポストの位置は変えずに4回プログラムを走らせた。



結果のダウンロード(data.xls)

データの番号(No.)を横軸、超音波センサの値(uss_data)を縦軸にとるとグラフは次のようになった。なお、各データは一定周期ごとに計測されたものである。



1つのポストは真左にあったため、データが2つに分かれてしまっているが、3つのポストがあることがよくわかる。No.10〜20の付近でもいくつかまとまった値が発生しているが、これは観測者(筆者)の足があった場所であり、4回の計測データは完全には一致していない。これに対し、ポストのデータは4回ともほとんど差がない。

ところで、計測中MIRSの動きは均一ではなく、何度か加速と減速を繰り返していた(下図)。


そこで、角度を横軸、超音波センサの値を縦軸にとりグラフを書き直すと次のようになった。


率直に言って、先ほどのグラフと大差ないように思われる。しかし、MIRSの回転速度が一定でない場合にはこのようにしてデータを扱わないと意味がない。

さて、このグラフをもとにポストの判定基準を考えることにする。ポストのデータはわずかながらも曲線を描いており、滑らかに変化する連続した値になっている。これに対し、人の足のように凹凸の激しい物体のデータは上下が激しく滑らかに変化する場所も少ない。そこで、ポストを次のように定義する。

ポストとは、極小値を中心に持ち、その両側の距離データは滑らかに増加する

計測したデータがこの定義に当てはまればその極小値がポストの中心だと判定できる。この判定をプログラムでどう実現するかについてはアルゴリズム詳細で述べる。

プログラムのアルゴリズム

従来の探索プログラムではMIRSの真左にあるポストを2度発見する可能性が高く、一度発見したポストの位置をもう一度確認(post_check)する必要があった。これはポスト探索に多くの時間を割くだけでなく、回転動作により誤差を蓄積することになる。この欠点を克服するのが本プログラムの目的である。

プログラムは次の3つのフェーズから成り立っている。

  1. 周囲の様子を超音波センサを使ってスキャンする
  2. データを元にポストの位置を判定する
  3. データを変換し、構造体に代入する

ソースファイル(post_search_t.c)のダウンロード

以下、各フェーズについて解説する。

Phase1
その場で360°回転しながら、一定時間ごとに回転角度・超音波センサの測定データ(以下「データ」)を取得する。このとき、回転角度が前回の測定時より一定角度(プログラム中でdefine済み)以上大きかったらその回転角度とデータを記録する。
これは、データを一定の角度ごとに記録するためで、MIRSを7秒程度で回転させた場合約40個のデータを取得できる。

Phase2
これが本プログラムの最も重要な処理である。ポストの判定は以下の手順で行われる。ソースファイルと比較しながら説明を読むとわかりやすいかもしれない。

  1. USS_MAX(control.hで定義)より大きいデータを削除する(対応するフラグを1にする)
  2. 削除されていない(フラグが0の)最初のデータを探す。このとき全てのデータが削除されていれば(フラグが全て0以外)、Phase3に進む
  3. 削除されていない(フラグが0の)最小のデータを探す
  4. 3で見つけたデータX(n)とその1つ後のデータX(n+1)の差dXを取り、X(n+1)+dX≒X(n+2)が成り立つかどうか調べる。もし成り立つならその後のデータ、後々のデータ…というふうに最大6回調べ、成り立った回数をaft_cntとして記録する。ただし、成り立たなくなった時点でカウントをやめ、5に移る
  5. 4の処理を、同様に前のデータX(n-k)に関して行う。成り立った回数はbef_cntに記録
  6. bef_cnt+aft_cntが、一定の回数(デフォルトは2回)以上なら、3で見つけたデータをポストデータとし(フラグを2にする)、4、5で判定に使ったデータは全て削除する(フラグを1にする)。もし一定の回数未満なら、3で見つけたデータのみ削除する(フラグを1にする)
  7. 2へもどる

この処理を最後まで行えば、最終的にフラグが2の角度とデータがポストとの角度と距離を示すことになる。

Phase3
データの測定時に使用する超音波センサは真左より少しずれているので、この補正を行う。また、データにMIRSの半径とポストの半径を足し、MIRS−ポスト間の距離に換算する。
換算した角度とデータを、引き数が1ならpost構造体へ、それ以外ならpost_tmp構造体に代入する。代入されるデータは、.ang[] .dis[] .number の3種類。


注意:なお、実際のMIRS0602にはこれに角度ごとのソート機能、差が一定角度以下になるデータの消去機能(ポストを発見してもMIRSがポストまで直線的にたどり着けない場合の対策)を追加したプログラムを使用した。これは完了報告書からダウンロードできるファイルに含まれているためそちらを参照して欲しい。

デバッグ文解説

本プログラム(関数)を呼び出すと、その実行結果をデバッグ文としてディスプレイに表示する。この際、別途ファイルに出力することはしないので、デバッグ文をファイルとして得たい場合はfprintf()文を直接追加するか、linux特有の機能であるリダイレクションを用いて標準出力をファイルに切り替える必要がある。
本プログラムを1回呼び出して終了するmainプログラム(もちろん初期化なども行う)を作成し、その結果をファイルに出力することによってデバッグ文を得たあと、各文の意味を追加して以下に示した。


既知のバグ

現段階(2007/2/27)までに判明している、本プログラムのバグ(or 欠点)とその対処法について以下にまとめる。これ以上のバグがあるとは考えにくいが、多くのパターンのポスト配置で試験を行っていないため本プログラムに対する検討が不十分であることもまた事実である。

バグ(or 欠点)の内容原因対処法
超音波センサが突発的に9999の値しか返さなくなることがあるこの現象はプログラムの試験中に発覚したもので、原因はいまのところ不明。ただし、超音波センサやFPGAボードなどの回路は正常動作していたので、超音波センサの計測タスクとの連携がうまく行われていないのではないかと思われる。また、どのMIRSでも発生する現象ではないようである有効な対処法は確立されていないが、プログラムを数回動かして発見したポストの個数が最も多いデータを使用する、というのが緊急回避策としてあげられる
回転にあまりに時間をかけるとセグメンテーション違反でプログラムが停止するデータを記録する構造体の要素数が100になっているためこの数を超えるデータを取ろうとすると発生する構造体の要素数を増やす、回転速度を上げる、など。回転時間は7〜8秒程度が良い
ポストに良く似た形状の物や、壁をポストとして認識してしまう長い壁やポストとよく似た形状の物は最初に定義した『ポストの定義』を満たしてしまうため高確率でこれをポストと認識してしまうポストの定義をもう少し厳しくすれば解消できるが、現在の超音波センサはあまり精度がよくないためポストを発見しにくくなってしまう。
また、本番の競技場では周囲にポスト以外のものがないためこのまま使用しても特に問題はないことが確認済みである

おわりに

標準プログラムpost_search()の弱点を克服する目的で開発したpost_search_t()だが、結果的に以下の性能を得ることができた。当初は想定していなかったような性能も、実際に試験を行うことにより確認できた。
ただし、これらの性能はMIRSにより若干異なる可能性がある。

  1. 同じポストを重複して発見することがない(試用期間中は一度も起こらなかった)
  2. パラメータの調節が不十分でMIRSの回転が均一でなくても結果に影響が少ない
  3. 引数と最終的な出力内容はpost_search()と全く同一なので移植が容易
  4. 遠方にあるポストも正確に検出可能。最大射程距離1.7〜2[m](機体差あり)
  5. MIRSから見て一部が重なって見えるポストでもある程度までは検出可能
  6. 精度がよい。標準関数であるpost_check()を行わなくても良いほどの高精度を実現(実証済み)

項目1〜3はプログラムを設計する時点で目標としたもので、4〜6については結果的に得られた利点である。特に、6については予想以上の精度を実現でき、これにより『ポスト確認』という工程を省略することが可能となった(本MIRSのプログラムでも省略している)。これは大変よい進歩だと思う。

構想1ヶ月、製造3日、改良4日、試験1週間というまだまだ未熟なプログラムで未知のバグもあるが、このプログラムを基板に、または参考にして、後輩がより良い探索プログラムを完成させてくれることを期待する。

関連文書