|
沼津高専 電子制御工学科 |
||||||
|
||||||
|
改訂記録 |
||||||
|
版数 |
作成日 |
作成者 |
承認 |
改訂内容 |
||
|
A01 |
2007.2.18 |
紅林 |
平田 |
初版 |
||
目次
1. はじめに
2. route1
3. route2
4. route3
5. route4
6. route5
7. bypass
8. 終わりに
1．はじめに
本ドキュメントは、MIRS0702詳細設計書に規定された最短ルート算出関数の動作を実現するプログラムの製造仕様を述べる。
構成は大きく５つに分かれ、1つ目は「スタート地点とポストを結ぶ直線の算出関数（route1）」、２つ目は「最短ルート算出関数（route2）」、３つ目は「現在の位置情報から次のルートを選択する関数（route3）」、４つ目は「得られたポスト情報が競技場内のものかどうかの識別関数（route4）」、５つ目は「例外処理時のルート処理関数（route5）」である。
また、本ドキュメント内では、簡単のためポストおよびMIRS本体は「点」と考えた。
2．route1
これはスタート地点から各ポストを結ぶ直線を求める関数である。
MIRS0702 技術調査書に記述するように、三角関数の正弦定理、余弦定理を用いる。
右図の数字はポスト番号である。
今、「1→4→2→5→3」の順番でポスト間を移動したとする。
・スタート地点とポスト１は移動した量がそのまま求める直線となる（直線A’とする）。
・スタート地点とポスト４を結ぶ直線は、余弦定理から得られる。
このとき、A’とB’のなす角も求めておく（φ１とする）。
・スタート地点とポスト２を結ぶ直線は、B’、C、ｃを用いるが、ｃの更新が必要である。
更新後は余弦定理で求めることができる。
・この後はC’を求めた方法を繰り返せば、すべての直線を得られる。
3．route2
route1で得られたA’、B’、C’、D’、E’を用いて、ポストを順番通りに移動するルートを算出する関数である。
今回も基本的に三角関数の正弦定理、余弦定理を用いる。
まず、route1で得られた情報をポスト番号の順番に入れ替える。
・ポスト1からポスト2までのルートは余弦定理から得られる。
A’とB’のなす角を正弦定理で求め、θ1とする。
・ポスト2からポスト3までのルートはφ3を更新して余弦定理から得られる。
route1とほぼ同様の理論で求められる。
4．route3
post_select（またはpost_select2）で関数コールされる。dis_select[]、ang_select[]に保存された情報が競技場内の物かどうかを選別し、競技場外の物ならばその情報を削除する関数である。
この関数は2つの引数を持つ。
route3(double rs_dis,double rs_ang)
また、返り値は、情報が真であれば“0”、偽であれば“9999”を返す。
理論はroute1の応用である。
１．1回のポスト探索で得られた複数のポスト情報に対し、スタート地点と各ポストを結ぶ直線を算出する。（route1）
２．直線は極座標系の量なので、ｘｙ直行座標系の量に変換する。
３．変換結果が競技場を定義する条件から外れたかどうかを判断して、結果を返り値として返す。
5．route4
現在の位置情報から次の移動量を選択する関数である。
選択される元情報はpost_select（またはpost_select2）で得られたポスト情報である（dis_select[]、ang_select[]に保存されている）。
スタート地点では、角度に関係なく距離がもっとも短いポストを選択する。
以降は、もっとも外側に在るポストに接近する。
つまり、最初に接近したポストが競技場の右半面に在った場合、周回角度がもっとも小さいポストを選択する。左半面の場合は、周回角度がもっとも大きいポストを選択する（ＭＩＲＳ0702の周回方向は反時計回りである）。
6．route5
この関数は例外動作の際のルート・動作を決定するものである。
構成は２つに分かれる。1つ目は、ポスト探索の結果、１つもポストを発見しなかった場合の動作（route5：case1）。2つ目は、選択したポスト情報が偽者（審判等）だった場合の動作（route5：case2）である。
route5：case1のアルゴリズムについて説明する。
ＭＩＲＳ0702はポストへ接近後、スタート地点と今いるポストを結ぶ直線を求めている（route1）。この直線と競技場中心の極座標から、今いるポストから競技場中心までの移動量を算出し、中心へ移動する。
中心へ移動後、その場回転を行い、回転盤による中心探索のためにＭＩＲＳの向きを整える。ただし、回転量と方向は場合による。例えば、競技場中心を原点O’とする直行座標x’y（route1等で考えている直交座標とは別のもの）を考えたとき、ＭＩＲＳが「スタート→第3象限→第4象限→原点（競技場中心）」と移動したとする。この場合、ポストが存在する可能性があるのは右半面であり、ＭＩＲＳが競技場右半面（x’軸の正の方向）を向いていたほうが探索・移動量計算が楽である。そのため回転量と方向は、この場合に適したものとなる。
その場回転後、回転盤による探索とポストの選別を行い、選択したポストに移動する。
ただし、route1に渡す情報は「ポスト間移動」したときの移動量なので、「例外動作を行う前にいたポスト」と「例外動作後に接近したポスト」を結ぶ直線を計算する必要がある。この仮想移動量の計算はbypass関数（bypass.c）で行う。
route5：case2のアルゴリズムについて説明する。
この関数はポスト探索が2回目以降に使用する。
ポスト周回時の探索でポストを複数発見したとする。
次に接近するポストを選択後、そのポストへ移動する前に正対補正を行う。このとき正対補正が成功すれば目標ポストへ移動する。正対補正が成功しなかった場合は、今のポスト情報を破棄し、探索時に得たポスト情報から次の目標ポストを検索する。
これを正対補正可能なポストが選択される、または探索時に得たポスト情報が無くなるまで繰り返す。
探索時に得たポスト情報が無くなった場合は、一度mainに戻った後、route5：case1をコールし競技場中心へ移動する。
7．bypass
post_approach後mainでコールされる。
動作内容は通常動作と例外動作（route5：case1実行時）で分かれる。
MIRS0702はポスト間移動後、「今移動した移動量（dis_now、ang_now）」と、「その前の移動量（dis_defore、ang_defore）」を保存、更新している。
通常動作時は更新を行う。
例外処理時は、「例外動作を行う前にいたポスト」と「例外動作後に接近したポスト」を結ぶ直線を計算し、更新を行う。
例外処理時は、dis_now、ang_nowには、ポストから競技場中心までの移動量、dis_defore、ang_deforeには競技場中心から次のポストまでの移動量が格納される。
dis_beforeとdis_now、これらのなす角を用いて、余弦定理から仮想直線の長さ、および正弦定理から仮想直線とdis_beforeのなす角を算出する。
これらを「今移動した移動量（dis_now、ang_now）」としてdis_now、ang_nowに上書きする。
8．終わりに
本ドキュメント内で説明した関数を含んだファイルroute_a.c、route_b.cを提供する。
route_a.cにはroute1、route2、route3が、route_b.cにはroute4、route5が含まれている
これらの関数は、スタート地点を原点O(0,0)として計算しているので、競技場の中心を原点とする場合などは改良が必要である。
また、route1はMIRSの移動がポスト間移動であることが前提で計算している。「ポスト→競技場中心→ポスト」という動作に適用できるかどうかは未調査（たぶん出来ない）。
|
関連文書 |